三维SLAM算法LeGO-LOAM源码阅读（一）

LeGO-LOAM是LOAM的增强版，在LOAM的基础上增加了回环检测。论文地址：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/Shan_Englot_IROS_2018_Preprint.pdf

它分别由点云分割、特征提取、激光里程计、激光建图组成，算法思想与RGBD-SLAM有些类似，系统通过接收来自三维激光雷达的输入并输出6自由度姿态估计。整个系统分为五个模块，第一个是分割，将单个扫描的点云投影到一个固定范围的图像上进行分割，然后第二个是将分割的点云发送到特征提取模块。第三个，激光雷达里程计使用从上一个模块中提取的特征找到在连续扫描过程中机器人位姿的转换，这些信息最终用于激光雷达用点云方式的建图中。第五个模块是融合激光雷达里程测量和建图的姿态估计结果，并输出最终的姿态估计。

从论文中可以得到，从地面特征中确认机器人当前位姿z，roll，pitch，而在每一帧点云数据匹配的过程中得到机器人位姿的x，y，yaw。

接下来看看代码，从CMakeLists中我们可以看到，它依赖于PCL、OpenCV、GTSAM三个库，以前没用过GTSAM库，它的定义是：

GTSAM是一个在机器人领域和计算机视觉领域用于平滑（smoothing）和建图（mapping）的C++库。它与g2o不同的是，g2o采用稀疏矩阵的方式求解一个非线性优化问题，而GTSAM是采用因子图（factor graphs）和贝叶斯网络（Bayes networks）的方式最大化后验概率。

在这个工程中，具备了imageProjection（图像投影）、featureAssociation（特征关联）、mapOptmization（地图优化）、transformFusion（位姿优化）四个可执行文件，这比LOAM的四个main函数程序加上lib中的一堆文件要人性化一点。

唯一的头文件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;

而 PointTypePose指的是具备姿态角的特定点：

struct PointXYZIRPYT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    float roll;
    float pitch;
    float yaw;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

typedef PointXYZIRPYT  PointTypePose;

根据算法思想，我们首先看到imageProjection.cpp，这个程序是对三维点云进行投影。下列是在投影过程中主要用到的点云：

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


pcl::PointCloud<PointType>::Ptr fullCloud;//投影后的点云
pcl::PointCloud<PointType>::Ptr fullInfoCloud;//整体的点云

pcl::PointCloud<PointType>::Ptr groundCloud;//地面点云
pcl::PointCloud<PointType>::Ptr segmentedCloud;//分割后的部分
pcl::PointCloud<PointType>::Ptr segmentedCloudPure;//分割后的部分的几何信息
pcl::PointCloud<PointType>::Ptr outlierCloud;//在分割时出现的异常

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

cloud_msgs::cloud_info segMsg;

它的具体信息如下：

Header header 

int32[] startRingIndex
int32[] endRingIndex

float32 startOrientation
float32 endOrientation
float32 orientationDiff

bool[]    segmentedCloudGroundFlag
uint32[]  segmentedCloudColInd 
float32[] segmentedCloudRange

接下来的流程完全依赖于回调函数ImageProjection::cloudHandler：

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    copyPointCloud(laserCloudMsg);
    findStartEndAngle();
    projectPointCloud();
    groundRemoval();
    cloudSegmentation();
    publishCloud();
    resetParameters();
}

在这个函数里依次调用了点云的复制、寻找始末角度、点云投影、地面检测、点云分割与发布。其中点云的复制是将rosmsg转化为pcl点云。

1.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;
}

2.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<float>(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;
        }
    }

3.groundRemoval是利用不同的扫描圈来表示地面，进而检测地面是否水平。例如在源码中的七个扫描圈，每两个圈之间进行一次比较，角度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加入到groundCloud点云。

4.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<int>(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<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;

                    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<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
            }
        }
    }

再回到cloudSegmentation当中，可以看到这是对点云分为地面点与可被匹配的四周被扫描的点，将其筛选后分别纳入被分割点云。

    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;
        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;
                        }
                    }
                    if (groundMat.at<int8_t>(i,j) == 1)
                    {
                        //地面点云每隔5个点纳入被分割点云
                        if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
                            continue;
                    }
                    //segMsg是自定义rosmsg
                    //是否是地面点
                    segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
                    //当前水平方向上的行数
                    segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
                    //深度
                    segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(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<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);
                    }
                }
            }
        }
    }

5.在publishCloud函数中我们可以看到，在我们计算的过程中参考系均为机器人自身参考系，frame_id自然是base_link。

imageProjection程序中，输入是雷达数据传入的点云信息，输出的是被分割后的点云，用来匹配特征。接下来需要调用到featureAssociation.cpp中的函数，这个程序用来特征聚类从而达到里程计的作用，避免博客太长，下一篇再写。