LIO_SAM框架的一些学习记录和细节
开源代码地址：https://github.com/TixiaoShan/LIO-SAM
5、mapOptimization细节
cornerOptimization()，角点优化函数。首先将当前帧的一个角点变化到map系，然后寻找离他最近的五个角点，判断这五个角点是否在一条直线上。如果是，保留三个变量，原始角点，距离变量，flag。
surfOptimization()，平面点优化函数。首先选择最近的五个平面点，判断其是否可以组成平面，如果可以，则保留原始平面点，距离变量，flag。
combineOptimizationCoeffs()，联合优化函数。将上述的角点和平面点的相关信息一起推入laserCloudOri和coeffSel两个量中。
LMOptimization()，LM优化过程，最繁琐的一块。使用的是张继的LOAM的一套，推导很麻烦，反正一句话，优化了transformTobeMapped这个量，但说是LM，看起来是牛顿欧拉法。 transformUpdate()，更新优化后得到的帧间变换。但注意这个也是更新transformTobeMapped，不同之处在于加权了IMU的原始信息，为啥怎么相信IMU呢？。
addOdomFactor()，将相邻两帧之间的关键帧加入因子图，增量用transformTobeMapped的值。
addGPSFactor()，如果没有关键帧，或者首尾关键帧距离小于5m，不添加gps因子，位姿协方差很小，没必要加入GPS数据进行校正，每隔5m添加一个GPS里程计。
addLoopFactor()，闭环边对应两帧的索引，闭环边的位姿变换。加入因子图后，清空loopIndexQueue();
加入cloudKeyPoses3D以及cloudKeyPoses6D。poseCovariance保留的是位姿的协方差，判断是否需要加GPS因子用的。最后更新一下transformTobeMapped。cornerCloudKeyFrames和surfCloudKeyFrames压入当前帧的角点和平面点信息。 updatePath()，把当前帧的位姿信息压入globalPath。
correctPoses()，当存在了回环后，需要及时更新cloudKeyPoses3D以及cloudKeyPoses6D的位姿信息，然后globalPath也要更新一下。
publishOdometry()，发布odom消息以及lidar_link到odom的tf变换。increOdomAffine是优化过后的当前帧位姿odom在map系下的表示，之后在和imu加权一下。pubLaserOdometryIncremental.publish(laserOdomIncremental)，发布这一里程计，给imu预积分使用。
publishFrames()，发布一些cloud和path之类。
6、loopClosureThread()回环线程
回环线程需要用到icp，这是一个非常耗时的操作，因此回环需要精确确定，而且回环的间隔要较大，这里不只是两个节点要相隔一段时间，也需要两个回环之间不要靠太近。
performLoopClosure() detectLoopClosureExternal，检测外部回环的程序，这里没有用到 detectLoopClosureDistance，判断回环两个节点之间的时间戳 loopFindNearKeyframes()，提取key附近的点云包括角点和平面点。之后liyongICP得到两个关键帧之间的位姿变换关系，加入因子图。loopIndexQueue压入当前信息，loopIndexContainer压入防止多次将一个回环加入因子图和展示。 visualizeLoopClosure()，在rviz展示回环。
7、visualizeGlobalMapThread()线程
以一定的频率发布GlobalMap，和提供saveMap的服务saveMapService