激光雷达Lidar和RTK的标定1----录制RTK和Lidar bag包以及数据预处理

录制RTK和Lidar bag包以及数据预处理

---

开个新坑，lidar和RTK的标定一直是机器人和自动驾驶中的一个重要问题，由于直接测量很难获得准确的RTK和雷达之间的位姿关系，这里我们探索下如何在matlab下，使用RTK数据和激光雷达点云数据对RTK和激光雷达的位姿关系进行求解（优化）。

这里我们开个新坑，大致会分成几个部分进行，包括

1.点云数据和RTK数据在matlab下的预处理

2.nlopt在windows下的安装和对应matlab环境搭建

3.优化算法设计和实现

注意：这里的目的主要是从算法原理和数据验证上找到最适合标定的方法，所以会在matlab上进行开发，同时采集数据时雷达和RTK运动要有丰富的姿态变化，速度不要太快（否则不同传感器的时间误差将产生较大影响），这种情况下可以假设雷达每帧（scan）是瞬时产生，不考虑旋转的时间误差。

---

硬件部分

1.velodyne VLP32激光雷达
2.RTK/INS组合导航
3.工控机（电脑）

软件部分

ros-melodic
matlab
nlopt优化库

---

接下来是bag包录制，这里默认需要的人有一定的基础，不会详细说每个步骤

1.bag包录制

既然是标定组合导航RTK和Lidar之间的位姿转换关系，那么录制相应包即可，流程如下
1.使用ros读取组合导航RTK位置和姿态数据，进行坐标转换（局部坐标系或UTM坐标系等均可）；
2.RTK数据按照ROS tf格式转换（transform格式）；
3.ros下启动雷达（驱动已安装）和RTK驱动/转换；
4.录制bag包（velodyne激光雷达点云的topic和RTK发布的odom transform对应topic）

2.数据解析

获得bag包后，例如091602.bag，在matlab下进行解析。

1.首先是提取雷达的点云数据和RTK odom数据（transform）

 bag=rosbag('091602.bag');

point_cloud = select(bag,'Time',[bag.StartTime bag.EndTime],'Topic','/velodyne_points');
cgi610_path = select(bag,'Time',[bag.StartTime bag.EndTime],'Topic','/imu_data');

2.提取rtk数据

for i=1:N
    
    p = readMessages(cgi610_path,i);
    %提取时间戳
    t0=p{1, 1}.Header.Stamp.Sec;
    t1=p{1, 1}.Header.Stamp.Nsec;
    time610(i) = t0*10e8+t1;

    %提取位置
    x(i)= p{1, 1}.Transform.Translation.X;
    y(i)= p{1, 1}.Transform.Translation.Y;
    z(i)= p{1, 1}.Transform.Translation.Z;
    %提取四元数
    q2=-p{1, 1}.Transform.Rotation.X;
    q3=-p{1, 1}.Transform.Rotation.Y;
    q1=p{1, 1}.Transform.Rotation.Z;
    q0=p{1, 1}.Transform.Rotation.W;
    % q0 q1 q2 q3 --->  w x y z
    % 四元数转换欧拉角 roll pitch yaw
    yaw0(i) = atan2(2*(q0*q1+q2*q3),1 - 2*(q1^2+q2^2));
    roll0(i) = -asin(2*(q0*q2 - q1*q3));
    pitch0(i) = -atan2(2*(q0*q3+q1*q2),1 - 2*(q2^2+q3^2));
       
end

3.提取雷达数据

for i=1:M
    pp = readMessages(point_cloud,i);
    pp=pp{1, 1};
    %提取雷达时间戳
    t0=pp.Header.Stamp.Sec;
    t1=pp.Header.Stamp.Nsec;
    time(i) = t0*10e8+t1;
    %直接提取雷达点云信息
    xyz{i} =readXYZ(pp);  
end

提取到的信息将进行下一步处理

ps：
完整代码

close all
clear
clc

an=180/pi;
bag=rosbag('091602.bag');

point_cloud = select(bag,'Time',[bag.StartTime bag.EndTime],'Topic','/velodyne_points');
cgi610_path = select(bag,'Time',[bag.StartTime bag.EndTime],'Topic','/imu_data');

N = cgi610_path.NumMessages;
M = point_cloud.NumMessages;

for i=1:M
    
    pp = readMessages(point_cloud,i);
    pp=pp{1, 1};
  
    t0=pp.Header.Stamp.Sec;
    t1=pp.Header.Stamp.Nsec;
    time(i) = t0*10e8+t1;
    %      cloud_data= pp.Data;
    %      cloudSize=length(cloud_data);

    xyz{i} =readXYZ(pp);  
    
    uu=0;
end

for i=1:N
    
    p = readMessages(cgi610_path,i);  
    t0=p{1, 1}.Header.Stamp.Sec;
    t1=p{1, 1}.Header.Stamp.Nsec;
    time610(i) = t0*10e8+t1;

    x(i)= p{1, 1}.Transform.Translation.X;
    y(i)= p{1, 1}.Transform.Translation.Y;
    z(i)= p{1, 1}.Transform.Translation.Z;

    q2=-p{1, 1}.Transform.Rotation.X;
    q3=-p{1, 1}.Transform.Rotation.Y;
    q1=p{1, 1}.Transform.Rotation.Z;
    q0=p{1, 1}.Transform.Rotation.W;
    
    % q0 q1 q2 q3 --->  w x y z
    %  roll pitch yaw
    
    yaw0(i) = atan2(2*(q0*q1+q2*q3),1 - 2*(q1^2+q2^2));
    roll0(i) = -asin(2*(q0*q2 - q1*q3));
    pitch0(i) = -atan2(2*(q0*q3+q1*q2),1 - 2*(q2^2+q3^2));
       
end


for i=1:M
    
    for j=1:1:N
        err(j)=abs(time610(j)-time(i));
    end
    
    [~,b]=min(err);
    
    px(i)=x(b);
    py(i)=y(b);
    pz(i)=z(b);
    

    roll(i)=roll0(b);
    pitch(i)=pitch0(b);
    yaw(i)=yaw0(b);
      
end

save pt_path091602.mat  xyz  time roll pitch yaw px py pz

figure

plot3(px,py,pz,'k.')
hold on
axis equal

figure
plot(R*an,'.')
hold on
plot(P*an,'.')
hold on
plot(Y*an,'.')
hold on
legend('roll','pitch','yaw')
title('610 path')


figure
plot(roll*an,'.')
hold on
plot(pitch*an,'.')
hold on
plot(yaw*an,'.')
hold on
legend('roll','pitch','yaw')