前言：

LIO-SAM后端优化部分写在了mapOptimization.cpp文件中，本部分主要进行了激光帧的scan-to-map匹配，回环检测以及关键帧的因子图优化。本部分主要有两个环节同步进行，一个单独开辟了回环检测线程，另外一个是lidar数据的回调函数。

功能简介:

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

订阅：

• 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匹配

scan-to-map匹配出现在雷达数据的回调函数laserCloudInfoHandler中，其是激光里程计最基本的位姿估计方法，其进行的操作有：当前帧位姿的初始化、组建局部地图、进行当前帧到局部地图的匹配和位姿优化、用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合。

1.位姿初始化

（1）如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）

（2）后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿（是因子图联合优化后的最优位姿），得到当前帧激光位姿 // note:利用imu里程计信息设置优化的初值

注意：此处的当前帧和上一帧之间的位姿变换 = 由上一帧的imu里程计位姿的逆*当前帧的imu里程计位姿。

2.局部地图map

（1）对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下。这里采用的kdtree进行搜索，是将历史所有关键帧集合存入kdtree中进行搜索。

（2）对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中。局部地图map中的点云超过一定数量时，就会清空下一帧的点云进行scan-to-map时进行重新创建。因此局部地图并不是每一帧点云到来时都进行重新，创建一次，只需要往点云地图中添加新的点云即可。

3.scan-to-map位姿优化

前面位姿初始化时，我们设置的当前帧的位姿主要是通过imu里程计得到，这里对上面的初始化位姿进行优化。

3.1 当前激光帧角点寻找局部map匹配点

（1）更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了。

（2）计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数。

3.2 当前激光帧平面点寻找局部map匹配点

（1）更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了。

（2）计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数。

3.3 scan-to-map优化

上面的角点到直线的距离以及平面点到平面的距离，就是非线性优化中的约束项，并以此构建残差。

对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped

3.4 与imu原始RPY数据加权融合

此处imu使用的是一个九轴的imu相比与六轴imu多出一个全局的imu位姿，此处imu原始RPY就是一个全局的imu位姿的roll、pitch和yaw角。

更新当前帧位姿的roll, pitch, z坐标；因为是小车，roll、pitch是相对稳定的，不会有很大变动，一定程度上可以信赖imu的数据，z是进行高度约束。

二：闭环检测线程

在此.cpp文件的节点中单独开辟了一个回环检测线程，在检测到回环后，在回环检测线程会对当前帧位姿进行一个icp的优化位姿。

1.选取候选闭环帧

在历史关键帧中查找与当前关键帧距离最近的关键帧集合，选择时间相隔较远的一帧作为候选闭环帧，此处同样使用的是kdtree的方法进行搜索。

这里时间的间隔设置的是30s，必须满足时间上超过一定的阈值，才认为是一个有效的回环。

2.提取当前帧特征点集合和组建候选帧区域map

提取当前关键帧特征点集合，降采样；提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样，目的是使用scan-to-map的方法进行优化位姿。

3.scan-to-map优化：icp

执行scan-to-map优化，调用icp方法，得到优化后位姿，构造闭环因子需要的数据，在因子图优化中一并加入更新位姿。

注意：

• 前面也有scan-to-map优化，是基于非线性优化的方法来进行优化的；此处采用的icp点云配准的方法，这种ICP方法视为基于线性优化的变体。

• 闭环的时候没有立即更新当前帧的位姿，而是添加闭环因子，让图优化去更新位姿。

通过调用PCL的icp库可以得到闭环帧到当前帧较为准确的位姿变换，用此位姿变换去矫正当前帧位姿，并作为约束因子加入到gtsam中，等待因子图优化环节一起优化。

三：因子图优化

因子图优化也是写在了雷达数据的回调函数laserCloudInfoHandler中，因子图优化联合了雷达里程计信息（包含IMU信息）、GPS信息和闭环信息一起进行优化。因子图优化是全局关键帧优化，前面的优化都是对当前帧的局部优化。

1.添加关键帧

计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧，非关键帧则跳出，不进行因子图优化。

2.添加激光里程计因子

激光里程计因子是一个帧间约束，前一帧和当前帧（3.3中scan-to-map之后的位姿）的位姿变换。

3. 添加GPS因子

gps的数据会先被一个成熟的节点（在module_navsat.launch文件）处理，我们直接调用的gps的处理结果。

本项目中作者对gps的使用较为谨慎。

4.添加闭环因子

闭环因子就是就是闭环检测线程中计算的当前帧和闭环帧之间的位姿变换。

5.执行因子图优化

执行了因子图优化后，优化器中所有的关键帧位姿都进行了优化。

执行完因子图优化后，清空一下这一轮优化使用的因子图（里面的优化因子），历史数据不会清掉，ISAM保存起来了，只是清空因子图不是清空的优化器。

到此为之，不管是全局优化还是局部优化都完成了，有几个变量需要注意：

// note：当前位姿的一个最优估计(最准确的)，通过以下两种方式更新：

// 1.如果这帧是普通帧，通过scan to map得到

// 2.若果这帧是关键帧，先通过scan to map更新，再通过因子图优化更新

transformTobeMapped[6]

// 历史关键帧位姿

// note：所有的雷达帧位姿会在这里更新 （如果是普通帧，则是因子图优化之前的san-map得到的位姿，如果是关键帧，则是因子图优化得到的位姿）

pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;

pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D;

pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D;

四：发布激光里程计

此处发布了一个增量式里程计信息，用于IMU预计分时使用。此外还有一个优化后的最优位姿，用于IMU里程计使用。

增量式里程计：当前帧（scan to map 后的位姿，没经过回环因子图优化）与前一帧（因子图优化后的位姿）之间的位姿变换 （这是一个平滑的结果）。