图优化简单案例

提示

---

前言

理解图优化理论第一步，搞清楚三要素：误差项、优化变量、协方差，当然开局讲这三个概念有点复杂，所以先借鉴三个简单例子讲基本概念。本例在现有博客基础上进行了案例扩充，由浅入深，便于初学者理解。

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、SLAM图优化

图是由节点和边构成的一种结构。
在机器人SLAM中，机器人位姿是节点，表示机器人状态量$p_i$（位置、姿态），用以下形式表示
$$p_i=[x,y,z,q_x,q_y,q_z,q_w]$$
前三项为机器人位置，后四项为机器人姿态的四元数表示。
各个位姿之间的变换关系构成边（旋转R、平移t）
$$T_{ij}=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$
构建误差项，已有当前观测$x_i$，再通过$T_{ij}$将$x_j$变换至$x_i$，理论上若无误差，则变换后的值应与$x_i$相等，但实际上存在误差，因此作差后得到误差项，构建最小二乘问题，使得整个图的边误差最小
$$\underset{R,t}{\min }E=\sum _{ij}\Vert x_i-T_{ij}{x}_j{\Vert }^2$$

二、相机位姿图优化案例

与上述不同的是，相机位姿图优化中，通常采用机器人对路标点的观测来构建边。
如下图所示案例，假设机器人在初始位置$x_0$处，此时观测到路标$l_0$在前方2m处，通过轮式里程计观测到机器人向前移动1m后，这时再看路标点$l_0$在前方0.8m处，此时机器人最优位姿和路标最优位姿如何计算。

在这个案例中，我们将路标作为顶点：
$x_0=0（初始点在0处）$
$l_0=x_0+2（路标点在前方2m处）$
$x_1=x_0+1（轮式里程计测得前进1m）$
$l_0=x_1+0.8（路标点在前方0.8m处）$
用图表示如下

对上式移项构建误差项得
$f_0=x_0-0（初始点在0处）$
$f_1=l_0-x_0-2（路标点在前方2m处）$
$f_2=x_1-x_0-1（轮式里程计测得前进1m）$
$f_3=l_0-x_1-0.8（路标点在前方0.8m处）$

误差平方和函数如下
$$\begin{gathered} F=\sum _i{f}_i^2=f_0^2+f_1^2+f_2^2+f_3^2 \\ =(x_0-0{)}^2+(l_0-x_0-2{)}^2+(x_1-x_0-1{)}^2+(l_0-x_1-0.8{)}^2 \end{gathered}$$
求误差平方和最小，将上式对每个变量进行求偏导，令偏导为0，即为函数极值点
$$\begin{gathered} \frac{\partial F}{\partial x_0}=2x_0-2(l_0-x_0-2)-2(x_1-x_0-1)=0 \\ \Rightarrow 3x_0-x_1+l_0=-3 \end{gathered}$$
$$\begin{gathered} \frac{\partial F}{\partial x_1}=2(x_1-x_0-1)-2(l_0-x_1-0.8)=0 \\ \Rightarrow -x_0+2x_1-l_0=0.2 \end{gathered}$$
$$\begin{gathered} \frac{\partial F}{\partial l_0}=2(l_0-x_0-2)+2(l_0-x_1-0.8)=0 \\ \Rightarrow -x_0-x_1+2l_0=2.8 \end{gathered}$$
将上面三式写成矩阵形式得
$$\left[\begin{array}{ccc}3& -1& 1\\ -1& 2& -1\\ -1& -1& 2\end{array}\right]\left[\begin{array}{c}{x}_0\\ x_1\\ l_0\end{array}\right]=\left[\begin{array}{c}-3\\ 0.2\\ 2.8\end{array}\right]$$
求解线性方程组，相当于调整后满足这些边得条件，路标点和机器人得位置为
$$\left[\begin{array}{c}{x}_0\\ x_1\\ l_0\end{array}\right]=\left[\begin{array}{c}0\\ 1.07\\ 1.93\end{array}\right]$$

三、信息矩阵

实际定位中，传感器的精度是有差别的，因此我们会对传感器赋于不同的置信度，比如本例中，我们认为轮式里程计的精度很高，则将其权重赋值为10，对应误差项为$f_2$，则误差平方和函数如下
$$\begin{gathered} F=\sum _i{f}_i^2=f_0^2+f_1^2+10f_2^2+f_3^2 \\ =(x_0-0{)}^2+(l_0-x_0-2{)}^2+10(x_1-x_0-1{)}^2+(l_0-x_1-0.8{)}^2 \end{gathered}$$
计算过程与上节一致，因此求得路标和机器人位置得
$$\left[\begin{array}{c}{x}_0\\ x_1\\ l_0\end{array}\right]=\left[\begin{array}{c}0\\ 1.01\\ 1.9\end{array}\right]$$
该结果显示$x_1$的值接近1，因此更加相信轮式里程计的数据，这就是信息矩阵的作用。

四、相机观测

本节介绍开头所说的三要素。（顶点为优化变量，边为运动/观测约束）

1.高斯牛顿法

目标函数：
$$\Delta x^{\ast }=\underset{x}{\mathrm{argmin}}\Vert e(x+\Delta x){\Vert }_p^2$$
$p$——协方差
误差项：
$$e(x+\Delta x)\approx e(x)+J(x)\Delta x$$
$J(x)$——Jacobian
令目标函数关于$\Delta x$的导数为0（类似上节中误差平方和函数导数为0），得到高斯牛顿方程，即增量方程：
$$J^T{P}^{-1}J\Delta x=-J^T{P}^{-1}e$$
其中
$J^T{P}^{-1}J=H$
$-J^T{P}^{-1}e=b$
因此增量方程可以写为
$$H\Delta x=b$$
非线性优化的思想是从某个的初始值开始,不断地寻找下降方向 ∆x 来找到目标函数的最优解,即不断地求令以下目标函数值达到最小值中的增量 ∆x 。

例：一次观测
1.误差项：e为像素平面上的重投影误差(2x1)
2.优化变量：x为相机位姿$T_1$(6x1)、路标点$P_1$(3x1)
3.协方差：P为像素误差(2x2)
Jacobian矩阵如下：
$$J^{2\ast 9}=\frac{\partial e}{\partial x}=\left[\begin{array}{c}\frac{\partial e}{\partial T_1}\frac{\partial e}{\partial P_1}\end{array}\right]=\left[\begin{array}{c}{J}_T^{2\ast 6}{J}_p^{2\ast 6}\end{array}\right]$$

---

总结

简单的图优化实例，帮助理解概念。