GWH Blog

LeGo-LOAM中的ImageProjection节点

发表于 更新于 分类于 ， ，
本文字数： 4.9k 阅读时长 ≈ 4 分钟

LeGo-LOAM中的ImageProjection节点

LeGo-LOAM是在LOAM基础上改进的激光雷达定位与建图方法，它对地面点进行了优化、提出了两步LM优化算法，加入了闭环检测和全局因子图优化等。
本系列博客只说明了关键代码的功能实现，以及一些关键地方的数学推导，详细的解释都写在了代码注释中，注释后的代码在融合相机后会传到github。同时也列出来一些问题提醒自己有条件时实际去验证。

这个模块接收1种消息：

1
2
// 接收原始点云的topic，注意使用成员函数作为回调函数的写法，pointCloudTopic来自"utility.h"
subLaserCloud_ = nh_.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);

这个模块做一些点云预处理的操作，主要是进行深度图转换、地面分割、物体分割的操作。

主要的接口函数就是原始点云的消息接收函数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
      // 将ROS格式的点云消息转换为pcl的，存给成员变量
      copyPointCloud(laserCloudMsg);
      // Start and end angle of a scan
      findStartEndAngle();
      // Range image projection
      projectPointCloud();
      // Mark ground points
      groundRemoval();
      // Point cloud segmentation
      cloudSegmentation();
      // Publish all clouds
      publishCloud();
      // Reset parameters for next iteration
      resetParameters();
}

1、findStartEndAngle()

这个函数找到一帧点云中的起始点和终止点的角度，存在自定义格式的消息segMsg_中。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
/**
* @brief    找到一帧点云的起始和终止点的角度
* 
* 1、将这些数据保存在自定义的点云消息segMsg_中
*/
void findStartEndAngle(){
   // 因为velodyne雷达顺时针旋转，所以atan2(..)函数前面需要加一个负号，参见A-LOAM代码
   segMsg_.startOrientation = -atan2(laserCloudIn_->points[0].y, laserCloudIn_->points[0].x);
   segMsg_.endOrientation   = -atan2(laserCloudIn_->points[laserCloudIn_->points.size() - 1].y, laserCloudIn_->points[laserCloudIn_->points.size() - 1].x) + 2 * M_PI;
   // 同LOAM中的方法一样，保证  PI < orientationDiff < 3*PI
   if (segMsg_.endOrientation - segMsg_.startOrientation > 3 * M_PI) {
       segMsg.endOrientation -= 2 * M_PI;
   } else if (segMsg_.endOrientation - segMsg_.startOrientation < M_PI)
       segMsg_.endOrientation += 2 * M_PI;
   segMsg_.orientationDiff = segMsg_.endOrientation - segMsg_.startOrientation;
}

这个代码说明，输入的点云最好是有序的点云（按照每一列点云采集的顺序存储的），自定义的消息格式为：

1
2
3
4
5
6
7
8
9
10
11
12
Header header 

int32[] startRingIndex		# 表示在第i条激光线中计算激光点曲率时的开始点在点云segmentedCloudColInd中的索引
int32[] endRingIndex		# 表示在第i条激光线中计算激光点曲率时的最后一点在点云segmentedCloudColInd中的索引

float32 startOrientation	# 当前帧点云初始点的水平角度
float32 endOrientation		# 当前帧点云结束点的水平角度
float32 orientationDiff		# 当前帧点云初始点与结束点之间的角度差值（约为2pi）

bool[]    segmentedCloudGroundFlag 	# 用于标记点云segmentedCloud中的点是否是地面点，true-地面点，false-分割点
uint32[]  segmentedCloudColInd 		# 点云segmentedCloud中的激光点在rangeMat_中的列索引
float32[] segmentedCloudRange 		# 点云segmentedCloud中的激光点的深度

2、projectPointCloud()

将点云投影成rangeMat表示，rangeMat的大小是N_SCAN * Horizon_SCAN，对于HDL-64E，rangeMat的大小就是64 * 2000。这个函数就是计算输入点云laserCloudIn中每一点在rangeMat中的行索引和列索引的。行索引计算如下：

1
2
3
4
5
6
7
thisPoint.x = laserCloudIn_->points[i].x;
thisPoint.y = laserCloudIn_->points[i].y;
thisPoint.z = laserCloudIn_->points[i].z;
// 计算当前遍历点的行索引rowIdn（即激光雷达线数，从下到上为0 ~ N_SCAN-1）
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;

列索引计算如下：

1
2
3
4
5
6
7
8
9
// 计算当前遍历点的列索引columnIdn（以velodyne雷达x轴作为image的中间列索引位置，顺时针索引减小，逆时针索引增加）
// atan2(y,x)函数计算的是x+yi的幅角，返回值的范围是(-Pi,Pi]
// 这里计算的是atan2(x,y)，得到的是点(x,y)与y轴的夹角
// TODO::这里没有判断雷达的线扫是否过半，若一圈的点云占据了370°，那么多出来的10°就会对转换后的只表示360°范围的rangeMat_造成影响
// 这里的坑看他是怎么填平的？
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
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;

这里计算列索引可用如下的图形描述：

雷达x轴处的点云被投影到了rangeMat的中间，并随着雷达顺时针的线扫rangMat向左填充，雷达转过-x轴rangMat从最右侧向中间填充。这里将点云投影成rangeMat的原因完全是因为工程上实现BFS比较方便。

3、groundRemoval()

这个函数主要用来标记地面点，首先地面点一般出现在竖直视场角0°之下，所以遍历rangeMat的行时可以做一个减枝。对于rangeMat中在同一列，相邻行的两激光点，计算这两个点与水平面的夹角，夹角小于10°，这两个点都被标记为地面点，代码如下：

1
2
3
4
5
6
7
8
9
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) {
    labelMat_.at<int>(i, j) = 0;
    labelMat_.at<int>(i + 1, j) = 0;
}

一个改进的地面分割方法：

在x-y平面上，将点云等分为一系列的扇形区域，设每个扇形区域的角度为$\Delta \alpha$，则一共可以划分$\frac{2 \pi}{\Delta \alpha}$个扇形区。通过下面的公式将激光点映射到对应的扇区：
$$
S_i = floor (\frac{atan2 + \pi}{\Delta \alpha})
$$
对于每一个扇区，将该区域的点按照到圆心的距离再次等间距划分为一个个子区域：

如此将一个点的三维坐标(x,y,z)转化到u-z平面的(p,z)坐标：
$$
p_i = (u_i,z_i)^T \quad u_i = \sqrt{x_i^2 + y_i^2}
$$
对子区域中的所有点，计算平均值后取得该子区域的平均点（二维坐标形式）。地面分割算法通过计算从圆心向外相邻两子区域的平均点所在直线与激光雷达水平面的夹角，按照夹角大小是否小于给定的阈值判断其是否属于地面点。设当前两相邻平均点的直线公式为$z = ku + b$，则具体的判定标准如下:

  1. 直线斜率必须小于给定阈值，即地面的上升角度不应该太大；
  2. 直线的斜率小于给定阈值的同时，直线的截距也应该小于给定阈值，即位于高台上的点应该不被选为地面点；

4、cloudSegmentation()

点云聚类进行物体分割，物体分割的目的是判定两激光点是否同属于一个物体

如上图所示，O点是激光雷达的原点，A,B是激光雷达系中任意相邻的俩激光点，分别对应激光束OA、OB，其夹角为$\alpha$，$\alpha$的大小只取决于激光雷达的水平或者垂直角分辨率。Lego-LOAM中通过判定$\beta$角是否大于60°来判定两点是否属于同一物体。可这样计算，不失一般性，假设OA与OB中较长的一个为OA，公式如下：
$$
d_1 = ||OA|| \quad d_2 = ||OB||
$$

$$
\beta = arctan \frac{||BH||}{||AH||} = arctan\frac{d_2 sin \alpha}{d_1 - d_2 cons \alpha}
$$
在代码实现上是使用标准的广度优先算法（BFS）完成的，经典的BFS算法，一般使用stl::queue辅助实现，作者嫌弃效率低，使用足够大的一维数组代替了stl::queue。物体分割完成之后要判断分割的有效性，有效分割满足这俩条件之一即可：

  1. 分割得到的cluster点集大小>=30；
  2. 当前cluster点数仍>=5，且包含的竖直方向上激光线数目>=3；
听说打赏我的人，最后都找到了真爱。