[代码阅读]lego-loam 代码细节详解(一)-image_projection

1 image_projection

input topic：
velodyne_points	点云的输入话题，在utils.h里面可以修改
output topic：
full_cloud_projected
full_cloud_info
ground_cloud	地面点云
segmented_cloud	只去除了outlier的点云（降采样）
segmented_cloud_pure	去除了outlier点和地面点的所有的点云(没有降采样)
segmented_cloud_info	自定义消息类型
outlier_cloud	不属于规则聚类的点

以上加粗的三个话题是传递给下一个节点所需的。

这个cpp的划分十分工整，在雷达的回调函数里进行了以下7个功能的处理；

• 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();

下面，对每个函数的功能做一个详细的解答；

1.1 copyPointCloud(laserCloudMsg)

概述：将ROS_MSG -> PCL Poindcloud

没什么好说的，就是将点云的消息类型从ROS格式转换成了PCL的格式。

1.2 findStartEndAngle()

找到雷达旋转的开始的角度和结束的角度

atan2()范围在$(-\pi ,\pi )$之间

所以：

startOrientation $\in (-\pi ,\pi )$

endOrientation $\in (\pi ,3\pi )$

理想情况下，雷达扫描点的起始点在-x轴附近，结束点也在-x附近。但是不能保证结束点就是比$\pi$稍小，起始点比$-\pi$稍大。二者相减正好≈$2\pi$。再加上结束点加了$2\pi$。

这种情况下，需要对end点加上2 $\pi$

这两种特殊情况的处理，来保证e-s的值始终在2$\pi$范围附近。

1.3 projectPointCloud()

这里的verticalAngle算的是当前点的垂直角度，以velodyne16为例，范围在(-15°，+15°)之间。

rowIdn是将垂直的度数范围(0,30)除以16线的分辨率(也就是2°一线),得到每一条线的ring号。

当然，velodyne是不需要这么麻烦的，直接每个点都有一个ring值。实际代码可以对此进行优化，跳过这一部分。

这里以velodyne16为例，其垂直分辨率16，水平分辨率1800.本函数将整个点云数据投影成一个16*1800的二维平面图

需要注意的是：

thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;

每个点的强度值，整数部分代表行号，小数部分代表列号。后面会通过intensity来恢复这个点。

fullCloud里根据0-1800*16的范围来保存每个点；

1.4 groundRemoval()

标记地面点

通过在16*1800的下半面，也就是8*1800的部分，通过计算每个点和其相同列的上一个点之间的向量，判断该向量和地面的夹角，小于一定的阈值则认为是平面，在groundMat中标记为1；

同时，在labelmat中，该点被标记为-1.

1.5 cloudSegmentation()

重点：通过bfs进行四邻域搜素。不理解的可以看一下四邻域的BFS搜索。

1.5.1 labelComponents()

对所有的点进行遍历，并通过BFS进行搜索判断是否是同一个聚类。聚类的分割以角度值为阈值，其计算如下图所示。

使用当前点和其四邻域的点进行一个角度$\beta$的计算。分别获取当前点和邻域点的深度，深度值大的为$d_1$，小的为$d_2$，$\alpha$是两个点之间的分辨率，垂直和水平方向分别为2°和360/1800°。由此可以计算出$\beta$角。

如果$\beta$大于一定的阈值，说明两者之间没有突变，所以可以认为是同一个聚类(如上图)，加入到队列当中。继续下一步的四邻域搜索。

labelMat

value	类型
-1	地面点
0	未标记
1,2,3… =>统一都是count的值	同一个count值代表同一个聚类
999999	无效点

四邻域搜索完成之后，会统计这次四邻域搜索的cluster的size大小

• size>30:认为是有效分割，count值++；
• 3<size<=30:判断当前聚类在X方向(纵坐标，0-15)上有多少个点，如果大于3个，则仍然认为是聚类。
• 其他size值，认为是无效点，标记为999999。

1.5.2 对3w个点再次遍历

分别有以下几个操作：

• 1.对于outliers，每隔5的倍数保存一次，1800/5=360。可以认为是一种降采样，每1度保留一个outlier
• 2.对于地面点，每隔5的倍数保存一次。
• 3.对于以上两个标签都不是的点，保存到segmentedCloud，并准备发送给下一个节点。

1.6 publishCloud();

将各类型的点云发布出去

1.7 resetParameters();

参数重置