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的匹配残差、位置(平移)残差、角度(旋转)残差。位置、角度两残差顶多就是对匹配位姿的约束，防止点云匹配的结果和初值差太多，真正的扫描匹配的主角是点云匹配残差。
点击Cartographer前后端梳理 - 古月居 (guyuehome.com)可查看全文
