Joeybee的博客

上一节中我们看过了findCorrespondingSurfFeatures和calculateTransformationSurf函数。中求点到直线法向量的代码和findCorrespondingSurfFeatures求平面法向量有很大区别，所以我们详细介绍一下。然后calculateTransformationCorner由点到线特征的距离求雅可比来优化位姿的代码和calculateTransformationSurf类似，我们不多关注了：！！！

在ISAM2中，由于处理的是非线性问题，状态变量的误差可能会变得非常大，导致线性化近似不再有效。当状态变量的误差超过这个阈值时，ISAM2会重新线性化这些变量，以保持优化过程的准确性。滤波器通过将点云中的点分配到一个三维体素网格中，并只保留每个体素（或“叶”）中的一个点（通常是中心点或最近的点），从而减少点云数据的数量。创建的线程是并发执行的，这意味着它们可以同时运行，而不受主线程的直接控制。下面我们来看mapOptimization的私有变量定义，不详细解释，筛一些有意思的说一下。

correctPoses()主要用来判断是否发生了回环检测，如果有回环就更新当前关键帧的位姿和位置。laserCloudCornerLastDS等是当前帧的角点经位姿变化到地图坐标系下的点云数据。发布的数据有历史关键帧位置数据cloudKeyPoses3D，这就是rviz里面轨迹的那条线。至此，lego-loam主要的代码就都注释完啦！完结撒花，咱们lio-sam再见！laserCloudSurfFromMapDS是子地图数据。

我们先来看和主线程并列的回环检测线程，然后再来看函数。

transformAftMapped是上一帧优化后的位姿，transformTobeMapped是当前帧优化后的位姿。所以我们按cloudKeyPoses3D就是空的，先看return之后的代码，也就是下一个函数downsampleCurrentScan()；初始的cloudKeyPoses3D就是空的，它的值会在saveKeyFramesAndFactor()函数中更新，所以可以。我的理解就是，当前的平移量差值△r，是在上一次位姿优化后，由于位姿变换产生的，我们先把它变换回上一次的位姿，也就是左乘。

发布的数据有离群点，角特征点和面特征点。skipFrameNum初值为1。所以frameCount == 2时才发布，也就是两帧一发布，建图的频率比雷达里程计慢一半。！！终于把featureAssociation注释完啦！！下一节进入mapOptmization！！

回到runFeatureAssociation()函数。回到Feature Association，我们来看下一个函数：integrateTransformation();

上一次我们已经看完了特征提取，这次我们进入特征匹配的内容！！！systemInitedLM的初始值为false。

我们接着看特征提取的第二个函数。

此时点云被投影到第一个点的imu坐标系中。补充：！！！imu的测量值是变化，不可以直接累积，在imu数据预处理中，通过一重积分、二重积分，算出积分值再累积！！！//imuPointLast的初值-1。在回调函数中：插值细节补充！！！

这个主函数就是一个很典型的ros框架的主函数！！主要来看FeatureAssociation类和runFeatureAssociation函数！！

咱们接着往下看！！！

马上就注释完啦!!加油！！我们继续看回调函数 publishCloud()

继续来看回调函数。

/用于确保在创建新对象时，对象的内存对齐符合Eigen库的要求。关键字用于声明一个变量或函数是在另一个文件中定义的。在一些激光雷达系统中，不同的环编号代表不同的扫描层或不同的方位角，这个字段有助于区分这些不同的层或角度。//两种不同的点云格式对应不同的激光雷达，对于Velodyne VLP-16这种激光雷达的数据结构就是XYZIR。分别表示边缘特征和表面特征的阈值,很奇怪边缘特征和表面特征的阈值为什么是一样的？//分别表示水平和垂直方向上的角分辨率，单位为弧度。结构体的对象在内存中正确对齐，以提高性能。

private://常规ros初始化三板斧之一，nh非常有用，可以用来发布数据、接收数据、设置参数、获取参数。//接收原始点云数据//从发布的数据可以看出，做了哪些处理：提取地面点，分割点云（刨除聚类中比较零散的点，然后再对大的聚类做分割），刨除异常点（或者是不可视的点），保留有效点云。具体的内容看下面的代码。//解决了上一篇中的一个问题！！！之所以没有定义PointXYZIR是因为pcl中有现成的定义！！！//这两个没有看懂，后续具体看用法。