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

1.引言

论文解读:无人驾驶算法学习（九）：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;//检查上下左右连续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 // 该点类型有4个元素
    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)
       {
           thisPoint.x = laserCloudIn->points[i].x;
           thisPoint.y = laserCloudIn->points[i].y;
           thisPoint.z = laserCloudIn->points[i].z;
           //计算竖直方向上的点的角度以及在整个雷达点云中的哪一条水平线上
           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);
           //在rangeMat矩阵中保存该点的深度，保存单通道像素值
           rangeMat.at(rowIdn, columnIdn) = range;

           thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;

           index = columnIdn  + rowIdn * Horizon_SCAN;
           fullCloud->points[index] = thisPoint;

           fullInfoCloud->points[index].intensity = range;
       }
   }

• 4.groundRemoval(){}

   //利用不同的扫描圈来表示地面，进而检测地面是否水平。例如在源码中的七个扫描圈，每两个圈之间进行一次比较，角度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加入到groundCloud点云
      void groundRemoval(){
       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);
        }
    }

};