SLAM——图优化篇

1.图优化的入门

在上节中，直接用俩俩匹配搭建一个里程计，但是如果：1.错误匹配，整个图就会看起来很奇怪，就是错了。2.误差会累积，常见的现象是：相机转过去的过程能作对，但转回来就很奇怪。3.效率低。由于地图拼接中累计误差是个很严重的问题，所以要保证每次匹配都精确无误，而这是很难实现的。所以我们把所有整的信息都考虑进来，成为一个全slam问题。

姿态图（原理部分）
姿态图，是由相机姿态构成的一个图（graph）。这里的图，从图论意义上来说，一个图有节点（位姿）和边构成（R，t）

在最简单的情况下节点代表相机的各个位姿（四元数或者矩阵）：$p=[x,y,z,qx,qy,qz,qw]$

而边则代表各个位姿的转换关系：$T=\begin{bmatrix}R^{3\times3}&t^{3\times1}\\0&1\end{bmatrix}$

则我们对于简单的回环来说可以大概表示为: $1\overset{_{1}^{2}\textrm{T}}{\rightarrow}2\overset{_{2}^{3}\textrm{T}}{\rightarrow}3\overset{_{3}^{4}\textrm{T}}{\rightarrow}4$

用高博的图即：

像这样子的vo，我们除了对边进行优化之外，没有比较好的方法了，构造最小二乘法问题，使得整个VO的边误差最小，则有:

$\underset{R,t}{min}=minE=\sum_{i,j}||x_{i}-T_{i,j}x_{j}||_{2}^{2}$

于是，我们的工作转移到了对代价函数： $x_{i}-T_{i,j}x_{j}$ 进行最小优化。

怎么把SLAM问题表示成图呢？

（参考高博的博客加上自己的一些理解）
图：顶点（需要调整的变量，即最小二乘中的决策变量）和边（观测方程z（测出来的东西）），然后我们是以$e_{k}=z_{k}-h(x_{k})$为误差，以 $\underset{x}{min}F_{k}(x_{k})=|e_{k}|$ 为目标函数。

那么优化变量（即顶点）： 可以是一个机器人的pose（6自由度下为4x4的变换矩阵T或者3自由度下的x,y,）也可以是一个空间点（三维空间的[x,y,z]或者二维像素的[x,y]）。观测方程（这里的观测方程，我之前纠结了好久，但是通俗的讲就是你传感器测出来的数据计算出的你想要求的变量）也有很多形式：
1.机器人俩个Pose之间的变换；
2.机器人在某个Pose处用激光测量到了某个空间点，得到了它离自己的距离和角度。
3.机器人在某个Pose处用相机观测到了某个空间点，得到了它的像素坐标。

在图中，以顶点为优化变量（在最小二乘中为决策变量），以边为观测方程（就是测出来的东东）。由于边可以链接一个或多个顶点，所以我们把它的形式写成更广义的 $z_{k}=h(x_{k1},x_{k2},...)$ ，以不限制顶点数量的意思。
1.机器人俩个pose之间的变换；——一条Binary Edge(二元边)，边的方程为： $T_{1}=\Delta T\cdot T_{2}$
2.机器人在某个Pose处用相机观测到了某个空间点，得到它的像素坐标；——Binary Edge，顶点为一个3D Pose：T和一个空间点 $x=[x,y,z]^{T}$，观测数据为像素坐标$z=[u,v]^{T}$,观测方程为：$z=C(Rx+t)$

总而言之，顶点就是优化变量（三维空间点或者Pose），边就是观测方程（T或者二维坐标）

2.图优化的深入理解

图优化是什么？在graph-based SLAM中，机器人的位姿是一个节点或顶点（vetex），位姿之间的关系构成边（edge），比如t+1时刻和t时刻之间的odometry关系构成边，或者由视觉计算出来的位姿转换矩阵也可以构成边。一旦图构成了，就要调整机器人的位姿去尽量满足这些边构成约束。

图优化SLAM能分解成俩个任务：
1.构建图，机器人位姿当做顶点，位姿关系当做边，称为前端。
2.优化图，调整机器人位姿顶点尽量满足边的约束，称为后端。

图优化过程中：先堆积数据，机器人位姿为构建的顶点。边是位姿之间的关系，可以是编码器数据计
算的位姿，也可以是通过ICP匹配计算出来的位姿，还可以是闭环检测的位姿关系。

左图为建完图未经优化的图，而右图为调整顶点满足边的约束，最后得到的地图。

—————一个帮助理解的例子：——————–
如下图所示，假如机器人初始点在0处，然后机器人向前移动，通过编码器测得是1，再编码器又测得了它向后0.8，结果通过闭环检测却发现它又回到了起点。可以看出，编码器出现了点小问题，那么需要机器人这几个状态中的位姿到底是怎么样的才最好的满足这些条件呢？

建立关系：
$f_{1}=x_{0}=0;f_{2}=x_{1}-x_{0}-1=0;f_{3}=x_{2}-x_{1}+0.8=0;f_{4}=x_{2}-x_{0}=0$

为了使的总体的误差最小，使用最小二乘如下：
∑4i=1||fi||22=