绘制lidar点云pose

输入点云序列，得到点云的运动轨迹，并生成周围点云拼接得到的点云地图

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>#include <iostream>
#include <pcl/registration/icp.h>
#include <pcl/registration/ndt.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
//打印矩阵，临时用，就不讲究float和double的通用了，也可以template
void printMat4(Eigen::Matrix4f mat)
{std::cout<<"lidar matrix is :"<<std::endl;for(int i=0;i<mat.rows();i++){for(int j=0;j<mat.cols();j++){std::cout<<mat(i,j)<<" , ";}std::cout<<std::endl;}
}
//保存轨迹，用的是变换矩阵的3*4子矩阵，按照行主序展开。用逗号做分隔。
void writePoseList(const std::vector<Eigen::Matrix4d>& matrices, const std::string& filename)
{std::ofstream file(filename);if (!file.is_open()){std::cout << "Failed to open file: " << filename << std::endl;return;}// 写入每个矩阵到文件for (size_t i = 0; i < matrices.size(); i++){const Eigen::Matrix4d& matrix = matrices[i];// 写入序号file << i + 1 << ",";// 写入矩阵元素,不写入最后一行for (int row = 0; row < 3; row++){for (int col = 0; col < 4; col++){file << matrix(row, col);// 在每个元素之间添加逗号，除了最后一个元素if (row != 2 || col != 3)file << ",";}}file << std::endl;}file.close();
}
//读取外参imu2lidar
Eigen::Matrix4d loadExtrinsic()
{Eigen::Matrix4d extrinsic;extrinsic << 0.70710678, 0.70710678, 0.0, -1.477,-0.70710678, 0.70710678, 0.0, -0.77,0.0, 0.0, 1.0, -0.66,0.0, 0.0, 0.0, 1.0;return extrinsic;
}
//读取imu的变换矩阵的3*4子矩阵，按照行主序展开。用 空格做分隔。得到一个imu的按照时间排序的pose-position信息（不是pp的增量，是相对起点坐标系的结果）
std::vector<Eigen::Matrix4d> loadIMU()
{std::vector<Eigen::Matrix4d> imu_pp;std::string filename = "/home/apollo/zr/code/imu_pose_0914_nn.txt";std::ifstream file(filename);if (!file.is_open()){std::cout << "Failed to open file: " << filename << std::endl;return imu_pp;}std::string line;while (std::getline(file, line)){Eigen::Matrix4d matrix = Eigen::Matrix4d::Identity();double value;std::stringstream ss(line);// 读取每行的值并赋给矩阵for (int i = 0; i < 13; i++){ss >> value;if(i==0)continue;matrix((i-1) / 4, (i-1) % 4) = value;}imu_pp.push_back(matrix);}file.close();return imu_pp;
}
//src到tgt的icp匹配，得到src点云到icp点云的变换矩阵
Eigen::Matrix4f icp_calib(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_src, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tgt)
{// pcl::PointCloud<pcl::PointXYZI>::Ptr cloudAll(//     new pcl::PointCloud<pcl::PointXYZI>);// *cloudAll = *cloud_src + *cloud_tgt;// pcl::io::savePCDFileBinary(path_out+"All.pcd", *cloudAll);double icpMaxCorrespondenceDistance_=3;//100int icpMaximumIterations_=100;//100double icpTransformationEpsilon_=1e-10;double icpEuclideanFitnessEpsilon_=0.001;double icpFitnessScoreThresh_=0.3;//0.3Eigen::Matrix4f transInit = Eigen::Matrix4f::Identity();pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;icp.setMaxCorrespondenceDistance(icpMaxCorrespondenceDistance_); icp.setMaximumIterations(icpMaximumIterations_);icp.setTransformationEpsilon(icpTransformationEpsilon_);icp.setEuclideanFitnessEpsilon(icpEuclideanFitnessEpsilon_);icp.setInputSource(cloud_src);icp.setInputTarget(cloud_tgt);pcl::PointCloud<pcl::PointXYZI>::Ptr transCurrentCloudInWorld(new pcl::PointCloud<pcl::PointXYZI>());icp.align(*transCurrentCloudInWorld,transInit.matrix());//迭代效果不理想，原因尚未定位到？？？Eigen::Matrix4f transWorldCurrent = Eigen::Matrix4f::Identity();if (icp.hasConverged() == false ) //|| icp.getFitnessScore() > icpFitnessScoreThresh_{std::cout << "ICP locolization failed    the score is   " << icp.getFitnessScore() << std::endl;return transWorldCurrent;} else {transWorldCurrent = icp.getFinalTransformation();// printMat4(transWorldCurrent);}return transWorldCurrent;
}
int main(int argc, char** argv)
{//***************load imu****************std::vector<Eigen::Matrix4d> imu_poses = loadIMU();//***************load extrinsic****************Eigen::Matrix4d extrinsic_mct2lidar;extrinsic_mct2lidar = loadExtrinsic();std::cout << "extrinsic_mct2lidar:\n" << extrinsic_mct2lidar << std::endl;Eigen::Matrix4d extrinsic_lidar2mct = extrinsic_mct2lidar.inverse();std::cout << "extrinsic_lidar2mct:\n" << extrinsic_lidar2mct << std::endl;std::string path = "/home/apollo/zr/code/genFeature0914/surf";Eigen::Matrix4d lidarPose = Eigen::Matrix4d::Identity();std::vector<Eigen::Matrix4d> lidarPoseList;pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_all(new pcl::PointCloud<pcl::PointXYZI>);for(int count=26;count<325;count++){//***************load lidar****************pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZI>);std::string pcd1 =path+std::to_string(count+1)+".pcd";std::cout << "pcd_file: " << count << "\n";if (pcl::io::loadPCDFile(pcd1, *cloud_src) < 0) {std::cout << "cannot open pcd_file: " << pcd1 << "\n";exit(1);}pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tgt(new pcl::PointCloud<pcl::PointXYZI>);std::string pcd2 =path+std::to_string(count)+".pcd";if (pcl::io::loadPCDFile(pcd2, *cloud_tgt) < 0) {std::cout << "cannot open pcd_file: " << pcd2 << "\n";exit(1);}//***************process icp****************Eigen::Matrix4f Rt_f = icp_calib(cloud_src,cloud_tgt);Eigen::Matrix4d Rt = Rt_f.cast<double>();lidarPose = lidarPose * Rt;lidarPoseList.push_back(lidarPose);pcl::PointCloud<pcl::PointXYZI>::Ptr currentFeatureCloudInWorld(new pcl::PointCloud<pcl::PointXYZI>);pcl::transformPointCloud (*cloud_src, *currentFeatureCloudInWorld, lidarPose);*cloud_all = *cloud_all+*currentFeatureCloudInWorld;}//保存lidar轨迹writePoseList(lidarPoseList, "/home/apollo/zr/code/pose_lidar0914.csv");//保存lidar建图pcl::io::savePCDFileBinary("/home/apollo/zr/code/lidar0914map.pcd", *cloud_all);return 0;
}

读取csv文件绘制行主序展开3*4位姿图

import numpy as np
from scipy.spatial.transform import Rotation
import csv
import os
from datetime import datetime
import time
#读取csv文件
def readcsv_convert(csv_file):#得到datas 二维listdatas = []with open(csv_file, 'r') as file:for line in file:line = line.strip().split(',')datas.append(list(map(float, line[1:])))#得到csv_data_list 一维list，每个元素都是矩阵形式# csv_data_list=[]# for data in datas:#     # 将列表转为 3x4 的矩阵#     matrix = np.reshape(data, (3, 4))#     # 创建一个 4x4 的旋转平移矩阵#     transform_matrix = np.identity(4)#     # 将矩阵的前三行赋值为输入的矩阵，最后一行为 [0, 0, 0, 1]#     transform_matrix[:3, :] = matrix#     transform_matrix[3, :] = [0, 0, 0, 1]#     # print(transform_matrix)#     csv_data_list.append(transform_matrix)return datas#保存二维list
def data_to_txt(txt_file,data_list):with open(txt_file, 'w') as txt_file:for item in data_list:# txt_file.write(str(item) + '\n')line = ','.join(map(str, item))txt_file.write(line + '\n')
#保存x,y,theta的pose-position增量或者pose-position
# def data_to_txt_plane(txt_file,x,y,theta):
#     with open(txt_file, 'w') as txt_file:
#         for i in range(len(x)):
#             txt_file.write(f'{x[i]}, {y[i]}, {theta[i]}\n')#从变换矩阵里提取平面运动信息：X，Y，Theta
def redrawMatrix2XYTheta(data):# 提取位置信息x_in = [row[3] for row in data]y_in = [row[7] for row in data]import numpy as npfrom scipy.spatial.transform import Rotation# 转换旋转矩阵为欧拉角euler_angles_list = []for row in data:# 提取相关数据rotation_matrix = np.array(row).reshape(3, 4)sub_matrix = rotation_matrix[:, :3]# 创建Rotation对象rotation = Rotation.from_matrix(sub_matrix)# 将旋转矩阵转换为欧拉角euler_angles = rotation.as_euler('xyz', degrees=True)euler_angles_list.append(euler_angles)theta_in = [row[2] for row in euler_angles_list]return x_in,y_in,theta_in
#绘制平面运动信息，输入为x，y，theta的全局值，不是增量值
def draw(x,y,theta,lenOfDirection):import matplotlib.pyplot as pltplt.plot(x, y, 'bo')# 绘制点的方向for xi, yi, ti in zip(x, y, theta):dx = lenOfDirection * np.sin(np.deg2rad(ti))dy = lenOfDirection * np.cos(np.deg2rad(ti))plt.arrow(xi, yi, dx, dy, head_width=0.2, color='red')# 设置图形坐标轴范围# plt.xlim(min(x) -1, max(x) + 1)# plt.ylim(min(y) -1, max(y) + 1)plt.xlim(-15, 15)plt.ylim(-45, 5)# 显示图形plt.show()#齐次化函数 ：3*4->4*4
def homogenize_matrix(matrix):new_matrix = [row + [0] for row in matrix]  # 在每一行末尾添加0new_row = [0] * 4  # 创建全为0的行new_row[3] = 1  # 在最后一列添加1new_matrix.append(new_row)  # 添加到新的矩阵return new_matrix# # 示例用法
csv_file = 'pose_lidar0914.csv'  # 输入的 CSV 文件名
csv_data_list = readcsv_convert(csv_file)
# x_mct,y_mct,theta_mct = redrawMatrix2XYTheta([row[1:] for row in csv_data_list])
x_mct,y_mct,theta_mct = redrawMatrix2XYTheta(csv_data_list)
# drawGPS(RPY_n, gps_n)
draw(x_mct,y_mct,theta_mct,0.9)
# data_to_txt_plane("mct_pose.csv",x_mct,y_mct,theta_mct)

#rpy转旋转矩阵输出csv文件

import numpy as np
from scipy.spatial.transform import Rotation
import csv
import os
from datetime import datetime
import timedef euler_to_rotation_matrix(roll, pitch, yaw):# 将角度转换为弧度roll_rad = np.deg2rad(roll)pitch_rad = np.deg2rad(pitch)yaw_rad = np.deg2rad(yaw)# 输入已经是弧度# roll_rad = roll# pitch_rad = pitch# yaw_rad = yaw# 创建旋转对象并进行转换r = Rotation.from_euler('xyz', [roll_rad, pitch_rad, yaw_rad], degrees=False)rotation_matrix = r.as_matrix()# 验证 ： 使用 numpy.linalg 模块中的函数将旋转矩阵转换为欧拉角# euler_angles = np.degrees(np.around(np.array([#     np.arctan2(rotation_matrix[2, 1], rotation_matrix[2, 2]),#     np.arctan2(-rotation_matrix[2, 0], np.sqrt(rotation_matrix[2, 1]**2 + rotation_matrix[2, 2]**2)),#     np.arctan2(rotation_matrix[1, 0], rotation_matrix[0, 0])# ]), decimals=2))return rotation_matrix#GPS转墨卡托，注意此时坐标系就是墨卡托系了，既不是imu也不是gnss了
def gps_to_Mercator(x,y):a = 6378.137e = 0.0818192k0 = 0.9996E0 = 500N0 = 0ret = [0,0]zoneNum = int(x / 6) + 31lamda0 = (zoneNum - 1) * 6 - 180 + 3lamda0 = lamda0 * np.pi / 180.0phi = y * np.pi / 180.0lamda = x * np.pi / 180.0v = 1.0 / np.sqrt(1 - np.power(e,2) * np.power(np.sin(phi),2))A = (lamda - lamda0) * np.cos(phi)T = np.tan(phi) * np.tan(phi)C = np.power(e,2) * np.cos(phi) * np.cos(phi) / (1 - np.power(e,2))s = (1 - np.power(e,2) / 4.0 - 3 * np.power(e,4) / 64.0 - 5 * np.power(e,6) / 256.0)* phi \- (3 * np.power(e,2) / 8.0 + 3 * np.power(e,4) / 32.0 + 45 * np.power(e,6) / 1024.0) * np.sin(2 * phi) \+ (15 * np.power(e,4) / 256.0 + 45 * np.power(e,6) / 1024.0) * np.sin(4 * phi) \- 35 * np.power(e,6) / 3072.0 * np.sin(6 * phi)UTME = E0 + k0 * a * v * (A + (1 - T + C) * np.power(A,3) / 6 + (5 - 18 * T + np.power(T,2)) * np.power(A,5) / 120)UTMN = N0 + k0 * a * (s + v * np.tan(phi) * (np.power(A,2) / 2 + (5 - T + 9 * C + 4 * np.power(C,2)) * np.power(A,4) \/ 24  + (61 - 58 * T + np.power(T,2))* np.power(A,6) / 720.0))ret[0] = UTME * 1000ret[1] = UTMN * 1000return ret#读取lidar数据的时间戳，从实际时间格式转unix格式
def read_lidar_timestamp(file_dir):files = os.listdir(file_dir)#files.sort(key=lambda x: int(x.replace("lidar6611_","").split('_')[0]))files.sort(key=lambda x: int(x.replace("lidar6600_","")[0]))Length_lidar = len(files)lidar_stamp = np.zeros((Length_lidar))i = 0char_fix = '_'list_lidar_time = []for Num in files:temp = Num.split(char_fix)[-1]list_lidar_time.append(temp.replace(".pcd",""))#使用 datetime.strptime 函数将时间字符串 list_lidar_time[i] 转换为 datetime 对象。strptime 函数使用给定的格式字符串 "%Y-%m-%d-%H-%M-%S.%f" 解析时间字符串，将其转换为 datetime 对象，并将结果赋给 time_lidar 变量。time_lidar = datetime.strptime(list_lidar_time[i],"%Y-%m-%d-%H-%M-%S.%f")#将 datetime 对象 time_lidar 转换为时间戳。首先，time_lidar.timetuple() 函数返回 time.struct_time 对象，表示 datetime 对象的日期和时间部分。然后，time.mktime 函数将 time.struct_time 对象转换为秒数的时间戳。接下来，将时间戳乘以 1000.0，然后加上微秒部分的毫秒表示 time_lidar.microsecond / 1000.0。最后，将结果转换为整数，并将其赋值给 lidar_stamp[i] 变量。lidar_stamp[i] = int(time.mktime(time_lidar.timetuple()) * 1000.0 + time_lidar.microsecond / 1000.0)i = i+1return lidar_stamp#读取imu-gnss数据的时间戳，从实际时间格式转unix格式
def read_gps_timestamp(list_gpstime):gps_stamp = np.zeros((len(list_gpstime)))for k in range(0,len(list_gpstime)):   time_gps = datetime.strptime(list_gpstime[k],"%Y/%m/%d %H:%M:%S:%f")      gps_stamp[k] = int(time.mktime(time_gps.timetuple()) * 1000.0 + time_gps.microsecond / 1000.0)return gps_stamp#插值
def interpolation(lidar_stamp, gps_stamp, RPY_o, gps_o):length_lidar = len(lidar_stamp)length_gps = len(gps_stamp)RPY = np.zeros((3,length_lidar))gps = np.zeros((3,length_lidar))j = 0for m in range(0, length_lidar):for j in range(j,length_gps):if lidar_stamp[m] <= gps_stamp[j]:diff_time = gps_stamp[j]-gps_stamp[j-1]diff_lg = abs(lidar_stamp[m] - gps_stamp[j])diff_gl = abs(gps_stamp[j-1] - lidar_stamp[m])frac_gl = diff_lg/diff_timefrac_lg = diff_gl/diff_timeRPY[0,m] = frac_gl*RPY_o[0,j-1]+frac_lg*RPY_o[0,j]RPY[1,m] = frac_gl*RPY_o[1,j-1]+frac_lg*RPY_o[1,j]RPY[2,m] = frac_gl*RPY_o[2,j-1]+frac_lg*RPY_o[2,j]gps[0,m] = frac_gl*gps_o[0,j-1]+frac_lg*gps_o[0,j]gps[1,m] = frac_gl*gps_o[1,j-1]+frac_lg*gps_o[1,j]gps[2,m] = frac_gl*gps_o[2,j-1]+frac_lg*gps_o[2,j] break   return RPY, gpscsv_data_list=[]
# 读取csv文件并将gnss坐标系转换为墨卡托坐标系
def readcsv_convert(csv_file,file_dir):length_gps = len(open(csv_file).readlines())-1offset = np.zeros((3,length_gps))ret_c = np.zeros((3,length_gps))gps_o = np.zeros((3,length_gps))RPY_o = np.zeros((3,length_gps))list_gpstime = []n = 0with open(csv_file, 'r') as csv_file:reader = csv.reader(csv_file)next(reader)for row in reader:  list_gpstime.append(row[0])       gps_o[0,n] = row[2]gps_o[1,n] = row[1]gps_o[2,n] = row[3]RPY_o[0,n] = row[4]RPY_o[1,n] = row[5]RPY_o[2,n] = row[6]n = n+1csv_file.closegps_stamp = read_gps_timestamp(list_gpstime)lidar_stamp = read_lidar_timestamp(file_dir)[RPY_n, gps_n] = interpolation(lidar_stamp, gps_stamp, RPY_o, gps_o)for p in range(0,RPY_n.shape[1]):rotation_matrix = euler_to_rotation_matrix(RPY_n[0,p], RPY_n[1,p], RPY_n[2,p])ret = gps_to_Mercator(gps_n[0,p], gps_n[1,p])ret_c[0,p] = ret[0]ret_c[1,p] = ret[1]ret_c[2,p] = gps_n[2,p]flattened_matrix = rotation_matrix.flatten(order='C').tolist() #行主序if(p!=0):offset[0,p] = ret_c[0,p]-ret_c[0,0]offset[1,p] = ret_c[1,p]-ret_c[1,0]offset[2,p] = ret_c[2,p]-ret_c[2,0] csv_data_list.append([lidar_stamp[p],flattened_matrix[0],flattened_matrix[1],flattened_matrix[2], offset[0,p],flattened_matrix[3],flattened_matrix[4],flattened_matrix[5],offset[1,p],flattened_matrix[6],flattened_matrix[7],flattened_matrix[8],offset[2,p]])return RPY_n, gps_n,csv_data_list#保存pp矩阵
def data_to_txt(txt_file,data_list):with open(txt_file, 'w') as txt_file:for item in data_list:# txt_file.write(str(item) + '\n')line = ','.join(map(str, item))txt_file.write(line + '\n')
# 保存x,y,theta
def data_to_txt_plane(txt_file,x,y,theta):with open(txt_file, 'w') as txt_file:for i in range(len(x)):txt_file.write(f'{x[i]}, {y[i]}, {theta[i]}\n')# 从pp矩阵中提取x,y,theta
def redrawMCT(data):# 提取位置信息x_in = [row[3] for row in data]y_in = [row[7] for row in data]import numpy as npfrom scipy.spatial.transform import Rotation# 转换旋转矩阵为欧拉角euler_angles_list = []for row in data:# 提取相关数据rotation_matrix = np.array(row).reshape(3, 4)sub_matrix = rotation_matrix[:, :3]# 创建Rotation对象rotation = Rotation.from_matrix(sub_matrix)# 将旋转矩阵转换为欧拉角euler_angles = rotation.as_euler('xyz', degrees=True)euler_angles_list.append(euler_angles)theta_in = [row[2] for row in euler_angles_list]return x_in,y_in,theta_in# 绘制gps
def drawGPS(RPY_n, gps_n):x=[ele*1e7-1187744343.4 for ele in gps_n[0]]y=[ele*1e7-319735719.5 for ele in gps_n[1]]theta=[ele for ele in RPY_n[2]]print(x[0])print(y[0])draw(x,y,theta,50)#绘制坐标和方向
def draw(x,y,theta,lenOfDirection):import matplotlib.pyplot as pltplt.plot(x, y, 'bo')# 绘制点的方向for xi, yi, ti in zip(x, y, theta):dx = lenOfDirection * np.sin(np.deg2rad(ti))dy = lenOfDirection * np.cos(np.deg2rad(ti))plt.arrow(xi, yi, dx, dy, head_width=0.2, color='red')# 设置图形坐标轴范围plt.xlim(min(x) -1, max(x) + 1)plt.ylim(min(y) -1, max(y) + 1)# plt.xlim(min(x) -1, max(x) + 1)# plt.ylim(min(x) -1, max(x) + 1)# 显示图形plt.show()# 齐次化
def homogenize_matrix(matrix):new_matrix = [row + [0] for row in matrix]  # 在每一行末尾添加0new_row = [0] * 4  # 创建全为0的行new_row[3] = 1  # 在最后一列添加1new_matrix.append(new_row)  # 添加到新的矩阵return new_matrix#转换到lidar坐标系
def cvt2LidarPose(matrix1,imupose):# 提取位置信息x_in = []y_in = []import numpy as npfrom scipy.spatial.transform import Rotation# 转换旋转矩阵为欧拉角euler_angles_list = []rotation_matrix = np.array(imupose[0]).reshape(3, 4)imupose_homo = homogenize_matrix(rotation_matrix)print(matrix1)print(imupose_homo)lidar_pose1 = np.dot(matrix1, imupose_homo)lidar_pose = np.dot(rotation_matrix, matrix1)# lidar_pose = rotation_matrix*matrix1sub_matrix = lidar_pose[:3, :3]# print('lidar_pose:')# print(lidar_pose)# print('lidar_pose1:')# print(lidar_pose1)# print(sub_matrix)# rotation = Rotation.from_matrix(sub_matrix)# euler_angles = rotation.as_euler('xyz', degrees=True)# print(euler_angles)for row in imupose:# 提取相关数据rotation_matrix = np.array(row).reshape(3, 4)lidar_pose = np.dot(rotation_matrix, matrix1)# lidar_pose = rotation_matrix*matrix1sub_matrix = lidar_pose[:3, :3]# print(rotation_matrix)# print(sub_matrix)# 创建Rotation对象rotation = Rotation.from_matrix(sub_matrix)# 将旋转矩阵转换为欧拉角euler_angles = rotation.as_euler('xyz', degrees=True)euler_angles_list.append(euler_angles)x_in.append(lidar_pose[0,3])y_in.append(lidar_pose[1,3])theta = [row[2] for row in euler_angles_list]x=x_iny=y_inreturn x,y,theta# # 示例用法
csv_file = 'lidar_imu_0914.csv'  # 输入的 CSV 文件名
file_dir = "lidar_0914"
txt_file = 'imu_pose_0914.csv'  # 输出的 TXT 文件名
RPY_n, gps_n,csv_data_list = readcsv_convert(csv_file, file_dir)
data_to_txt(txt_file,csv_data_list)
x_mct,y_mct,theta_mct = redrawMCT([row[1:] for row in csv_data_list])
# drawGPS(RPY_n, gps_n)
draw(x_mct,y_mct,theta_mct,0.5)
data_to_txt_plane("mct_pose.csv",x_mct,y_mct,theta_mct)import math
# 将角度转换为弧度
angle_rad = math.radians(45)
# 计算正弦值
sin45 = math.sin(angle_rad)
cos45 = math.cos(angle_rad)
# imu2lidar
# matrix2 = np.array([[cos45, sin45, 0, -1.477], [sin45, -cos45, 0, -0.77],[0, 0, -1, -0.66],[0, 0, 0, 1]])
# mct2lidar  注意imupose已经转到MCT下了，用的mct的pose，所以只需要从mct转到lidar下的外参
matrix2 = np.array([[cos45, sin45, 0, -1.477], [-sin45, cos45, 0, -0.77],[0, 0, 1, -0.66],[0, 0, 0, 1]])
matrix1 = np.linalg.inv(matrix2)
print("imu2lidar:")
print(matrix2)
print("lidar2imu:")
print(matrix1)
x_lidar,y_lidar,theta_lidar = cvt2LidarPose(matrix1,[row[1:] for row in csv_data_list])
draw(x_lidar,y_lidar,theta_lidar,0.5)
data_to_txt_plane("lidar_pose.csv",x_lidar,y_lidar,theta_lidar)