– 第六章 后端图优化理论
6.1 图优化理论
课程介绍：
- 这期开始不讲滤波器的内容了（gmapping），只讲图优化（karto、cartographer）。
- 但是滤波器在视觉slam之中的应用还是非常广泛的。
- 三种slam方向：
滤波估计当前位姿，**平滑（图优化）**估计过去位姿，**预测（暂未接触）**估计未来位姿；
– 6.1.1 Graph-based SLAM
- 2Dslam问题会比较简单，因为只是欧式空间的问题，不用考虑李代数等问题。
- 激光和视觉的差异：
- 激光一般只进行pose位姿优化（精度较高）；节点表示机器人位姿；
- 视觉精度比较差，除了pose还有其他的；
- 回环检测：走了一圈之后，不同的两个节点1、5到了同样的位置，产生一个回环，可以根据边（约束）计算相对空间变换；即
节点1和5相互满足两条约束。
- 求解目标：由于里程计的误差，导致误差不为零，上述两条边误差不为零，所以需要使误差尽可能趋向零；这个是图优化的目标；
- 抽象问题：求解两个节点的位姿变换过程，由于旋转矩阵涉及sin和cos计算，实际上是非线性计算，即图优化slam依然是可以抽象成，求解非线性最小二乘问题；
- 前端和后端的差别：
- 前端：节点和节点之间构建边（约束）、节点连线过程，以及之前的过程都是前端问题；
- 后端：节点连成边后，构建了图后，让误差最小化的就是后端优化；优化是预测和观测问题；
– 6.1.2 非线性最小二乘
- 状态方程f(x)可以认为是一个预测过程；
- x是预测值；
- 状态向量是机器人位姿robot pose；
- z表示的是观测值，即传感器值；由于噪声，f(x)和z不相等；
- 由于数学算法的局限，需要把非线性问题，转化成一个线性的问题求解；
- 勘误：f2(x) = z’2 z2
- 目标：找到最优的x，让观测和预测的误差最小化；
- 人造的传感器通常服从高斯分布（因为人造的模块化、相互独立等因素）**。**自然界造物一般服从贝塔分布或其他；
- 直接想法的解释：极值点的导数必定零；
- 非线性问题：需要线性化才能求解，使用泰勒展开方法；
- 误差函数有个一阶的近似，高阶的余项可忽略
- 化简过程：
- 非线性最小二乘求解过程：
- Ax = b，还是一个适定方程，刚好有一个解。
- 不断迭代求解到最后就是一个机器人位姿，graph-based位姿图优化问题；
– 6.1.3 非线性最小二乘在slam中的应用
- 图的构建
- 两者特点
- 里程计：时间上是连续的，连续的机器人位姿才可以构建为图；
- 回环检测：时间上是不连续的；不同时间的节点可以通过边（约束）匹配构建成图；
- 误差函数
- xi和xj之间的相对位姿（节点j在节点i的坐标，即xi逆*xj）通过帧间匹配算法求得（ICP、NDT等）；
- 观测值Zij和**预测值Zij’**的相对坐标计算，观测值从传感器得，预测值从里程计推算。
- 误差函数定义：即两个位姿变换关系，**Zij的逆*Zij’**矩阵。
- 实际上上述问题都是第一节课提到的坐标位姿变换关系问题，这就是所谓的误差函数。
- 误差函数的矩阵形式，实际就是上一段观测值和预测值推导的内容；
- 这就是为什么说2Dslam问题是机器人位姿图问题，抽象。
- 接下来的问题，实际上就是非线性函数的线性化问题；
- 误差函数的线性化
- 上图的是一个jacobian矩阵，属于稀疏的；
- 因为jacobian是稀疏的向量，所以H矩阵（黑森矩阵）也是稀疏的；
- 最终也是一个稀疏矩阵；
- H（黑森）矩阵是一个稀疏矩阵，就可以进行快速的求解；
- 已知H、b，则由（H▽X = b），可求解▽X；
- 但是还需要解决坐标系的转化问题，需要增加固定的坐标系给xi、xj，如下图：
- 设第一帧位姿为世界坐标系的原点，即增加固定坐标系的约束，后续求解都在世界坐标系。△X1=0；如下图
- 构建线性系统
- 以上所有过程已经推导了后端图优化必备信息，可以开始构建线性系统：
- 上图构建了线性的系统，另外这是我们作业的内容。
- 上图就是图优化的求解根本流程；
- 即：
- 1、构建线性系统；
- 2、求解线性系统；
- 3、迭代求解，直到收敛；
- 所以调用库的时候，只要提供误差函数就好了，因为流程都是一致的。
6.2 cartographer代码讲解
在整体的原有框架中代码主要分为两个部分：cartographer和cartographer_ros。
- cartographer主要负责处理来自雷达、IMU和里程计的数据并基于这些数据进行地图的构建，是cartographer理论的底层实现。建图过程主要分为前端Local累加式建图与后端Global位姿图构建与回环匹配。
- cartographer_ros则基于ros的通信机制获取传感器的数据并将它们转换成cartographer中定义的格式传递给cartographer处理，与此同时也将cartographer的处理结果发布用于显示或保存，在低层算法和机器人操作系统ROS之间提供了交互的接口。
其中输入项中Range Data的激光数据为必须，其余均为非必须。
整体的建图可分为Local前端部分和Global后端回环部分。
特点：
- 基于图优化的slam算法
- 比较完善的匹配系统，包含建图和定位；
- 目前效果最好的开源激光slam系统；
- 有人专门维护，不断更新增加新特性，从没停止；
2Dslam代码。（源码用了大量C++高级特性）
– 6.2.1 代码查看流程：
流程分析：https://blog.csdn.net/weixin_44492854/article/details/103705166
1、从入口global_trajectory_builder.cc（mapping->internal）进入。
– 6.2.2 具体代码分析
- 前端匹配
1、global_trajectory_builder.cc
子图匹配，帧间匹配，前端局部子图匹配
CSM暴力求解：
打分：似然场和TSDF评价体系，本质相似。
- 外插器实现：保存一段时间的位姿用来计算线速度和角速度
- CSM实现：
- 后端优化
1、入口为pose_graph_2d文件，AddNode函数；
- 增加节点和计算约束（边）；
- 增加队列即可。
2、AddWorkItem函数：
- 判断work队列；
3、DrainWorkQueue函数才是调用队列
work_item实际上就是约束。
回环检测及回环优化
- 多线程：AddWorkItem，增加工作队列
- 回环成功：说明需要进行优化
- 回环–优化–回环–优化……
- FCSM：fast correlative scan match 快速关联扫描匹配
- 地图的层数depth？
- 分支定界：求最优解
