0. 简介
最近在研究整个SLAM框架的改进处，想着能不能从Cartographer中找到一些亮点可以用于参考。所以这一篇博客希望能够梳理好Cartographer前后端优化，并从中得到一些启发。carto整体是graph-based框架，前端是scan-map匹配，后端是SPA优化。前端又分为CSM+Ceres两个部分，完成匹配后则会进入子图生成维护中。在子图维护以及优化后放入后端优化，完成全局地图优化和回环检测。下图来自cartographer 代码分析
前端优化
前端负责数据的初步处理、局部地图的构建以及与即时定位相关的工作。主要流程包括：
- 传感器数据输入：前端接受来自多种传感器的数据输入，包括激光雷达（LiDAR）、惯性测量单元（IMU）、里程计等。
- 数据预处理：
- 时间同步：不同传感器的数据时间戳可能不一致，需要进行时间同步。
- 数据滤波：对 LiDAR 数据等进行滤波，移除异常或噪声数据。
- 转换坐标系：将数据转换到统一的坐标系中。
- 局部地图构建：
- 扫描匹配：将当前帧的 LiDAR 数据与上一帧或者局部地图进行匹配，估计机器人的位置和姿态。
- 插值和融合：利用 IMU 数据进行插值，融合不同传感器的信息，减少漂移。
- 生成子图：
- 前端会在特定时间或距离阈值内生成一个子图（Submap）。这个子图包含一段时间内的传感器数据，用来表示局部地图。
- 发送到后端：局部优化完成后，将子图数据传输给后端进行全局优化。
子图流程
子图在前端和后端之间起到重要的连接作用。它的主要流程如下：
- 子图生成：
- 前端会在一定时间间隔或移动距离达到阈值后，生成一个新的子图。
- 子图代表在这个时间段或距离内的局部地图数据，通常包含来自 LiDAR、IMU、相机等传感器的数据。
- 每个子图在生成时会在局部进行优化，包括与邻近帧的扫描匹配，生成初步的位姿估计。
- 子图固定：
- 一旦子图生成并初步优化完成，它就被固定，以便后续优化中使用。
- 固定的子图包含其位姿估计和点云数据。
- 子图插入（下面都是是后端对子图的操作）：
- 前端将生成的子图传递给后端，后端将它插入全局位姿图中。
- 后端根据前端的初步位姿估计，将新子图与全局图进行连接，确保其在全局地图中的初始位置正确。
- 子图连接：
- 子图会在后端进行相邻子图的连接，以确保当前子图与邻近子图保持一致。
- 这包括回环检测，如果检测到闭环，则会标记两个相关子图之间的连接关系，为后续的全局优化做准备。
- 全局优化：
- 子图的位姿经过图优化算法进行全局优化（如 Ceres Solver），从而调整它们在全局地图中的相对位置和方向。
- 优化完成后，每个子图的位姿在全局地图中得到修正。
- 生成全局地图：
- 优化后的子图拼接形成完整的全局地图。
- 子图的位姿修正确保了全局地图的精度和一致性。
后端优化
后端负责全局的地图优化、回环检测和地图更新。主要流程包括：
- 子图数据接收：
- 后端接收来自前端的子图数据，包括子图的初始位姿、点云和其他传感器信息。
- 每个子图在前端进行过初步的局部优化，后端会根据它们之间的相对位姿进行进一步优化。
- 构建位姿图：
- 后端为每个子图建立位姿节点，将其作为位姿图（Pose Graph）中的一个节点。
- 位姿图中的边表示相邻子图或检测到回环的子图之间的相对位姿关系。
- 检测回环：
- 回环检测是后端优化的重要步骤，可以提高全局地图的一致性。
- 利用特征匹配或扫描匹配的方法，后端搜索当前子图与先前已处理过的子图之间的相似性，以检测是否存在回环。
- 如果检测到回环，将在位姿图中添加边，表示当前子图与历史子图之间的关系。
- 添加其他约束：
- 根据惯性测量单元（IMU）、GPS、里程计等其他传感器数据，后端可以为位姿图中的节点和边添加更多的约束。
- 这些约束有助于在优化时保持全局一致性和准确性。
- 全局图优化：
- 在构建了完整的位姿图并添加各种约束后，利用图优化算法（如Ceres Solver）对整个位姿图进行全局优化。
- 通过最小化每个节点和边的残差误差，优化算法计算出全局一致的子图位姿。
- 子图位置调整：
- 全局优化后，每个子图的位置和方向将得到调整，使它们在全局地图中准确对齐。
- 这有助于在拼接各个子图时确保地图的整体一致性。
- 地图更新：
- 子图的优化位姿用于重新生成最终的全局地图。
- 所有子图拼接后，生成的高分辨率地图将更准确地反映整个环境。
- 反馈给前端：
- 优化后的位姿图可以反馈给前端，帮助前端进行更准确的局部定位。
- 同时，地图的优化也将进一步指导后端未来的优化方向。
1. 前端优化
Carto先用了CSM做粗匹配，然后用Ceres做精匹配，并使用了分支定界的方法来加速。
1.1 CSM优化
由于Ceres scan matcher这是一个实时的局部优化，需要一个好的初始位姿估计。所以需要real time CSM把位姿估计器传来的预测值更新为一个好的初值，如果没有real time CSM，就还用位姿估计器传来的预测值作初值。Ceres Scan Matcher以初值作为先验，并找到最佳的点，该点就是通过scan match获得的在子地图中的位置，实现方式是 interpolating the submap and sub-pixel aligning the scan. 前端的两个scan matcher其实都是 scan to map 问题，让当前观测和已知环境最匹配。
CSM说白了就是穷举。我先对scan在map中的pose做个大致估计（例如通过里程计来预测），然后在这个pose周围穷举所有的pose，找到匹配最好的那个。
详细可以参考：前端 3. Real Time Correlative Scan Matcher。相关代码可以参考CSM快速匹配与多分辨率匹配代码实现。
1.2 Ceres
在通过real time CSM或者位姿估计器传来的预测值作为初值后，然后需要通过 ceres scan matcher 优化才能插入到子图中。前端的两个scan matcher其实都是 scan to map 问题，让当前观测和已知环境最匹配。ceres匹配对初值要求相当高，匹配后的结果会考虑其与初始位置偏差进行权重化，说明 cartographer认为其匹配后的值应该与初值相差不大。 Ceres扫描匹配器优化包含了三个残差项：点云与grid的匹配残差、位置(平移)残差、角度(旋转)残差。位置、角度两残差顶多就是对匹配位姿的约束，防止点云匹配的结果和初值差太多，真正的扫描匹配的主角是点云匹配残差。
详细可以参考：前端 4. Ceres scan matcher，代码可以参考cartographer代码学习笔记-CeresScanMatcher2D
2. 子图构建
对于子图的相关处理都集中在成员函数InsertRangeData()函数，其还调用了另一个重要函数AddSubmap()，为了方便理解，根据下图进行讲解一下：
其上的每个方形代表一个子图 Submap2D，根据上面的分析知道submaps_最多同时存在两个子图，这里假设现在submaps_中存储的子图为子图1与子图2。那么子图1中插入的数据定然比子图2多options_.num_range_data()个点云，因为只有子图一达到options_.num_range_data()个点云时，子图2才被创建，同时会删除submaps_最前面的子图（这里假设为子图0，未在上图画出）。后续到来的点云会同时插入到子图1与子图2，也就是说，这两个子图的点云是存在交集的。
当子图2点云达到了options_.num_range_data()个，也就是此时子图1的数据为2倍的options_.num_range_data()，会把子图1标记为完成状态，同时从 submaps_ 中删除该子图，这样子图2代替了之前子图1的位置，同时会再创建子图3添加到 submaps_ 之中。也就是说，此时submaps_中包含了子图2与子图3，然后再继续往子图2,3插入数据，所以子图2与子图3也存在交集，依次循环下去。相关的代码注释参考：cartographer 生成子图
子图的状态主要是给后台的线程提供的。一开始子图的状态都是kActive的，当它完成之后(kFinished的状态)就会与所有旧的节点 计算约束此外新增的节点也会与所有完成的子图计算约束。这里的主要计算是在后端优化中实现的
3. 后端优化
首先后端优化是从一个整理来考虑的，其基于 global 系。利用到到子图与子图之间的变换关系，可是在前端(基于local系)过程中，子图位姿是单个相对于 local系 的位姿，可以说多个子图之间他们是没有直接联系到一起的。除了子图，节点也一样，前端的节点都是根据活跃子图计算出相对于 local系 的位姿，但是却没有和前面已经完成的子图联系到一起，也显得比较孤立。
但是在后端优化中，这些都被考虑了进去，其估算出来的节点位姿，不仅希望其在活跃的子图位姿比较准确，还希望其相对于其他子图的位姿也比较准确，并且子图与子图之间的位姿也比较准确。同时呢，还要这些准确的位姿传递给到前端，因为全局优化是以一定频率持续优化，基于前面的优化再优化，而不是只优化一次。
在后端优化中，节点和子图的位姿是通过最小化误差函数来进行调整的。这个误差函数可以包含多种约束，如里程计测量、IMU数据、固定的参考帧位姿、特征点观测等等。优化的目标是找到一组位姿，使得所有的约束都得到满足，并且使得整个系统的一致性最好。
3.1 SPA后端优化
Cartographer的后端优化是借用SPA（Sparse Pose Adjustment）优化算法的思想. 其主要步骤如下:
- 确定两个节点在global坐标系下的相对位姿变换.
- 通过其它方式再次获取这两个节点的相对位姿变化
- 对这两个相对位姿变换的差的最小二乘问题进行求解
- 进行求解后得到一个增量$$\Delta{x}$$, 将当前位姿加上这个增量就得到了优化后的位姿
- Cartographer使用的是ceres进行位姿图优化. 值得注意的是, global坐标系到现在(提到后端部分)才出现, 前端都是在local坐标系下做的
SPA（Sparse Pose Adjustment）优化是一种位姿图优化技术（Pose Graph），其优化的对象是所有节点的世界位姿。在一些视觉SLAM方案中，节点由视觉关键帧组成，我们根据前端里程计在关键帧之间构建相对位姿约束 —— 边，如经典的 ORB-SLAM。
Cartographer (以下简称Carto) 稍有不同，Carto 中的节点共有两类 —— 关键帧节点和子图节点，关键帧当然指的是激光关键帧，而子图（submap）是由连续的若干个激光关键帧拼接到一起形成的子地图。这两类节点都是我们的优化对象，所有的约束（边）都是在这两类节点之间构建，同类节点之间不构建约束（我们暂且这么认为，实际Carto中稍有不同）
3.2 回环优化
回环的作用在于告诉你，你的累积误差(drift)有多大。而优化的任务在于，把这个累积的误差，分摊到所有的节点之间，以使得总体的误差最小化
4. 参考链接
https://zhuanlan.zhihu.com/p/533451244
https://blog.csdn.net/qq_41663016/article/details/130637263
https://zhuanlan.zhihu.com/p/364377540
评论（0）
您还未登录，请登录后发表或查看评论