Lego-Loam代码阅读（转）

转自https://wykxwyc.github.io/tags/#LeGO-LOAM

目录

imageProjecion.cpp
imageProjecion
cloudHandler
findStartEndAngle
projectPointCloud
groundRemoval
cloudSegmentation
publishCloud
resetParameters
labelComponents

imageProjecion.cpp

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

imageProjecion

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

订阅话题：订阅来自velodyne雷达驱动的topic
    "/velodyne_points"(sensor_msgs::PointCloud2)，订阅的subscriber是subLaserCloud。
发布话题，这些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)

然后分配内存(对智能指针初始化)，初始化各类参数。
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. 重置参数

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

void findStartEndAngle()进行segMsg的开始和结束姿态的标记。因为开始和结束时的角度无法确定，而且两者之间的相对误差也是在一个范围之内的，所以代码要对这个问题进行处理。具体过程如下：

计算开始和结束的角度值segMsg.startOrientation和segMsg.endOrientation。
考虑结束的角度比开始时的角度值小的问题，对它进行处理。

// 开始和结束的角度差一般是多少？
// 一个velodyne 雷达数据包转过的角度多大？
// segMsg.endOrientation - segMsg.startOrientation范围为(0,4PI)
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的范围为(PI,3PI),一圈大小为2PI，应该在2PI左右
segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;

projectPointCloud

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

计算竖直角度，用atan2函数进行计算。
通过计算的竖直角度得到对应的行的序号rowIdn，rowIdn计算出该点激光雷达是水平方向上第几线的。从下往上计数，-15度记为初始线，第0线，一共16线(N_SCAN=16)。
求水平方向上的角度horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;。
根据水平方向上的角度计算列向量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的连在了一起，非常巧妙。

接着在thisPoint.intensity中保存一个点的位置信息rowIdn+columnIdn / 10000.0，fullInfoCloud的点保存点的距离信息；

groundRemoval

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

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

cloudSegmentation

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

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

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

}

labelComponents

void labelComponents(int row, int col)对点云进行标记。

用queueIndX，queueIndY保存进行分割的点云行列值，用queueStartInd作为索引。
求这个点的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度来决定是否要将这个点加入保存的队列。加入的话则假设这个点是个平面点。
然后进行聚类，聚类的规则是：
如果聚类超过30个点，直接标记为一个可用聚类，labelCount需要递增；
如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数
竖直方向上超过3个也将它标记为有效聚类
标记为999999的是需要舍弃的聚类的点，因为他们的数量小于30个