Lidar和Imu 标定和重定位

雨幕丶

46人浏览 · 2025-06-02 23:24:41

雨幕丶 · 2025-06-02 23:24:41 发布

1.标定

目前开源的lidar和imu标定算法都是基于ROS1的，最近需要标定robosense_helios和imu，通过rosbags将ROS2数据包转为ROS1的bag。

标定前需要确定lidar和imu的时间戳是否一致，robosense支持使用系统时间(config中修改)，imu也使用系统时间；
点云数据类型是XYZIRT（Cmakelists中修改）

1.1 lidar_align

这个算法是标定里程计和lidar之间的外参的，有博主在此基础上对里程计的回调函数进行修改，参考[1]，可以从bag中读取 "sensor_msgs/Imu"类型的IMU数据。

实际使用过程发现标定结果和预估的相差很大（两者的坐标系相同，因此旋转应该接近单位矩阵，平移可以拿尺子量），后来这个方法就先弃用了。

1.2 港大 LI-Init

最初运行 robosense.launch 时，出现了“缺少强度字段”的警告。排查发现 robosense_ros 中的 Intensity 类型是 std::uint8_t，而 LiDAR 驱动使用的是 float。将两处类型统一后，警告虽消失，但程序卡在数据激励阶段。后来参考[2]的做法，将 Helios 点云转换为 Velodyne 格式，并使用 LI-Init 的 velodyne.launch 启动，系统能跑起来。然而，标定结果和建图效果并不理想。经过多次调整 config 和 launch 文件中的参数，地图基本正常，但平移仍存在偏差，尤其是 Z 轴——实测误差约 8 cm，而标定值为 14 cm。

1.3 浙大lidar_IMU_calib

源码只支持VLP-16，有博主适配了RS_Helios5515 [3] ，正常运行没有问题，唯一的小问题是平移z轴存在1cm的误差，实际应该是0.86cm

 0.9999799, - 0.0020486,  0.0060094,  0.0275791;
 0.0020832,   0.9999813, -0.0057559,  0.0149441;
-0.0059975,  0.0057683,   0.9999654, -0.0947816

2.重定位

2.1 S-FAST_LIO

FAST_LIO的简化版本，状态量不再使用复杂的流行，而是Sophus表示。扩展了速腾雷达的使用以及重定位。

初始位姿通过config文件提供，然后赋值给状态量state_point；

然后是读取先验全局地图和ikd_tree地图，重定位通过ikd_tree匹配实现的，个人理解是基于滤波的方法需要提供较为准确的初始值，实际使用需要将初始位姿改为rviz提供。

2.2 liorf_localization

LIO-SAM的扩展版本，订阅初始位姿话题 ( RViz界面通过“2D Pose Estimate”按钮发布的初始位姿），回调函数保存initialize_pose[]数据；

载入先验地图loadGlobalMap()；

通过ICP计算当前帧点云和全局点云的变换 systemInitialize()

注：目前还没测试这个方法，只是看了下代码实现重定位的思路

梳理LIO-SAM和FAST_LIO的扩展

FAST_LIO_SLAM：在FAST-LIO2的基础上，添加SC-PGO(Loop detection and Pose-graph Optimization)模块，通过加入ScanContext全局描述子，进行回环修正(SC-PGO模块与FAST-LIO2解耦)。

FAST_LIO_LC：在FAST_LIO_SLAM的基础上添加了：

基于Radius Search 基于欧式距离的回环检测搜索，增加回环搜索的鲁棒性；
回环检测的优化结果，更新到FAST-LIO2的当前帧位姿中，幷进行ikdtree的重构，进而更新submap。

FAST_LIO_SAM：

将LIO-SAM的后端GTSAM优化部分移植到FAST-LIO2的代码中，数据传输处理环节更加清晰。
增加关键帧的保存，可通过rosservice的指令对地图和轨迹进行保存。
FAST_LIO_SLAM中的后端优化，只使用了GPS的高层进行约束，GPS的高层一般噪声比较大，所以添加GPS的XYZ三维的postion进行GPS先验因子约束。

SC-LIO-SAM：Scan Context + LIO-SAM

2.3 LIO-SAM_based_relocalization

基于LIO-SAM实现的轻量级地图重定位系统，本质上和liorf_localization没太大区别，只是使用NDT的输出作为ICP初值，得到更精准的变换。

2.4 SC-LIO-SAM_based_relocalization

在 LIO-SAM_based_relocalization 中新增 Scan Context 检测模块，可在获取当前时刻数据后自动确定其在已有地图中的初始位姿，无需人工提供初值。该方法需要搭配SC-LIO-SAM建图使用。

也就是先运行SC-LIO-SAM得到PCD以及SCDs地图，然后再运行SC-LIO-SAM_based_relocalization实现重定位。

2.4.1 SC-LIO-SAM 相比LIO-SAM修改的代码

具体参考《回环检测 Scan Contex 及扩展》3.1节内容

2.4.2 SC-LIO-SAM_based_relocalization

1.构造函数

订阅 /initialpose话题，回调函数获取初始位姿数据；
载入SC_LIO-SAM保存的关键帧scan context描述子文件SCDs；
载入关键帧点云Scans；
载入关键帧位姿optimized_poses.txt

2.点云回调函数

新增了关于当前帧和回环帧通过ICP计算当前帧初始位姿（T_map_lidar(cur_frame)）的方法。如果这个初始位姿计算比较准确，重定位效果很好。(scan to scan)

3.线程globalLocalizeThread

通过performSCLoopClosurel()或者Rviz 提供了初始位姿后，先执行ICPLocalizeInitialize()函数，函数内部根据初始位姿对当前帧和全局地图ICP（ICP 收敛且 fitness score 合格认为成功，否则是失败）: (scan to map)

ICP失败，执行死循环，提示“Offer A New Guess Please”，意思就是初始位姿有问题，这个时候只能通过rviz再次提供位姿；
ICP成功认为initializedFlag = Initialized ，执行ICPscanMatchGlobal()，重定位成功后，最新的关键帧和map进行匹配，计算的是T_map_odom

4.总结

①重定位测试方法：

指定任意一秒数据测试重定位

// ros1
rosbag play  xxxx.bag --start xx           

// ros2
ros2 bag play xxx  --start-offset xx

②三个重要的坐标系

map系：以构建地图的第一帧为基准；
lidar_odom系：以重定位帧为基准；
lidar系：以lidar实时位置为基准。

2.4.3 代码修改思路

录制的bag是三百秒左右，指定bag的起始时间在200s之前，都可以自动找到回环帧，然后完成重定位；但起始时间指定200s之后，找不到回环帧，重定位失败。

调试思路：首先确定，在重定位失败的那一秒之前几秒，重定位成功找到的回环帧是哪一帧，

202 成功
[Loop found] Nearest distance: 0.249986 btn 59 and 29.

203 成功
min_dist: 0.300107 , SC_DIST_THRES: 0.4
[Loop found] Nearest distance: 0.300107 btn 59 and 29.

204 失败
min_dist: 0.258476 , SC_DIST_THRES: 0.4
[Loop found] Nearest distance: 0.258476 btn 59 and 5.
[Loop found] yaw diff: 24 deg.
SC loop found! between 59 and 5.
ICP fitness test failed (33.274 > 0.1). Reject this SC loop.

205 失败
min_dist: 0.273431 , SC_DIST_THRES: 0.4
[Loop found] Nearest distance: 0.273431 btn 59 and 5.
[Loop found] yaw diff: 24 deg.
SC loop found! between 59 and 5.
ICP fitness test failed (38.0234 > 0.1). Reject this SC loop.

206 失败
[Loop found] Nearest distance: 0.299515 btn 59 and 5.
[Loop found] yaw diff: 18 deg.
SC loop found! between 59 and 5.
ICP fitness test failed (30.0051 > 0.1). Reject this SC loop.

很明显，重定位成功找到的回环帧是第29帧，重定位失败找到的回环帧是第5帧，然后导致后续ICP计算的fitness很大，最终导致重定位失败。

修改：

首先根据自己情况修改 SC_DIST_THRES阈值，distanceBtnScanContext() 函数会计算两个scan context之间的距离，值越小表示两个 Scan Context 越相似，计算的距离会和SC_DIST_THRES比较，大于阈值认为该帧不作为回环帧；

2. performSCLoopClosure() 最终计算当前帧(重定位帧)的位姿 T_map_lidar(curframe)

其次是detectLoopClosureID() 只会返回一个距离最小（最相似）的回环帧索引，然后performSCLoopClosure()函数中获取回环帧的索引，进而得到回环帧的位姿，回环帧的位姿粗略的认为是当前帧的位姿，把当前帧点云变换到地图系中，作为ICP的源点云；ICP的目标点云是回环帧以及前后25个关键帧组成的点云，两个点云进行ICP计算得分FitnessScore。（注意，此时的ICP不要带初值（踩坑））

按照作者原来的逻辑如果ICP没有收敛或者得分比较大，就直接return了，也就是用scan context这个方法不能提供一个合理的初值，transformInTheWorld[]数组的值为0，ICPLocalizeInitialize()判断该数组为0也是return，导致程序处于等待状态，这个时候需要rviz提供一个初值（也是读完线程代码才理解的）我觉得得把ICP没有收敛或者大于阈值中的return注释掉，也就是scan context这个方法提供的初值不合理也继续往下执行，ICPLocalizeInitialize()函数把initializedFlag标志为设置为Initializing，这个时候终端提示“Offer A New Guess Please”，才合理。

注：刚刚只是把代码变得合理一些，但是重定位失败的问题还是没有解决

以我bag的206s为例，人为的设置这一帧的回环帧是第29帧（因为前面重定位成功的回环帧就是第29帧），发现是可以重定位成功的，说明回环帧找的有问题（实际找的是第5帧），在detectLoopClosureID()把十个候选回环帧的信息都打印出来是可以找到第29帧的，因此修改这部分代码，把十个候选回环帧的索引作为函数返回值，performSCLoopClosure() 获取到候选回环帧后依次和当前帧进行ICP，最终选择FitnessScore最小的作为最终的回环帧，按照这个思路修改后，206~210s这几帧重定位成功。

注：scan context找到最相似的回环帧进行ICP得到的fitnessscore并不一定是最小的。

但是在211s 发现新的问题，所有的候选回环帧和当前帧ICP的到FitnessScore分数都很大（当时忘记截图记录了，下面的日志其实不是211s这个时间的，但反馈差不多）。

[Testing SC candidate] loop_idx=3
    [ICP] loop_idx=3 converged=1 fitness=15.0104
      [Rejected by ICP] loop_idx=3, fitness=15.0104 >= threshold(0.1)
  [Testing SC candidate] loop_idx=5
    [ICP] loop_idx=5 converged=1 fitness=13.0291
      [Rejected by ICP] loop_idx=5, fitness=13.0291 >= threshold(0.1)
  [Testing SC candidate] loop_idx=2
    [ICP] loop_idx=2 converged=1 fitness=14.9827
      [Rejected by ICP] loop_idx=2, fitness=14.9827 >= threshold(0.1)
  [Testing SC candidate] loop_idx=4
    [ICP] loop_idx=4 converged=1 fitness=14.236
      [Rejected by ICP] loop_idx=4, fitness=14.236 >= threshold(0.1)
  [Testing SC candidate] loop_idx=6
    [ICP] loop_idx=6 converged=1 fitness=13.0227
      [Rejected by ICP] loop_idx=6, fitness=13.0227 >= threshold(0.1)
  [Testing SC candidate] loop_idx=0
    [ICP] loop_idx=0 converged=1 fitness=13.0812
      [Rejected by ICP] loop_idx=0, fitness=13.0812 >= threshold(0.1)
  [Testing SC candidate] loop_idx=8
    [ICP] loop_idx=8 converged=1 fitness=13.5542
      [Rejected by ICP] loop_idx=8, fitness=13.5542 >= threshold(0.1)
  [Testing SC candidate] loop_idx=7
    [ICP] loop_idx=7 converged=1 fitness=13.0252
      [Rejected by ICP] loop_idx=7, fitness=13.0252 >= threshold(0.1)
  [Testing SC candidate] loop_idx=1
    [ICP] loop_idx=1 converged=1 fitness=13.0789
      [Rejected by ICP] loop_idx=1, fitness=13.0789 >= threshold(0.1)

说明这里的问题和找到的回环帧没关系（一段连续时间的回环帧应该是相同或者差一两帧的），因此怀疑是直接把回环帧的位姿认为是当前帧位姿这一步有问题，而且结合绿色框返回的yaw角很大，确认了我的怀疑，

不能直接把回环帧的位姿认为是当前帧的位姿，先计算候选回环帧的 yaw 差 （调用 distanceBtnScanContext()），然后对回环帧位姿做个yaw角补偿后，才认为是当前帧位姿的粗略值，修改后211s之后的帧可以重定位成功。

后来在新的数据测试，遇到新的问题：回环帧位姿（红色箭头），scan to scan优化的位姿（黄色箭头），此时重定位是失败的。和使用rviz人为手点相比，优化后的位姿在（x,y）是没有太大差距，主要是 yaw 角相差很大，导致scan to map失败。算法层面没有找到原因在哪，这帧数据的后面几帧是可以自动重定位的，如下图所示。所以从工程角度，把初始位姿的yaw角每隔固定数值(如30或者45)进行旋转，然后和map匹配，直到fitness 小于阈值。实际测试，这种情况出现的次数比较少，上述方法只是对自动重定位的补救。（TODO）

2.4.4 ROS2修改及使用组件(component)

注：lio-sam有ros2版本，也是后来才发现的。增加的重定位代码几乎都是c++相关的，在作者ros2版本添加重定位相关代码应该是最方便的。

总结需要注意的地方：

自定义msg的时间戳使用 std_msgs/Header header；
config参数类型一定要和get_parameter()对应；
ros2使用的msg

// ros1
sensor_msgs::PointCloud2 tempCloud;
// ros2
sensor_msgs::msg::PointCloud2 tempCloud;

2.模板函数，获取msg的时间戳

template<typename T>
double ROS_TIME(T msg)
{
    return rclcpp::Time(msg).seconds();
}

3.关于路径问题，参考fast_lio，在cmake添加

add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")

代码是通过 ROOT_DIR 获取功能包的路径，注意此时路径带 “/”。省去了在配置文件关于路径的拼写。

4.lio-sam依赖GTSAM，ros1可以通过命令行安装，但是ros2需要通过源码编译安装。建议安装4.2.0，注意使用系统的eigen库）

cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DEigen3_DIR=/usr/include/eigen3 ..

会遇到找不到libmetis-gtsam.so 的问题

error while loading shared libraries: libmetis-gtsam.so: cannot open shared object file: No such file or director

是因为这个库在 /usr/local/lib/ 文件中，但是程序默认找的位置是路径 /usr/lib

# find / -name libmetis-gtsam.so
/usr/local/lib/libmetis-gtsam.so

两种解决方法：

① 只移动当前需要的.so文件
sudo cp /usr/local/lib/libmetis-gtsam.so /usr/lib/

② 将/usr/local/lib添加到环境变量中,程序在运行时就会优先在该目录中查找共享库
export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH   (在当前终端或者写进 ~/.bashrc)

5.后续就是解决CPU占用率过高问题：

CPU占用率过高的原因可能是因为ICPscanMatchGlobal()执行太频繁，把休眠时间加大可以很大程度解决cpu占用率问题

①节点进程内通信：目前代码有四个进程，需要使用 component ，将进程合并为一个，然后使用IPC（intra-process）实现进程内通信，显著减少cpu占用；

②scan2MapOptimization 使用GPU加速，参考 LIO-SAM-GPU-ScanToMapOpt

(实际测试发现在室内轨迹可能会漂移，需要把 mappingSurfLeafSize 和 mappingCornerLeafSize 变小)；

③ 代码中关于匹配的方法由 ICP 改为 GICP；

2.5 动态点云剔除

参考 removert 实现动态点云剔除（后处理）。

原地图

静态点云地图

静态点云和动态点云（黄色）

2.6 MID-360 倒置安装

对比 Helios 和 MID-360 建图和重定位以及重定位精度的区别。

1.视场角

对比两个传感器的视场分布，MID360 的点云主要集中在上半空间，而 Helios 则以下半空间为主。实际环境中的障碍物几乎来自下半部分，因此将 MID360 采用倒置安装。

2.代码修改

①配置文件外参修改

    # lidar -> imu
    extrinsicTrans: [0.0,  0.0, 0.0]
    extrinsicRot: 
      [ 1.0, 0.0, 0.0,
        0.0, -1.0, 0.0,
        0.0, 0.0, -1.0 ]
    extrinsicRPY:
      [ 1.0, 0.0, 0.0,
        0.0, -1.0, 0.0,
        0.0, 0.0, -1.0 ]

②

lio-sam关于外参这里写的不是很严谨，首先 imuConverter 中是把IMU数据变换到 Lidar坐标系，因此需要的外参是 IMU 到 Lidar 的变换，需要把配置文件的外参求逆；

其次是 IMUPreintegration 中外参的定义使用硬编码，没有用到配置文件参数

gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));

修改后的code：

//1. 参数
    std::vector<double> rot_il_vec(9), trans_il_vec(3);
    declare_parameter("extrinsicRot",   std::vector<double>{1,0,0, 0,1,0, 0,0,1});
    declare_parameter("extrinsicTrans", std::vector<double>{0,0,0});
    get_parameter("extrinsicRot",   rot_il_vec);
    get_parameter("extrinsicTrans", trans_il_vec);

    R_il << rot_il_vec[0], rot_il_vec[1], rot_il_vec[2],
           rot_il_vec[3], rot_il_vec[4], rot_il_vec[5],
           rot_il_vec[6], rot_il_vec[7], rot_il_vec[8];
    t_il = Eigen::Vector3d(trans_il_vec[0], trans_il_vec[1], trans_il_vec[2]);

    // 求逆 IMU→LiDAR = T_li
    R_li = R_il.transpose();
    t_li = -R_li * t_il;
    Q_li = Eigen::Quaterniond(R_li).normalized();

    extRot  = R_li;
    extQRPY = Q_li;

//2. imu convert lidar：IMU→LiDAR
sensor_msgs::msg::Imu ParamServer::imuConverter(const sensor_msgs::msg::Imu &imu_in)
{
    sensor_msgs::msg::Imu imu_out = imu_in;

    // rotate acceleration
    Eigen::Vector3d acc(imu_in.linear_acceleration.x,
                        imu_in.linear_acceleration.y,
                        imu_in.linear_acceleration.z);
    
    // livox 内置的六轴imu的加速度单位是g 这里要还原到m/s^2
    if(imuType == 0)  
        acc = acc * imuGravity;

    acc = extRot * acc;                      // R_li
    imu_out.linear_acceleration.x = acc.x();
    imu_out.linear_acceleration.y = acc.y();
    imu_out.linear_acceleration.z = acc.z();
    // std::cout<<" linear_acceleration.z: "<< acc.z() <<std::endl;

    // rotate gyroscope
    Eigen::Vector3d gyr(imu_in.angular_velocity.x,
                        imu_in.angular_velocity.y,
                        imu_in.angular_velocity.z);
    gyr = extRot * gyr;                      // R_li
    imu_out.angular_velocity.x = gyr.x();
    imu_out.angular_velocity.y = gyr.y();
    imu_out.angular_velocity.z = gyr.z();

    // rotate orientation
    Eigen::Quaterniond q_from(imu_in.orientation.w,
                              imu_in.orientation.x,
                              imu_in.orientation.y,
                              imu_in.orientation.z);
    Eigen::Quaterniond q_final;
    
    if(imuType == 0)
        q_final = extQRPY;
    else if(imuType == 1)
        q_final = q_from * extQRPY;
    else{
        RCLCPP_FATAL(get_logger(),"imu_type can only be one of 0 or 1");
        rclcpp::shutdown();
    }

    q_final.normalize();

    imu_out.orientation.x = q_final.x();
    imu_out.orientation.y = q_final.y();
    imu_out.orientation.z = q_final.z();
    imu_out.orientation.w = q_final.w();

    // check validity
    double norm = q_final.norm();
    if (norm < 0.1) {
        RCLCPP_ERROR(this->get_logger(), "Invalid quaternion, please use a 9-axis IMU!");
        rclcpp::shutdown();
    }

    return imu_out;
}

//3. IMUPreintegration 构造函数
    gtsam::Rot3  R_li_gtsam( R_li.cast<double>() );
    gtsam::Rot3  R_il_gtsam( R_il.cast<double>() );
    gtsam::Point3 t_li_gtsam( t_li.x(), t_li.y(), t_li.z() );
    gtsam::Point3 t_il_gtsam( t_il.x(), t_il.y(), t_il.z() );

    imu2Lidar = gtsam::Pose3(R_li_gtsam, t_li_gtsam); // IMU→LiDAR
    lidar2Imu = gtsam::Pose3(R_il_gtsam, t_il_gtsam); // LiDAR→IMU

③

实际测试对比，MID360和Helios的重定位精度几乎是一致的；CPU占用率更低（点云更少）。