LeGO-LOAM代码阅读

整体流程

• 多线程消息交互
    不同线程之间的消息接收和发送见下表，去除了用于rviz显示的消息。

函数类名称	接收消息	发布消息
imageProjection	/kitti/velo/pointcloud	/segmented_cloud /segmented_cloud_info /outlier_cloud
FeatureAssociation	/imu/data /segmented_cloud /segmented_cloud_info /outlier_cloud	/laser_cloud_corner_last /laser_cloud_surf_last /outlier_cloud_last /laser_odom_to_init
mapOptimization	/imu/data /laser_cloud_corner_last /laser_cloud_surf_last /outlier_cloud_last /laser_odom_to_init	/aft_mapped_to_init
TransformFusion	/laser_odom_to_init /aft_mapped_to_init	/integrated_to_init

ImageProjection.cpp

程序流程

  主要工作流程都包含在函数中，在“/velodyne_points”信息的响应函数cloudHandler中被调用：

转换点云消息类型

  在函数copyPointCloud中，包含两部分内容：

• 将接收的ROS格式点云转换为PointXYZI格式的PCL点云，然后去除NAN点。
• 如果ROS点云包含ring也就是行号，存储到PointXYZIR格式的PCL点云中。但要求is_dense==true，即没有无效点。

找到每个扫描开始和结束的角度

  在函数findStartEndAngle中，计算了开始和结束的角度，计算雷达扫过的角度。

生成 Range Image

  在函数projectPointCloud中，将点投影到Range Image上，并存入fullCloud和fullInfoCloud。
  根据x，y坐标算出columnIdn，根据ring或者x，y和z的夹角计算出rowIdn。fullCloud的intensity存储的是columnIdn和rowIdn的编码，fullInfoCloud的intensity存储的是x，y，z的平方和。

标记地面点

  在函数groundRemoval中，计算地面点并存入groundCloud。
  在最低的groundScanInd个Scan中，计算上下两线之间点的XYZ位置得到两线之间的俯仰角，如果俯仰角在10度以内，则两个点都是地面点。
  将地面点和无效点的类别设为-1，不参与后续的聚类。
  如果有程序在接收地面点消息，将地面点存入groundCloud并发送。

点云聚类

  在函数cloudSegmentation和labelComponents中。对点云进行聚类，并生成segmented_cloud，segmented_cloud_pure和segmented_cloud_info三个消息。
  首先在函数labelComponents中，使用宽度优先进行聚类，在邻域内进行探索。如果一个聚类有超过30个点，或在有超过5个点，并超过3行，被视为有效聚类。
  然后在函数cloudSegmentation中，得到消息需要的信息：

• 对每个Scan记录第五个点和最后第五个点的位置。
• 对于无效聚类和地面点，只选取一小部分加入外点和地面点集合。
• 在segMsg中记录点是否是地面点，columnIdn和Range值；segmentedCloudPure记录点和对于的聚类。

点云发布

  向外发布七个消息，内容如下：

• 点云聚类相关信息segMsg
• 外点集合outlierCloud
• 包括稀疏采样地面点的聚类后点云segmentedCloud,
• 所有点云fullCloud和fullInfoCloud
• 稠密的地面点集合groundCloud
• 记载了聚类结果的点云 segmentedCloudPure

其他函数

空间分配函数 allocateMemory

  进行指针和数组的空间分配和赋初值，以及设计邻域遍历方法。

变量初值设置函数 resetParameters

  将指针和数组重新赋初值。

参数和变量

groundMat：
1） groundMat.at(i,j) = 0，初始值；
2） groundMat.at(i,j) = 1，有效的地面点；
3） groundMat.at(i,j) = -1，无效地面点；

rangeMat
1） rangeMat.at(i,j) = FLT_MAX，浮点数的最大值，初始化信息；
2） rangeMat.at(rowIdn, columnIdn) = range，保存图像深度；

labelMat
1） labelMat.at(i,j) = 0，初始值；
2） labelMat.at(i,j) = -1，无效点；
3）labelMat.at(thisIndX, thisIndY) = labelCount，不同类有效点；
4）labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999，无效聚类点。

FeatureAssociation.cpp

  程序的内容执行分为对于imu和地图数据的处理以及在runFeatureAssociation函数中运行的程序主体。主要完成特征匹配和里程计位姿估计。
##数据处理函数

• imuHandler和AccumulateIMUShiftAndRotation函数
  从IMU中，可以得到以下参数：线加速度，角速度和旋转角。通过旋转角在加速度中去除重力的影响。同时积分计算给出当前位姿的估计。
• laserCloudHandler，laserCloudInfoHandler和outlierCloudHandler函数
  保存对应的消息，设置对应的控制变量为true。控制变量影响runFeatureAssociation函数的执行。

runFeatureAssociation函数

  代码分为两部分：特征提取和匹配、位姿估计。分两部分按顺序介绍各个函数的功能。

特征提起和匹配

• adjustDistortion函数
    估计激光开始时刻的rpy角，利用IMU插补将激光都投影到开始的时刻。
• calculateSmoothness函数
    根据同一行扫描的前后五个点计算点的光滑度。
• markOccludedPoints函数
  

  如上图所示，去除因为遮挡产生的，不可靠的特征点。

• extractFeatures函数
    将每一行扫描分成六份，光滑度排序和邻域抑制之后得到平面特征，线特征。每一份提取2个线特征，20个次一级的线特征。提取4个面特征，次一级的面特征通过对没有被选为线特征的点降采样得到。

位姿估计

• updateInitialGuess函数
    根据IMU得到位姿估计的初值。
• updateTransformation函数
    根据线特征和面特征，计算激光相较于前一帧的相对位姿。
• integrateTransformation函数
    融合计算激光的相较于第一帧的位姿。

mapOptimization.cpp

  mapOptimization中共运行三个线程，loopClosureThread，visualizeGlobalMapThread和mapOptimization::run。下面依次对三个线程进行分析。

loopClosureThread线程

  每一秒进行一次回环检测。

detectLoopClosure函数

  首先基于kd树搜索找到半径historyKeyframeSearchRadius范围内，时间差超过30秒帧作为回环帧。取出回环帧前后各historyKeyframeSearchNum帧关键帧点云，拼接，降采样。

performLoopClosure

  在当前关键帧点云和detectLoopClosure生成的局部地图中，使用icp进行scan-to-model的对齐。把对齐的结果作为约束加入gtsam作为边，执行isam优化。

visualizeGlobalMapThread线程

  线程包括两个功能publishGlobalMap和在最后保存结果点云。

publishGlobalMap函数

  可视化当前点500米范围内的关键帧，采用先对关键帧降采样，点云拼接之后再进行降采样的方式来降低数据规模。

结果保存

  降采样保存面特征和线特征和publishGlobalMap得到的全局点云。保存轨迹。

局部优化

  主要流程在run函数中，按照执行顺序介绍各部分功能。

数据接收

  在各个回调函数中，接收Imu数据和FeatureAssociation发送的laser_cloud_corner_last，laser_cloud_surf_last，outlier_cloud_last和laser_odom_to_init消息。前两个分别是次一级的线特征和面特征。后两个是外点和全局位姿。

transformAssociateToMap函数

  根据当前时刻和上一时刻FeatureAssociation函数估计的位姿transformSum，transformBefMapped，计算两帧之间的位姿变换。根据位姿变换，更新上一个时刻估计的位姿transformAftMapped，保存在transformTobeMapped中。

extractSurroundingKeyFrames函数

  根据是否执行回环采用不同的局部地图构建逻辑。
  首先介绍进行回环的情况，如果进行回环，就只从时间上相邻的帧中进行选择。如果recentCornerCloudKeyFrames中的点云由于轨迹才开始，或者刚发生回环数量不够， 清空recentCornerCloudKeyFrames，按照时间顺序依次从新到旧加入surroundingKeyframeSearchNum个最新的点云。如果recentCornerCloudKeyFrames中的点云数量足够，去除recentCornerCloudKeyFrames中最旧的一帧，加入最新的帧。
  不进行闭环的情况下，在cloudKeyPoses3D中进行半径surroundingKeyframeSearchRadius内的邻域搜索，根据搜索结果去除和增加surroundingKeyPosesDS中的点。
  对选择的局部地图的面特征和线特征进行降采样，得到laserCloudCornerFromMapDS和laserCloudSurfFromMapDS。

downsampleCurrentScan函数

  对laserCloudCornerLast，laserCloudSurfLast，laserCloudOutlierLast，laserCloudSurfTotalLast分别进行降采样。其中laserCloudSurfLastDS和laserCloudOutlierLastDS相加，得到laserCloudSurfTotalLast。

scan2MapOptimization函数

  scan2MapOptimization函数的实现分为四个部分，计算面特征和线特征雅可比的surfOptimization和cornerOptimization函数，优化位姿的LMOptimization和更新位姿的transformUpdate。通过计算的雅可比矩阵优化位姿，根据新的位姿变换更新特征匹配，重新计算雅可比矩阵，迭代优化位姿。
  cornerOptimization对laserCloudCornerLastDS每一个点在laserCloudCornerFromMapDS中找到距离最近的五个点，如果能够拟合成直线，就计算点到直线的距离。
  surfOptimization对laserCloudSurfTotalLastDS中每一个点在laserCloudSurfFromMapDS中找到距离最近的五个点，如果能够拟合成平面，计算点到平面的距离。
  LMOptimization根据上面两个函数求出的雅可比矩阵，对位姿进行优化。
  transformUpdate把最终得到的位姿和IMU测量进行插值。

saveKeyFramesAndFactor函数

  根据和上一个关键帧的距离判断当前帧是否为作关键帧，如果当前帧被选为关键帧，就加入gtsam对为子进行优化。

correctPoses函数

  检测是否发生了回环，如果发生了回环，根据回环结果更新位姿，同时清空局部地图。

TransformFusion.cpp

  融合高频的里程计信息和低频的优化后的位姿，得到最终的轨迹。融合的函数是transformAssociateToMap逻辑和mapOptimization.cpp中相同。

对Kitti数据集的适配

  应用到kitti数据集存在以下问题：

• kitti测试数据集中激光经过了去畸变处理
• kitti raw data中的imu频率太低
• LOAM和LeGO都假设点云数据是按列存储的原始数据，所以需要投影到range mat上来转换成按行排序。kitti去完畸变之后是按行存储的，会影响对于startOrientation和endOrientation的估计，去畸变也影响了到RangeMap的投影和后续的Scan构成。比较直接的问题就是无法准确的判断出每一条扫描开始和结束的位置，也就无法计算出整个激光扫过的角度。需要在projectPointCloud作额外改动。用atan2差值来判断扫描行的方法是不可取的，3.14和-3.14其实是同一个方向。Kitti存储数据的结构实际上是螺旋式的。

其他代码特性

• std::numeric_limits::quiet_NaN()：产生NaN。
• PointXYZIR：ROS增加的pcl点云结构，额外包含了laser ring number.
• ros::Publisher::getNumSubscribers：Returns the number of subscribers that are currently connected to this Publisher.
• pcl::PointCloud< PointT >::is_dense：True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
• void pcl::removeNaNFromPointCloud ：Removes points with x, y, or z equal to NaN. Paragram index means the mapping (ordered): cloud_out[i] = cloud_in[index[i]].
• gtsam::Cal3_S2::between：“Between”, subtracts calibrations. between(p,q) == compose(inverse§,q).

参考

• https://github.com/wykxwyc/LeGO-LOAM_NOTED LeGO-LOAM代码注释版
• https://zhuanlan.zhihu.com/p/159525107 LeGO-LOAM-transformAssociateToMap()解析
• https://zhuanlan.zhihu.com/p/109379384 从零开始做自动驾驶定位(八): 点云畸变补偿
• https://wykxwyc.github.io/2019/01/21/LeGO-LOAM-code-review-mapOptmization/ LeGO-LOAM 源码阅读笔记（mapOptmization.cpp）
• https://www.cnblogs.com/ReedLW/p/9005621.html 激光雷达slam之LOAM中的坐标转换与IMU融合