今天要解析的代码是lins，对应论文《LINS: A Lidar-Inertial State Estimator for Robust and Efficient Navigation》，来自港科大刘明组，注释过的代码和论文链接附在文章最后。LINS这个名字与VINS相似，可以理解为Lidar-Inertial System的缩写。

lins也衍生自loam，算是在lego-loam基础上的改进版本，主要的改进在于：

1. 基于滤波器实现lidar与imu紧耦合的前端里程计，这也是论文最主要的工作；
2. scan2map匹配后就会利用gtsam对所有关键帧进行一次全局优化，而不仅仅是检测到回环的时候。

查看cmake文件可以看到，代码主要分为4个功能模块：image_projection_node，lins_fusion_node，lidar_mapping_node，transform_fusion_node，其中第1、3模块基本上完全是从loam、lego-loam照搬过来的，我在之前SC-Lego-LOAM解析的文章中也详细分析过，本文就不再赘述，第4个模块并未起到实际作用。需要额外说明的是，由于第1、3模块基本直接从lego-loam搬过来的，坐标系的定义方式与lins本身是不同的，所以在调用1、3模块相关代码前和后都会对坐标系进行一次变换，这只是习惯问题，本文不做细致分析。剩下的篇幅，重点来分析下lins_fusion_node，即利用Iterative ESKF对lidar和imu数据进行紧耦合，实现一个鲁棒的激光里程计。

需要特别解释的是，论文中的状态都是相对位姿和误差，假设frame m和frame n是相邻两帧激光位姿，中间会夹杂40帧IMU数据（论文中雷达10Hz，IMU400Hz），即frame m+1, frame m+2, …, frame n-2, frame n-1, 当frame m+1等到来时都会累积相对于frame m的位姿，这也是本文中的“状态”和“状态误差”中的“状态”，当frame n到来会进行IEKF的更新，然后重新开始一个周期来进行IEKF的预测和更新。

接下来先上一个图，这是我总结的一个大概的思维导图（真没想到逼乎可以直接粘上来这个。。。）

lins_fusion_node思维导图

LinsFusion lins的回调函数接受激光角/面点，IMU等所需的消息并push到buffer里，它有一个重要的成员变量estimator真正的负责状态估计。IMU的回调函数imuCallback会触发状态估计performStateEstimation，最终调用estimator的一系列成员函数完成状态估计。执行performStateEstimation函数时存在几种不同的状态：

1. 第一帧点云；
2. 第二帧点云和IMU消息；
3. 第三帧及以后的点云和IMU消息。

对于状态1：会依次调用processFirstPointCloud->processPCL->processFirstScan对estimator的预积分器preintegration和滤波器filter_进行创建和简单的初始化，也会将当前scan作为将来的target进行匹配；

对于状态2：会依次调用processPointClouds->processImu->processPCL->processSecondScan。其中processImu会通过estimator的preintegration_进行中值积分，计算出来一个第2帧和第3帧激光之间的相对位姿估计，然后processSecondScan会以preintegration_积分的结果为初值，以点云匹配的形式计算出第2帧和第3帧激光之间的相对位姿估计，然后对filter进行初始化，需要注意的是，滤波器中主要保存了两种信息：(1)相对位姿，两帧激光雷达数据中会存在40个IMU消息（论文中激光10Hz，IMU400Hz），每当IMU到来都会以前一个激光帧时刻的位姿为坐标系累积相对位姿；（2）相对位姿误差协方差。因此，滤波器的状态是相对位姿误差，需要至少存在两帧激光雷达数据才能真正完成初始化。余下的一些操作包括估计实话的roll和pitch，初始化全局位姿globalState_。

状态3才是正常的一般数据处理处理流程，lins的processPointClouds会对一组scan和IMU（40个左右）进行处理，分别通过调用estimator的processImu和processPCL->processScan来完成。

• IMU的处理在filter_的predict函数中，过程与preintegration_中大致相同，都是用IMU中的角速度和线加速度，对相对上一关键帧的位姿进行更新，重新计算误差状态微分方程的雅可比阵和误差状态转移矩阵，对误差状态协方差进行更新，这里对应论文中的公式（5）-（11）；
• processScan会调用performIESKF函数，从名字也可以看出来这是代码核心中的核心。filter_.predict已经利用IMU进行了kalman滤波器的预测这一步，performIESKF则是利用激光的观测，对误差状态进行迭代更新，这一部分对应论文的公式（15）-（18）和（4）；需要注意的是这里的状态和方差都是在上一关键帧位姿的作为坐标系表示的。integrateTransformation是将相对位姿进行累积，计算全局坐标系下的位姿globalState_；为了使得滤波器能够继续进行下去，需要将当前时刻的状态和误差状态协方差在当前位姿下表示，说起来有点绕。IEKF的预测和更新得到的是k+1时刻的状态，在k时刻位姿下的表示，而在下一个预测和更新周期内，预测和更新都需要相对于当前时刻（k+1）的位姿进行，因此需要将状态进行一次变换，主要是对速度和重力加速度估计进行一次变换，和状态中的位置和姿态分量直接归零，这些都是在filter_->reset(1)中完成的，对应论文公式（19）；余下的一些操作就是根据当前最优的全局位姿估计和重力加速度分量，重新估计全局的roll和pitch角并修正globalState_。

接下来正常情况下维持在状态3，循环执行filter_.predict和performIESKF进行卡尔曼滤波的预测和更新步骤。

到这里文章的主体部分已经结束了，比较简洁，只是对代码核心部分的结构和数据流走向进行了梳理，激光点云特征提取和建图都是在其他论文和代码里多次出现过的，本专栏其他文章也有过解析不再赘述。这篇文章是对自己学习的总结，也给想简单了解的读者提供一个引导，感兴趣的读者可以对照论文和代码再深入理解。文章写的比较仓促，代码也是分几次看完的，写注释的时候可能理解不够，如有错误和歧义的话，也欢迎大家留言指出讨论。

2022.6.5更新

周末又重新读了lins中状态估计部分lins_fusion_node代码，主要想对卡尔曼滤波器有更多的思考和理解，这里记录几点想法。

1. 误差状态卡尔曼滤波ESKF。ESKF(Error State Kalman Filter)区别于我们常说的对于导航状态的EKF，基于导航状态的EKF是对位姿的预测和更新，而ESKF则是在导航状态EKF预测的基础上，对误差状态进行更新（误差状态的预测为0），并加回到导航状态中。只所以说是在导航状态EKF预测的基础上，是因为观测对误差状态的雅可比，与观测与导航状态的雅可比有关。相关内容可以在Sola的《Quaternion kinematics for the error-state Kalman filter》中找到，这篇论文中公式很多，但真的值得好好读；
2. 迭代误差状态卡尔曼滤波iESKF。这与ESKF的不同之处在于更新过程，并不是对于误差状态一步求解，而是计算误差状态的变化量，再加到误差状态上去，如此迭代。这部分代码中说来源于ROVIO，这部分推导我还没有深入学习，大家基础好的可以找来看一下；
3. 关于robocentric。我们常见的状态预测都是以第一帧为单位阵的，也就是世界坐标系的原点，所谓的状态都是在这个世界坐标系下的，lins中则是以机器人为中心的，即状态都是相对于上一激光帧的，因此经过IMU预测和新的lidar点云观测预测后，状态的参考系会转换成当前的坐标系，这也就是filter_->reset(1)的意义；
4. 关于状态的参考系。位置、姿态、速度、重力g都是相对于上一关键帧的，因此在reset时位置和姿态需要置零，速度和g需要做姿态变换（传统的非robocentric的g是在第一帧位姿下的？），而且g的模长还会规范化为9.81，bias是在IMU/车体坐标系的，不需要什么额外操作。
5. 关于初始化。论文中说外参和加速度计bias是离线标定时获取的，陀螺仪的bias是静止时的输出；速度的初始化是根据前两帧点云匹配得到的位置计算的，这也是为什么在第二帧才完成了初始化；全局姿态的初始化，yaw可以认为是0，pitch和roll可以从加速度计测量值的投影计算得到；
6. estimator中的preintegration_只使用了一次，也就是第一帧点云和第二帧点云之间，因为这个时候滤波器还没有进行初始化，不能调用predict进行积分。这里是通过预积分进行的；
7. 初始化完成后的IMU预测是estimator通过filter_->predict()在滤波器中计算的(这部分没有使用预积分，而是直接积分的)，而更新是在estimator中直接计算保存在linState_中的，并通过filter_->update()直接保存到了filter；
8. 关于绝对位姿。滤波器状态估计部分实际上是对相对关键帧的导航状态进行估计，误差状态进行更新，并加到了导航状态，想要获得绝对位姿（相对于第一帧的位姿）还需要进行一次积分。对于姿态而言，虽然通过对滤波器状态的积分可以给出来一个绝对姿态估计，但是通过对重力分量估计（重力加速度在初始帧坐标系/世界坐标系下的表示）的投影也可以直接计算出roll和pitch，用来修正绝对姿态（一定是这个更好么？没有IMU的参与，绝对位姿一定会累积发散，带上IMU那么roll和pitch的误差就会维持在比较低的水平么？）。