LeGO-LOAM 源码阅读笔记(imageProjecion.cpp)

LeGO-LOAM是一种在LOAM之上进行改进的激光雷达建图方法，建图效果比LOAM要好，但是建图较为稀疏，计算量也更小了。

本文原地址：wykxwyc的博客

github注释后LeGO-LOAM源码：LeGO-LOAM_NOTED
关于代码的详细理解，建议阅读：

1.地图优化代码理解

2.图像重投影代码理解

3.特征关联代码理解

4.LeGO-LOAM中的数学公式推导

以上博客会随时更新，如果对你有帮助，请点击注释代码的github仓库右上角star按钮，你的鼓励将给我更多动力。

imageProjecion.cpp概述

imageProjecion.cpp进行的数据处理是图像映射，将得到的激光数据分割，并在得到的激光数据上进行坐标变换。

imageProjecion

imageProjecion()构造函数的内容如下：

1. 订阅话题：订阅来自velodyne雷达驱动的topic
   • "/velodyne_points"(sensor_msgs::PointCloud2)，订阅的subscriber是subLaserCloud。
2. 发布话题，这些topic有：
   • "/full_cloud_projected"(sensor_msgs::PointCloud2)
   • "/full_cloud_info"(sensor_msgs::PointCloud2)
   • "/ground_cloud"(sensor_msgs::PointCloud2)
   • "/segmented_cloud"(sensor_msgs::PointCloud2)
   • "/segmented_cloud_pure"(sensor_msgs::PointCloud2)
   • "/segmented_cloud_info"(cloud_msgs::cloud_info)
   • "/outlier_cloud"(sensor_msgs::PointCloud2)

然后分配内存(对智能指针初始化)，初始化各类参数。

上述的cloud_msgs::cloud_info是自定义的消息类型，其具体定义如下：

Header header 

int32[] startRingIndex  // 长度：N_SCAN
int32[] endRingIndex    // 长度：N_SCAN

float32 startOrientation
float32 endOrientation
float32 orientationDiff

// 以下长度都是 N_SCAN*Horizon_SCAN
bool[]    segmentedCloudGroundFlag 
uint32[]  segmentedCloudColInd 
float32[] segmentedCloudRange

关于上面的自定义消息，另外还需要说明的是，segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;或者segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;表示的是将第0线和第16线点云横着排列后。每一线点云有一个startRingIndex和endRingIndex,表示这一线点云中的一部分，如下图绿色部分。
黑色部分是整体这一线点云中筛选出来满足条件的。

cloudHandler

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)是这个文件中最主要的一个函数。由它调用其他的函数：

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    copyPointCloud(laserCloudMsg);
    findStartEndAngle();
    projectPointCloud();
    groundRemoval();
    cloudSegmentation();
    publishCloud();
    resetParameters();
}

• 整体过程如下：
  1.复制点云数据
  2.找到开始和结束的角度
  3.移除地面点
  4.点云分块
  5.发布处理后的点云数据
  6.重置参数

上面第一部分复制点云数据函数copyPointCloud(laserCloudMsg) 是将ROS中的sensor_msgs::PointCloud2ConstPtr类型转换到pcl点云库指针。

velodyne 雷达数据

首先从VLP给的雷达数据手册上（63-9243 Rev B User Manual and Programming Guide,VLP-16.pdf）查找一下它的坐标系定义：

velodyne雷达在上面的坐标系下输出"/velodyne_points"(sensor_msgs::PointCloud2) 的点云数据。其数据格式可以理解为x,y,z,intensity 这4个量（它的定义比sensor_msgs::PointCloud要复杂一点，但本质还是这几个量）。

findStartEndAngle

void findStartEndAngle()进行关于segMsg(cloud_msgs::cloud_info segMsg)的三个内容的计算：
1）计算开始角度（segMsg.startOrientation）；
2）计算结束角度（segMsg.endOrientation）；
3）计算雷达转过的角度（开始和结束的角度差，segMsg.orientationDiff）。

关于具体计算，需要清楚整个雷达的坐标系定义，参考上面雷达坐标系的那张图。

另外在计算segMsg.startOrientation和segMsg.endOrientation时，atan2(..)函数取了负数的原因是：雷达旋转方向和坐标系定义下的右手定则正方向不一致。参考下图：

projectPointCloud

void projectPointCloud()将激光雷达得到的数据看成一个16x1800的点云阵列。然后根据每个点云返回的XYZ数据将他们对应到这个阵列里去。

1. 计算竖直角度，用atan2函数进行计算。
2. 通过计算的竖直角度得到对应的行的序号rowIdn，rowIdn计算出该点激光雷达是水平方向上第几线的。从下往上计数，-15度记为初始线，第0线，一共16线(N_SCAN=16)。
3. 求水平方向上的角度horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;。
4. 根据水平方向上的角度计算列向量columnIdn。
   这里对数据的处理比较巧妙，一开始觉得很奇怪，但后来发现这样做其实让数据更不容易失真。
   计算columnIdn主要是下面这三个语句：

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;

先把columnIdn从horizonAngle:(-PI,PI]转换到columnIdn:[H/4,5H/4],然后判断columnIdn大小，再讲它的范围转换到了[0,H] (H:Horizon_SCAN)。
这样就把扫描开始的地方角度为0与角度为360的连在了一起，非常巧妙。
5. 接着在thisPoint.intensity中保存一个点的位置信息rowIdn+columnIdn / 10000.0，fullInfoCloud的点保存点的距离信息；

具体的转换过程可以看下面这个两张图：

各种标记的含义

• groundMat：
  1） groundMat.at(i,j) = 0，初始值；
  2） groundMat.at(i,j) = 1，有效的地面点；
  3） groundMat.at(i,j) = -1，无效地面点；

• rangeMat
  1） rangeMat.at(i,j) = FLT_MAX，浮点数的最大值，初始化信息；
  2） rangeMat.at(rowIdn, columnIdn) = range，保存图像深度；

• labelMat
  1） labelMat.at(i,j) = 0，初始值；
  2） labelMat.at(i,j) = -1，无效点；
  3）labelMat.at(thisIndX, thisIndY) = labelCount，平面点；
  4）labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999，需要舍弃的点，数量不到30。

groundRemoval

void groundRemoval()由三个部分的程序组成。

1. 由上下两线之间点的XYZ位置得到两线之间的俯仰角，如果俯仰角在10度以内，则判定(i,j)和(i,j+1)为地面点,groundMat[i][j]=1，否则，则不是地面点，进行后续操作；
2. 找到所有点中的地面点，并将他们的labelMat标记为-1，rangeMat[i][j]==FLT_MAX 判定为无效的另一个条件。
3. 如果有节点订阅groundCloud，那么就需要把地面点发布出来。具体实现过程：把点放到groundCloud队列中去。这样就把地面点和非地面点标记并且区分开来了。

cloudSegmentation

void cloudSegmentation()进行的是点云分割的操作，将不同类型的点云放到不同的点云块中去，例如outlierCloud，segmentedCloudPure等。具体步骤：

1. 首先判断点云标签，这个点云没有进行过分类（在原先的处理中没有被分到地面点中去或没有分到平面中），则通过labelComponents(i, j);对点云进行分类。进行分类的过程在labelComponents中进行介绍。
2. 分类完成后，找到可用的特征点或者地面点(不选择labelMat[i][j]=0的点)，按照它的标签值进行判断，将部分界外点放进outlierCloud中。continue继续处理下一个点。
3. 然后将大部分地面点去掉，省下的那些点进行信息的拷贝与保存操作。
4. 最后如果有节点订阅SegmentedCloudPure，那么把点云数据保存到segmentedCloudPure中去。

labelComponents

void labelComponents(int row, int col)对点云进行标记。通过标准的BFS算法对点进行标记：以(row，col)为中心向外面扩散，判断(row,col)是否属于平面中一点。

• BFS过程：
  1.用queueIndX，queueIndY保存进行分割的点云行列值，用queueStartInd作为索引。
  2.求这个点的4个邻接点，求其中离原点距离的最大值d1最小值d2。根据下面这部分代码来评价这两点之间是否具有平面特征。注意因为两个点上下或者水平对应的分辨率不一样，所以alpha是用来选择分辨率的。

// alpha代表角度分辨率，
// Y方向上角度分辨率是segmentAlphaY(rad)
if ((*iter).first == 0)
		alpha = segmentAlphaX;
else
		alpha = segmentAlphaY;

// 通过下面的公式计算这两点之间是否有平面特征
// atan2(y,x)的值越大，d1，d2之间的差距越小,越平坦
angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));

• 在这之后通过判断角度是否大于60度来决定是否要将这个点加入保存的队列。加入的话则假设这个点是个平面点。
• 然后进行聚类，聚类的规则是：
  1.如果聚类超过30个点，直接标记为一个可用聚类，labelCount需要递增；
  2.如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数
  3.竖直方向上超过3个也将它标记为有效聚类
  4.标记为999999的是需要舍弃的聚类的点，因为他们的数量小于30个

publishCloud

void publishCloud()发布各类点云数据。

// 发布各类点云内容
void publishCloud(){
	// 发布cloud_msgs::cloud_info消息
    segMsg.header = cloudHeader;
    pubSegmentedCloudInfo.publish(segMsg);

    sensor_msgs::PointCloud2 laserCloudTemp;

	// pubOutlierCloud发布界外点云
    pcl::toROSMsg(*outlierCloud, laserCloudTemp);
    laserCloudTemp.header.stamp = cloudHeader.stamp;
    laserCloudTemp.header.frame_id = "base_link";
    pubOutlierCloud.publish(laserCloudTemp);

	// pubSegmentedCloud发布分块点云
    pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
    laserCloudTemp.header.stamp = cloudHeader.stamp;
    laserCloudTemp.header.frame_id = "base_link";
    pubSegmentedCloud.publish(laserCloudTemp);

    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);
    }

    if (pubFullInfoCloud.getNumSubscribers() != 0){
        pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";
        pubFullInfoCloud.publish(laserCloudTemp);
    }
}

---

resetParameters

void resetParameters()具体代码：

// 初始化/重置各类参数内容
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;

    std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint);
    std::fill(fullInfoCloud->points.begin(), fullInfoCloud->points.end(), nanPoint);
}

---

（imageProjection.cpp完）