SC-LEGO-LOAM 扩展以及深度解析

前言

本作者在16年大学开始接触ROS后，逐步向着机器人建图导航方面扩展，尤其是对激光雷达方向比较感兴趣，目前打算针对近阶段的SC-LEGO-LOAM进行分析讲述。从ScanContext和Lego LOAM两个部分进行分析阐述。一方面也是记录自己的学习成果，另一方面也是帮助他人一起熟悉这篇20年的经典文章。

LOAM系列发展

轻量级，能在嵌入式设备上实时运行。
地面优化，在点云处理部分加入了分割模块，这样做能够去除地面点的干扰，只在聚类的目标中提取特征。其中主要包括一个地面的提取（并没有假设地面是平面），和一个点云的分割，使用筛选过后的点云再来提取特征点，这样会大大提高效率。
在提取特征点时，将点云分成小块，分别提取特征点，以保证特征点的均匀分布。在特征点匹配的时候，使用预处理得到的segmenation标签筛选(点云分割为边缘点类和平面点类两类)，再次提高效率。
两步L-M优化法估计6个维度的里程计位姿。先使用平面点优化高度，同时利用地面的方向优化两个角度信息。地面提取的平面特征用于在第一步中获得[tzt_ztz,θroll\theta_{roll}θroll，θpitch\theta_{pitch}θpitch]；再使用边角点优化剩下的三个变量，通过匹配从分段点云提取的边缘特征来获得其余部分的变换[txt_xtx,tyt_yty，θyaw\theta_{yaw}θyaw]。匹配方式还是scan2scan，以这种方式分别优化，效率提升40%，但是并没有造成精度的损失

集成了回环检测以校正运动估计漂移的能力（即使用gtsam作回环检测并作图优化，但是本质上仍然是基于欧式距离的回环检测，不存在全局描述子）。

LIO-SAM

该文章作为LeGO-LOAM作者的正统续作，也是近年来比较有了解价值的多传感器融合里程计，为此我们拿出来说一说。LIO-SAM实际上是LeGO-LOAM的扩展版本，添加了IMU预积分因子和GPS因子，去除了帧帧匹配部分。

论文认为loam系列文章存在一些问题：将其数据保存在全局体素地图中，难以执行闭环检测；没有结合其他绝对测量（GPS，指南针等）；当该体素地图变得密集时，在线优化过程的效率降低。为此作者决定使用因子图的思想优化激光SLAM，引入四种因子：IMU预积分因子；激光雷达里程因子；GPS因子；闭环因子。 下面是文章中主要的贡献点。

该图对应了文中的两个因子图。当中主要有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 paramsInitParams();// subscribersubLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic.c_str(), 1,&ImageProjection::cloudHandler, this);// publisherpubFullCloud = 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 cloudcopyPointCloud(laserCloudMsg);// 2. Start and end angle of a scanfindStartEndAngle();// 3. Range image projectionprojectPointCloud();// 4. Mark ground pointsgroundRemoval();// 5. Point cloud segmentationcloudSegmentation();// 6. Publish all cloudspublishCloud();// 7. Reset parameters for next iterationresetParameters();}

该回调函数调用了七个函数，完成了对单帧激光雷达数据的处理。下面我们对该流程进行梳理，并详细介绍地面分割方法。

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 projectionfloat 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, groundfor (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 pointsgroundMat.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 scanfor (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 pointfromIndX = queueIndX[queueStartInd];fromIndY = queueIndY[queueStartInd];--queueSize;++queueStartInd;// Mark popped pointlabelMat.at<int>(fromIndX, fromIndY) = labelCount;//检查上下左右四个邻点// Loop through all the neighboring grids of popped gridfor (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter) {// new indexthisIndX = fromIndX + (*iter).first;thisIndY = fromIndY + (*iter).second;// index should be within the boundaryif (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;elsealpha = 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 pointsif (feasibleSegment == true) {++labelCount;} else { // segment is invalid, mark these pointsfor (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 odometryfor (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 latersegMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i, j) == 1);// mark the points' column index for marking occlusion latersegMsg.segmentedCloudColInd[sizeOfSegCloud] = j;// save range infosegMsg.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 InfosegMsg.header = cloudHeader;pubSegmentedCloudInfo.publish(segMsg);// 2. Publish cloudssensor_msgs::PointCloud2 laserCloudTemp;pcl::toROSMsg(*outlierCloud, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubOutlierCloud.publish(laserCloudTemp);// segmented cloud with groundpcl::toROSMsg(*segmentedCloud, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubSegmentedCloud.publish(laserCloudTemp);// projected full cloudif (pubFullCloud.getNumSubscribers() != 0) {pcl::toROSMsg(*fullCloud, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubFullCloud.publish(laserCloudTemp);}// original dense ground cloudif (pubGroundCloud.getNumSubscribers() != 0) {pcl::toROSMsg(*groundCloud, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubGroundCloud.publish(laserCloudTemp);}// segmented cloud without groundif (pubSegmentedCloudPure.getNumSubscribers() != 0) {pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubSegmentedCloudPure.publish(laserCloudTemp);}// projected full cloud infoif (pubFullInfoCloud.getNumSubscribers() != 0) {pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);laserCloudTemp.header.stamp = cloudHeader.stamp;laserCloudTemp.header.frame_id = "base_link";pubFullInfoCloud.publish(laserCloudTemp);}}

resetParameters：清空成员变量，准备下一次的处理。

…详情请参照古月居