• LOAM框架后端优化总结


    LOAM,即Lidar Odometry and Mapping in Real Time

    在这里插入图片描述

    一、Lidar Mapping 原理

    在 Lidar Mapping 部分,通过与多次扫描构成的地图进行匹配,消除Lidar Odometry 中利用两帧之间计算位姿产生的漂移,从而对轨迹进行优化;
    在这里插入图片描述
    输入:

    1. T K + 1 L T^L_{K+1} TK+1L表示lidar odometry 模块输出的 k + 1 k+1 k+1时刻相 对于起始帧的位姿变换;
    2. 𝑇 𝐾 𝑊 𝑇_𝐾^𝑊 TKW表示截止到k时刻已经别lidar mapping优化后的位姿
    3. P ˉ 𝐾 + 1 \bar{P}_{𝐾+1} PˉK+1表示 k + 1 k+1 k+1时刻去畸变(对齐到末尾时刻)的单帧点云;
    4. 𝑄 k 𝑄_k Qk表示截止到 k k k时刻已经构建好的点云地图;

    流程:

    1. 通过 T K + 1 L T^L_{K+1} TK+1L 𝑇 𝐾 𝑊 𝑇_𝐾^𝑊 TKW可以求取出优化前的 T K + 1 W T^W_{K+1} TK+1W
    2. 通过 T K + 1 W T^W_{K+1} TK+1W P ˉ 𝐾 + 1 \bar{P}_{𝐾+1} PˉK+1可以求出世界坐标系下的点云 𝑄 ˉ K + 1 \bar{𝑄}_{K+1} QˉK+1
    3. 通过 𝑄 ˉ K + 1 \bar{𝑄}_{K+1} QˉK+1 𝑄 k 𝑄_k Qk 的配准,可以得到优化后的 T K + 1 W T^W_{K+1} TK+1W,进而得到优化后的 𝑄 K + 1 {𝑄}_{K+1} QK+1

    关键:

    1. 按照同样的方式从 𝑄 ˉ K + 1 \bar{𝑄}_{K+1} QˉK+1中提取特征点,但是比激光里程计中多十倍;
    2. 为了寻找特征点的匹配,把地图 𝑄 k 𝑄_k Qk 存储在10m 的立方体区域中,同时将其中与 𝑄 ˉ K + 1 \bar{𝑄}_{K+1} QˉK+1有交集的区域建立成KD-tree;
    3. 对于 𝑄 ˉ K + 1 \bar{𝑄}_{K+1} QˉK+1中的每个特征点在交集(KD-tree)搜索邻域, 得到邻域点并保留其中的对应线和对应面;
    4. 通过约束优化角点到线平面点到面的距离,来优化位姿 T K + 1 W T^W_{K+1} TK+1W

    二、 通过 T K + 1 L T^L_{K+1} TK+1L 𝑇 𝐾 𝑊 𝑇_𝐾^𝑊 TKW可以求取出优化前的 T K + 1 W T^W_{K+1} TK+1W以及 𝑄 ˉ K + 1 \bar{𝑄}_{K+1} QˉK+1

    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    即可计算出绕yxz的旋转角,并基于优化前的位姿 T K + 1 W T^W_{K+1} TK+1W P ˉ 𝐾 + 1 \bar{P}_{𝐾+1} PˉK+1,求取待配准点云 𝑄 ˉ K + 1 \bar{𝑄}_{K+1} QˉK+1

    三、 Scan-To-SubMap匹配

    如果使用全部的地图数据,在计算效率上会大打折扣,所以,建图环节将整个空间三维区域划分为多个子cube(边长为10m的立方体),并接着计算当前帧位姿所在的cube索引,然后在当前帧位姿附近 5 ∗ 5 ∗ 5 5*5*5 555个cube中寻找最有效的cube作为 𝑄 k 𝑄_k Qk,用以代替全局地图,优化得到最终的姿态变换矩阵 T K + 1 W T^W_{K+1} TK+1W

    首先将特征地图存入kd树,设置迭代循环。对于边特征点,先转到全局坐标系,再在KD树中搜索5个邻近点。接着求5个邻域点的均值点,并构建协方差矩阵,进行特征值分解,通过最大特征值对应的特征向量构建直线。

    对于面特征点,上一章帧间匹配通过kd树查找3个最邻近点构成平面,然后通过论文中公式计算点到面的距离。本章地图匹配则仍然是先搜索5个最近点,由于这5个点在一个平面上,代码中直接通过矩阵运算求解5个点构成平面的法向量。

    得到当前帧线特征点和面特征点与地图配准后,构建loss函数求解。

    如果邻域点分布在一条线段上,那么matD1中的一个特征值就会明显大于另外两个,matV1中与较大特征值对应的特征向量就代表线的方向。
    如果邻域点分布在一块平面上,那么matD1中的一个特征值就会明显小于另外两个,matV1中与较小特征值对应的特征向量就代表平面的方向。

  • 相关阅读:
    排序2:希尔排序(缩小增量排序)
    独孤九剑第四式-K近邻模型(KNN)
    Jetsonnano B01 笔记7:Mediapipe与人脸手势识别
    项目终于收尾了,第一次体验到专业项目管理软件的魅力
    【ARM CoreLink 系列 6 -- DMC-400控制器简介】
    mmlab 做实验
    2-3.spring源码--BeanFactoryPostProcessor
    杰理之关于提升 BLE 传输速率说明【篇】
    STM32CubeMX驱动INA226芯片
    递归的总结和案例
  • 原文地址:https://blog.csdn.net/weixin_44156680/article/details/125921620