前言
本作者在16年大学开始接触ROS后，逐步向着机器人建图导航方面扩展，尤其是对激光雷达方向比较感兴趣，目前打算针对近阶段的SC-LEGO-LOAM进行分析讲述。从ScanContext和Lego LOAM两个部分进行分析阐述。一方面也是记录自己的学习成果，另一方面也是帮助他人一起熟悉这篇20年的经典文章。
LOAM系列发展
LOAM
LOAM作为该系列的鼻祖，在前几年kitti数据集中常年霸占榜首，文中的作者所写的代码由于可读性不高，所以有很多人对代码进行了重构。
论文中作者目标是使用一个三维空间中运动的多线激光雷达来实现激光里程计+建图的功能。为了可以同时获得低漂移和低复杂度，并且不需要高精度的测距和惯性测量。本文的核心思想是将定位和建图的分割，通过两个算法：一个是执行高频率的里程计但是低精度的运动估计（定位），另一个算法在比定位低一个数量级的频率执行匹配和注册点云信息（建图和校正里程计）。
第一个算法为了保证高频率，文中使用scan-to-scan匹配，利用前后两帧的位姿信息来实现粗略的位姿估计（当中包含有：特征点提取【按线束分割 计算曲率
删除异常点
按曲率大小筛选特征点】
帧间匹配【投影到上一时刻，并计算损失函数】
迭代优化【利用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的自动求导，代替了手推的解析求导，效率会低一些）。整个代码目录如下：
LeGO-LOAM
LeGO-LOAM针对处理运算量做了优化，它的运算速度增加，同时相较于LOAM并没有牺牲精度。LeGO-LOAM作为近三年的论文，由于其代码开源以及对设备性能要求低等优点，到现在依然用的比较多。
论文里面主要是和LOAM对比，其相比LOAM具有以下五个特点
- 轻量级，能在嵌入式设备上实时运行。
- 地面优化，在点云处理部分加入了分割模块，这样做能够去除地面点的干扰，只在聚类的目标中提取特征。其中主要包括一个地面的提取（并没有假设地面是平面），和一个点云的分割，使用筛选过后的点云再来提取特征点，这样会大大提高效率。
- 在提取特征点时，将点云分成小块，分别提取特征点，以保证特征点的均匀分布。在特征点匹配的时候，使用预处理得到的segmenation标签筛选(点云分割为边缘点类和平面点类两类)，再次提高效率。
- 两步L-M优化法估计6个维度的里程计位姿。先使用平面点优化高度，同时利用地面的方向优化两个角度信息。地面提取的平面特征用于在第一步中获得[
,
，
]；再使用边角点优化剩下的三个变量，通过匹配从分段点云提取的边缘特征来获得其余部分的变换[
,
，
]。匹配方式还是scan2scan，以这种方式分别优化，效率提升40%，但是并没有造成精度的损失
- 集成了回环检测以校正运动估计漂移的能力（即使用gtsam作回环检测并作图优化，但是本质上仍然是基于欧式距离的回环检测，不存在全局描述子）。
LIO-SAM
该文章作为LeGO-LOAM作者的正统续作，也是近年来比较有了解价值的多传感器融合里程计，为此我们拿出来说一说。LIO-SAM实际上是LeGO-LOAM的扩展版本，添加了IMU预积分因子和GPS因子，去除了帧帧匹配部分。
论文认为loam系列文章存在一些问题：将其数据保存在全局体素地图中，难以执行闭环检测；没有结合其他绝对测量（GPS，指南针等）；当该体素地图变得密集时，在线优化过程的效率降低。 为此作者决定使用因子图的思想优化激光SLAM，引入四种因子：IMU预积分因子；激光雷达里程因子；GPS因子；闭环因子。 下面是文章中主要的贡献点。
- 里程计部分改为scan2localmap的匹配，特征提取部分去除了原LeGO-LOAM中的聚、分割并提取较为突出的边缘点和平面点，而是沿用LOAM中的边缘和平面点。（精度高一些，LeGO-LOAM主要考虑性能多一点）
- 维护两个因子图，预积分因子图可联合优化激光雷达odom和IMU，并估计IMU偏差，进行实时的里程计估算，这里将雷达位姿作为预测，而把imu作为观测，去更新imu的bias。全局因子图联合优化雷达odom和GPS因子以及回环因子。（即最终建图没有用到IMU优化后的轨迹，有点奇怪，这样做的好处就是优化速度快）
- 利用robot_localization中的EKF节点融合GPS和9轴IMU数据，得到提供初始姿态的gps odom。（这里初始化部分使用到了9轴IMU，参考VINS）
该图对应了文中的两个因子图。当中主要有4种因子，imu预积分因子（橙色）、激光里程计因子（绿色）、GPS因子（黄色）和回环因子（黑色）。在激光里程计部分，沿用LOAM中的特征提取方法，提取边缘点和平面点，将当前帧特征点分别与之前n+1帧的局部特征点地图进行匹配，分别建立点到直线和点到平面的约束，最后联合优化估计位姿。而GPS因子则是在位姿估计协方差矩阵变得很大的时候通过对其时间戳进行线性插值而添加进来。
下面是系统的节点图：
这里可以很清晰的看到系统中odometry部分有三个数据，/odometry/gps和/odometry/navsat是用robot_localization包融合GPS而得到的，而/odometry/imu是由优化后的激光odom和实时的imu_raw数据，通过imu预积分得到一个实时的预测的odom，并更新imu偏差。
ISCLOAM
ISCLOAM是一篇2020年发表在ICRA上面的论文，论文里面作者提出了一种基于scan context(18 IROS)改进的全局点云描述符，主要是编码了强度信息。在保存了ISC特征描述符后可以快速进行回环检测。
文中提供的代码在点云预处理、特征提取、位姿估计和LEGO-LOAM基本一致。文中主要的贡献在于回环检测部分：根据预处理的点云数据，实时提取ISC特征，并根据这些特征进行回环检测。在发现回环后，则开始后端优化，即接收当前的边缘、平面特征，以及odom和回环信息，然后进行全局一致性优化，这里采用GTSAM完成图优化,此处使用了全局特征子，和SC-LeGO-LOAM一样。
文中使用的ISC特征和SC特征对比，在回环检测部分，两个关键步骤，一是计算ISC特征，二是特征相似性计算。
首先是提取ISC特征，用一张图像来表示，其中像素值存的是点云的强度。ISC描述子同时编码了LiDAR点云的几何（位置）和密度信息，密度信息的描述子表示了周围环境的反射率，该信息对于不同地点是独一无二的。更容易避免回环检测失误。
ISC特征的相似性计算部分这里分两步计算，第一步计算其空间距离得分，第二步计算其强度距离得分。这里进行两个阶段的检测，首先是快速的几何结构匹配，然后密度结构重识别。
bool ISCGenerationClass::is_loop_pair(ISCDescriptor &desc1,
ISCDescriptor &desc2,
double &geo_score,
double &inten_score) {
int angle = 0;
geo_score = calculate_geometry_dis(desc1, desc2, angle); // 计算几何距离
if (geo_score > GEOMETRY_THRESHOLD) {
inten_score = calculate_intensity_dis(desc1, desc2, angle); // 计算强度得分
if (inten_score > INTENSITY_THRESHOLD) {
return true;
}
}
return false;
}
double ISCGenerationClass::calculate_geometry_dis(const ISCDescriptor &desc1, const ISCDescriptor &desc2, int &angle) {
double similarity = 0.0;
// 几何结构匹配
for (int i = 0; i < sectors; i++) { // 列
int match_count = 0;
// 矩阵逐元素异或
for (int p = 0; p < sectors; p++) {
int new_col = p + i >= sectors ? p + i - sectors : p + i;
for (int q = 0; q < rings; q++) { // 行
// 由于二值矩阵的列向量表示方位角，因此LiDAR的旋转可以通过矩阵的列移动反映．
// 因此，为了检测视角变化，我们需要计算具有最大几何相似度的列移动
if ((desc1.at<unsigned char>(q, p) == true && desc2.at<unsigned char>(q, new_col) == true)
|| (desc1.at<unsigned char>(q, p) == false && desc2.at<unsigned char>(q, new_col) == false)) {
match_count++;
}
}
}
if (match_count > similarity) {
similarity = match_count;
angle = i;
}
}
return similarity / (sectors * rings);
}
double ISCGenerationClass::calculate_intensity_dis(const ISCDescriptor &desc1, const ISCDescriptor &desc2, int &angle) {
double difference = 1.0;
// 密度结构重识别 计算上一步最佳列旋转后的和候选帧的ISC的密度相似度
// 取ISC对应列的余弦距离的均值
double angle_temp = angle;
for (int i = angle_temp - 10; i < angle_temp + 10; i++) {
int match_count = 0;
int total_points = 0;
for (int p = 0; p < sectors; p++) {
int new_col = p + i;
if (new_col >= sectors)
new_col = new_col - sectors;
if (new_col < 0)
new_col = new_col + sectors;
for (int q = 0; q < rings; q++) {
match_count += abs(desc1.at<unsigned char>(q, p) - desc2.at<unsigned char>(q, new_col));
total_points++;
}
}
double diff_temp = ((double) match_count) / (sectors * rings * 255);
if (diff_temp < difference)
difference = diff_temp;
}
return 1 - difference;
}
ICP && NDT
ICP和NDT原本是作为点云与点云的直接匹配方法来作为前端里程计估计的方法，但是目前由于LOAM系列的出现，导致这种方法已经很少使用。此处我们提到ICP和NDT是想说明其在回环检测的作用，由于回环检测需要寻找到回环点。以LeGO-LOAM算法为代表的回环检测就是使用ICP+欧式距离的方法来寻找到回环点。而以SC-LeGO-LOAM算法为代表的回环检测使用了scan context系列提取的全局特征子来进行查找，同时有些方法会在此基础上再加入ICP来提升回环检测后重定位的精度，方便图优化。这两种方法各有千秋，可以根据需求进行选择。
回环检测
在LOAM系列中回环检测主要存在有四种方法
- 传统的领域距离搜索+ICP匹配
- 基于scan context系列的粗匹配+ICP精准匹配的回环检测
- 基于scan context的回环检测
- 基于Intensity scan context+ICP的回环检测
在参考很多大佬的比对结果中我们发现，传统的领域距离搜索+ICP匹配是这三个方法中最耗时的，相较于基于scan context的回环检测而言利用类梯度下降法做ICP匹配需要消耗大量的计算资源。在使用基于scan context的回环检测时，会出现误检和漏检情况，而基于Intensity scan context的回环检测，对于scan context建图精度未有太大提升，但漏检的情况要少很多。
–>
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();
}该回调函数调用了七个函数，完成了对单帧激光雷达数据的处理。下面我们对该流程进行梳理，并详细介绍地面分割方法。
- copyPointCloud：该函数主要的功能是通过
pcl::fromROSMsg函数将ROS的PointCloud保存成PCL的PointCloud，并通过pcl::removeNaNFromPointCloud函数对nan噪点进行了滤除，从而避免了后面的计算中出现各种异常情况。 - findStartEndAngle：这个函数主要为了计算起止角度范围。因为运动，会导致多线激光雷达的第一个点和最后一个点并不是严格的360°，会存在一定的畸变。为此需要计算出起止角度，并将差值放在
segMsg变量中，成员变量segMsg的类型cloud_msgs::cloud_info是作者自定义的。它保存了当前帧的一些重要信息，包括起至角度，每个线的起至序号，及成员变量fullCloud中每个点的状态。 - projectPointCloud：该函数中将激光点云按照角度展开成图像的形式，计算所在行列和深度。其中行表示激光线束数量，列表示每个线上同一时刻扫描到的点。这里以
x轴的负方向开始逆时针列序列号逐渐递增，即图像中的从左到右。以Mat图像保存深度，其中点集的行用fullCloud 和 fullInfoCloud表示和深度用fullInfoCloud表示。这里的fullInfoCloud内部存放该帧下的所有数据，该处的点云相比于copyPointCloud函数输入的点云数据，主要的不同之处在于，当前的点云根据计算出来的行列，重新排列了顺序。例如，激光雷达本身可能是先列后行的方向输出的点云，或者是livox雷达那种非重复扫描出来的结果，这里都会被重新组织成先行后列的顺序保存。
void ImageProjection::projectPointCloud() {
// 激光点云投影成二维图像，行表示激光线束数量，列表示每一个线上扫描到的点（0.1s扫描一圈，一个圆圈摊平就是360度）
// 计算点云深度，保存到深度图中
// range image projection
float verticalAngle, horizonAngle, range;
size_t rowIdn, columnIdn, index, cloudSize;
PointType thisPoint;
cloudSize = laserCloudIn->points.size();
for (size_t i = 0; i < cloudSize; ++i) {
thisPoint.x = laserCloudIn->points[i].x;
thisPoint.y = laserCloudIn->points[i].y;
thisPoint.z = laserCloudIn->points[i].z;
// find the row and column index in the iamge for this point
// 计算竖直方向上的点的角度以及在整个雷达点云中的哪一条水平线上
if (useCloudRing == true) {
// 用vlp的时候有ring属性
rowIdn = laserCloudInRing->points[i].ring;
} else {
// 其他lidar需要根据计算垂直方向的俯仰角？
verticalAngle =
atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
}
if (rowIdn < 0 || rowIdn >= N_SCAN)
continue;
// 水平方向上的角度，一行1800个像素点
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
// round是四舍五入
columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2;
if (columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;
if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
continue;
// 每个点的深度
range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
if (range < sensorMinimumRange)
continue;
// 在rangeMat矩阵中保存该点的深度，保存单通道像素值
rangeMat.at<float>(rowIdn, columnIdn) = range;
thisPoint.intensity = (float) rowIdn + (float) columnIdn / 10000.0;
index = columnIdn + rowIdn * Horizon_SCAN;
fullCloud->points[index] = thisPoint;
fullInfoCloud->points[index] = thisPoint;
fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
}
}- groundRemoval：该步骤主要是提前滤除地面，从贴近地面的几个线中提取地面点。取七个扫描圈，每两个圈之间进行一次比较，角度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加入到
groundCloud点云中。地面的点云会在groundMat中标记为1，labelMat中标记-1 ，不会参与后面的分割。
void ImageProjection::groundRemoval() {
// 利用不同的扫描圈来表示地面，进而检测地面是否水平。
// 取七个扫描圈，每两个圈之间进行一次比较，角度相差10°以内的我们可以看做是平地。
// 并且将扫描圈中的点加入到groundCloud点云
size_t lowerInd, upperInd;
float diffX, diffY, diffZ, angle;
// groundMat
// -1, no valid info to check if ground of not
// 0, initial value, after validation, means not ground
// 1, ground
for (size_t j = 0; j < Horizon_SCAN; ++j) { // 每行
for (size_t i = 0; i < groundScanInd; ++i) { // 每列
// 只用7个光束检测地面
lowerInd = j + (i) * Horizon_SCAN;
upperInd = j + (i + 1) * Horizon_SCAN;
// intensity在投影的时候已经归一化
if (fullCloud->points[lowerInd].intensity == -1 ||
fullCloud->points[upperInd].intensity == -1) {
// no info to check, invalid points
groundMat.at<int8_t>(i, j) = -1;
continue;
}
diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
angle = atan2(diffZ, sqrt(diffX * diffX + diffY * diffY)) * 180 / M_PI;
// 相邻圈小于10度
if (abs(angle - sensorMountAngle) <= 10) {
groundMat.at<int8_t>(i, j) = 1;
groundMat.at<int8_t>(i + 1, j) = 1;
}
}
}
// extract ground cloud (groundMat == 1)
// mark entry that doesn't need to label (ground and invalid point) for segmentation
// note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan
for (size_t i = 0; i < N_SCAN; ++i) {
for (size_t j = 0; j < Horizon_SCAN; ++j) {
if (groundMat.at<int8_t>(i, j) == 1 || rangeMat.at<float>(i, j) == FLT_MAX) {
labelMat.at<int>(i, j) = -1;
}
}
}
if (pubGroundCloud.getNumSubscribers() != 0) {
for (size_t i = 0; i <= groundScanInd; ++i) {
for (size_t j = 0; j < Horizon_SCAN; ++j) {
if (groundMat.at<int8_t>(i, j) == 1)
groundCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]);
}
}
}
}- cloudSegmentation：这一部分主要是将地面点与异常点排除之后，对非地面点云的分割，并生成局部特征的步骤。这个函数主要完成了两个任务，一是通过广度优先搜索，从非地面点中找出所有连成片的入射角比较小的patch上的点，并在labelMat标注patch的编号（从1开始）
void ImageProjection::labelComponents(int row, int col) {
// use std::queue std::vector std::deque will slow the program down greatly
// 特征检测，检测点与其临近点的特征
float d1, d2, alpha, angle;
int fromIndX, fromIndY, thisIndX, thisIndY;
bool lineCountFlag[N_SCAN] = {false};
queueIndX[0] = row;
queueIndY[0] = col;
int queueSize = 1;
int queueStartInd = 0;
int queueEndInd = 1;
allPushedIndX[0] = row;
allPushedIndY[0] = col;
int allPushedIndSize = 1;
while (queueSize > 0) {
// Pop point
fromIndX = queueIndX[queueStartInd];
fromIndY = queueIndY[queueStartInd];
--queueSize;
++queueStartInd;
// Mark popped point
labelMat.at<int>(fromIndX, fromIndY) = labelCount;
//检查上下左右四个邻点
// Loop through all the neighboring grids of popped grid
for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter) {
// new index
thisIndX = fromIndX + (*iter).first;
thisIndY = fromIndY + (*iter).second;
// index should be within the boundary
if (thisIndX < 0 || thisIndX >= N_SCAN)
continue;
// at range image margin (left or right side)
if (thisIndY < 0)
thisIndY = Horizon_SCAN - 1;
if (thisIndY >= Horizon_SCAN)
thisIndY = 0;
// prevent infinite loop (caused by put already examined point back)
if (labelMat.at<int>(thisIndX, thisIndY) != 0)
continue;
// d1与d2分别是该点与某邻点的深度
d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY),
rangeMat.at<float>(thisIndX, thisIndY));
d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY),
rangeMat.at<float>(thisIndX, thisIndY));
if ((*iter).first == 0)
alpha = segmentAlphaX;
else
alpha = segmentAlphaY;
// angle其实是该特点与某邻点的连线与XOZ平面的夹角，这个角代表了局部特征的敏感性
angle = atan2(d2 * sin(alpha), (d1 - d2 * cos(alpha)));
// 如果夹角大于60°，则将这个邻点纳入到局部特征中，该邻点可以用来配准使用
if (angle > segmentTheta) {
queueIndX[queueEndInd] = thisIndX;
queueIndY[queueEndInd] = thisIndY;
++queueSize;
++queueEndInd;
labelMat.at<int>(thisIndX, thisIndY) = labelCount;
lineCountFlag[thisIndX] = true;
allPushedIndX[allPushedIndSize] = thisIndX;
allPushedIndY[allPushedIndSize] = thisIndY;
++allPushedIndSize;
}
}
}
// check if this segment is valid
// 当邻点数目达到30后，则该帧雷达点云的几何特征配置成功
bool feasibleSegment = false;
if (allPushedIndSize >= 30)
feasibleSegment = true;
else if (allPushedIndSize >= segmentValidPointNum) {
int lineCount = 0;
for (size_t i = 0; i < N_SCAN; ++i)
if (lineCountFlag[i] == true)
++lineCount;
if (lineCount >= segmentValidLineNum)
feasibleSegment = true;
}
// segment is valid, mark these points
if (feasibleSegment == true) {
++labelCount;
} else { // segment is invalid, mark these points
for (size_t i = 0; i < allPushedIndSize; ++i) {
labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
}
}
}二是把所有地面点和刚分割出来的labelCount上的点合并保存在segmentedCloud中，此时当前帧的点已经有效的分割成多个类，这也是该node需要传递给下一个node进行特征提取和匹配的点云。并在segMsg中对应位置处保存每个点的属性（该点是不是地面，深度，属于第几列）。同时由于我们不需要那么多地面的点云，所以使用5个采一个的策略来实现地面点的降采样。
void ImageProjection::cloudSegmentation() {
// segmentation process
// /在排除地面点与异常点之后，逐一检测邻点特征并生成局部特征
for (size_t i = 0; i < N_SCAN; ++i)
for (size_t j = 0; j < Horizon_SCAN; ++j)
if (labelMat.at<int>(i, j) == 0)
labelComponents(i, j);
int sizeOfSegCloud = 0;
// extract segmented cloud for lidar odometry
for (size_t i = 0; i < N_SCAN; ++i) {
segMsg.startRingIndex[i] = sizeOfSegCloud - 1 + 5;
for (size_t j = 0; j < Horizon_SCAN; ++j) {
// 如果是特征点或者是地面点，就可以纳入被分割点云
if (labelMat.at<int>(i, j) > 0 || groundMat.at<int8_t>(i, j) == 1) {
// outliers that will not be used for optimization (always continue)
if (labelMat.at<int>(i, j) == 999999) {
if (i > groundScanInd && j % 5 == 0) {
outlierCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]);
continue;
} else {
continue;
}
}
// majority of ground points are skipped
// 地面点云每隔5个点纳入被分割点云
if (groundMat.at<int8_t>(i, j) == 1) {
if (j % 5 != 0 && j > 5 && j < Horizon_SCAN - 5)
continue;
}
// mark ground points so they will not be considered as edge features later
segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i, j) == 1);
// mark the points' column index for marking occlusion later
segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
// save range info
segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at<float>(i, j);
// save seg cloud 把当前点纳入分割点云中
segmentedCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]);
// size of seg cloud
++sizeOfSegCloud;
}
}
segMsg.endRingIndex[i] = sizeOfSegCloud - 1 - 5;
}
// extract segmented cloud for visualization
// 在当前有节点订阅便将分割点云的几何信息也发布出去
if (pubSegmentedCloudPure.getNumSubscribers() != 0) {
for (size_t i = 0; i < N_SCAN; ++i) {
for (size_t j = 0; j < Horizon_SCAN; ++j) {
if (labelMat.at<int>(i, j) > 0 && labelMat.at<int>(i, j) != 999999) {
segmentedCloudPure->push_back(fullCloud->points[j + i * Horizon_SCAN]);
segmentedCloudPure->points.back().intensity = labelMat.at<int>(i, j);
}
}
}
}
}- publishCloud：下面j就是与ROS相关的发布任务了。包括该帧的segMsg，完整点云fullCloud/fullInfoCloud（步骤3），地面点云（步骤4），发从非地面提取出来的点和降采样的地面点（步骤5），外点（步骤5,实际为比较小的patch上的点【因此两个叠加会导致角度差较大，且点云数不满足需求】）。
void ImageProjection::publishCloud() {
// 1. Publish Seg Cloud Info
segMsg.header = cloudHeader;
pubSegmentedCloudInfo.publish(segMsg);
// 2. Publish clouds
sensor_msgs::PointCloud2 laserCloudTemp;
pcl::toROSMsg(*outlierCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubOutlierCloud.publish(laserCloudTemp);
// segmented cloud with ground
pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubSegmentedCloud.publish(laserCloudTemp);
// projected full cloud
if (pubFullCloud.getNumSubscribers() != 0) {
pcl::toROSMsg(*fullCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubFullCloud.publish(laserCloudTemp);
}
// original dense ground cloud
if (pubGroundCloud.getNumSubscribers() != 0) {
pcl::toROSMsg(*groundCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubGroundCloud.publish(laserCloudTemp);
}
// segmented cloud without ground
if (pubSegmentedCloudPure.getNumSubscribers() != 0) {
pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubSegmentedCloudPure.publish(laserCloudTemp);
}
// projected full cloud info
if (pubFullInfoCloud.getNumSubscribers() != 0) {
pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubFullInfoCloud.publish(laserCloudTemp);
}
}- resetParameters：清空成员变量，准备下一次的处理。