LeGo-LOAM框架后端优化总结

LeGo-LOAM是发表于IROS2018年的文章，全称为：Lightweight and Ground-Optimize Lidar Odometry and Mapping on Variable Terrain.

一、Lidar Mapping 原理

流程：

1. transformAsscoiateToMap()
   根据trasnformSum 和 transformAftMapped得到transformTobeMapped
2. extractSurroundingKeyFrames()
   提取周围关键帧组成submap
3. downSampleCurrentScan()
   下采样当前帧
4. scan2MapOptimization()
   scan to map 的优化
5. saveKeyFramesAndFactor()
   保存关键帧和因子
6. correctPoses()
   校正位姿

其中1、3、4与LOAM中的处理基本一致。

二、提取周围关键帧组成SubMap

在extractSurroundingKeyFrames()函数中，若回环检测功能开启，则加载历史中最近的50个关键帧形成点云地图（当组成SubMap的关键帧少于50帧时，直接添加即可；当组成SubMap的关键帧等于50帧时，添加新的关键帧之前需要剔除最初的关键帧）；若回环检测功能关闭，则加载历史中最近的50个关键帧形成点云地图。 （对于已有的SubMap每次需要删除不在周围区域的关键帧）。

三、保存关键帧和因子

选择关键帧：当前帧和之前帧的距离大于0.3米

        bool saveThisKeyFrame = true;
        if (sqrt((previousRobotPosPoint.x-currentRobotPosPoint.x)*(previousRobotPosPoint.x-currentRobotPosPoint.x)
                +(previousRobotPosPoint.y-currentRobotPosPoint.y)*(previousRobotPosPoint.y-currentRobotPosPoint.y)
                +(previousRobotPosPoint.z-currentRobotPosPoint.z)*(previousRobotPosPoint.z-currentRobotPosPoint.z)) < 0.3){
            saveThisKeyFrame = false;
        }
        // saveThisKeyFrame为false，并且cloudKeyPoses3D不为空
        if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())
        	return

gtsam插入先验因子：

gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]), Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));
initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])));

gtsam插入里程计因子，更新关键帧：

gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]),Point3(transformLast[5], transformLast[3], transformLast[4]));
gtsam::Pose3 poseTo   = Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]));
gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->points.size()-1, cloudKeyPoses3D->points.size(), poseFrom.between(poseTo), odometryNoise));
initialEstimate.insert(cloudKeyPoses3D->points.size(), Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4])));

更新gtsam：

isam->update(gtSAMgraph, initialEstimate);
isam->update();

gtSAMgraph.resize(0);
initialEstimate.clear();

四、回环检测

回环检测可以消除漂移(drift)，通过ICP算法对比当前帧和之前帧是否匹配，如果匹配则进行图优化。回环检测在loopClosureThread中进行。

基本选择原则：

1 将关键帧点云建立kdtree，根据当前位置点，搜索出一定距离范围内的点云；
2 同时遍历距离由近到远的点，其时间与当前时间间隔在30s以上认为检测到回环；
3 根据搜到的回环帧，合并周围的其周围多帧的点云，以进行后续的回环检测；

如果检测到回环之后，接着进行ICP匹配，然后进行图优化。

icp配准：

1 将当前帧点云与回环检测出的邻近帧点云进行icp配准，得到位姿变换矩阵；
2 更新图，进行优化；