前言

预积分量约束相邻两帧的状态量（位置、速度、姿态），同时考虑到IMU的零偏的性质，即短时间内变换速率比较慢，因此可以认为两帧之间的零偏不变，也就是还可以约束两帧的零偏变换。

同时，在slam问题中，IMU预积分通常会和其它传感器的约束一起构建一个优化问题

无论是VIO还是LIO，IMU和其它传感器的标定结果往往对最终的里程记和建图性能有着显著的影响。同样，在多传感器融合算法中，传感器之间的标定结果的精度对多传感器融合的效果也有着非常大的影响。

在lidar-imu融合的算法中，lidar和IMU之间的外参标定非常重要，在一些数据集上往往有着经过良好标定的结果，然而，绝大多数情况下，或者实际自己应用时，传感器之间的外参需要自己去标定。

视觉VIO中预积分和视觉的优化建模

用VIO-MONO论文中的图来举例

黄色代表IMU预积分，其只对相邻帧发生约束，五角星代表视觉地图点，同一个地图点可以通过重投影误差对多个关键帧的位姿形成约束

LIO-SAM中预积分和lidar位姿的优化建模

其中蓝色圆圈代表关键帧位姿，黄色矩形代表关键帧速度和零偏，红色矩形代表IMU预积分约束，它可以约束相邻帧的位姿、速度和零偏，绿色矩形代表激光雷达里程计的帧间约束，其约束相邻两帧的位置和姿态。

这里包括了lidar里程计因子和预积分因子，是预积分节点因子图的优化模型。

IMU与其它床拿起标定方法

无论是VIO还是LIO，IMU和其它传感器的标定结果往往对最终的里程记和建图性能有着显著的影响。同样，在多传感器融合算法中，传感器之间的标定结果的精度对多传感器融合的效果也有着非常大的影响。

在lidar-imu融合的算法中，lidar和IMU之间的外参标定非常重要，在一些数据集上往往有着经过良好标定的结果，然而，绝大多数情况下，或者实际自己应用时，传感器之间的外参需要自己去标定。

LIO-SAM中，作者推荐了一个Lidar-IMU标定的功能包来实现lidar和IMU之间的外参标定。

推荐的功能github链接

通常来讲，lidar和IMU之间的旋转外参相比于平移外参对里程记的性能有着更显著的影响。因此，条件有限的情况下，可以只标定lidar和IMU之间的旋转外参，平移外参则可以通过手工测量等方式获取。

该旋转标定算法本质上是⼀个手眼标定算法。即先计算出两帧lidar之间的旋转（通过ICP、NDT等点云配准算法）。然后计算出两帧IMU之间的旋转（IMU陀螺仪积分），然后通过手眼标定的方式求解出外参。和VINS-Mono中相机IMU旋转外参初始化的过程⼀致，具体方式为：

上面等式成立的原因就是，IMU和激光雷达的外参是不随时间变化的
即

进行化简即：

用四元数来表示旋转，四元数的乘法可以通过一个公式转成矩阵的乘法。

化简一下即

最一开始的四元数等式可转为，矩阵乘向量的形式

之后采集若干组IMU和lidar的相对旋转平移数据，进行超定方程组的建立，如下：

相当于已知一个4n×4大小的矩阵，求出⼀个4×1向量的最优解
这是一个Ab=0的线性最小二乘问题，可以用最小二乘的公式直接求解。

但是A的矩阵通常比较大，用最小二乘的公式会涉及的到逆，时间会慢些，可以用SVD方法求解。

lidar_imu_calib 功能包

理解完IMU和其它传感器的标定原理之后，可以通过lidar_imu_calib 功能包进行 imu和lidar的外参标定

安装步骤：

mkdir -p catkin_ws/src   
cd catkin_ws/src
git clone https://github.com/chennuo0125-HIT/lidar_imu_calib.git
cd ..
catkin_make -DCATKIN_WHITELIST_PACKAGES="ndt_omp;lidar_imu_calib"
1
2
3
4
5

执行步骤：
1 录制imu和lidar的数据rosbag包

rosbag record /imu /lidar_points
1

注意要改成自己的消息名称

2 设置配置文件

lidar_topic: lidar data topic name
imu_topic: imu data topic name
bag_file: *.bag file record imu and lidar data topic
1
2
3

雷达的消息名称
imu的消息名称
rosbag包的路径

3 运行功能包的roslaunch

roslaunch lidar_imu_calib calib_exR_lidar2imu.launc
1

调试IMU数据

如果需要调试IMU数据可以使用如下代码：

  void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
    {
        sensor_msgs::Imu thisImu = imuConverter(*imuMsg);  

         debug IMU data
         cout << std::setprecision(6);
         cout << "IMU acc: " << endl;
         cout << "x: " << thisImu.linear_acceleration.x << 
               ", y: " << thisImu.linear_acceleration.y << 
               ", z: " << thisImu.linear_acceleration.z << endl;
         cout << "IMU gyro: " << endl;
         cout << "x: " << thisImu.angular_velocity.x << 
               ", y: " << thisImu.angular_velocity.y << 
               ", z: " << thisImu.angular_velocity.z << endl;
         double imuRoll, imuPitch, imuYaw;
         tf::Quaternion orientation;
         tf::quaternionMsgToTF(thisImu.orientation, orientation);
         tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
         cout << "IMU roll pitch yaw: " << endl;
         cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

上面代码输出了IMU的角速度和线加速度，并利用tf将四元数转成欧拉角。