激光雷達與陀螺儀標定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的數據及結果

也有以激光雷達為參考坐標系的標定結果：