LVI-SAM 代码解析

浏览开源代码LVI-SAM，记录自己的理解。

概述

项目分为7个部分（程序）：visual_feature、visual_odometry、visual_loop、imuPreintegration、imageProjection、featureExtraction、mapOptimization。
LIO-SAM中各模块之间的ROS消息收发结构。

LIO-SAM的因子图结构如下：

该项目整体流程如下：

1. IMU实时推算/优化——imuPreintegration。该模块定于Lidar odometry消息，与IMU数据一起进行优化，优化结果作为实时输出的校正值。
2. 激光运动畸变校正——imageProjection。利用当前帧起止时刻之间的IMU数据、IMU里程计数据计算预积分，得到每一时刻的激光点位姿，从而变换到初始时刻激光点坐标系下，实现校正。
3. 提取特征——featureExtraction。对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点特征。
4. scan-to-map匹配——mapOptimization。提取局部关键帧map的特征点，与当前帧特征点执行scan-to-map匹配，更新当前帧的位姿。
5. 因子图优化——mapOptimization。添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿。
6. 闭环检测——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

只处理点云数据。

1. 存放到cloudQueue中，size<=2 return，始终保持有3帧数据。
2. 取出第0帧，并pop。第0帧作为timeScanCur，第1帧作为timeScanNext，第2帧（最新）暂不使用？
3. 确保is_dense==true、fields[i].name中有“ring”（激光线数）和“time”（相对于雷达帧时间的delta t），否则shutdown。

deskewInfo

加imu和odo的锁

imuDeskewInfo

1. 从imuQueue中pop出timeScanCur-0.01之前的IMU数据。
2. 直接从IMU的消息中获取roll、pitch、yaw？？？
3. 计算(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 = d_i-5+d_i-4+d_i-3+d_i-2+d_i-1-10*+d_i+d_i+1+d_i+2+d_i+3+d_i+4+d_i+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点云信息处理

1. updateInitialGuess

• 若系统未初始化，使用点云中的IMU 姿态角初始化。
• 如果odom（VINS）可用，根据点云信息中的odom x/y/z/r/p/y 计算地图坐标系中的位置，odom中信息是相对起点第一帧的里程计信息，然后返回。
• 如果imu可用、odom不可用，使用IMU信息更新本帧的姿态信息，注意仅更新姿态信息。

2. 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中，然后降采样关键帧（地图）中的点云。

3. downsampleCurrentScan 将当前帧点云中的角点和面点下采样。
4. scan2MapOptimization
   当角点和面点的数量都满足要求时，将附近帧（地图）corner和surf点放入kdtree，进行cornerOptimization、surfOptimization、combineOptimizationCoeffs。
   最大迭代30次，当deltaR < 0.05 && deltaT < 0.05时，停止迭代。
   其中用到了多线程对for循环处理点云进行加速：

#pragma omp parallel for num_threads(numberOfCores)
1

• 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融合，并进行限幅。

5. saveKeyFramesAndFactor

• saveFrame 当该帧与上一关键帧之间的roll、pitch、yaw、xyz变化大于阈值时，作为取作关键帧，进行下面的全图优化。
• addOdomFactor 将本帧与上个关键帧之间建立帧间约束，这里使用本次激光估计的位姿与上次优化的关键帧位姿之间的delta为约束。

gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
1

• 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);
1
2
3

• addLoopFactor VINS输出的回环帧和帧间相对位姿作为约束。
• isam为ISAM2，把新建graph update后，再次update的原因？一次不会收敛？
• 将本次优化结果更新到cloudKeyPoses3D和cloudKeyPoses6D中，并计算边缘化结果，同时把该帧的降采样后的corner和surf点加入到map中。

7. correctPoses
   若插入了GPS factor或Loop factor，则根据优化结果，更新cloudKeyPoses3D和cloudKeyPoses6D。3D只保存key frame的xyz，6D保存key frame的位姿。为什么分开保存？
8. publishOdometry
   发布最新关键帧的位姿到/lidar/mapping/odometry，
9. publishFrames

gpsHandler GPS处理

将GPS数据放入gpsQueue。

loopHandler 回环帧处理

限制本次回环距离上次的时间>=5s。

performLoopClosure：

1. loopFindKey 根据消息中的回环时间戳，查找对应的激光关键帧在cloudKeyPoses6D中的索引，不是精确时间：loop_time_pre前的关键帧和loop_time_cur后的关键帧。
2. loopFindNearKeyframes 把1中找到loop_time_cur帧和loop_time_pre及其附近帧中点云转换到map系下，放到一组点云中，再进行降采样。需要保证当前帧点云数量>=300，历史点云数量>=1000。
3. 若两个回环帧的距离<25m，不对当前帧的点进行变换；否则把当前帧的点云按照两帧的delta_xyz进行补偿，便于进行ICP。
4. ICP最大查找距离25m*2，当前帧点云作为source，历史帧点云作为target，进行ICP。
5. 若icp.getFitnessScore() < 0.3 && icp.hasConverged() == true，计算当前帧的位姿，与历史帧构成回环帧。icp.getFitnessScore() 作为回环噪声。

loopClosureThread

每0.5s进行一次performLoopClosureDetection。

1. 在最后一个激光关键帧的10m内所有的激光关键帧。
2. 保留一个30s之外的历史关键帧。
3. 将两个帧的时间作为参数传入performLoopClosure进行回环，同loopHandler中的处理。

visualizeGlobalMapThread

每0.2s执行一次publishGlobalMap，仅是发布关键帧和点云。

To be continued。。。