LeGo-LOAM激光雷达定位算法源码阅读(一)

文章目录

1.引言

2.CMakeLists.txt及头文件解读

3.imageProjection.cpp解读

1.引言

论文解读:无人驾驶算法学习(9):LeGo-LOAM激光雷达定位算法

原论文：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/Shan_Englot_IROS_2018_Preprint.pdf

LeGO-LOAM在LOAM在此基础上，增加了回环检测，分别由点云分割、特征提取、激光里程计和激光地图组成。该系统通过接收三维激光雷达的输入和输出6自由度姿态来估计。

整个系统分为五个模块：

(1)第一个是分割，将单个扫描点云投影到固定范围的图像上进行分割；

(2)二是将分割点云发送到特征提取模块；

(3)第三是激光雷达里程计利用从上一个模块中提取的特性，在连续扫描过程中找到机器人位置的转换；

(4)第四种这些信息最终用于激光雷达点云的构图；

(5)第五个模块是结合激光雷达里程测量和构图的姿态估计结果，并输出最终姿态估计。

节点与主题关系图：

2.CMakeLists.txt及头文件解读

CMakeLists.txt中:

include_directories(

${catkin_INCLUDE_DIRS}

include

${PCL_INCLUDE_DIRS}

${OpenCV_INCLUDE_DIRS}

${GTSAM_INCLUDE_DIR}

)，说明它依赖于PCL、OpenCV、GTSAM三个库

在这个项目中，有imageProjection(图像投影)，featureAssociation(特征关联)，mapOptmization(地图优化)，transformFusion(位置优化)四个可执行文件，然后逐一分析算法原理。

唯一的头文件utility.h定义一些普通的东西，先看看头文件。

首先PointType是带intensity的PointXYZ：

typedef pcl::PointXYZI PointType;

这些参数是根据雷达品牌确定的，如16线、1800点的数据：

// VLP-16

extern const int N_SCAN = 16;

extern const int Horizon_SCAN = 1800;

extern const float ang_res_x = 0.2.///水平上每条线的间隔为0.2°

extern const float ang_res_y = 2.0////每条线在垂直方向上的间隔为2°

extern const float ang_bottom = 15.0 0.1；///垂直方向的起始角度为负角，与水平方向相差15.1°

extern const int groundScanInd = 7./用多少个扫描圈来表示地面

点云分割的必要参数：

extern const float sensorMountAngle = 0.0;

extern const float segmentTheta = 1.0472；//点云分割角度跨度上限(π/3)

extern const int segmentValidPointNum = 5.//检查上下左右连续5点作为分割特征依据

extern const int segmentValidLineNum = 3.//检查上下左右连续三线作为分割特征依据

extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI;//转成弧度

extern const float segmentAlphaY = ang_res_y / 180.0 * M_PI;//转成弧度

而 PointTypePose指具有姿态角的具体点：

//添加新的PointT常规操作类型

struct PointXYZIRPYT定义点类型结构

{

PCL_ADD_POINT4D // 这种类型有四个元素

PCL_ADD_INTENSITY;

float roll;

float pitch;

float yaw;

double time;

EIGEN_MAKE_ALIGNED_OPERATOR_NEW// 确保new操作符对齐操作

} EIGEN_ALIGN16;// 强制SSE对齐

typedef PointXYZIRPYT PointTypePose;

3.imageProjection.cpp解读

根据算法思想，我们首先看到imageProjection.cpp，该程序投影三维点云。以下是投影过程中主要使用的点云：

pcl::PointCloud::Ptr laserCloudIn;////雷达直接传出的点云

pcl::PointCloud::Ptr fullCloud;////投影后的点云

pcl::PointCloud::Ptr fullInfoCloud;///整体点云

pcl::PointCloud::Ptr groundCloud;///地面点云

pcl::PointCloud::Ptr segmentedCloud;//分割后的部分

pcl::PointCloud::Ptr segmentedCloudPure;//分割后部分的几何信息

pcl::PointCloud::Ptr outlierCloud;///分割过程中的异常

另外，使用自创rosmsg表示点云信息：

cloud_msgs::cloud_info segMsg;

1.回调函数触发核心函数void cloudHandler(){}

copyPointCloud(laserCloudMsg);//点云复制

findStartEndAngle()

projectPointCloud()//点云投影

groundRemoval()///地面检测

cloudSegmentation()//点云分割

publishCloud()//点云发布

resetParameters(); ////清除此次点云变量

2.findStartEndAngle(){}

///转换起点和最后点的角度

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;

}

3.projectPointCloud(){}

///逐一计算点云深度，并将深度点云保存至fullInfoCloud中

void projectPointCloud()

{

float verticalAngle, horizonAngle, range;

size_t rowIdn, columnIdn, index, cloudSize;

PointType thisPoint;

cloudSize = laserCloudIn->points.size();

for (size_t i = 0; i < cloudSize; i)

{

size_t lowerInd, upperInd;

float diffX, diffY, diffZ, angle;

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

groundMat.at(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(i,j) = 1;

groundMat.at(i+1,j) = 1;

}

}

}

for (size_t i = 0; i < N_SCAN; ++i){

for (size_t j = 0; j < Horizon_SCAN; ++j){

if (groundMat.at(i,j) == 1 || rangeMat.at(i,j) == FLT_MAX){

labelMat.at(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(i,j) == 1)

groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);

}

}

}

}

5.cloudSegmentation(){}关键部分

//首先调用了labelComponents函数，

//该函数对特征的检测进行了详细的描述，

//并且是针对于某一特定的点与其邻点的计算过程。

void labelComponents(int row, int col)

{

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)

{

fromIndX = queueIndX[queueStartInd];

fromIndY = queueIndY[queueStartInd];

--queueSize;

++queueStartInd;

labelMat.at(fromIndX, fromIndY) = labelCount;

//检查上下左右四个邻点

for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter)

{

thisIndX = fromIndX + (*iter).first;

thisIndY = fromIndY + (*iter).second;

if (thisIndX < 0 || thisIndX >= N_SCAN)

continue;

if (thisIndY < 0)

thisIndY = Horizon_SCAN - 1;

if (thisIndY >= Horizon_SCAN)

thisIndY = 0;

if (labelMat.at(thisIndX, thisIndY) != 0)

continue;

//d1与d2分别是该特定点与某邻点的深度

d1 = std::max(rangeMat.at(fromIndX, fromIndY),

rangeMat.at(thisIndX, thisIndY));

d2 = std::min(rangeMat.at(fromIndX, fromIndY),

rangeMat.at(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(thisIndX, thisIndY) = labelCount;

lineCountFlag[thisIndX] = true;

allPushedIndX[allPushedIndSize] = thisIndX;

allPushedIndY[allPushedIndSize] = thisIndY;

++allPushedIndSize;

}

}

}

bool feasibleSegment = false;

//当邻点数目达到30后，则该帧雷达点云的几何特征配置成功

if (allPushedIndSize >= 30)

feasibleSegment = true;

else if (allPushedIndSize >= segmentValidPointNum)

{

int lineCount = 0;

for (size_t i = 0; i < N_SCAN; ++i)

if (lineCountFlag[i] == true)

++lineCount;

if (lineCount >= segmentValidLineNum)

feasibleSegment = true;

}

if (feasibleSegment == true)

{

++labelCount;

}

else

{

for (size_t i = 0; i < allPushedIndSize; ++i)

{

labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999;

}

}

}

6.cloudSegmentation(){}

//可以看到这是对点云分为地面点与可被匹配的四周被扫描的点，

//将其筛选后分别纳入被分割点云

void cloudSegmentation()

{

//这是在排除地面点与异常点之后，逐一检测邻点特征并生成局部特征

for (size_t i = 0; i < N_SCAN; ++i)

for (size_t j = 0; j < Horizon_SCAN; ++j)

if (labelMat.at(i,j) == 0)

labelComponents(i, j);

int sizeOfSegCloud = 0;

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(i,j) > 0 || groundMat.at(i,j) == 1)

{

//离群点或异常点的处理

if (labelMat.at(i,j) == 999999)

{

if (i > groundScanInd && j % 5 == 0)

{

outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);

continue;

}

else

{

continue;

}

}

if (groundMat.at(i,j) == 1)

{

//地面点云每隔5个点纳入被分割点云

if (j%5!=0 && j>5 && j(i,j) == 1);

//当前水平方向上的行数

segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;

//深度

segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at(i,j);

//把当前点纳入分割点云中

segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);

++sizeOfSegCloud;

}

}

segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;

}

//如果在当前有节点订阅便将分割点云的几何信息也发布出去

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(i,j) > 0 && labelMat.at(i,j) != 999999)

{

segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);

segmentedCloudPure->points.back().intensity = labelMat.at(i,j);

}

}

}

}

}

7.publishCloud(){}

//在我们计算的过程中参考系均为机器人自身参考系，frame_id自然是base_link。

void publishCloud(){

segMsg.header = cloudHeader;

pubSegmentedCloudInfo.publish(segMsg);

sensor_msgs::PointCloud2 laserCloudTemp;

pcl::toROSMsg(*outlierCloud, laserCloudTemp);

laserCloudTemp.header.stamp = cloudHeader.stamp;

laserCloudTemp.header.frame_id = "base_link";

pubOutlierCloud.publish(laserCloudTemp);

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

}

}

};