如何基于ROS学习激光SLAM?

lio-sam，紧耦合的雷达和IMU激光slam方法

imageProjection部分：

主要的功能为点云去畸变，需要点云中每个点包含有time这个标签。

首先imuQueue将每一个imu原始信息变换到雷达系下，依次入列。

imuRotX，imuRotY，imuRotZ，以及imuTime这几个队列里面存放的是相邻两帧之间的imu角速度积分得到的旋转角度，第一个为0。

这里cloudInfo的初始值由OdomMsg提供。本来imu用来进行角度畸变去除，odom进行平移畸变去除。但是由于平移畸变太小，所以没有用。

ProjectPointCloud(), 此函数的主要作用为利用去畸变得到的点云做rangeMat的投影。根据点的实际位置算出其相应的行列坐标。

cloudExtraction(), 此函数的主要作用为统计有效点的数量，利用之前的rangeMat，只取在其上有效的点。点的总数量为count。另外，该函数还根据不同的ring获得了每一个ring的起始点云索引。

最后，publishClouds()函数的作用为发布提取的有效的点云以及cloudInfo。

cloudInfo包含如下信息：

imuPreintegration部分

transfusion这个对象主要的作用为可视化轨迹和odom，作用不是很大，subLaserOdometry接受的话题为mapping部分的里程计消息，然后作为一个低频的lidar odom入列。

subImuOdometry接受的话题为预积分后的imu里程计消息，这个消息作为一个高频的消息，首先入列。然后根据当前时刻和上一帧的雷达里程计时刻之间的imu odom消息。根据坐标变换得到两个时刻之间的坐标增量。用该增量乘之前的雷达里程计，发布imu频率的odom，以及path。

IMUPreintegration对象是imu预积分的主要对象。该对象订阅imu的原始消息以及lio_sam/mapping/odometry_incremental。

其中odometryHandler主要作用为接受低频的mapping的odom消息，加入imu预积分的因子图中，然后优化imu预积分。注意，由于imu的消息在另一个进程中不断入列，所以在while循环退出后需要继续进行一下imu预积分（当imu频率不是很高的时候）。

imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);

imuHandler的作用就是根据每次更新的bias以imu的频率输出。

FeatureExtraction部分

主要的功能为提取去畸变后的角点和平面点。

calculateSmoothness()根据公式计算每一个点云的光滑度，这里注意，当点在建筑物的边缘上时，曲率很大，必然是角点。

markOccludedPoints()主要的功能为滤除遮挡点，遮挡点定义如下，A点和B点是相邻点，但是A点在B点后面很多，所以A点之前的5个点很容易被遮挡，因此他们被滤除。平行点。当激光照射到一个和激光入射角接近的平面时，很容易误识别角点，所以这些点被滤除。

extractFeatures()首先空间区域被均匀分成六份，每个区域的点按照光滑度有小到大排列，每个区域至多取20个角点。另外，当取得一个角点时，为防止聚集，他周围10个相邻像素的点都会被滤除。

publishFeatureCloud() 该函数的主要作用为发布特征点云。

cloudInfo包含有cloud_corner和cloud_surface两个点云。

mapOptimization部分

主要的功能就在于构建因子图实现全局优化，重点的函数是帧间匹配的LM算法。

laserCloudHandler()接受imageProjection节点中去畸变后的点云，该点云有三部分的消息，去畸变的点云的角点和平面点，imu的原始信息，imu里程计的信息。

1. cornerOptimization()，角点优化函数。首先将当前帧的一个角点变化到map系，然后寻找离他最近的五个角点，判断这五个角点是否在一条直线上。如果是，保留三个变量，原始角点，距离变量，flag。
2. surfOptimization()，平面点优化函数。首先选择最近的五个平面点，判断其是否可以组成平面，如果可以，则保留原始平面点，距离变量，flag。
3. combineOptimizationCoeffs()，联合优化函数。将上述的角点和平面点的相关信息一起推入laserCloudOri和coeffSel两个量中。
4. LMOptimization()，LM优化过程，最繁琐的一块。使用的是张继的LOAM的一套，推导很麻烦，反正一句话，优化了transformTobeMapped这个量，但说是LM，看起来是牛顿欧拉法。
5. transformUpdate()，更新优化后得到的帧间变换。但注意这个也是更新transformTobeMapped，不同之处在于加权了IMU的原始信息，为啥怎么相信IMU呢？。

1. addOdomFactor()，将相邻两帧之间的关键帧加入因子图，增量用transformTobeMapped的值
2. addGPSFactor()，如果没有关键帧，或者首尾关键帧距离小于5m，不添加gps因子，位姿协方差很小，没必要加入GPS数据进行校正，每隔5m添加一个GPS里程计。
3. addLoopFactor()，闭环边对应两帧的索引，闭环边的位姿变换。加入因子图后，清空loopIndexQueue();
4. 加入cloudKeyPoses3D以及cloudKeyPoses6D。poseCovariance保留的是位姿的协方差，判断是否需要加GPS因子用的。最后更新一下transformTobeMapped。cornerCloudKeyFrames和surfCloudKeyFrames压入当前帧的角点和平面点信息。
5. updatePath()，把当前帧的位姿信息压入globalPath。

loopClosureThread()回环线程

回环线程需要用到icp，这是一个非常耗时的操作，因此回环需要精确确定，而且回环的间隔要较大，这里不只是两个节点要相隔一段时间，也需要两个回环之间不要靠太近。

1. detectLoopClosureExternal，检测外部回环的程序，这里没有用到
2. detectLoopClosureDistance，判断回环两个节点之间的时间戳
3. loopFindNearKeyframes()，提取key附近的点云包括角点和平面点。之后liyongICP得到两个关键帧之间的位姿变换关系，加入因子图。loopIndexQueue压入当前信息，loopIndexContainer压入防止多次将一个回环加入因子图和展示。

visualizeGlobalMapThread()线程

以一定的频率发布GlobalMap，和提供saveMap的服务saveMapService