LVI-SAM 代码解析
浏览开源代码LVI-SAM，记录自己的理解。
概述
项目分为7个部分（程序）：visual_feature、visual_odometry、visual_loop、imuPreintegration、imageProjection、featureExtraction、mapOptimization。
LIO-SAM中各模块之间的ROS消息收发结构。
LIO-SAM的因子图结构如下：
该项目整体流程如下：
- IMU实时推算/优化——imuPreintegration。该模块定于Lidar odometry消息，与IMU数据一起进行优化，优化结果作为实时输出的校正值。
- 激光运动畸变校正——imageProjection。利用当前帧起止时刻之间的IMU数据、IMU里程计数据计算预积分，得到每一时刻的激光点位姿，从而变换到初始时刻激光点坐标系下，实现校正。
- 提取特征——featureExtraction。对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点特征。
- scan-to-map匹配——mapOptimization。提取局部关键帧map的特征点，与当前帧特征点执行scan-to-map匹配，更新当前帧的位姿。
- 因子图优化——mapOptimization。添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿。
- 闭环检测——mapOptimization。在历史关键帧中找候选闭环匹配帧，执行scan-to-map匹配，得到位姿变换，构建闭环因子，加入到因子图中一并优化。
imuPreintegration
订阅IMU和/lidar/mapping/odometry消息，估计IMU bias，与lidar进行图优化输出实时预积分结果。
IMU处理线程中，将IMU消息放入队列中，积分最新IMU数据，获得最新位置并发布到odometry/imu和/lidar/imu/path。
Lidar odometry处理线程中，将Lidar的odometry作为观测，与IMU预积分factor、bias between factor共同优化后，作为IMU线程的最新状态，继续预积分输出。
疑问：两个线程有公用内容，但是未用mutex保护！！！！
使用 ISAM2 优化器，为了保证运行速度，每加入100个factor就reset一次，reset前手动进行边缘化操作。
参考点：优化完成后，进行失败检测failureDetection，优化出的速度>30m/s，或imu bias >0.1m/ss、rad/s，重设优化器。
imageProjection 激光点云运动校正
利用imuPreintegration中的信息对每帧激光的每一时刻激光点进行运动畸变校正。
订阅IMU消息，存放到imuQueue中。
订阅/vins/odometry/imu_propagate_ros，存放到odomQueue中。
订阅点云数据：
cachePointCloud
只处理点云数据。
- 存放到cloudQueue中，size<=2 return，始终保持有3帧数据。
- 取出第0帧，并pop。第0帧作为timeScanCur，第1帧作为timeScanNext，第2帧（最新）暂不使用？
- 确保is_dense==true、fields[i].name中有“ring”（激光线数）和“time”（相对于雷达帧时间的delta t），否则shutdown。
deskewInfo
加imu和odo的锁
imuDeskewInfo
- 从imuQueue中pop出timeScanCur-0.01之前的IMU数据。
- 直接从IMU的消息中获取roll、pitch、yaw？？？
- 计算(timeScanCur，timeScanNext + 0.01]区间内每个轴的陀螺角速度的累计角度到imuRotX、imuRotY、imuRotZ输出，每个IMU时刻作为数组一个元素。
odomDeskewInfo
从imu_propagate_ros发出的消息中，获取timeScanCur之后的一个预积分位姿作为cloudInfo和transBegin的位姿，获取timeScanNext之后的一个预积分位姿作为transEnd，需要保证两个位姿之间未进行优化，即相同的resetID。
根据transBegin和transEnd计算odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre。
projectPointCloud
对0～激光雷达线数内的点，滤除1m内的点，将三维距离存放到rangeMat（二维）。
findRotation： 从imuRot中找该点对应的旋转，时间在两个IMU之间时，用前后两帧rot根据时间做插值获取。
findPosition：设置pos为0，为何没有用odom获取？
计算相对第0个雷达点的位姿，然后将该点的坐标转换到第0个点时刻的坐标。不是相对timeScanCur的？？？
将坐标存放到fullCloud->points一维数组中。
cloudExtraction
将rageMat中的点按照行的顺序存放到cloudInfo.pointRange中，并记录每行对应的pointRange的起止索引。同时将fullCloud->points存储到extractedCloud中。
publishClouds();
resetParameters();
featureExtraction 点云特征提取
对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点（用曲率的大小进行判定）。
订阅imageProjection发布的/lidar/deskew/cloud_info。
calculateSmoothness
计算每个点的曲率，存储到cloudSmoothness
曲率计算公式：
dr = di-5+di-4+di-3+di-2+di-1-10*+di+di+1+di+2+di+3+di+4+di+5
curvature = dr * dr
markOccludedPoints
标记遮挡点和平行光束点.
遮挡点：10个水平像素内，距离超过0.3m，认为存在遮挡。
平行光束点：前后点的距离差大于距离的0.02倍。
extractFeatures
提取corner和surface点。
- 遍历扫描线，每根扫描线扫描一周的点云划分为6段，针对每段提取20个角点、不限数量的平面点，加入角点集合、平面点集合
- 认为非角点的点都是平面点，加入平面点云集合，最后降采样。
publishFeatureCloud();
mapOptimization
main中创建3个任务：mapOptimization、loopDetectionthread、visualizeMapThread。
mapOptimization任务
mapOptimization创建后会订阅多个消息，包括雷达点云/lidar/feature/cloud_info、odometry/gps、/vins/loop/match_frame。
输出消息：/lidar/mapping/trajectory、/lidar/mapping/odometry
laserCloudInfoHandler Lidar点云信息处理
- updateInitialGuess
- 若系统未初始化，使用点云中的IMU 姿态角初始化。
- 如果odom（VINS）可用，根据点云信息中的odom x/y/z/r/p/y 计算地图坐标系中的位置，odom中信息是相对起点第一帧的里程计信息，然后返回。
- 如果imu可用、odom不可用，使用IMU信息更新本帧的姿态信息，注意仅更新姿态信息。
- extractSurroundingKeyFrames
cloudKeyPoses3D中存放的是laser帧的x、y、z和intensity（在cloudKeyPoses3D中的index）。
cloudKeyPoses6D中存放的是laser帧的xyz、rpy、time和intensity（在cloudKeyPoses6D中的index）。
- 使用kd-tree方法搜最后一个关键帧附近的关键帧。为什么不是根据Guess提取near关键帧？？
- 降采样帧。
- 将10s内的帧也放到局部帧中。
- 多线程并行处理点云：将最后一帧与局部帧中距离大的帧去除 （为什么还要把近10s内的帧放进去呢？前面kd-tree获取附近帧的时候已经把距离近的放进去了）。将corner和surf点根据cloudKeyPoses6D中的位姿计算新的点云，放到laserCloudCornerFromMap和laserCloudSurfFromMap中，然后降采样关键帧（地图）中的点云。
- downsampleCurrentScan 将当前帧点云中的角点和面点下采样。
- scan2MapOptimization
当角点和面点的数量都满足要求时，将附近帧（地图）corner和surf点放入kdtree，进行cornerOptimization、surfOptimization、combineOptimizationCoeffs。
最大迭代30次，当deltaR < 0.05 && deltaT < 0.05时，停止迭代。
其中用到了多线程对for循环处理点云进行加速：
#pragma omp parallel for num_threads(numberOfCores)
- cornerOptimization 多线程并行处理
–将corner点用updateInitialGuess得到的位姿转换到地图坐标系中，从附近帧corner点云中ktree搜索最近的5个点。
–如果五个点的距离都小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了。
–计算该corner点到线的距离、垂向量等参数，作为角点参数存储起来。 - surfOptimization 多线程并行处理
–将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了。
–计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数。 - combineOptimizationCoeffs 把匹配好的corner点和surf点都放入laserCloudOri，并且存储相应的参数。
- LMOptimization scan-to-map优化，对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存到transformTobeMapped。
- 迭代完成后，会对计算的位姿与IMU原始roll、pitch融合，并进行限幅。
- saveKeyFramesAndFactor
- saveFrame 当该帧与上一关键帧之间的roll、pitch、yaw、xyz变化大于阈值时，作为取作关键帧，进行下面的全图优化。
- addOdomFactor 将本帧与上个关键帧之间建立帧间约束，这里使用本次激光估计的位姿与上次优化的关键帧位姿之间的delta为约束。
gtSAMgraph.add(BetweenFactor(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
- addGPSFactor
距离第一帧小于5m，或x、y边缘化误差同时小于阈值，或距上次建立gpsfactor的gps点小于5m，不建立gpsfactor。为什么？
直接用当前帧时间±0.2s内的GPS进行优化，忽略0.2s的运动？
设置GPS的最小观测噪声为1m？如果观测实在太差，而噪声给的不准确怎么办？
当边缘化上帧后，pose方差小于阈值时不添加此GPSFactor。
Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f);
noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);
gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
- addLoopFactor VINS输出的回环帧和帧间相对位姿作为约束。
- isam为ISAM2，把新建graph update后，再次update的原因？一次不会收敛？
- 将本次优化结果更新到cloudKeyPoses3D和cloudKeyPoses6D中，并计算边缘化结果，同时把该帧的降采样后的corner和surf点加入到map中。
- correctPoses
若插入了GPS factor或Loop factor，则根据优化结果，更新cloudKeyPoses3D和cloudKeyPoses6D。3D只保存key frame的xyz，6D保存key frame的位姿。为什么分开保存？ - publishOdometry
发布最新关键帧的位姿到/lidar/mapping/odometry， - publishFrames
gpsHandler GPS处理
将GPS数据放入gpsQueue。
loopHandler 回环帧处理
限制本次回环距离上次的时间>=5s。
performLoopClosure：
- loopFindKey 根据消息中的回环时间戳，查找对应的激光关键帧在cloudKeyPoses6D中的索引，不是精确时间：loop_time_pre前的关键帧和loop_time_cur后的关键帧。
- loopFindNearKeyframes 把1中找到loop_time_cur帧和loop_time_pre及其附近帧中点云转换到map系下，放到一组点云中，再进行降采样。需要保证当前帧点云数量>=300，历史点云数量>=1000。
- 若两个回环帧的距离<25m，不对当前帧的点进行变换；否则把当前帧的点云按照两帧的delta_xyz进行补偿，便于进行ICP。
- ICP最大查找距离25m*2，当前帧点云作为source，历史帧点云作为target，进行ICP。
- 若icp.getFitnessScore() < 0.3 && icp.hasConverged() == true，计算当前帧的位姿，与历史帧构成回环帧。icp.getFitnessScore() 作为回环噪声。
loopClosureThread
每0.5s进行一次performLoopClosureDetection。
- 在最后一个激光关键帧的10m内所有的激光关键帧。
- 保留一个30s之外的历史关键帧。
- 将两个帧的时间作为参数传入performLoopClosure进行回环，同loopHandler中的处理。
visualizeGlobalMapThread
每0.2s执行一次publishGlobalMap，仅是发布关键帧和点云。
To be continued。。。