LeGO-LOAM代码和原理解析(1)-Image Projection
文章目录
- 前言
- 一、ImageProjection成员变量
- 二、构造函数ImageProjection()
-
- 1.分配内存allocateMemory
- 三、回调函数cloudHandler
-
- 1.copyPointCloud
- 2.findStartEndAngle
- 3.projectPointCloud
- 4.groundRemoval
- 5.cloudSegmentation
- 6.labelComponents
- 6.resetParameters
- 总结
前言
imageProjecion.cpp进行的数据处理是图像映射，将得到的激光数据分割，并在得到的激光数据上进行坐标变换。
一、ImageProjection成员变量
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;
具体定义如下：
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
二、构造函数ImageProjection()
main()函数通过构造函数订阅和发布消息。代码如下：
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>
(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
pubFullCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_projected", 1);
pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_info", 1);
pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2> ("/ground_cloud", 1);
pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud", 1);
pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud_pure", 1);
pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info> ("/segmented_cloud_info", 1);
pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2> ("/outlier_cloud", 1);
...
allocateMemory();//为指针分配内存，否则出现野指针错误，初始化一次
resetParameters();//容器清空，参数初始化化，为下一次循环做准备，回调函数中循环
其中pointCloudTopic就是输入点云的topic，本代码中为/os1_points 。
1.分配内存allocateMemory
初始化：为成员变量指针分配内存，否则出现野指针错误，在构造函数中初始化一次。
代码如下：
void allocateMemory(){
laserCloudIn.reset(new pcl::PointCloud<PointType>());
laserCloudInRing.reset(new pcl::PointCloud<PointXYZIR>());
fullCloud.reset(new pcl::PointCloud<PointType>());
fullInfoCloud.reset(new pcl::PointCloud<PointType>());
groundCloud.reset(new pcl::PointCloud<PointType>());
segmentedCloud.reset(new pcl::PointCloud<PointType>());
segmentedCloudPure.reset(new pcl::PointCloud<PointType>());
outlierCloud.reset(new pcl::PointCloud<PointType>());
fullCloud->points.resize(N_SCAN*Horizon_SCAN);
fullInfoCloud->points.resize(N_SCAN*Horizon_SCAN);
segMsg.startRingIndex.assign(N_SCAN, 0);
segMsg.endRingIndex.assign(N_SCAN, 0);
segMsg.segmentedCloudGroundFlag.assign(N_SCAN*Horizon_SCAN, false);
segMsg.segmentedCloudColInd.assign(N_SCAN*Horizon_SCAN, 0);
segMsg.segmentedCloudRange.assign(N_SCAN*Horizon_SCAN, 0);
std::pair<int8_t, int8_t> neighbor;//上下左右偏移
neighbor.first = -1; neighbor.second = 0; neighborIterator.push_back(neighbor);
neighbor.first = 0; neighbor.second = 1; neighborIterator.push_back(neighbor);
neighbor.first = 0; neighbor.second = -1; neighborIterator.push_back(neighbor);
neighbor.first = 1; neighbor.second = 0; neighborIterator.push_back(neighbor);
allPushedIndX = new uint16_t[N_SCAN*Horizon_SCAN];
allPushedIndY = new uint16_t[N_SCAN*Horizon_SCAN];
queueIndX = new uint16_t[N_SCAN*Horizon_SCAN];
queueIndY = new uint16_t[N_SCAN*Horizon_SCAN];
}
三、回调函数cloudHandler
代码会在回调函数内进行循环，需注意成员变量容器的清空，否则会内存泄露，本代码由构造函数中resetParameters()函数进行clear
由它调用其他的函数。代码如下：
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
// 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.copyPointCloud
本函数将ROS中的sensor_msgs::PointCloud2ConstPtr类型转换到pcl点云库指针。代码如下：
void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
cloudHeader = laserCloudMsg->header;
cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
// Remove Nan points
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
// have "ring" channel in the cloud
if (useCloudRing == true){
pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
if (laserCloudInRing->is_dense == false)// 指定points中的数据是否全部是有限的，此时为true；当数据集中包含有Inf/NaN等值时，此时为false。
{
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
}
}
2.findStartEndAngle
本函数将起始点与最末点进行角度的转换
并保存到segMsg：
1）计算开始角度（segMsg.startOrientation）；
2）计算结束角度（segMsg.endOrientation）；
3）计算雷达转过的角度（开始和结束的角度差，segMsg.orientationDiff）。
代码如下（示例）：
//将起始点与最末点进行角度的转换
void findStartEndAngle(){
//计算角度时以x轴负轴为基准(-pi~-0~0~pi)
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;
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;
}
下图依次是atan2和-atan的取值范围，以及正常和特殊情况下开始角度和结束角度
3.projectPointCloud
本函数逐一计算点的深度，将具有深度的点云保存至fullInfoCloud中。
以16为例，将激光雷达得到的数据看成一个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。
5. 接着在thisPoint.intensity中保存一个点的位置信息rowIdn+columnIdn / 10000.0，存入fullInfoCloud点云。
代码如下：
//逐一计算点云深度，并具有深度的点云保存至fullInfoCloud中;本函数将整个点云数据投影成一个16*1800的二维平面图
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 iamge for this point
if (useCloudRing == true){
rowIdn = laserCloudInRing->points[i].ring;
}
else{
//计算竖直方向上的点的角度以及在整个雷达点云中的哪一条水平线上
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;
//计算水平方向上点的角度与所在线数
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
//round是四舍五入取偶
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;
//当前点与雷达的深度
range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
if (range < sensorMinimumRange)
continue;
rangeMat.at<float>(rowIdn, columnIdn) = range;
// 每个点的强度值，整数部分代表行号，小数部分代表列号。后面会通过intensity来恢复这个点
thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
index = columnIdn + rowIdn * Horizon_SCAN;
fullCloud->points[index] = thisPoint;
fullInfoCloud->points[index] = thisPoint;
fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
}
4.groundRemoval
本函数利用不同的扫描圈来表示地面，进而检测地面是否水平。例如在源码中的七个扫描圈，每两个圈之间进行一次比较，角度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加入到groundCloud点云。
代码如下：
//利用不同的扫描圈来表示地面，进而检测地面是否水平。例如在源码中的七个扫描圈，每两个圈之间
//进行一次比较，角度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加入到groundCloud点云
void groundRemoval(){
size_t lowerInd, upperInd;
float diffX, diffY, diffZ, angle;
// groundMat
// -1, no valid info to check if ground of not
// 0, initial value, after validation, means not ground
// 1, ground
for (size_t j = 0; j < Horizon_SCAN; ++j){
for (size_t i = 0; i < groundScanInd; ++i){
lowerInd = j + ( i )*Horizon_SCAN;
upperInd = j + (i+1)*Horizon_SCAN;
if (fullCloud->points[lowerInd].intensity == -1 ||
fullCloud->points[upperInd].intensity == -1){
// no info to check, invalid points
groundMat.at<int8_t>(i,j) = -1;
continue;
}
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){
groundMat.at<int8_t>(i,j) = 1;
groundMat.at<int8_t>(i+1,j) = 1;
}
}
}
// extract ground cloud (groundMat == 1)
// mark entry that doesn't need to label (ground and invalid point) for segmentation
// note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan
for (size_t i = 0; i < N_SCAN; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
labelMat.at<int>(i,j) = -1;
}
}
}
if (pubGroundCloud.getNumSubscribers() != 0){
for (size_t i = 0; i <= groundScanInd; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (groundMat.at<int8_t>(i,j) == 1)
groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
}
}
}
}
各种标记的含义
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。
5.cloudSegmentation
本函数作为本程序的关键部分，首先调用了labelComponents函数，该函数对特征的检测进行了详细的描述，并且是针对于某一特定的点与其邻点的计算过程。
具体步骤：
1)首先判断点云标签，这个点云没有进行过分类（在原先的处理中没有被分到地面点中去或没有分到平面中），则通过labelComponents(i, j);对点云进行分类。
2)分类完成后，找到可用的特征点或者地面点(不选择labelMat[i][j]=0的点)，按照它的标签值进行判断，将部分界外点放进outlierCloud中。continue继续处理下一个点。
3)然后将大部分地面点去掉，剩下的那些点进行信息的拷贝与保存操作。
4)最后如果有节点订阅SegmentedCloudPure，那么把点云数据保存到segmentedCloudPure中去。
代码如下：
void cloudSegmentation(){
//这是在排除地面点与异常点之后，逐一检测邻点特征并生成局部特征
for (size_t i = 0; i < N_SCAN; ++i)
for (size_t j = 0; j < Horizon_SCAN; ++j)
if (labelMat.at<int>(i,j) == 0)
labelComponents(i, j);
int sizeOfSegCloud = 0;
// extract segmented cloud for lidar odometry
for (size_t i = 0; i < N_SCAN; ++i) {
segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;
for (size_t j = 0; j < Horizon_SCAN; ++j) {
//如果是被认可的特征点或者是地面点，就可以纳入被分割点云
if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
//离群点或异常点的处理
if (labelMat.at<int>(i,j) == 999999){
if (i > groundScanInd && j % 5 == 0){
outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
continue;
}else{
continue;
}
}
// majority of ground points are skipped
if (groundMat.at<int8_t>(i,j) == 1){
//地面点云每隔5个点纳入被分割点云
if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
continue;
}
// mark ground points so they will not be considered as edge features later
segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
// mark the points' column index for marking occlusion later
segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
// save range info
segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at<float>(i,j);
// save seg cloud
segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
// size of seg cloud
++sizeOfSegCloud;
}
}
segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
}
// extract segmented cloud for visualization
if (pubSegmentedCloudPure.getNumSubscribers() != 0){
for (size_t i = 0; i < N_SCAN; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (labelMat.at<int>(i,j) > 0 && labelMat.at<int>(i,j) != 999999){
segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
segmentedCloudPure->points.back().intensity = labelMat.at<int>(i,j);
}
}
}
}
}
6.labelComponents
本函数对点云进行标记。通过标准的BFS算法对点进行标记：以(row，col)为中心向外面扩散，判断(row,col)是否属于平面中一点。
具体过程：
1)用queueIndX，queueIndY保存进行分割的点云行列值，用queueStartInd作为索引。
2)求这个点的4个邻接点，求其中离原点距离的最大值d1最小值d2。根据下面这部分代码来评价这两点之间是否具有平面特征。注意因为两个点上下或者水平对应的分辨率不一样，所以alpha是用来选择分辨率的。
3)通过判断角度是否大于60度来决定是否要将这个点加入保存的队列,然后生长聚类。
4)如果聚类超过30个点，直接标记为一个可用聚类;如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数,竖直方向上超过3个也将它标记为有效聚类。
代码如下：
//首先调用了labelComponents函数，
//该函数对特征的检测进行了详细的描述，
//并且是针对于某一特定的点与其邻点的计算过程。
void labelComponents(int row, int col){
// use std::queue std::vector std::deque will slow the program down greatly
float d1, d2, alpha, angle;
int fromIndX, fromIndY, thisIndX, thisIndY;
bool lineCountFlag[N_SCAN] = {false};//聚类后竖直方向跨越的数量
queueIndX[0] = row;
queueIndY[0] = col;
int queueSize = 1;
int queueStartInd = 0;
int queueEndInd = 1;
allPushedIndX[0] = row;
allPushedIndY[0] = col;
int allPushedIndSize = 1;
//queueSize指的是在特征处理时还未处理好的点的数量，
//因此该while循环是在尝试检测该特定点的周围的点的几何特征
while(queueSize > 0){
// Pop point
fromIndX = queueIndX[queueStartInd];
fromIndY = queueIndY[queueStartInd];
--queueSize;
++queueStartInd;
// Mark popped point
labelMat.at<int>(fromIndX, fromIndY) = labelCount;
//检查上下左右四个邻点
for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
// new index
thisIndX = fromIndX + (*iter).first;
thisIndY = fromIndY + (*iter).second;
// index should be within the boundary
if (thisIndX < 0 || thisIndX >= N_SCAN)
continue;
// at range image margin (left or right side)
if (thisIndY < 0)
thisIndY = Horizon_SCAN - 1;
if (thisIndY >= Horizon_SCAN)
thisIndY = 0;
// prevent infinite loop (caused by put already examined point back)
if (labelMat.at<int>(thisIndX, thisIndY) != 0)
continue;
//d1与d2分别是该特定点与某邻点的深度
d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY),
rangeMat.at<float>(thisIndX, thisIndY));
d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY),
rangeMat.at<float>(thisIndX, thisIndY));
//该迭代器的first是0则是水平方向上的邻点，否则是竖直方向上的，设置相应的角分辨率。
if ((*iter).first == 0)
alpha = segmentAlphaX;
else
alpha = segmentAlphaY;
//这个angle其实是该特定点与某邻点的连线与XOZ平面的夹角，
//这个夹角代表了局部特征的敏感性
angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
//如果夹角大于60°，则将这个邻点纳入到局部特征中，该邻点可以用来配准使用
if (angle > segmentTheta){
queueIndX[queueEndInd] = thisIndX;
queueIndY[queueEndInd] = thisIndY;
++queueSize;
++queueEndInd;
labelMat.at<int>(thisIndX, thisIndY) = labelCount;
lineCountFlag[thisIndX] = true; // 聚类中包括thisIndX环
allPushedIndX[allPushedIndSize] = thisIndX;
allPushedIndY[allPushedIndSize] = thisIndY;
++allPushedIndSize;
}
}
}
// check if this segment is valid
bool feasibleSegment = false;
// 聚类超过30个点，直接标记为一个可用聚类
if (allPushedIndSize >= 30)
feasibleSegment = true;
//如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数
else if (allPushedIndSize >= segmentValidPointNum){
int lineCount = 0;
for (size_t i = 0; i < N_SCAN; ++i)
if (lineCountFlag[i] == true)
++lineCount;
//竖直方向上超过3个也将它标记为有效聚类
if (lineCount >= segmentValidLineNum)
feasibleSegment = true;
}
// segment is valid, mark these points
if (feasibleSegment == true){
++labelCount;
}else{ // segment is invalid, mark these points
for (size_t i = 0; i < allPushedIndSize; ++i){
labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
}
}
}
其中d1、d2、alpha和angle的关系如下图所示：
6.resetParameters
因为循环体在类的成员函数里面，因此需要对容器类成员变量进行clear操作，以及在每次循环后对一些成员变量重新初始化。
代码如下：
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);
}
该处使用的url网络请求的数据。
总结
本节点对每一帧的激光数据的预处理。包括了地面的提取（没有统一平面作为地面的假设），点云的实时分割。