文章目录
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);
}
}
};