前言
本作者在16年大学开始接触ROS后，逐步向着机器人建图导航方面扩展，尤其是对激光雷达方向比较感兴趣，目前打算针对近阶段的SC-LEGO-LOAM进行分析讲述。从ScanContext和Lego LOAM两个部分进行分析阐述。一方面也是记录自己的学习成果，另一方面也是帮助他人一起熟悉这篇20年的经典文章。
LOAM系列发展
LOAM
LOAM作为该系列的鼻祖，在前几年kitti数据集中常年霸占榜首，文中的作者所写的代码由于可读性不高，所以有很多人对代码进行了重构。
论文中作者目标是使用一个三维空间中运动的多线激光雷达来实现激光里程计+建图的功能。为了可以同时获得低漂移和低复杂度，并且不需要高精度的测距和惯性测量。本文的核心思想是将定位和建图的分割，通过两个算法：一个是执行高频率的里程计但是低精度的运动估计（定位），另一个算法在比定位低一个数量级的频率执行匹配和注册点云信息（建图和校正里程计）。
第一个算法为了保证高频率，文中使用scan-to-scan匹配，利用前后两帧的位姿信息来实现粗略的位姿估计（当中包含有：特征点提取【按线束分割→ \rightarrow→ 计算曲率 → \rightarrow→ 删除异常点 → \rightarrow→按曲率大小筛选特征点】 ⇒ \Rightarrow⇒ 帧间匹配【投影到上一时刻，并计算损失函数】 ⇒ \Rightarrow⇒迭代优化【利用LM来实现位姿的迭代优化，并估算出这一帧数据中的点和上一帧数据中点的对应关系】）。
第二个算法为了保证高精度，则是使用了map-to-map的匹配。其优点是精度高，误差累计小；缺点就是计算量大，实时性压力大。当我们在第一步有了里程计校正后的点云数据后，接下来我们就可以做一个map-to-map的匹配了。但是map-to-map存在计算量大的问题，因此 我们可以让其执行的频率降低。这样的高低频率结合就保证了计算量的同时又兼具了精度。
值得注意的是LOAM仅仅是一个激光里程计算法，没有闭环检测，也就没有加入图优化框架。只是将SLAM问题分为两个算法并行运行：一个odometry算法，10Hz；另一个mapping算法，1Hz，最终将两者输出的pose做整合，实现10Hz的位姿实时输出。两个算法都是使用点云中提取出尖锐的边点和平整的面点作为特征点，然后进行特征点匹配，来估计lidar的位姿以及对位姿进行fine tune。
A-LOAM
A-LOAM相较于LOAM而言舍去了IMU对信息修正的接口，同时A-LOAM使用了Ceres库完成了LM优化和雅克比矩阵的正逆解。A-LOAM可读性更高，便于上手。简而言之，A-LOAM就是LOAM的清晰简化，版本。
A-LOAM的代码清晰度确实很高，整理的非常简洁，主要是使用了Ceres函数库代替了张继手推的ICP优化求解部分（用Ceres的自动求导，代替了手推的解析求导，效率会低一些）。整个代码目录如下：
…详情请参照古月居
SC-LeGO-LOAM算法详解
SC-LeGO-LOAM是在LeGO-LOAM的基础上新增了基于Scan context的回环检测，在回环检测的速度上相较于LeGO-LOAM有了一定的提升。下面我们将会对SC-LeGO-LOAM算法的代码进行详细分析。
首先我们从代码的launch文件开始入手进行分析，README文件中提示，我们需要从run.launch文件入手。
<launch> ........ <node pkg="lego_loam" type="imageProjection" name="imageProjection" output="screen"/> <node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/> <node pkg="lego_loam" type="mapOptmization" name="mapOptmization" output="screen"/> <node pkg="lego_loam" type="transformFusion" name="transformFusion" output="screen"/> </launch>
从中我们可以看到launch文件中主要依赖四个node，其中最后一个node主要输出了一些数据坐标系的转换，对文章的理解影响不会很大，主要功能的是由前面3个node来实现的，而且是存在数据流的依次传递和处理。
imageProjection.cpp
首先我们先来看一下imageProjection这一个node节点，这个部分主要是对激光雷达数据进行预处理。包括激光雷达数据获取、点云数据分割、点云类别标注、数据发布。
该文件中订阅了激光雷达发布的数据，并发布了多个分类点云结果，初始化lego_loam::ImageProjection构造函数中，nodehandle使用~来表示在默认空间中。
ImageProjection::ImageProjection() : nh("~") { // init params InitParams(); // subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic.c_str(), 1, &ImageProjection::cloudHandler, this); // publisher pubFullCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_projected", 1); pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_info", 1); pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2>("/ground_cloud", 1); pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud", 1); pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud_pure", 1); pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info>("/segmented_cloud_info", 1); pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud", 1); // 离群点或异常点 nanPoint.x = std::numeric_limits<float>::quiet_NaN(); nanPoint.y = std::numeric_limits<float>::quiet_NaN(); nanPoint.z = std::numeric_limits<float>::quiet_NaN(); nanPoint.intensity = -1; allocateMemory(); resetParameters(); }
在构造函数中除了使用allocateMemory对点云进行reset、resize、assign等重置、赋值等操作以外。主要的函数是ImageProjection::cloudHandler，该函数内部清晰记录了激光雷达数据流的走向
////
void ImageProjection::cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) { // 1. Convert ros message to pcl point cloud copyPointCloud(laserCloudMsg); // 2. Start and end angle of a scan findStartEndAngle(); // 3. Range image projection projectPointCloud(); // 4. Mark ground points groundRemoval(); // 5. Point cloud segmentation cloudSegmentation(); // 6. Publish all clouds publishCloud(); // 7. Reset parameters for next iteration resetParameters(); }
该回调函数调用了七个函数，完成了对单帧激光雷达数据的处理。下面我们对该流程进行梳理，并详细介绍地面分割方法。
featureAssociation.cpp
其次featureAssociation这一个node节点，主要是特征提取。代码中先初始化了lego_loam::FeatureAssociation，用来订阅了上一节点发出来的分割出来的点云，点云的属性，外点以及IMU消息，并设置了回调函数。其中IMU消息的订阅函数较为复杂，它从IMU数据中提取出姿态，角速度和线加速度，其中姿态用来消除重力对线加速度的影响。然后函数FeatureAssociation::AccumulateIMUShiftAndRotation用来做积分，包括根据姿态，将加速度往世界坐标系下进行投影。再根据匀加速度运动模型积分得到速度和位移，同时，对角速度也进行了积分。
void FeatureAssociation::imuHandler(const sensor_msgs::Imu::ConstPtr &imuIn) { double roll, pitch, yaw; tf::Quaternion orientation; tf::quaternionMsgToTF(imuIn->orientation, orientation); tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); // 对加速度进行坐标变换 // 进行加速度坐标交换时将重力加速度去除，然后再进行xxx到zzz,yyy到xxx,zzz到yyy的变换。 // 去除重力加速度的影响时，需要把重力加速度分解到三个坐标轴上，然后分别去除他们分量的影响，在去除的过程中需要注意加减号（默认右手坐标系的旋转方向来看）。 // 在上面示意图中，可以简单理解为红色箭头实线分解到红色箭头虚线上（根据pitchpitchpitch进行分解）,然后再按找rollrollroll角进行分解。 // 原文链接：https://blog.csdn.net/wykxwyc/article/details/98317544 float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81; float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81; float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81; imuPointerLast = (imuPointerLast + 1) % imuQueLength; // 将欧拉角，加速度，速度保存到循环队列中 imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); imuRoll[imuPointerLast] = roll; imuPitch[imuPointerLast] = pitch; imuYaw[imuPointerLast] = yaw; imuAccX[imuPointerLast] = accX; imuAccY[imuPointerLast] = accY; imuAccZ[imuPointerLast] = accZ; imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x; imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y; imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z; // 积分计算得到位移 AccumulateIMUShiftAndRotation(); }
上面的回调函数仅仅是考虑该如何读取数据，而未涉及点云该如何处理，也没有说怎么与IMU数据做融合。由于代码太多而且很多都是纯粹的数学推导计算，为此我们选取runFeatureAssociation中的一些重要的思想来详写。
/
// 主程序入口 void FeatureAssociation::runFeatureAssociation() { // 有新数据进来才执行 if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud && std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 && std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05) { newSegmentedCloud = false; newSegmentedCloudInfo = false; newOutlierCloud = false; } else { return; } /** 1. Feature Extraction */ adjustDistortion(); // imu去畸变 calculateSmoothness(); // 计算光滑性 markOccludedPoints(); // 距离较大或者距离变动较大的点标记 extractFeatures(); // 特征提取（未看懂） publishCloud(); // cloud for visualization /** 2. Feature Association */ if (!systemInitedLM) { checkSystemInitialization(); return; } updateInitialGuess(); // 更新初始位姿 // 一个是找特征平面，通过面之间的对应关系计算出变换矩阵。 // 另一个部分是通过角、边特征的匹配，计算变换矩阵。 updateTransformation(); integrateTransformation(); // 计算旋转角的累积变化量。 publishOdometry(); publishCloudsLast(); // cloud to mapOptimization }
…详情请参照古月居
到这里对SC-LeGO-LOAM的所有流程已经理清楚，同时这部分代码可以在我个人的Github项目中看到，如果各位感兴趣的可以下载测试学习。
