(02)Cartographer源码无死角解析-(53) 2D后端优化→位姿图优化理论(SPA)讲解、核型函数调用流程

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证

一、前言

通过前面的一系列博客，已经完成了2D点云扫描匹配：含相关性暴力搜索匹配以及ceres扫描匹配的讲解。总得来说，关于Cartographer前端数据处理以及前端位姿优化的相关代码已经深入解析。那么接下来，就是对后端部分代码讲解，这里暂时还是以2D后端优化为例，后续再向3D扩展。后端优化的的代码主要集中在如下文件：

src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc

源码中的实现，主要参考SPA论文：Efficient Sparse Pose Adjustment for 2D Mapping
论文翻译：https://blog.csdn.net/u014527548/article/details/106238658

二、位姿图理论

Cartographer 后端优化是使用位姿图的方式，如果了解g2o的朋友应该知道，图的核心在于点(节点)与边的建立，边主要起到约束作用，Cartographer 位姿图中包含了两种约束：子图内\color{red}子图内子图内与子图间\color{red}子图间子图间约束，另外在回环检测中会计算子图间约束。

1、什么是图

由节点和边组成的一种数据结构, 节点之间的关系可以是任意的, 图中任意两节点之间都可能相关(存在边)。

2、什么是位姿图

SPA 论文中对位姿图的定义: 位姿图是一组通过非线性约束连接的机器人位姿, 这些非线性约束是从对附近位姿共有的特征的观察中获得的。位姿图是一种图, 节点代表位姿, 边代表 2 个位姿间的相对坐标变换(也叫约束)，如下三角形表示机器人位姿, 三角形之间的连线表示约束(坐标变换)：

如果对于图优化比较陌生的朋友可以参考本人博客：史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)

3.什么是优化

由于前端里程计会有累计误差, 那有没有一种方法可以将这种累计误差减小甚至消除掉呢?这就是优化的目的与作用。

左边表示没有优化之前、中间表示优化之后、右边表示真实地图。可以明显看到优化之后的精度提升较大。

三、残差项构建

通过前面博客的了解，可以知道优化问题核心就是在于残差项的构建，这里粘贴一下源码中的图示：

总的来说，可以分为如下几个步骤

第一步：\color{blue} 第一步：第一步：确定 2 个节点在 global 坐标系下的相对位姿变换。
第二步：\color{blue} 第二步：第二步：通过其他方式再次获取这 2 个节点的相对位姿变换
第三步：\color{blue} 第三步：第三步：对这 2 个相对位姿变换的差的最小二乘问题进行求解
第四步：\color{blue} 第四步：第四步：进行求解之后会得到一个增量