前言

LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。

LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。

其中建图优化节点整体如下图

在这篇博客中 https://www.guyuehome.com/39723 分析了 点云匹配前戏之初值计算及局部地图构建

经过上篇博客，

• 激光雷达当前帧有了初值的估计，那么则可以利用初值投到地图坐标系下
• 构建了局部地图

有了上面两步，就可以构建角点和面点的：

• 残差
• 梯度方向

其中残差就是点到直线/面 的距离，方向就是距离缩小的方向。

本篇主要分析：如何构建角点面点的残差及梯度

角点面点的残差及梯度代码解析

点云配准的代码在 mapOptmization.cpp 中

在点云的回调函数中，调用了 点云配准 的函数 ：scan2MapOptimization();

函数功能：得到一个旋转和平移，使得当前帧最好的贴合到局部地图中去

    void scan2MapOptimization()
    {

        if (cloudKeyPoses3D->points.empty())
            return;

如果没有关键帧，那也没办法做当前帧到局部地图的匹配，则直接return

        if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
        {
         ... （配准过程）
        }

判断当前帧的角点数和面点数是否足够
默认 角点要求10 面点要求100

else {
            ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
        }

角点或者面点的数量不够，则终端提示信息

下面来看配准过程

            kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
            kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

分别把角点和面点 局部地图构建 kdtree

            for (int iterCount = 0; iterCount < 30; iterCount++)
            {

迭代求解，迭代30次
里面是手写的优化器
lio-sam是继承了 loam和lego-loam中的 高斯牛顿的 优化方法

角点残差及梯度构建

cornerOptimization();

角点优化
来看里面具体内容

    void cornerOptimization()
    {
        updatePointAssociateToMap();

将当前帧的先验位姿（初值估计那来的） 将欧拉角转成eigen的形式

        for (int i = 0; i < laserCloudCornerLastDSNum; i++)
        {

遍历当前帧的角点

pointOri = laserCloudCornerLastDS->points[i];

取出该点

pointAssociateToMap(&pointOri, &pointSel);

将该点从当前帧通过初始的位姿变换到地图坐标系下去

kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

在角点地图里面寻找距离当前点比较近的5个点

if (pointSearchSqDis[4] < 1.0) {

计算找到的点中距离当前点最远的点，如果距离太大那说明这个约束不可信，就跳过

                for (int j = 0; j < 5; j++) {
                    cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
                    cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
                    cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
                }
                cx /= 5; cy /= 5;  cz /= 5;

计算找到的5个点的均值

                float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
                for (int j = 0; j < 5; j++) {
                    float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
                    float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
                    float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
                    a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
                    a22 += ay * ay; a23 += ay * az;
                    a33 += az * az;
                }
                a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;
                matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
                matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
                matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;

计算协方差矩阵

cv::eigen(matA1, matD1, matV1);

特征值分解 为 证明这5个点是一条直线

 if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {

这是线特征 ， 要求 最大 特征值 大于3倍的次大特征值
判断不是线特征，则直接不处理这个特征点了

                    float x0 = pointSel.x;
                    float y0 = pointSel.y;
                    float z0 = pointSel.z;

本次处理的特征点

                    float x1 = cx + 0.1 * matV1.at<float>(0, 0);
                    float y1 = cy + 0.1 * matV1.at<float>(0, 1);
                    float z1 = cz + 0.1 * matV1.at<float>(0, 2);
                    float x2 = cx - 0.1 * matV1.at<float>(0, 0);
                    float y2 = cy - 0.1 * matV1.at<float>(0, 1);
                    float z2 = cz - 0.1 * matV1.at<float>(0, 2);

进行了一个直线的构建
通过点的均值往两边拓展
最大特征值对应的特征向量对应的就是直线的方向向量。所以用的matV1的第0行

现在有了 一个点，和构建的两个点，下面要求整个点到构建的两个点形成直线的距离和方向，即残差与残差方向

                    float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                    + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                                    + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));

这部分求得就是

叉乘的结果就是OC，垂直所在平面向上

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

这个就是AB的模长

                    float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                              + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;

                    float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                               - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

                    float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                               + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

这部分求得是

也就是垂线的方向

                    float ld2 = a012 / l12;
                    float s = 1 - 0.9 * fabs(ld2);

求距离 也就是残差，根据三角形面积
一个简单的核函数 ， 残差越大 权重 越 低

                    coeff.x = s * la;
                    coeff.y = s * lb;
                    coeff.z = s * lc;
                    coeff.intensity = s * ld2;

残差 x y z 代表方向 intensity 为大小

                    if (s > 0.1) {
                        laserCloudOriCornerVec[i] = pointOri;
                        coeffSelCornerVec[i] = coeff;
                        laserCloudOriCornerFlag[i] = true;
                    }

如果残差小于10cm，就认为是一个有效的约束

面点残差及梯度构建

下面来看面点的优化部分

    void surfOptimization()
    {

updatePointAssociateToMap();

将当前帧的先验位姿（初值估计那来的） 将欧拉角转成eigen的形式

        for (int i = 0; i < laserCloudSurfLastDSNum; i++)
        {

遍历当前帧每个面点

pointOri = laserCloudSurfLastDS->points[i];

取出 该点

pointAssociateToMap(&pointOri, &pointSel); 

将该点从当前帧通过初始的位姿变换到地图坐标系下去

kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

在面点地图里面寻找距离当前点比较近的5个点

            Eigen::Matrix<float, 5, 3> matA0;// 五个点 3个未知量 所以是 5*3
            Eigen::Matrix<float, 5, 1> matB0;
            Eigen::Vector3f matX0;

下面不是通过特征值分解 来求特征向量的 通过超定方程来求解

            matA0.setZero();
            matB0.fill(-1);//填充为-1
            matX0.setZero();

平方方程 Ax + By + Cz + 1 = 0

            if (pointSearchSqDis[4] < 1.0) {

最大距离不能超过1m

                for (int j = 0; j < 5; j++) {
                    matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                    matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                    matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
                }

填充 matA0 矩阵

                matX0 = matA0.colPivHouseholderQr().solve(matB0);

求解 Ax = b 这个超定方程

                float pa = matX0(0, 0);
                float pb = matX0(1, 0);
                float pc = matX0(2, 0);
                float pd = 1;

求出来x的就是这个平面的法向量

                for (int j = 0; j < 5; j++) {
                    if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
                             pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                             pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
                        planeValid = false;
                        break;
                    }
                }

将每个点代入平面方程，计算点到平面的距离，如果距离大于0.2m，认为这个平面曲率偏大，就是无效的平面

                if (planeValid) {
                    float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

计算残差，点到平面的距离

                    float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
                            + pointSel.y * pointSel.y + pointSel.z * pointSel.z));

残差到权重的换算 分母部分 点离激光越远 则 分母越大，那么权重就越大，

                   coeff.x = s * pa;
                    coeff.y = s * pb;
                    coeff.z = s * pc;
                    coeff.intensity = s * pd2;

残差 x y z 代表梯度方向 intensity 为大小

                    if (s > 0.1) {
                        laserCloudOriSurfVec[i] = pointOri;
                        coeffSelSurfVec[i] = coeff;
                        laserCloudOriSurfFlag[i] = true;
                    }

如果权重大于阈值，就认为是一个有效的约束

总结