zysss_的博客

该流程图描述了LIO-SAM整个系统的话题是如何相互作用的，流程图原文件在这里。不需要积分分享给大家。

这部分主要是讲如何添加odom因子、gps因子、回环因子和获取优化后的信息给到下一次优化使用，讲解函数void saveKeyFramesAndFactor()

回环检测思路描述：主要是通过里程计判断回环，根据最后一个关键帧的平移信息，寻找离他15米内的其它关键帧，并且两帧的时间戳相差要大于30s，上面两个条件都满足则认为是找到了回环帧，开始用icp匹配计算位姿，算完后把两帧索引，两帧相对位姿，噪声（icp得分）放入回环约束队列中，对回环因子的使用，和因子图的更新在函数saveKeyFramesAndFactor()中，后面会详解

分别将局部地图的角点和面点放入局部地图的kdtree中（注意这里的角点面点不是当前帧的，是局部地图的，在上一个函数中筛选出来的），LIO-SAM中初值计算及局部地图构建中有详细解析局部地图构建，extractSurroundingKeyFrames()函数构建的局部地图是把那些点都筛选出来了，但是还没有加入kdtree中，局部地图其实就是通过kd_tree提取离当前帧一定距离内的关键帧

下面详解的两个函数主要作用 是更新当前匹配结果的初始位姿 和 提取当前帧相关的关键帧并且构建点云局部地图

- 提取角点：根据之前算的sp、ep，即每个scan的起始和结束id，来将每根的所有点进行6等份，当点不是遮挡点并且曲率大于边缘点门限则认为是角点，每一份只找最多20个点，将这些点手机进cornerCloud，每提取一个角点的时候都将该点左右各5个点设置为遮挡点避免太集中（设定为遮挡点后就通过不了角点判断了）- 提取面点：把提取出来的有效点云转成pcl格式pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud

这是imageProjection.cpp中的 **ImageProjection IP**类前端点云预处理其实就是提取运动补偿信息、用提取的信息对点进行补偿到最开始的点的时刻、保存有效点云数据信息和确定计算曲率的点索引的起止点，实际操作在原始点云回调函数中

实际操作简述： 说白了就是拿到优化后的全局位姿和imu的局部增量位姿，然后加起来就是当前时刻的位姿了，具体操作在下面第4点，这样就能高频输出比较准确的位姿了，全局位姿在后端优化比较久

这里将通过代码解析预积分这个节点的运行顺行。

（个人理解为，假设调整第 k 时刻的位姿，但是 k 到 k+1时刻的IMU积分项是不会变的，因为这就是个固定的测量值，但是根据最上面的公式可以看到，当调整 k 时刻位姿时 第 k+1 时刻就要重新积分，而在图优化中，关键帧的位姿在不停的调整是必然且正常的，每次调整都要使得第 k+1 帧的位姿重新积分很耗时间）乘了以后，所有状态量都和世界坐标系无关，都变成只和当前起始的 第 k 帧有关，即和前一个关键帧相关。首先看 IMU 原始积分的公式 ，两个关键帧。实际的系统状态是离散的，则变成如下。......

实际上此种地⾯分离算法有⼀些简单，我们可以结合激光雷达安装⾼度等其他先验信息进⾏优化,避免将桌子这些当作地面点。LeGO-LOAM中前端改进中很重要的⼀点就是充分利⽤了地⾯点，那⾸先⾃然是提取对地⾯点的提取。理想情况下， 应该接近0,考虑到⼀⽅⾯激光雷达安装也⽆法做到绝对⽔平，另⼀⽅⾯，地⾯也不是绝。对⽔平，因此，这个⾓度会略微⼤于0,考虑到作者实际在草坪之类的场景下运动，因此这个值被设置成。如上图，相邻的两个扫描线束的同⼀列打在地⾯上如AB点所⽰，他们的垂直⾼度差。，计算垂直⾼度差和⽔平⾼度差的⾓度。..

A-LOAM的整体流程框图如下：算法主要就是这3个cpp文件。

A-LOAM算法流程：主要运行以下3个cpp文件，算法流程框图在文末

在后端的当前帧和地图匹配的时候，我们就需要从地图⾥寻找线特征和⾯特征的约束对，此时，由于没有了线束信息，我们就需要采取额外的操作来判断其是否符合线特征和⾯特征的给定约束。注意这里的A、B、C不是之前的ABC，A=A/D···，下式就是上式整体除以D，下式也就是三个未知数，五个⽅程（也是找5个点），写成矩阵的形式就是⼀个5×3⼤⼩的矩阵，求出结果之后，我们还需要对结果进⾏校验，来观察其是否符合平⾯约束，具体就是分别求出5个点到求出平⾯的距离，如果太远，则说明该平⾯拟合不成功。，这里利用平面方程构建约束。...

不同于前端的scan-to-scan的过程，LOAM的后端是scan-to-map的算法，具体来说就是把当前帧和地图进⾏匹配，得到更准的位姿同时也可以构建更好的地图。由于是scan-to-map的算法，因此计算量会明显⾼于scan-to-scan的前端，所以，后端通常处于⼀个低频的运⾏频率，但是由于scan-to-map的精度往往优于scan-to-scan，因此后端也有着⽐起前端来说更⾼的精度。.........

算法流程主要用作梳理整个雷达里程计，先在理解上知道代码会做什么，和这样做的原因，然后再看代码实现，代码实现中的注释和算法流程的梳理是一 一对应的，可以对比着代码看这个流程代码位于"laserOdometry.cpp"的main函数开始，以下为该函数的运行步骤notic:订阅到的点云是经过特征点处理与均匀化的，具体订阅的消息见 A-LOAM中的特征提取及均匀化-算法流程+代码注释 中“每个scan提取特征”代码末尾的发布ros::spin() 和 ros::spinOnce() 区别及详解 总的来说就是，sp

我们知道激光雷达的⼀帧数据是过去⼀段时间⽽⾮某个时刻的数据，因此在这⼀帧时间内的激光雷达或者其载体通常会发⽣运动，因此，这⼀帧数据的原点都不⼀致，会导致⼀些问题，⽐如这个代表⼀辆⻋往墙的⽅向运动，原则上这个墙应该是⽔平的，但是由于上述原因，最终该帧的数据如下。..................

1. 先看 (b) ，取A点的距离（距离为雷达到A点），取B点的距离，计算两个点的列id之差 2. 根据 **列id之差**判断两点是否邻近，只有邻近才有意义去判断剔除 3. 先判断 *depthA-depthB>0.3* 满足的话则将A点的**左边5个点**设置为无效点置1 4. 然后判断 *depthB-depthA>0.3* 满足的话则将B点的**右边5个点**设置为无效点置1 5. 然后看 (a) ，**计算点与其邻近点的距离**，距离过大则很可能为平行点 6. 先看平行点的情况：取B点

这个函数是个回调函数，在订阅到点云数据的时候才会运行，ALOAM中的点云接收频率为先判断初始化，等待几帧来到后才认定为初始化成功进行线束id计算与每个点的时间戳，并将点云存到其对应的个数组中标记每个线束的最左5与最右5不参与曲率计算开始曲率计算，：这里实际处理是将全部点云都参与曲率计算了，计算曲率的公式可以这样认为x1+x2+x3+x4+x5−10∗x6+x7+x8+x9+x10+x11开始特征提取，用到第4点的曲率信息。.................................