本文围绕LeGO-LOAM的imageProjection部分展开，介绍点云分割流程。包括订阅点云数据回调处理、点云预处理、投影映射到图像、地面移除、点云分割、发布点云数据及重置参数等步骤，还详细阐述了各步骤涉及的函数及原理，分割后点云将用于特征提取和配准。
学习文章：
[1] LeGO-LOAM源码解析2： imageProjection
[2] LeGo-LOAM 源码解析-WinFrom控件库
[3] LeGO-LOAM批注版
整合拼起来的，自用（24.04.06）
备注（24.04.07）：
本来想接着拼合阅读featureAssociation部分，但是尝试了一下搞出来发现结构并不是很清晰，而且也不懂里面的原理。找了一篇文章发现写的比较清晰，打算按照按文章和代码批注版进行学习。文章链接：LeGO-LOAM分析之特征提取（二）
LeGO-LOAM源码解析1 ： 算法整体框架和utility.h
imageProjection——点云分割
随着点云信息的输入，首先来到的是imageProjection.cpp文件，在这里主要的工作对点云的分割，并将分割好的点云送往featureAssociation.cpp。LOAM把所有的实现全部都放在了mian函数中，逻辑结构和代码分层不够明显，也存在一定的冗余；与LOAM中的代码书写有了较大的不同，LeGO-LOAM虽然也是分为四个部分，四个mian函数，但是对每一个部分都定义了一个类对函数和变量进行封装，结构脉络清晰
将激光点云处理成图像
总体流程：订阅点云数据回调处理 -> 点云转换到pcl预处理 -> 截取一帧激光数据 -> 投影映射到图像 -> 地面移除 -> 点云分割 -> 发布点云数据 -> 重置参数;
点云预处理基本流程：
- 接收新的一帧cloud数据；
- 初始化临时变量数据；
- 将ros cloud 转换成pcl point格式；
- 采用pcl库剔除无效值；
- 统计雷达数据扫描起始角度和终止角度，包括角度差范围；
- 将无序的点云数据，根据3d雷达（如16线 velodyne）的基本扫描原理，转换为有序点云，包括水平index和垂直index。可认为将3d点云转换为2维平面图像存储。
- 地面和非地面分割；
- 点云聚类
- 发布点云
ImageProjection类私有对象
定义节点句柄：
private:
ros::NodeHandle nh;
定义接受点云的订阅者：
ros::Subscriber subLaserCloud;
定义一系列与点云相关的发布者：
ros::Publisher pubFullCloud;
ros::Publisher pubFullInfoCloud;
ros::Publisher pubGroundCloud;
ros::Publisher pubSegmentedCloud;
ros::Publisher pubSegmentedCloudPure;
ros::Publisher pubSegmentedCloudInfo;
ros::Publisher pubOutlierCloud;
定义一系列点云存储：
//雷达传出的点云
pcl::PointCloud<PointType>::Ptr laserCloudIn;
pcl::PointCloud<PointXYZIR>::Ptr laserCloudInRing;
//全部点云
pcl::PointCloud<PointType>::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix
//整体点云
pcl::PointCloud<PointType>::Ptr fullInfoCloud; // same as fullCloud, but with intensity - range
//地面点云
pcl::PointCloud<PointType>::Ptr groundCloud;
//分割后的点云
pcl::PointCloud<PointType>::Ptr segmentedCloud;
//分割后的几何信息
pcl::PointCloud<PointType>::Ptr segmentedCloudPure;
//异常点云
pcl::PointCloud<PointType>::Ptr outlierCloud;
PointType nanPoint; // fill in fullCloud at each iteration
定义三个矩阵分别代表点云投影后的rang image 、 标签矩阵和地面矩阵：
cv::Mat rangeMat; // range matrix for range image
cv::Mat labelMat; // label matrix for segmentaiton marking
cv::Mat groundMat; // ground matrix for ground cloud marking
int labelCount;
一帧点云的起始角和结束角：
float startOrientation;
float endOrientation;
定义自定义的rosmsg和一些用于BFS的索引（点云分割时使用）：
cloud_msgs::cloud_info segMsg; // info of segmented cloud
std_msgs::Header cloudHeader;
std::vector<std::pair<int8_t, int8_t> > neighborIterator; // neighbor iterator for segmentaiton process
uint16_t *allPushedIndX; // array for tracking points of a segmented object
uint16_t *allPushedIndY;
uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed
uint16_t *queueIndY;
关于自定义的ros消息类型cloud_msgs的结构如下：
Header header
//分割的起始和结束的索引 长度：N_SCAN
int32[] startRingIndex
int32[] endRingIndex
//分割的起始和结束的角度和差值
float32 startOrientation
float32 endOrientation
float32 orientationDiff
//与点云分割相关 长度都是 N_SCAN*Horizon_SCAN
bool[] segmentedCloudGroundFlag # true - ground point, false - other points
uint32[] segmentedCloudColInd # point column index in range image
float32[] segmentedCloudRange # point range
main
主函数相对来说比较简单，主要是初始化节点、定义自定义类ImageProjection的对象，以及等待回调函数，对应代码如下：
int main(int argc, char** argv){
ros::init(argc, argv, "lego_loam");
ImageProjection IP;
ROS_INFO("\033[1;32m---->\033[0m Image Projection Started.");
ros::spin();
return 0;
}
在 main 函数中，只是实例化了一个对象 IP ，所以 一旦创建了一个对象，就进入到了 Class 中去执行构造函数。
ImageProjection():
//关于ROS中句柄定义时nh("~")与nh的一些小区别：以ros::init(argc, argv, "lego_loam");为例，
//若定义方式为nh("~")，则使用该句柄发布任何主题的消息时，其消息名称前都会自动加上/lego_loam/消息名；
//而若定义方式为nh时，其消息名称则直接为/消息名。
nh("~"){
// 订阅来自velodyne雷达驱动的topic ("/velodyne_points")
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 1, &ImageProjection::cloudHandler, this);
// 转换成图片的点云
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);
//无效点
// std::numeric_limits<float>::quiet_NaN()返回值为：NAN 或另一实现定义的NAN
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();
}
class 中的构造函数中，订阅了原始的激光点云数据（subLaserCloud）然后就进入到了 回调函数（ImageProjection::cloudHandler）对点云数据进行处理。
回调函数cloudHandler
// 主要步骤
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) //ros点云消息
{
// 将ros消息转换为pcl点云
copyPointCloud(laserCloudMsg);
// 计算出点云出于哪一条扫描线的第几个点的计算方法（和LOAM一样）
findStartEndAngle(); //扫描的开始结束角度（一圈）
// 距离图像投影
projectPointCloud();
// 提取地面点
groundRemoval();
// 快速的点云分割
cloudSegmentation();
// 发布所有点云
publishCloud();
// 为下一次迭代重置参数
resetParameters();
}
copyPointCloud()转换点云消息格式
由于激光雷达点云消息在传递过程中使用的是ROS 类型的消息，所以在处理的过程中统一需要对消息类型转换，通常的办法就是使用 PCL 库
// 复制点云 将ros消息转换为pcl点云
// 使用pcl库函数;
void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
// 1. 读取ROS点云转换为PCL点云
cloudHeader = laserCloudMsg->header;
// cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
//2.移除无效的点云 Remove Nan points
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
// 3. have "ring" channel in the cloud or not
// 如果点云有"ring"通过，则保存为laserCloudInRing
// 判断是不是使用了 velodyne 的激光雷达
if (useCloudRing == true){
pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
if (laserCloudInRing->is_dense == false) {
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
}
}
这个函数主体内容分为三个步骤，其中的第三步注意区分自己使用的激光雷达是否存在 CloudRing，没有这个的激光雷达接下来的步骤都不会去执行，所以在实际跑这个框架的时候一定要注意这个地方
findStartEndAngle()寻找帧的初始角和结束角
对于雷达旋转一圈，其实并不是从0度按某一方向旋转到360度，而是在起始时有一定偏差，而旋转也不是严格的360度，一般情况下要比360度稍微多几度。注意，我们的雷达坐标系为右前上（xyz），所以我们要找到点云的起始角和结束角。这里是以x轴为基准，采用的办法是使用收到的第一个点云点和结束的点云点的角度作为该帧点云的起始角和结束角。
我们一般认为
θ
s
t
a
r
t
∈
[
−
π
,
π
]
\theta_{start} \in [ − π , π ]
θstart∈[−π,π]而
θ
e
n
d
∈
[
−
π
,
3
π
]
\theta_{end} \in [-\pi,3\pi]
θend∈[−π,3π]
而两者的角度差不会与
2
π
2\pi
2π相差太多。
由上图可知，雷达是顺时针旋转的，所以计算角度
α
\alpha
α是需要添加一个负号:
α
=
−
arctan
y
/
x
\alpha=-\arctan y/x
α=−arctany/x，对角度进行转换(这里到最后就是以x负轴为
−
π
-\pi
−π的顺时针计算角度)：
void findStartEndAngle(){
// 雷达坐标系：右->X,前->Y,上->Z
// 雷达内部旋转扫描方向：Z轴俯视下来，顺时针方向（Z轴右手定则反向）
// atan2(y,x)函数的返回值范围(-PI,PI],表示与复数x+yi的幅角
// segMsg.startOrientation范围为(-PI,PI]
// segMsg.endOrientation范围为(PI,3PI]
// 因为内部雷达旋转方向原因，所以atan2(..)函数前面需要加一个负号
segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x); //第一个点
// 下面这句话怀疑作者可能写错了，laserCloudIn->points.size() - 2应该是laserCloudIn->points.size() - 1，laserCloudIn->points.size() - 1：取点云空间的大小
segMsg.endOrientation = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y, //最后一个点
laserCloudIn->points[laserCloudIn->points.size() - 2].x) + 2 * M_PI;
// 开始和结束的角度差一般是多少？
// 一个velodyne 雷达数据包转过的角度多大？
// 雷达一般包含的是一圈的数据，所以角度差一般是2*PI，一个数据包转过360度
// segMsg.endOrientation - segMsg.startOrientation范围为(0,4PI)
// 如果角度差大于3Pi或小于Pi，说明角度差有问题，进行调整。
if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI)
{
// 说明终点 角度为 pi - theta + 2*pi 起点的角度为 -pi + theta
segMsg.endOrientation -= 2 * M_PI; // 这样的话 endOrientation 就不需要 + 2*pi了
}
else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
segMsg.endOrientation += 2 * M_PI; // 终点角度 < 0 (-pi左右), 起点>0 (pi左右)
// segMsg.orientationDiff的范围为(PI,3PI),一圈大小为2PI，应该在2PI左右
segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
}
这个函数在 LOAM 代码里面也存在。
首先通过一个反正切函数，找到一帧激光点云起始时刻和结束时刻的航向角，第二步的作用是将一帧激光点云的航向角度限制在 [pi , 3pi] 之间，方便后续计算出一帧激光点云的时间。
projectPointCloud()点云重投影
距离图像投影
将3D point cloud投影映射到2D range image
将激光雷达数据投影成一个16x1800（依雷达角分辨率而定）的点云阵列
// 这里将激光点云投影到了一个二维矩阵上（实际还存储在了一个向量数组中）。横坐标代表激光点云属于的扫描线（总共是16或者32或者64）
// 纵坐标则表示它是这条线上的第几个数据。每一个矩阵元素的值是它距离激光设备的距离，在函数中变量名为rang。
// 逐一计算点云深度，并具有深度的点云保存至fullInfoCloud中;本函数将整个点云数据投影成一个16*1800的二维平面图
void projectPointCloud(){
float verticalAngle, horizonAngle, range; //（翻译：垂直角（z方向一点与原点连线，该线和xy平面的夹角）、水平角（与y轴夹角）、范围：每一个矩阵元素的值是它距离激光设备的距离）
// unsigned int（size_t）
size_t rowIdn, columnIdn, index, cloudSize; //（翻译：行号、列号、指数、点云大小）
PointType thisPoint;
// 指的是一条激光线的点云数？No，取得的是一帧点云的大小（不丢失的话，16*1800）
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;
// 计算竖直方向上的角度（雷达的第几线，此处只算出来竖直方向上的角度）
verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
// rowIdn计算出该点激光雷达是竖直方向上第几线的
// 从下往上计数，-15度记为初始线，第0线，一共16线(N_SCAN=16)
rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
if (rowIdn < 0 || rowIdn >= N_SCAN) //超过范围则不算
continue;
// 行
// -------------------------------------------------------------------------------------------------------------
// 烈
// atan2(y,x)函数的返回值范围(-PI,PI],表示与复数x+yi的幅角
// 下方角度atan2(..)交换了x和y的位置，计算的是与y轴正方向的夹角大小(关于y=x做对称变换)
// 这里是在雷达坐标系，所以是与正前方y轴的夹角大小
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
// round函数进行四舍五入取整
// 这边确定不是减去180度??? 不是
// 雷达水平方向上某个角度和水平第几个点的关联关系???关系如下：
// horizonAngle:(-PI,PI],columnIdn:[H/4,5H/4]-->[0,H] (H:Horizon_SCAN)
// 下面是把坐标系绕z轴旋转,对columnIdn进行线性变换
// x+==>Horizon_SCAN/2,x-==>Horizon_SCAN
// y+==>Horizon_SCAN*3/4,y-==>Horizon_SCAN*5/4,Horizon_SCAN/4
//
// 3/4*H
// | y+
// |
// (x-)H----------> H/2 (x+)
// |
// | y-
// 5/4*H H/4
//
//注：点数也是按顺时针的
// 得到角度后还要转换成列坐标
columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
if (columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;
// 经过上面columnIdn -= Horizon_SCAN的变换后的columnIdn分布：
// 3/4*H
// | y+
// H |
// (x-)---------->H/2 (x+)
// 0 |
// | y-
// H/4
//
if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
continue;
// 对于任意一个激光点, 根据求得的距离图像坐标 (rowIdn, columnIdn) 生成距离图像 rangeMat
// 当前点与雷达的深度
range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
rangeMat.at<float>(rowIdn, columnIdn) = range;
// columnIdn:[0,H] (H:Horizon_SCAN)==>[0,1800]
// 每个点的强度值，整数部分代表行号，小数部分代表列号。后面会通过intensity来恢复这个点
thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0; //点的位置信息
// 根据 距离图像坐标(rowIdn, columnIdn) 重新编排点序号 index , 放置到 fullCloud
// 列 行
index = columnIdn + rowIdn * Horizon_SCAN;
fullCloud->points[index] = thisPoint;
// 构建fullCloud
fullInfoCloud->points[index].intensity = range;
}
}
补充1：
补充2：
跳过，看不懂，看不懂
groundRemoval()提取地面点
首先。判断地面点的标志就是相邻两个激光线束扫描到点的坐标，如果两个坐标之间的差值 或者两个坐标之间的斜率大于一个设定的值，则将该点判断是地面点。所以此处用到了激光点云图片的深度信息
在提取地面点之前首先需要明确的一点，在激光雷达安装时是水平的，一般认为最中间的激光线束与水平面平行，所以对于一个16线的激光雷达而言，地面点在前7束激光线之中提取。即groundScanInd=7，遍历前7束所有点云：
// 提取地面点
//利用不同的扫描圈来表示地面，进而检测地面是否水平。例如在源码中的7个扫描圈，每两个圈之间
//进行一次比较，角度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加入到groundCloud点云
void groundRemoval(){
size_t lowerInd, upperInd;
float diffX, diffY, diffZ, angle;
// groundMat
// -1，没有有效的信息来检查是否接地
// 0，初始值，验证后，表示不接地
// 1，地
for (size_t j = 0; j < Horizon_SCAN; ++j){
// 前7个激光雷达扫描线束足够满足地面点的检测 所以只遍历 7 次
// groundScanInd 是在 utility.h 文件中声明的线数，groundScanInd=7
for (size_t i = 0; i < groundScanInd; ++i){
// 对每一个点，取当前的点和下一个扫描线的同样位置的点（在二维矩阵图像看来就是处在下一个row（行）同样colum（列）的点）。
lowerInd = j + ( i )*Horizon_SCAN;
upperInd = j + (i+1)*Horizon_SCAN;
// 初始化的时候用nanPoint.intensity = -1 填充
// 都是-1 证明是空点nanPoint
if (fullCloud->points[lowerInd].intensity == -1 ||
fullCloud->points[upperInd].intensity == -1){
groundMat.at<int8_t>(i,j) = -1;
continue;
}
// 由上下两线之间点的XYZ位置得到两线之间的俯仰角
// 如果俯仰角在10度以内，则判定(i,j)为地面点,groundMat[i][j]=1
// 否则，则不是地面点，进行后续操作
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;
if (abs(angle - sensorMountAngle) <= 10){
groundMat.at<int8_t>(i,j) = 1;
groundMat.at<int8_t>(i+1,j) = 1;
}
}
}
// 找到所有点中的地面点或者距离为FLT_MAX(rangeMat的初始值)的点，并将他们标记为-1
// rangeMat[i][j]==FLT_MAX，代表的含义是什么？ 无效点
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;
}
}
}
// 如果有节点订阅groundCloud，那么就需要把地面点发布出来
// 具体实现过程：把点放到groundCloud队列中去
if (pubGroundCloud.getNumSubscribers() != 0){ //有节点订阅groundCloud，往下执行
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]);
}
}
}
}
垂直相邻两点俯仰角小于10度就判定为地面点。
cloudSegmentation()点云分割
从论文中可以看到，分割函数的来源参考了两篇论文 （ Effificient Online Segmentation for Sparse 3D Laser Scans ，另外一篇 Fast Range Image-Based Segmentation of Sparse 3D Laser Scans for Online Operation）的方法 。
具体步骤：
1)首先判断点云标签，这个点云没有进行过分类（在原先的处理中没有被分到地面点中去或没有分到平面中），则通过labelComponents(i, j);对点云进行分类。
2)分类完成后，找到可用的特征点或者地面点(不选择labelMat[i][j]=0的点)，按照它的标签值进行判断，将部分界外点放进outlierCloud中。continue继续处理下一个点。
3)然后将大部分地面点去掉，剩下的那些点进行信息的拷贝与保存操作。
4)最后如果有节点订阅SegmentedCloudPure，那么把点云数据保存到segmentedCloudPure中去。
// 该部分为点云分割部分，去除一些分类比较小的点云集，也就是可以去除打在杂草和树叶上的那些点云，
// 分类打标签主要在labelComponents(int row, int col)子函数中实现
/*
本函数作为本程序的关键部分，首先调用了labelComponents函数，该函数对特征的检测进行了详细的描述，并且是针对于某一特定的点与其邻点的计算过程。
*/
void cloudSegmentation()
{
//这是在排除地面点与异常点之后，逐一检测邻点特征并生成局部特征
for (size_t i = 0; i < N_SCAN; ++i)
for (size_t j = 0; j < Horizon_SCAN; ++j)
// 如果labelMat[i][j]=0,表示没有对该点进行过分类
// 需要对该点进行聚类
if (labelMat.at<int>(i,j) == 0)
labelComponents(i, j); //对没有分过类的点进行聚类
int sizeOfSegCloud = 0;
// 为激光雷达里程计提取分段云
for (size_t i = 0; i < N_SCAN; ++i) {
// segMsg.startRingIndex[i]
// segMsg.endRingIndex[i]
// 表示第i线的点云起始序列和终止序列
// 以开始线后的第6点（0 - 1 + 5 = 4 != 6，我认为是开始后的第5个点（0 ~ 4），但根据后续的程序这里是否改成第6个点）为开始，以结束线前的第5点为结束
segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5; //我认为这里减1去掉
// 分段保存；对于因为点数太少而无法聚类的点的一部分（列数为5的倍数）设为界外点
for (size_t j = 0; j < Horizon_SCAN; ++j) {
// 找到可用的特征点（平面点）或者地面点(不选择labelMat[i][j]=0的点)，就可以纳入被分割点云
if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
// labelMat数值为999999表示这个点是因为聚类数量不够30而被舍弃的点
// 需要舍弃的点直接continue跳过本次循环，
// 当列数为5的倍数，并且行数较大，可以认为非地面点的，将它保存进异常点云(界外点云)中
// 然后再跳过本次循环
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;
}
}
// 如果是地面点,对于列数不为5的倍数的，直接跳过不处理（大多数地面点被跳过）
if (groundMat.at<int8_t>(i,j) == 1){
// 地面点云每隔5个点纳入被分割点云
if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
continue;
}
// 上面多个if语句已经去掉了不符合条件的点（无效点、离群点、地面点中每5个取一个、labelMat[i][j]=0的点），这部分直接进行信息的拷贝和保存操作
// 保存完毕后sizeOfSegCloud递增
// 标记地面点，以便它们以后不会被视为边缘（面）特征
segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1); //返回bool类型
// 标记点的列索引以便稍后标记遮挡（segmentedCloudColInd可存放28800个数据）
segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
// 保存范围信息
segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at<float>(i,j);
// 保存段云
segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]); //此时这里的：j + i*Horizon_SCAN == sizeOfSegCloud
// 段云的大小（即所有被保存起来点云的大小）
++sizeOfSegCloud;
}
}
// 以结束线前的第5个点为结束（减去++sizeOfSegCloud;运算多出来的1）
segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
}
// 如果有节点订阅SegmentedCloudPure,
// 那么把点云数据保存到segmentedCloudPure中去
if (pubSegmentedCloudPure.getNumSubscribers() != 0){
for (size_t i = 0; i < N_SCAN; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
// 需要选择不是地面点(labelMat[i][j]!=-1)和没被舍弃的点
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);
}
}
}
}
}
难啃
labelComponents()点云聚类函数
完成聚类
本函数对点云进行标记。通过标准的BFS算法对点进行标记：以(row，col)为中心向外面扩散，判断(row,col)是否属于平面中一点。
具体过程：
1)用queueIndX，queueIndY保存进行分割的点云行列值，用queueStartInd作为索引。
2)求这个点的4个邻接点，求其中离原点距离的最大值d1最小值d2。根据下面这部分代码来评价这两点之间是否具有平面特征。注意因为两个点上下或者水平对应的分辨率不一样，所以alpha是用来选择分辨率的。
3)通过判断角度是否大于60度来决定是否要将这个点加入保存的队列,然后生成聚类。
4)如果聚类超过30个点，直接标记为一个可用聚类;如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数,竖直方向上超过3个也将它标记为有效聚类
计算搜索点与其四邻域某点是否在一个平面，基本思路如下图所示，若计算出来的angle角度越大（一般要大于60度），则说明在一个平面的可能性大。
void labelComponents(int row, int col){ //聚类平面点
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;
// 标准的BFS
// BFS的作用是以(row，col)为中心向外面扩散，判断(row,col)是否是这个平面中一点
//queueSize指的是在特征处理时还未处理好的点的数量，因此该while循环是在尝试检测该特定点的周围的点的几何特征
while(queueSize > 0)
{
// pop point
fromIndX = queueIndX[queueStartInd]; // i（0 ~ 15）
fromIndY = queueIndY[queueStartInd]; // j（0 ~ 1799）
--queueSize; //即：接下来处理该点（用掉了）
++queueStartInd;
// make popped point
// labelCount的初始值为1，后面会递增
labelMat.at<int>(fromIndX, fromIndY) = labelCount; //第一遍：这里labelMat.at<int>(i, j)未赋值时为0，赋值后为1（初始值）
// neighbor=[[-1,0];[0,1];[0,-1];[1,0]]
// 遍历点[fromIndX,fromIndY]边上的四个邻点(上下左右)
// std::vector<std::pair<uint8_t, uint8_t> > neighborIterator
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（行范围：0 ~ 15）
if (thisIndX < 0 || thisIndX >= N_SCAN)
continue;
//注意列是环状，而行不是
// 是个环状的图片，左右连通
if (thisIndY < 0)
thisIndY = Horizon_SCAN - 1;
if (thisIndY >= Horizon_SCAN)
thisIndY = 0;
// 如果点[thisIndX,thisIndY]已经标记过（[thisIndX,thisIndY]为[fromIndX,fromIndY]的邻点）
// labelMat中，-1代表无效点，0代表未进行标记过，其余为其他的标记
// 如果当前的邻点已经标记过，则跳过该点。
// 如果labelMat已经标记为正整数，则已经聚类完成，不需要再次对该点聚类
// 防止无限循环（把已经检查过的点放回去）
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));
// alpha代表角度分辨率，
// 水平方向上角度分辨率是segmentAlphaX(rad)
// 竖直方向上角度分辨率是segmentAlphaY(rad)
// 该迭代器的first是0则是水平方向上的邻点，否则是竖直方向上的邻点，设置相应的角分辨率。
if ((*iter).first == 0)
alpha = segmentAlphaX; // 0.2度（相应弧度）
else
alpha = segmentAlphaY; //竖直方向（2度，相应弧度）
// 确定当前点（是平面点or？）
// 通过下面的公式计算这两点之间是否有平面特征
// atan2(y,x)的值越大，d1，d2之间的差距越小,越平坦
// 这个angle其实是该特定点与某邻点的连线与XOZ平面的夹角，这个夹角代表了局部特征的敏感性
angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
//若该搜索点的四邻域点在一个平面上，则保存，然后搜索这点的四邻域：
// 如果夹角大于60°，则将这个邻点纳入到局部特征中，该邻点可以用来配准使用
if (angle > segmentTheta){
// segmentTheta=1.0472<==>60度
// 如果算出角度大于60度，则假设这是个平面
queueIndX[queueEndInd] = thisIndX;
queueIndY[queueEndInd] = thisIndY;
++queueSize; //若此时queueSize > 0时，之后会以该点为基点继续向外扩散
++queueEndInd;
labelMat.at<int>(thisIndX, thisIndY) = labelCount;
lineCountFlag[thisIndX] = true; //聚类包括thisIndX环
allPushedIndX[allPushedIndSize] = thisIndX;
allPushedIndY[allPushedIndSize] = thisIndY;
++allPushedIndSize;
}
}
}
// （i，j）以及（i，j）周围处理完毕，平面点均被标记为labelCount（即：labelMat.at<int>(thisIndX, thisIndY) = labelCount），
// 但第一个即（i，j）也是被标记为labelCount（即：labelMat.at<int>(thisIndX, thisIndY) = labelCount），若（i，j）周围没有平面点，但他已经被标记好了
// |
bool feasibleSegment = false; // |
// |
// 如果聚类超过30个点，直接标记为一个可用聚类，labelCount需要递增 |
if (allPushedIndSize >= 30) //我觉得这里可以有效排除以上这种情况----------|
feasibleSegment = true;
else if (allPushedIndSize >= segmentValidPointNum){
// 如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数 |
int lineCount = 0;
for (size_t i = 0; i < N_SCAN; ++i)
if (lineCountFlag[i] == true)
++lineCount;
// 竖直方向上超过3个也将它标记为有效聚类 |
if (lineCount >= segmentValidLineNum)
feasibleSegment = true;
}
if (feasibleSegment == true){
++labelCount;
}else{
for (size_t i = 0; i < allPushedIndSize; ++i){
// 标记为999999的是需要舍弃的聚类的点（从平面点中选取的），因为他们的数量小于30个，且竖直方向上不超过3个
labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
}
}
}
聚类，超过30个点聚为一类，labelCount++；
小于30超过5，统计垂直方向聚类点数，超过3个也标记为一类；
若都不满足，则赋值999999表示需要舍弃的聚类点。
publishCloud()
发布各类点云数据
// 发布各类点云内容
void publishCloud(){
// 发布cloud_msgs::cloud_info消息
// 发布点云，包括采样后的地面点和分割后的点云
segMsg.header = cloudHeader; //只是一个头
pubSegmentedCloudInfo.publish(segMsg); //实际是通过segMsg消息传送点云
// 类似于指针类型
sensor_msgs::PointCloud2 laserCloudTemp;
// 发布离群后的点云 实参
pcl::toROSMsg(*outlierCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubOutlierCloud.publish(laserCloudTemp);
// pubSegmentedCloud发布分块点云（同1？）
pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubSegmentedCloud.publish(laserCloudTemp);
// rangeimage投影后的点云
if (pubFullCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*fullCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubFullCloud.publish(laserCloudTemp);
}
// 发布地面点
if (pubGroundCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*groundCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubGroundCloud.publish(laserCloudTemp);
}
// 分割后的点云，不包括地面点
if (pubSegmentedCloudPure.getNumSubscribers() != 0){
pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubSegmentedCloudPure.publish(laserCloudTemp);
}
// rangimage投影后的点云
if (pubFullInfoCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubFullInfoCloud.publish(laserCloudTemp);
}
}
resetParamenters() 参数重置
// 初始化/重置各类参数内容
// 因为循环体在类的成员函数里面，因此需要对容器类成员变量进行clear操作，以及在每次循环后对一些成员变量重新初始化。
void resetParameters(){
laserCloudIn->clear();
groundCloud->clear();
segmentedCloud->clear();
segmentedCloudPure->clear();
outlierCloud->clear();
rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
groundMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_8S, cv::Scalar::all(0));
labelMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32S, cv::Scalar::all(0));
labelCount = 1;
// nanPoint为-1
std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint);
std::fill(fullInfoCloud->points.begin(), fullInfoCloud->points.end(), nanPoint);
}
LeGO-LOAM首先进行地面分割，找到地面之后，对地面之上的点进行聚类。聚类的算法如下图，主要是根据斜率对物体做切割，最后得到地面和分割后的点云。上述步骤的关键是要理解如何进行地面分割和点云分割。
点云分割完成之后，接下来对分割后的点云提取特征，提取的特征的目的是进行点云配准，从而得出当前位姿。
扫一扫
1492
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
