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

接下来是featureAssociation.cpp，里程计部分，它分为特征点的提取与匹配两部分。这是一个非常长的代码，首先进入主函数：

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lego_loam");

    ROS_INFO("\033[1;32m---->\033[0m Feature Association Started.");

    FeatureAssociation FA;

    ros::Rate rate(200);
    while (ros::ok())
    {
        ros::spinOnce();

        FA.runFeatureAssociation();

        rate.sleep();
    }

    ros::spin();
    return 0;
}

我们可以看到FeatureAssociation的构造函数初始化了一些ros话题的收发，接着就是一个死循环不断的调用runFeatureAssociation（以每秒200HZ的频率），先在这里怀疑一下是否需要这么高的频率，转到构造函数，发现它接收上一个节点的分割点云、异常值以及IMU信息，却发送了很多点云的话题：

//没有找到接收这四个话题的节点，可能是供可视化处理的
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 1);
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 1);
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 1);
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 1);
//角部特征、平面特征以及里程计信息供建图时使用
pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
pubOutlierCloudLast = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2);
pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);

 另外在initializationValue函数中定义了降采样滤波器，那么点云名称最后加DS的就是降采样之后的点云。

再转到runFeatureAssociation函数：

    void runFeatureAssociation()
    {
        //判断分割点云和异常值的实时性
        if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
            std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
            std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05)
	{
            newSegmentedCloud = false;
            newSegmentedCloudInfo = false;
            newOutlierCloud = false;
        }
	else
	{
            return;
        }
        //进行形状特征的调整
        adjustDistortion();
        calculateSmoothness();
        markOccludedPoints();
        //提取特征
        extractFeatures();
        publishCloud();
        //在配准之前，检验LM法是否初始化，接下来就是里程计部分
        if (!systemInitedLM)
	{
            checkSystemInitialization();
            return;
        }
        //提供粗配准的先验以供优化
        updateInitialGuess();
        //优化并发送里程计信息
        updateTransformation();
        integrateTransformation();
        publishOdometry();
        //发送点云以供建图使用
        publishCloudsLast();
    }

也就是说，这个函数依次调用了如上的这些函数完成了一次特征的匹配与里程计的计算，在建图过程中它们将被反复调用。在整个建图过程中的特征配准，虽然是采取ICP的方法，但利用点到点的一一配准太浪费时间，在这里作者采取了点与线之间的距离作为优化的目标，所以以LM初始化检查为分界，这个程序分为特征提取与配准计算两个阶段。

一. 特征提取阶段

1.adjustDistortion函数，它用于计算点云之间的相对位姿与时间差，计算出相对移动的速度，并将得到的点云转换到IMU的座标系下。

//在这里是利用插值的方法，relTime指的是角度的变化率，乘上scanPeriod（0.1秒）便得到在一个扫描周期内的角度变化量
float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;

这一块对IMU的处理非常巧妙，在每次处理点云时，需要遍历每一个点，把第一个点作为初始座标，随后的点均以第一个点为参考做插值计算。

            if (imuPointerLast >= 0)
	    {
                float pointTime = relTime * scanPeriod;
                imuPointerFront = imuPointerLastIteration;
                while (imuPointerFront != imuPointerLast)
		{
                    if (timeScanCur + pointTime < imuTime[imuPointerFront])
		    {
                        break;
                    }
                    //imuQueLength是200，imuPointerFront在0~199之间轮转
                    imuPointerFront = (imuPointerFront + 1) % imuQueLength;
                }
                //要是已经是最新数据那就拿下标为imuPointerFront的数据作为Cur
                if (timeScanCur + pointTime > imuTime[imuPointerFront])
	        {
                    imuRollCur = imuRoll[imuPointerFront];
                    imuPitchCur = imuPitch[imuPointerFront];
                    imuYawCur = imuYaw[imuPointerFront];

                    imuVeloXCur = imuVeloX[imuPointerFront];
                    imuVeloYCur = imuVeloY[imuPointerFront];
                    imuVeloZCur = imuVeloZ[imuPointerFront];

                    imuShiftXCur = imuShiftX[imuPointerFront];
                    imuShiftYCur = imuShiftY[imuPointerFront];
                    imuShiftZCur = imuShiftZ[imuPointerFront];
                }
                //否则用插值计算当前数据
		else
		{
                    int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
                    float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
                                                     / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
                    float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
                                                    / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);

                    imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
                    imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
                    if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI)
		    {
                        imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
                    }
		    else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI)
		    {
                        imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
                    }
		    else
		    {
                        imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
                    }

                    imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
                    imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
                    imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;

                    imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
                    imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
                    imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
                }
                //如果是在点云中的第一个点，就将数据作为初始座标系
                if (i == 0)
		{
                    imuRollStart = imuRollCur;
                    imuPitchStart = imuPitchCur;
                    imuYawStart = imuYawCur;

                    imuVeloXStart = imuVeloXCur;
                    imuVeloYStart = imuVeloYCur;
                    imuVeloZStart = imuVeloZCur;

                    imuShiftXStart = imuShiftXCur;
                    imuShiftYStart = imuShiftYCur;
                    imuShiftZStart = imuShiftZCur;

                    if (timeScanCur + pointTime > imuTime[imuPointerFront])
		    {
                        imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];
                        imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];
                        imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];
                    }
		    else
		    {
                        int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
                        float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
                                                         / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
                        float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
                                                        / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
                        imuAngularRotationXCur = imuAngularRotationX[imuPointerFront] * ratioFront + imuAngularRotationX[imuPointerBack] * ratioBack;
                        imuAngularRotationYCur = imuAngularRotationY[imuPointerFront] * ratioFront + imuAngularRotationY[imuPointerBack] * ratioBack;
                        imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront] * ratioFront + imuAngularRotationZ[imuPointerBack] * ratioBack;
                    }

                    imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;
                    imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;
                    imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;

                    imuAngularRotationXLast = imuAngularRotationXCur;
                    imuAngularRotationYLast = imuAngularRotationYCur;
                    imuAngularRotationZLast = imuAngularRotationZCur;

                    updateImuRollPitchYawStartSinCos();
                }
                //如果不是第一个点
                else
		{
                    //把速度与位移转换回IMU的初始座标系下
                    VeloToStartIMU();
                    TransformToStartIMU(&point);
                }
            }

2.calculateSmoothness函数检测了点云特征点的曲率，具体方法是将点云的每一层拿出分析，计算每个点左右各五个点的偏差作为曲率，将曲率保存在cloudSmoothness中。

3.markOccludedPoints是去除一些瑕点，这些瑕点指的是在点云中可能出现的互相遮挡的情况，如果两个特征点距离过近（小于某个阈值），则取更近的那个点做下一步处理。

void markOccludedPoints()
{
    int cloudSize = segmentedCloud->points.size();
    //遍历每一个点
    for (int i = 5; i < cloudSize - 6; ++i)
    {
        //depth1和depth2分别是两点的深度
        float depth1 = segInfo.segmentedCloudRange[i];
        float depth2 = segInfo.segmentedCloudRange[i+1];
        //将可能存在遮挡的点去除，将远侧的点视为瑕点，这里的处理方法和LOAM不太一样，是直接采取比较点的下标，去掉其中一侧的6个点
        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i+1] - segInfo.segmentedCloudColInd[i]));

        if (columnDiff < 10)
        {
            if (depth1 - depth2 > 0.3)
            {
                 cloudNeighborPicked[i - 5] = 1;
                 cloudNeighborPicked[i - 4] = 1;
                 cloudNeighborPicked[i - 3] = 1;
                 cloudNeighborPicked[i - 2] = 1;
                 cloudNeighborPicked[i - 1] = 1;
                 cloudNeighborPicked[i] = 1;
            }
	    else if (depth2 - depth1 > 0.3)
	    {
                 cloudNeighborPicked[i + 1] = 1;
                 cloudNeighborPicked[i + 2] = 1;
                 cloudNeighborPicked[i + 3] = 1;
                 cloudNeighborPicked[i + 4] = 1;
                 cloudNeighborPicked[i + 5] = 1;
                 cloudNeighborPicked[i + 6] = 1;
            }
        }
        //diff1和diff2是当前点距离前后两个点的距离
        float diff1 = std::abs(segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i]);
        float diff2 = std::abs(segInfo.segmentedCloudRange[i+1] - segInfo.segmentedCloudRange[i]);
        //如果当前点距离左右邻点都过远，则视其为瑕点，因为入射角可能太小导致误差较大
        if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])
        cloudNeighborPicked[i] = 1;
    }
}

4.extractFeatures函数，不加注释导致看了很久。。。。首先它的做法是将每一层点云分成6份，每一份中，对点的曲率进行排序，sp和ep分别是这段点云的起始点与终止点。。。从而判断出角点与平面点，根据label值，分类为sharp，lesssharp，flat，lessflat。分类后分别提供给ros话题。下面是对角点特征的筛选：

for (int k = ep; k >= sp; k--)
{
    int ind = cloudSmoothness[k].ind;
    //曲率大于edgeThreshold（0.1）则视为角点，而且不能是地面点
    if (cloudNeighborPicked[ind] == 0 &&
    cloudCurvature[ind] > edgeThreshold &&
    segInfo.segmentedCloudGroundFlag[ind] == false)
    {
        largestPickedNum++;
        if (largestPickedNum <= 2)
	{
            cloudLabel[ind] = 2;
            cornerPointsSharp->push_back(segmentedCloud->points[ind]);
            cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
        }
											else if (largestPickedNum <= 20)
	{
            cloudLabel[ind] = 1;
            cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
        }
	else
	{
            break;
        }
        //防止重复处理
        cloudNeighborPicked[ind] = 1;
        for (int l = 1; l <= 5; l++)
	{
            int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
            if (columnDiff > 10)
                break;
            cloudNeighborPicked[ind + l] = 1;
        }
        for (int l = -1; l >= -5; l--)
	{
            int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
            if (columnDiff > 10)
                break;
            cloudNeighborPicked[ind + l] = 1;
        }
    }
}

二. 配准计算阶段

1.updateInitialGuess函数将当前时刻保存的IMU数据作为先验数据。

2.updateTransformation分别对平面特征与角点特征进行了匹配计算，首先找到各自对应的特征，下面是角部特征的函数：

    void findCorrespondingCornerFeatures(int iterCount)
    {
        int cornerPointsSharpNum = cornerPointsSharp->points.size();
        //对角部特征点依次进行处理
        for (int i = 0; i < cornerPointsSharpNum; i++)
	{
            //根据IMU数据的将点云转换到上一次扫描的位置中
            TransformToStart(&cornerPointsSharp->points[i], &pointSel);
            //每五次迭代寻找一次邻域点，否则使用上次的邻域查找
            if (iterCount % 5 == 0)
	    {
                kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
                int closestPointInd = -1, minPointInd2 = -1;
                //pointSearchSqDis是平方距离，阈值是25
                if (pointSearchSqDis[0] < nearestFeatureSearchSqDist)
		{
                    closestPointInd = pointSearchInd[0];
                    //得到最近邻所在的层数
                    int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);

                    float pointSqDis, minPointSqDis2 = nearestFeatureSearchSqDist;
                    for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++)
		    {
                        //最近邻需要在上下两层之内否则失败
                        if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5)
			{
                            break;
                        }

                        pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                     (laserCloudCornerLast->points[j].x - pointSel.x) +
                                     (laserCloudCornerLast->points[j].y - pointSel.y) *
                                     (laserCloudCornerLast->points[j].y - pointSel.y) +
                                     (laserCloudCornerLast->points[j].z - pointSel.z) *
                                     (laserCloudCornerLast->points[j].z - pointSel.z);

                        if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan)
			{
                            if (pointSqDis < minPointSqDis2)
			    {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                        }
                    }
                    for (int j = closestPointInd - 1; j >= 0; j--)
		    {
                        if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5)
			{
                            break;
                        }

                        pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                     (laserCloudCornerLast->points[j].x - pointSel.x) +
                                     (laserCloudCornerLast->points[j].y - pointSel.y) *
                                     (laserCloudCornerLast->points[j].y - pointSel.y) +
                                     (laserCloudCornerLast->points[j].z - pointSel.z) *
                                     (laserCloudCornerLast->points[j].z - pointSel.z);

                        if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan)
			{
                            if (pointSqDis < minPointSqDis2)
			    {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                        }
                    }
                }

                pointSearchCornerInd1[i] = closestPointInd;
                pointSearchCornerInd2[i] = minPointInd2;
            }
            //这里是开始计算点到直线的距离，tripod即三角形，根据三角形余弦定理计算距离并求偏导数
            if (pointSearchCornerInd2[i] >= 0)
	    {
                tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
                tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];

                float x0 = pointSel.x;
                float y0 = pointSel.y;
                float z0 = pointSel.z;
                float x1 = tripod1.x;
                float y1 = tripod1.y;
                float z1 = tripod1.z;
                float x2 = tripod2.x;
                float y2 = tripod2.y;
                float z2 = tripod2.z;

                float m11 = ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1));
                float m22 = ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1));
                float m33 = ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1));

                float a012 = sqrt(m11 * m11  + m22 * m22 + m33 * m33);

                float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

                float la =  ((y1 - y2)*m11 + (z1 - z2)*m22) / a012 / l12;

                float lb = -((x1 - x2)*m11 - (z1 - z2)*m33) / a012 / l12;

                float lc = -((x1 - x2)*m22 + (y1 - y2)*m33) / a012 / l12;

                float ld2 = a012 / l12;

                float s = 1;
                if (iterCount >= 5)
		{
                    s = 1 - 1.8 * fabs(ld2);
                }

                if (s > 0.1 && ld2 != 0)
		{
                    coeff.x = s * la;
                    coeff.y = s * lb;
                    coeff.z = s * lc;
                    coeff.intensity = s * ld2;

                    laserCloudOri->push_back(cornerPointsSharp->points[i]);
                    coeffSel->push_back(coeff);
                }
            }
        }
    }

平面特征与角部特征的计算类似，都使用了LM法进行迭代，具体计算在calculateTransformationSurf与calculateTransformationCorner当中，按下不表。

3.integrateTransformation函数是将IMU信息融入到位姿更新当中。

    void integrateTransformation()
    {
        float rx, ry, rz, tx, ty, tz;
        //transformSum是IMU的累计变化量，0、1、2分别是pitch、yaw、roll，transformCur是当前的IMU数据，AccumulateRotation是为了使局部座标转换为全局座标
        AccumulateRotation(transformSum[0], transformSum[1], transformSum[2],
                           -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);

        float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX)
                 - sin(rz) * (transformCur[4] - imuShiftFromStartY);
        float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX)
                 + cos(rz) * (transformCur[4] - imuShiftFromStartY);
        float z1 = transformCur[5] - imuShiftFromStartZ;

        float x2 = x1;
        float y2 = cos(rx) * y1 - sin(rx) * z1;
        float z2 = sin(rx) * y1 + cos(rx) * z1;

        tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
        ty = transformSum[4] - y2;
        tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
        //加入IMU当前数据更新位姿
        PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart,
                          imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);

        transformSum[0] = rx;
        transformSum[1] = ry;
        transformSum[2] = rz;
        transformSum[3] = tx;
        transformSum[4] = ty;
        transformSum[5] = tz;
    }

4.publishOdometry即是将上一条中计算出的位姿变为里程计信息与tf变换发送出去。

这就是第二个程序的主要流程，我们需要的就是里程计的信息。代码里面有很多坑，也没有注释，今后慢慢学习。。。