绘制lidar点云pose

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

#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 
//打印矩阵，临时用，就不讲究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::Ptr cloudAll(
    //     new pcl::PointCloud);
    // *cloudAll = *cloud_src + *cloud_tgt;
    // pcl::io::savePCDFileBinary(path_out+"All.pcd", *cloudAll);

    double icpMaxCorrespondenceDistance_=3;//100
    int icpMaximumIterations_=100;//100
    double icpTransformationEpsilon_=1e-10;
    double icpEuclideanFitnessEpsilon_=0.001;
    double icpFitnessScoreThresh_=0.3;//0.3
    Eigen::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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183

读取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 二维list
    datas = []
    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 np
    from 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 plt
    plt.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]  # 在每一行末尾添加0
    new_row = [0] * 4  # 创建全为0的行
    new_row[3] = 1  # 在最后一列添加1
    new_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)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101

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

import numpy as np
from scipy.spatial.transform import Rotation
import csv
import os
from datetime import datetime
import time

def 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.137
    e = 0.0818192
    k0 = 0.9996
    E0 = 500
    N0 = 0
    ret = [0,0]

    zoneNum = int(x / 6) + 31
    lamda0 = (zoneNum - 1) * 6 - 180 + 3
    lamda0 = lamda0 * np.pi / 180.0
    phi = y * np.pi / 180.0
    lamda = x * np.pi / 180.0
    v = 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 * 1000
    ret[1] = UTMN * 1000

    return 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 = 0
    char_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+1
    return 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 = 0
    for 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_time
                frac_lg = diff_gl/diff_time
                RPY[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, gps

csv_data_list=[]
# 读取csv文件并将gnss坐标系转换为墨卡托坐标系
def readcsv_convert(csv_file,file_dir):
    length_gps = len(open(csv_file).readlines())-1
    offset = 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 = 0

    with 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+1
    csv_file.close
    gps_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 np
    from 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 plt
    plt.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]  # 在每一行末尾添加0
    new_row = [0] * 4  # 创建全为0的行
    new_row[3] = 1  # 在最后一列添加1
    new_matrix.append(new_row)  # 添加到新的矩阵
    return new_matrix

#转换到lidar坐标系
def cvt2LidarPose(matrix1,imupose):
     # 提取位置信息
    x_in = []
    y_in = []
    import numpy as np
    from 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*matrix1
    sub_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*matrix1
        sub_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_in
    y=y_in
    return 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)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314