LIO-SAM后端中的点云配准

Rhys___

已于 2024-05-08 15:34:23 修改

阅读量1.3k

点赞数

分类专栏：激光SLAM-LOAM系列文章标签：算法 c++ 自动驾驶

于 2022-09-15 16:18:13 首次发布

本文链接：https://blog.youkuaiyun.com/zysss_/article/details/126869212

版权

本文深入解析LIO-SAM中点云配准的scan2MapOptimization()函数，涵盖角点和面点约束问题的构建。通过角点约束问题的cornerOptimization()，利用特征值分解构建直线约束；面点约束问题的surfOptimization()，通过平面方程确定有效平面。最后，统一角点面点约束，便于gtsam优化。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

点云配准的函数只有一个入口 scan2MapOptimization()，该函数主要作用是对点云配准进行优化问题构建

从运行流程开始解析

如果没有关键帧，则无法做当前帧到局部地图的匹配，所以后面的步骤都在描述第2次及以后进来的情况

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

判断当前帧的角点数和面点数是否足够，足够才开始构建优化问题

        if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)

分别将局部地图的角点和面点放入局部地图的kdtree中（注意这里的角点面点不是当前帧的，是局部地图的，在上一个函数中筛选出来的），LIO-SAM中初值计算及局部地图构建中有详细解析局部地图构建，extractSurroundingKeyFrames()函数构建的局部地图是把那些点都筛选出来了，但是还没有加入kdtree中，局部地图其实就是通过kd_tree提取离当前帧一定距离内的关键帧

// 分别把角点面点局部地图构建kdtree
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

迭代求解分角点约束问题构建和面点约束问题构建

角点约束问题构建

cornerOptimization()
流程综述：

遍历当前帧的角点，并将该点从当前帧通过初始的位姿转换到地图坐标系下去
然后根据kdtree寻找距离当前点比较近的5个点，下面以图示

其中黑色的点是map，蓝色的点是关键帧的点，两个都在世界坐标系下，由于当前帧与map的位姿变换是要求的量，所以这里画的时候会显示出有一定的距离，通过kdtree中距离的关系获取点的匹配，因为是构建点线约束，找到邻近的5个点通过特征值分解获取5个点构成的特征向量，该向量也是直线向量，里面画的红线和蓝线分别是两个当前帧的点通过kdtree找到的5个点连成的直线向量
在这里插入图片描述

根据最近的5个点构建协方差矩阵 matA1，对该矩阵进行特征值分解，当最大特征值大于3倍的次大特征值的时候才满足线特征
满足上面的条件后获取最大特征值对应的特征向量，该向量为直线向量，通过点的均值往该向量两侧拓展，构成一个线的两个端点x1，x2

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);

计算点到线的残差和垂线方向（及雅可比方向）

// 下面是计算点到线的残差和垂线方向（及雅克比方向）
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)));

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

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 =