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

1. imageProjecion.cpp概述

imageProjecion.cpp进行的数据处理是图像映射，将得到的激光数据分割，并在得到的激光数据上进行坐标变换。下列是在投影过程中主要用到的点云：

    pcl::PointCloud<PointType>::Ptr laserCloudIn;//接受到的来自激光Msg的原始点云数据
    pcl::PointCloud<PointXYZIR>::Ptr laserCloudInRing;//用 laserCloudInRing 存储含有具有通道R的原始点云数据

    //深度图点云：以一维形式存储与深度图像素一一对应的点云数据
    pcl::PointCloud<PointType>::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix
    //带距离值的深度图点云:与深度图点云存储一致的数据，但是其属性intensity记录的是距离值
    pcl::PointCloud<PointType>::Ptr fullInfoCloud; // same as fullCloud, but with intensity - range

    //注：所有点分为被分割点、未被分割点、地面点、无效点。
    pcl::PointCloud<PointType>::Ptr groundCloud;//地面点点云
    pcl::PointCloud<PointType>::Ptr segmentedCloud;//segMsg 点云数据:包含被分割点和经过降采样的地面点
    pcl::PointCloud<PointType>::Ptr segmentedCloudPure;//存储被分割点点云，且每个点的i值为分割标志值
    pcl::PointCloud<PointType>::Ptr outlierCloud;//经过降采样的未分割点

    //另外使用自创的rosmsg来表示点云信息
    // segMsg点云信息(存储分割结果并用于发送)
    cloud_msgs::cloud_info segMsg;

2.构造函数

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

1. 订阅话题：订阅来自velodyne雷达驱动的topic ,订阅的subscriber是subLaserCloud

   1. “/velodyne_points”(sensor_msgs::PointCloud2)

2. 发布话题，这些topic有：

   1. "/full_cloud_projected(sensor_msgs::PointCloud2)
   2. “/full_cloud_info”(sensor_msgs::PointCloud2)
   3. “/ground_cloud”(sensor_msgs::PointCloud2)
   4. “/segmented_cloud”(sensor_msgs::PointCloud2)
   5. “/segmented_cloud_pure”(sensor_msgs::PointCloud2)
   6. “/segmented_cloud_info”(cloud_msgs::cloud_info)
   7. “/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 # true - ground point, false - other points
uint32[]  segmentedCloudColInd # point column index in range image
float32[] segmentedCloudRange # point range 

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

3.cloudHandler

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

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

整体过程如下：
1.复制点云数据
       点云数据函数copyPointCloud(laserCloudMsg) 是将ROS中的sensor_msgs::PointCloud2ConstPtr类型转换到pcl点云库指针。
2.找到开始和结束的角度
3.点云投影
4.地面检测，移除地面点
5.点云分割
6.发布处理后的点云数据
7.重置参数，清空此次的点云变量

4.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个量。

5.findStartEndAngle

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

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

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

代码如下：

 //将起始点与最末点进行角度的转换
   void findStartEndAngle()
   {
   
       //计算角度时以x轴负轴为基准
       segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
       //因此最末角度为2π减去计算值
       segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y, laserCloudIn->points[laserCloudIn->points.size() - 2].x) + 2 * M_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;
   }

6.projectPointCloud

void projectPointCloud()是将点云逐一计算深度，将具有深度的点云保存至fullInfoCloud中。

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的点保存点的距离信息；

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

代码如下：

 //逐一计算点云深度，并具有深度的点云保存至fullInfoCloud中
    void projectPointCloud() {
   
        // range image projection
        float 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 ia