激光雷达与惯性测量单元标定Lidar和IMU的标定lidar_IMU_calib

一、环境编译

# init ROS workspace
mkdir -p ~/catkin_li_calib/src
cd ~/catkin_li_calib/src
catkin_init_workspace

# Clone the source code for the project and build it. 
git clone https://github.com/APRIL-ZJU/lidar_IMU_calib

# ndt_omp
wstool init
wstool merge lidar_IMU_calib/depend_pack.rosinstall
wstool update
# Pangolin
./build_submodules.sh
## build
cd ../..
catkin_make
source ./devel/setup.bash

论文地址：https://arxiv.org/pdf/2007.14759.pdf

二、数据录制

鉴于是基于NDT匹配算法的里程计计算，
数据录制需要注意：
1.需要在平面多的房间里录制；
2.xyz轴方向都需要移动
3.不要旋转过/移动过猛

rosbag -record -o out /velodyne_packets /你的IMU数据话题
查看数据：
rosrun nodelet nodelet standalone velodyne_pointcloud/CloudNodelet _calibration:="/home/XXX（your username）/catkin_velodyne（your velodyne catkin工作环境）/src/velodyne/velodyne_pointcloud/params/VLP16db.yaml"
然后播放bag包 rosbag play yourbagname.bag
在rviz中查看即可rosrun rviz rviz

三、适配launch

修改licalib_gui.launch文件
注意此处的velodyne数据为velodyne_packets

<?xml version="1.0"?>
<launch>
    <arg name="topic_imu"           default="/你的imu数据话题" />
    <arg name="path_bag"            default="/home/$(env USER)/bag包位置" />
    <arg name="bag_start"           default="0" />
    <arg name="bag_durr"            default="48" />
    <arg name="scan4map"            default="15" />
    <arg name="lidar_model"         default="VLP_16" />
    <arg name="ndtResolution"       default="0.5" /> <!-- 0.5 for indoor case and 1.0 for outdoor case -->

    <arg name="time_offset_padding" default="0.015" />
    <arg name="show_ui"    default="true" />

    <node pkg="li_calib" type="li_calib_gui" name="li_calib_gui" output="screen">
    <!-- <node pkg="li_calib" type="li_calib_gui" name="li_calib_gui" output="screen" clear_params="true" launch-prefix="gdb -ex run &#45;&#45;args">-->

        <param name="topic_imu"         type="string"   value="$(arg topic_imu)" />
        <param name="topic_lidar"       type="string"   value="/velodyne_packets" />
        <param name="LidarModel"        type="string"   value="$(arg lidar_model)" />
        <param name="path_bag"          type="string"   value="$(arg path_bag)" />
        <param name="bag_start"         type="double"   value="$(arg bag_start)" />
        <param name="bag_durr"          type="double"   value="$(arg bag_durr)" /> <!-- for data association -->
        <param name="scan4map"          type="double"   value="$(arg scan4map)" />
        <param name="ndtResolution"     type="double"   value="$(arg ndtResolution)" />

        <param name="time_offset_padding"   type="double"   value="$(arg time_offset_padding)" />
        <param name="show_ui"               type="bool"     value="$(arg show_ui)" /><!-- 我们用ui界面来进行操作 -->
    </node>

</launch>

三、运行及相关问题

运行：
在UI界面依顺序点击：初始化(Initialization)->初始化(Initialization)...直到初始化成功（判定标准看问题1.）->数据关联(Data Association)->初始化优化（Batch Optimization）->迭代优化（Refinement）->迭代优化（Refinement）...直至数据不再变换
问题：

1.初始化失败问题

有可能遇到
[Initialization] fails.的问题，此时将lidar_IMU_calib -> src -> core -> inertial_initializer.cpp中最后几行前，加入 std::cout << "Vector4d cov（2）value ：" << cov(2) << std::endl;这一行代码，一直初始化，直到输出结果为2.5左右即可初始化成功

  std::cout << "Vector4d cov" << cov(2) << std::endl;

  if (cov(2) > 0.25) {
    q_ItoS_est_ = q_ItoS_est;
    rotaion_initialized_ = true;
    return true;
  } else {
    return false;
  }
}

}

2.数据跑飞问题

如果你录取的数据，不是在平面多的房间里或者旋转过大，极有可能出现数据跑飞的问题，表现就是XYZ偏差超出正常值，这时候需要从录数据。

四、我的标定结果

精度无从考证。
我的是在小车上安装标定的，分别做了X方向的行进，一定高度的Z轴越野，以及带Y方向加速的旋转运动以进行标定，其中Z轴越野加减速较大！
附三次标定结果，我们取其平均数。



其平均数（以IMU为参考坐标系）：X轴偏移量23CM Y轴：8CM Z轴：15.5厘米，由于不知道激光雷达中心在何处，我们手工测量大改X轴22-24cm Y轴0cm Z轴12-18CM
我们舍弃Y轴数据只用X，Z轴数据，因为想要一个沿IMU的X方向行进的小车做Y轴的平移实在困难，我们就将其视为Y轴无偏移，因为二者我们都是手工测量将其置于小车中间的。
角度偏移取平均：R（绕X）0.5° P（绕Y）0° Y（绕Z）2.1°
以下使我们的小车平台

五、数据分析（作者示例数据分析）

我们看到x偏差为-10cm左右，y为0cm左右，z为-15cm左右。可以通过图中看到这是以IMU为参考系的激光雷达的距离！绕Z轴-90左右->即右手握住Z轴负方向旋转90度左右！示例标定结果很准确！虽然用的数据是Geogre01 但个人认为是Geogre03的数据及结果

也有以激光雷达为参考坐标系的标定结果：