legoloam学习总结

1. imageProjection过程

1.1 主要函数

这里主要完成点云的预处理过程，主要看cloudHandler函数，如下：

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){

        // 1. Convert ros message to pcl point cloud
        copyPointCloud(laserCloudMsg);
        // 2. Start and end angle of a scan
        findStartEndAngle();
        // 3. Range image projection
        projectPointCloud();
        // 4. Mark ground points
        groundRemoval();
        // 5. Point cloud segmentation
        cloudSegmentation();
        // 6. Publish all clouds
        publishCloud();
        // 7. Reset parameters for next iteration
        resetParameters();
    }

1. copyPointCloud函数：
   功能：
   （1）将rosmsg转化成pcl点云
   （2）去除NAN点
2. findStartEndAngle()函数：
   功能：
   （1）计算点云第一个点的水平角度
   （2）计算点云最后一个点的水平角度
3. projectPointCloud函数：
   功能：
   （1）对点云中所有的点进行循环，计算得到range图
   （2）range图：行数代表激光雷达的线数，总共16线，列数，代表旋转了几个分辨率0.2度，总共1800
   （3）按照index = columnIdn + rowIdn * Horizon_SCAN;的计算方式，把所有点放到fullcloud里
4. groundRemoval()函数：
   功能：
   （1）分割出地面点，主要看相邻两个扫描线上对应点与地面的夹角
5. cloudSegmentation函数：
   功能：
   （1）对点云（除掉地面点）进行分类，筛选出一些有效的点。

1.2 总结：

可以看出：
（1）只进行了点云的分割和筛选，没有对imu做处理，没有对畸变之类的做处理
（2）没有进行坐标的变换。

2. featureAssociation过程

2.1 主要函数

主要流程函数如下：

        adjustDistortion();

        calculateSmoothness();

        markOccludedPoints();

        extractFeatures();

        publishCloud(); // cloud for visualization
	
        /**
		2. Feature Association
        */
        if (!systemInitedLM) {
            checkSystemInitialization();
            return;
        }

        updateInitialGuess();

        updateTransformation();

        integrateTransformation();

        publishOdometry();

        publishCloudsLast(); // cloud to mapOptimization

1. adjustDistortion函数：
   （1）进行了点云坐标的变换
   （2）对segment出来的点云中的每一个点进行循环
   （3）计算每一个点的相对时间
   （4）在当前点时间前后寻找imu的计算数值
   （4）对于当前激光点，通过插值得到当前点对应的imu得出的位姿状态
   （5）将当前激光点的位置坐标进行变换，表达在本帧第一个点的车辆坐标系下

这个函数是对激光点进行处理，不涉及imu的问题

2. calculateSmoothness函数：
   （1）这个函数利用imageProjecttion中求出的range图，计算每个特征点的curvature，注意这里应该是横着的curvature，也就对应竖着的边。

3. markOccludedPoints函数
   去除遮挡点

4. extractFeatures函数：
   提取特征点
   （1）分成平面特征点和curvature变化剧烈的特征点

5. checkSystemInitialization函数：
   （1）用来初始化
   （2）将lesssharp和lessflat的特征点用来构建kd树
   （3）将transformSum赋初值，这里感觉很怪异，是用imu对应世界坐标系的pitch和roll作为初值的意思?

6. updateInitialGuess函数：
   这个函数对待求解的transformCur变量给一个初值，
   平移变换部分给的初值0；
   角度变换部分给的初值为imu积分出来的，这里为啥不用rollpitchyaw值直接算出来？而是要积分出来

7. updateTransformation函数：
   （1）找到平面特征对应点/sharp特征对应点
   （2）计算变换

8. integrateTransformation函数：
   积分得到一条路径

3. mapOptimization过程

3.1 主函数主要流程

如下主要函数run所示：

                transformAssociateToMap();

                extractSurroundingKeyFrames();

                downsampleCurrentScan();

                scan2MapOptimization();

                saveKeyFramesAndFactor();

                correctPoses();

                publishTF();

                publishKeyPosesAndFrames();

                clearCloud();

3.1.1 transformAssociateToMap函数

这个函数专门开贴讲过：
https://blog.csdn.net/l898985121/article/details/114844864

3.1.2 extractSurroundingKeyFrames函数

1. 如果周围的关键帧数目为0，就直接return
2. 不考虑loopclosure的情况
   将cloudKeyPoses3D结构放入kdtree，便于后续查找
   cloudKeyPoses3D这个结构存放的关键帧对应的车辆位姿
   在车辆当前位置为中心，50m为半径的圆内，查找并返回所有的关键帧
   将关键帧内的点放到一个点云里，并进行下采样，得到surroundingKeyPosesDS数据结构
   同时还有上一帧处理过的一个结构：surroundingExistingKeyPosesID
   下面又两个双重循环，完成两个数据结构的对齐，示意图如下：
   首先，有两个数据结构，分别存储了关键帧的id，这里就用1,2，3，4.。表示。
   
   在第一次双循环结束后得到下图。可以看到是将surroundingExistingKeyPosesID中的关键帧进行删除，删除那些不在surroundingKeyPosesDS中的关键帧。
   

第二次双循环后结果如下。可以看到是将surroundingKeyPosesDS中有，但是surroundingExistingKeyPosesID中没有的关键帧，添加到surroundingExistingKeyPosesID中。

总结：
surroundingKeyPosesDS存储的是，当前车辆位置处，车辆能看到的关键帧。surroundingExistingKeyPosesID存储的是上一个车辆位置处，车辆看到的关键帧，双循环的目的就是将上一次的看到的关键帧与这一次看到的进行对齐。

3. 考虑loopclosure的情况
   to be done

3.1.3 downsampleCurrentScan函数

将读入的一帧一帧数据进行降采样，得到稀疏的表示。

3.1.4 scan2MapOptimization函数

1. 当提取的地图上的特征点数目大于阈值，则进行优化

2. 将地图角点和地图平面点分别放进kdtree

3. 进行十次优化

4. 先进行角点相关系数求解

5. 再进行平面点相关系数求解

6. 最后进行一次LM优化
   这里的方法也和loam里差不多好像

7. 通过transformupdate函数，与imu的数据进行一次简单的加权平均融合

8. 注意：在这个函数中，并没有用到gtsam的优化

3.1.5 saveKeyFramesAndFactor函数

1. 以优化求解后的位姿作为机器人当前位姿

2. 看看这一帧和上一帧的位姿差别大不大（0.3m），大就可以更新

3. 再看看是不是第一帧

4. 如果是第一帧就直接插入gtsam位姿，并进行初始化

5. 如果不是第一帧

6. 则向gtsam中添加相应约束

7. isam进行更新，求得最新的车辆当前位置，并用这个位置更新transformAftMapped

8. 注意：transformAftMapped一共有两处更新，一处是完成LMoptimization后，会进行一次更新，另一次是在这里，进行图优化完成后的更新

9. 添加相应的关键帧

3.1.6 correctPoses 函数

1. 只有当loopclosed的时候进行一次
2. 对所有的cloudKeyPoses3D进行一次更新，利用的就是闭环后，完成图优化后的位姿

3.2 闭环检测线程主要流程

主要就是在一个while循环里，调用performLoopClosure

void loopClosureThread(){

        if (loopClosureEnableFlag == false)
            return;

        ros::Rate rate(1);
        while (ros::ok()){
            rate.sleep();
            performLoopClosure();
        }
    }

3.2.1 performLoopClosure函数

1. 先看看关键帧如果是空的，就退出
2. 通过detectLoopClosure找到待匹配的帧和局部地图点云
3. 利用icp进行匹配，得到一个位姿变换约束
4. 将该约束添加到gtsam中。并对gtsam进行更新。

3.2.2 detectLoopClosure函数

1. 将关键帧位姿cloudKeyPoses3D构造成kdtree
2. 在当前车辆位姿，也就是优化后的位姿附近搜索得到附近的关键帧位姿
3. 找到的附近点，要距离本次时间点大于30s，如果没有点大于30s，就退出
4. 如果有点大于30s了
5. 把最新的一帧keyframe取出来，把点存出来
6. 构建一个用于匹配的局部地图，见下面代码
   这段代码当中的±25是指，在所选取的帧，前后各选25帧，一共50帧累加到一起形成局部地图

for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j){
            if (closestHistoryFrameID + j < 0 || closestHistoryFrameID + j > latestFrameIDLoopCloure)
                continue;
            // 要求closestHistoryFrameID + j在0到cloudKeyPoses3D->points.size()-1之间,不能超过索引
            *nearHistorySurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[closestHistoryFrameID+j], &cloudKeyPoses6D->points[closestHistoryFrameID+j]);
            *nearHistorySurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[closestHistoryFrameID+j],   &cloudKeyPoses6D->points[closestHistoryFrameID+j]);
        }

7. 对局部地图点进行下采样。