【LOAM系列】四：LIO-SAM论文代码阅读笔记

LIO-SAM

IROS 2020 MIT Tixiao Shan

1、论文

1.1 概述

IMU预积分的运动估计对点云进行去畸变并产生激光里程计优化的初始估计。
激光雷达里程计用于估计IMU的零偏。
滑窗优化，在姿态优化的时候边缘化掉旧的激光雷达帧

LOAM是扫描到全局地图的匹配，松耦合方法

1.2 IMU预积分因子

系统状态
$$\begin{array}{c}\mathbf{x}={\left[{\mathbf{R}}^{\mathbf{T}},{\mathbf{p}}^{\mathbf{T}},{\mathbf{v}}^{\mathbf{T}},{\mathbf{b}}^{\mathbf{T}}\right]}^{\mathbf{T}}\end{array}$$
因子图优化：IMU预积分因子、激光雷达里程计因子、GPS因子、回环因子。当机器人姿态的变化超过用户定义的阈值时，图中会添加一个新的机器人状态节点x

IMU测量值
$$\begin{array}{rl}& {\hat{\omega }}_t={\omega }_t+{\mathbf{b}}_t^{\omega }+{\mathbf{n}}_t^{\omega }\\ & {\hat{\mathbf{a}}}_t={\mathbf{R}}_t^{\text{BW}}\left({\mathbf{a}}_t-\text{g}\right)+{\mathbf{b}}_t^{\text{a}}+{\mathbf{n}}_t^{\text{a}}\end{array}$$
欧拉法IMU预积分
$$\begin{array}{rl}& {\mathbf{v}}_{t+\Delta t}={\mathbf{v}}_t+\mathbf{g}\Delta t+{\mathbf{R}}_t\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_t^{\text{a}}-{\mathbf{n}}_t^{\mathbf{a}}\right)\Delta t\\ & \begin{array}{c}{\mathbf{p}}_{t+\Delta t}={\mathbf{p}}_t+{\mathbf{v}}_t\Delta t+\frac{1}{2}\mathbf{g}\Delta t^2+\frac{1}{2}{\mathbf{R}}_t\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_t^{\mathbf{a}}-{\mathbf{n}}_t^{\mathbf{a}}\right)\Delta t^2\end{array}\\ & {\mathbf{R}}_{t+\Delta t}={\mathbf{R}}_t\exp \left(\left({\hat{\omega }}_t-{\mathbf{b}}_t^{\omega }-{\mathbf{n}}_t^{\omega }\right)\Delta t\right)\end{array}$$
相对运动
$$\begin{array}{rl}& \Delta {\mathbf{v}}_{ij}={\mathbf{R}}_i^{\top }\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij}\right)\\ & \Delta {\mathbf{p}}_{ij}={\mathbf{R}}_i^{\top }\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2\right)\\ & \Delta {\mathbf{R}}_{ij}={\mathbf{R}}_i^{\top }{\mathbf{R}}_j.\end{array}$$

1.3 激光里程计因子

特征提取：通过评估局部区域上点的粗糙度/曲率来提取边缘和平面特征，粗糙度(曲率)大的点被划分为边缘特征，小的被分为平面特征

激雷达点云帧：边缘特征Fei + 平面特征Fpi Fi={Fei,Fpi}

关键帧选择：当机器人姿态的变化与先前的状态 xi相比，如果超过给定阈值时(1m和10°），选择此时的激光雷达帧Fi+1作为关键帧,因子图的中新的机器人状态节点Xi+1,然后丢弃掉两个关键帧之间的其他激光雷达帧。

LO因子生成过程：

1）建立子关键帧的体素地图 sub-keyframes for voxel map

滑动窗口方法构建点云地图，包含固定数目的最近激光雷达的扫描点云
提取n个最近的关键帧，sub-keyframes子关键帧集合，采用变换关系 {Ti−n, …, Ti} 将关联的子关键帧集合 {Fi−n, …, Fi} 转换到世界坐标系中,转换后的子关键帧会被合并成一个体素地图Mi，注意地图由两个子体素地图构成，定义边缘特征体素地图为Mei ，平面特征体素地图为 Mpi
M i = { M i e , M i p }  where :  M i e = ′ F i e ∪ ′ F i − 1 e ∪ … ∪ ′ F i − n e M i p = ′ F i p ∪ ′ F i − 1 p ∪ … ∪ ′ F i − n p . \begin{equation*}\begin{array}{l} {{{\mathbf{M}}_i} = \left\{ {{\mathbf{M}}_i^e,{\mathbf{M}}_i^p} \right\}} \\ {{\text{ where : }}{\mathbf{M}}_i^e{ = ^\prime }{\text{F}}_i^e{ \cup ^\prime }{\text{F}}_{i - 1}^e \cup \ldots { \cup ^\prime }{\text{F}}_{i - n}^e} \\ {{\mathbf{M}}_i^p{ = ^\prime }{\text{F}}_i^p{ \cup ^\prime }{\text{F}}_{i - 1}^p \cup \ldots { \cup ^\prime }{\text{F}}_{i - n}^p.} \end{array}\end{equation*} Mi​={Mie​,Mip​} where : Mie​=′Fie​∪′Fi−1e​∪…∪′Fi−ne​Mip​=′Fip​∪′Fi−1p​∪…∪′Fi−np​.​​
子体素地图下采样

2）扫描与子地图匹配 scan-matching

新激光雷达帧与世界坐标系下的子地图匹配，通过IMU预积分得到激光雷达帧的初始值，使用这个初值将新帧数据转换到世界坐标系下，然后进行边缘和平面特征匹配

特征距离计算

点到线的距离计算：从Fk+1帧中选取一个线特征的点 k(世界坐标系），在子地图中选取两个点：与 k 最近的点u ，以及在最近扫描线中选取和点 u相邻扫描线中最近的点v，这样的目的是防止 三点共线而无法构成三角形。

点到面的距离：从Fk+1帧中选取一个线特征的点 k(世界坐标系），在子地图中选取三个点：与 k 最近的点u ，u同一扫描线上最近点v，以及在最近扫描线中选取和点 u相邻扫描线中最近的点w，找到共面的三个点。
$$\begin{array}{rl}& {\mathbf{d}}_{e_k}=\frac{\left|\left({\mathbf{p}}_{i+1,k}^e-{\mathbf{p}}_{i,u}^e\right)\times \left({\mathbf{p}}_{i+1,k}^e-{\mathbf{p}}_{i,v}^e\right)\right|}{\left|{\mathbf{p}}_{i,u}^e-{\mathbf{p}}_{i,v}^e\right|}\\ & {\mathbf{d}}_{p_k}=\frac{\left|\begin{array}{c}\left({\mathbf{p}}_{i+1,k}^p-{\mathbf{p}}_{i,u}^p\right)\cdot [\left({\mathbf{p}}_{i,u}^p-{\mathbf{p}}_{i,v}^p\right)\times \left({\mathbf{p}}_{i,u}^p-{\mathbf{p}}_{i,w}^p\right)]\end{array}\right|}{\left|\left({\mathbf{p}}_{i,u}^p-{\mathbf{p}}_{i,v}^p\right)\times \left({\mathbf{p}}_{i,u}^p-{\mathbf{p}}_{i,w}^p\right)\right|},\end{array}$$
最小化距离
求解的是Ti+1，即i+1帧在世界坐标系下的位姿
$$\begin{array}{c}\underset{{\mathbf{T}}_{i+1}}{\min }\left\{\sum _{{\mathbf{p}}_{i+1,k}^e{\in }^{\prime }{F}_{i+1}^e}{\mathbf{d}}_{e_k}+\sum _{{\mathbf{p}}_{i+1,k}^p{\in }^{\prime }{F}_{i+1}^p}{\mathbf{d}}_{p_k}\right\}.\end{array}$$

$$\begin{array}{c}\Delta {\mathbf{T}}_{i,i+1}={\mathbf{T}}_i^{\top }{\mathbf{T}}_{i+1}\end{array}\text{\hspace{1em}(12)}$$

1.4 GPS因子

在估计的位置协方差大于接收到的GPS位置协方差的时候，才去增加一个GPS因子。
将GPS坐标转换到局部笛卡尔坐标系下；
如果GPS信号与激光雷达没有做硬件同步，那么我们根据激光雷达的时间戳在GPS中进行线性插值

1.5 回环因子

基于欧氏距离的闭环检测方法

当一个新的状态 Xi+1 被加入到因子图中，首先在因子图中进行搜索，找到欧式空间中接近状态Xi+1 的先验状态。状态节点X3 是其中一个返回的候选对象。然后用扫描匹配scan-match方式将关键帧Xi+1 与子关键帧集 {F3−m,⋯,F3,⋯,F3+m} 进行匹配。注意，关键帧 Fi+1 和过去的子关键帧在扫描匹配scan-match之前已经先被转换到世界坐标系W中 。然后我们就得到了相对变换 ΔT3,i+1 ，并将其作为一个闭环因子添加到因子图中。选择索引数m为12(也就是闭环检测的子地图包含25帧)，闭环与新状态Xi+1 的搜索范围设为15m。

2、代码

• LIO-SAM源码解析：准备篇
• LIO-SAM源码解析(一)：ImageProjection
• LIO-SAM源码解析(二)：FeatureExtraction
• LIO-SAM源码解析(三)：IMUPreintegration
• LIO-SAM源码解析(四)：MapOptimization

2.1 整体流程

代码结构图

因子图

ROS节点图

1、激光运动畸变校正。利用当前帧起止时刻之间的IMU数据、IMU里程计数据计算预积分，得到每一时刻的激光点位姿，从而变换到初始时刻激光点坐标系下，实现校正。

2、提取特征。对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点特征。

3、scan-to-map匹配。提取布局关键帧map的特征点，与当前帧特征点执行scan-to-map匹配，更新当前帧的位姿。

4、因子图优化。添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿。

5、闭环检测。在历史关键帧中找候选闭环匹配帧，执行scan-to-map匹配，得到位姿变换，构建闭环因子，加入到因子图中一并优化。

2.2 IMU预积分（ImuPreintegration）

调用GTSAM预积分器，直接 输入IMU的测量即可进行两帧之间的预积分，积分完成后输入先验位姿即可得到每帧IMU的位姿,

        // 用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预积分量，得到当前时刻的状态
        gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);

1）TransformFusion类

功能简介

主要功能是订阅激光里程计（来自MapOptimization）和IMU里程计，根据前一时刻激光里程计，和该时刻到当前时刻的IMU里程计变换增量，计算当前时刻IMU里程计；rviz展示IMU里程计轨迹（局部）。

订阅

1.订阅激光里程计，来自MapOptimization；
2.订阅imu里程计，来自ImuPreintegration。

发布

1.发布IMU里程计，用于rviz展示；
2.发布IMU里程计轨迹，仅展示最近一帧激光里程计时刻到当前时刻之间的轨迹。

2）ImuPreintegration类

功能简介

1.用激光里程计，两帧激光里程计之间的IMU预计分量构建因子图，优化当前帧的状态（包括位姿、速度、偏置）;
2.以优化后的状态为基础，施加IMU预计分量，得到每一时刻的IMU里程计。

订阅

1.订阅IMU原始数据，以因子图优化后的激光里程计为基础，施加两帧之间的IMU预计分量，预测每一时刻（IMU频率）的IMU里程计；
2.订阅激光里程计（来自MapOptimization），用两帧之间的IMU预计分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的IMU里程计，以及下一次因子图优化）。

发布

1.发布imu里程计；

2.3 激光运动畸变校正（ImageProjection）

功能简介

1.利用当前激光帧起止时刻间的imu数据计算旋转增量，IMU里程计数据（来自ImuPreintegration）计算平移增量，进而对该帧激光每一时刻的激光点进行运动畸变校正（利用相对于激光帧起始时刻的位姿增量，变换当前激光点到起始时刻激光点的坐标系下，实现校正）；
2.同时用IMU数据的姿态角（RPY，roll、pitch、yaw）、IMU里程计数据的的位姿，对当前帧激光位姿进行粗略初始化。

话题订阅
1.订阅原始IMU数据；
2.订阅IMU里程计数据，来自ImuPreintegration，表示每一时刻对应的位姿；
3.订阅原始激光点云数据。

发布话题

1.发布当前帧激光运动畸变校正之后的有效点云，用于rviz展示；
2.发布当前帧激光运动畸变校正之后的点云信息，包括点云数据、初始位姿、姿态角、有效点云数据等，发布给FeatureExtraction进行特征提取。

2.4 点云特征提取（FeatureExtraction）

功能简介

对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点（用曲率的大小进行判定）。1、计算当前激光帧点云中每个点的曲率

标记属于遮挡、平行两种情况的点，不做特征提取
点云角点、平面点特征提取.
遍历扫描线，每根扫描线扫描一周的点云划分为6段，针对每段提取20个角点：曲率大于阈值，则认为是角点，同一条扫描线上后5个点标记一下，不再处理， 避免特征聚集
不限数量的平面点：曲率小于阈值，则认为是平面点，平面点和未被处理的点，都认为是平面点，加入平面点云集合
认为非角点的点都是平面点，加入平面点云集合，最后降采样

订阅

订阅当前激光帧运动畸变校正后的点云信息，来自ImageProjection。

发布

1.发布当前激光帧提取特征之后的点云信息，包括的历史数据有：运动畸变校正，点云数据，初始位姿，姿态角，有效点云数据，角点点云，平面点点云等，发布给MapOptimization；
2.发布当前激光帧提取的角点点云，用于rviz展示；
3.发布当前激光帧提取的平面点点云，用于rviz展示。

2.5 因子图优化（MapOptimization）

功能简介

1.scan-to-map匹配：提取当前激光帧特征点（角点、平面点），局部关键帧map的特征点，执行scan-to-map迭代优化，更新当前帧位姿；
2.关键帧因子图优化：关键帧加入因子图，添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿；
3.闭环检测：在历史关键帧中找距离相近，时间相隔较远的帧设为匹配帧，匹配帧周围提取局部关键帧map，同样执行scan-to-map匹配，得到位姿变换，构建闭环因子数据，加入因子图优化。

/**
     * 订阅当前激光帧点云信息，来自featureExtraction
     * 1、当前帧位姿初始化
     *   1) 如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
     *   2) 后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光位姿
     * 2、提取局部角点、平面点云集合，加入局部map
     *   1) 对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
     *   2) 对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
     * 3、当前激光帧角点、平面点集合降采样
     * 4、scan-to-map优化当前帧位姿
     *   (1) 要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化
     *   (2) 迭代30次（上限）优化
     *      1) 当前激光帧角点寻找局部map匹配点
     *          a.更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
     *          b.计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
     *      2) 当前激光帧平面点寻找局部map匹配点
     *          a.更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
     *          b.计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
     *      3) 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
     *      4) 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
     *   (3)用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
     * 5、设置当前帧为关键帧并执行因子图优化
     *   1) 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
     *   2) 添加激光里程计因子、GPS因子、闭环因子
     *   3) 执行因子图优化
     *   4) 得到当前帧优化后位姿，位姿协方差
     *   5) 添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
     * 6、更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
     * 7、发布激光里程计
     * 8、发布里程计、点云、轨迹
    */
    void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)

订阅

1.订阅当前激光帧点云信息，来自FeatureExtraction；
2.订阅GPS里程计；
3.订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上。

发布

1.发布历史关键帧里程计；
2.发布局部关键帧map的特征点云；
3.发布激光里程计，rviz中表现为坐标轴；
4.发布激光里程计；
5.发布激光里程计路径，rviz中表现为载体的运行轨迹；
6.发布地图保存服务；
7.发布闭环匹配局部关键帧map；
8.发布当前关键帧经过闭环优化后的位姿变换之后的特征点云；
9.发布闭环边，rviz中表现为闭环帧之间的连线；
10.发布局部map的降采样平面点集合；
11.发布历史帧（累加的）的角点、平面点降采样集合；
12.发布当前帧原始点云配准之后的点云；

闭环检测

 * 闭环scan-to-map，icp优化位姿
      *1、在历史关键帧中查找与当前关键帧距离最近的关键帧集合，选择时间相隔较远的一帧作为候选闭环帧
 
 2、提取当前关键帧特征点集合，降采样；提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样
 
 3、执行scan-to-map优化，调用icp方法，得到优化后位姿，构造闭环因子需要的数据，在因子图优化中一并加入更新位姿
 
 注：闭环的时候没有立即更新当前帧的位姿，而是添加闭环因子，让图优化去更新位姿
 
 **把所有帧的位姿都转化为一个3D点数据，然后存在kd-tree中使用最近邻搜索查找**

3、lio-sam-mid360

在特征提取计算曲率的时候值计算了前后两个点的距离

float diffRange = cloudInfo.pointRange[i-2]  + cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 4
                            + cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2];            

            // 用当前激光点前后5个点计算当前点的曲率，平坦位置处曲率较小，角点处曲率较大；这个方法很简单但有效
            float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]
                            + cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]
                            + cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10
                            + cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]
                            + cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]
                            + cloudInfo.pointRange[i+5];    

在特征提取时分段处理，一共分为6段，每段角点提取40个，原来的代码是提取20个

 if (largestPickedNum <= 40){
                            cloudLabel[ind] = 1;
                            cornerCloud->push_back(extractedCloud->points[ind]);
                        }

Livox的点云格式

//livox的点云格式
struct LiovxPointCustomMsg
{
    PCL_ADD_POINT4D// 位置
    PCL_ADD_INTENSITY;// 激光点反射强度，也可以存点的索引
    float time;// 时间偏移，记录相对于当前帧第一个激光点的时差，第一个点time=0
    uint16_t ring;// 扫描线
    uint16_t tag;//livox的点的tag
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (LiovxPointCustomMsg,
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (float, time, time)
    (uint16_t, ring, ring) (uint16_t, tag, tag)
)

using PointXYZIRT = LiovxPointCustomMsg;

    std::deque<livox_ros_driver::CustomMsg> cloudQueue;
    livox_ros_driver::CustomMsg currentCloudMsg;//雷达数据结构改变

---