手把手计算入门级图优化例子（视觉SLAM）

最新推荐文章于 2024-08-14 15:14:01 发布

晚餐男孩

最新推荐文章于 2024-08-14 15:14:01 发布

阅读量838

点赞数 3

分类专栏： SLAM 文章标签： slam 计算机视觉

本文链接：https://blog.csdn.net/shanpenghui/article/details/110500499

版权

SLAM 专栏收录该内容

26 篇文章 25 订阅

订阅专栏

机器人定位问题已经从滤波方法逐渐变成优化方法，事实也证明基于优化方法的定位效果要比滤波方法效果好。其中，优化和图论结合的图优化方法几乎占据了头条，成为学习SLAM必须要掌握的一门知识。那么，什么是图优化呢？
本文注重手推，想理解图优化的童鞋请准备好笔和纸，和我一起推导，完成之后必然有所收获。

一、SLAM图优化

机器人的位姿是一个节点（位姿）和边构成（R，t）。
在最简单的情况下，节点代表相机的各个位姿（四元数或者矩阵）：
$p_i=[x,y,z,q_x,q_y,q_z,q_w]\tag{1}$

边则代表各个位姿的转换关系：
$T_{ji}=\begin{bmatrix}R_{3×3}&t_{3×1} \\0&1 \end{bmatrix} \tag{2}$

      简单的回环来说可以大概表示为：
$p_1\stackrel{T_{21}}{\longrightarrow} p_2\stackrel{T_{32}}{\longrightarrow} p_3\stackrel{T_{43}}{\longrightarrow} p_4\tag{3}$
      构造最小二乘问题，使得整个VO图的边误差最小：
$\underset{\text{R,t}}{min}= minE= \sum_{i,j}||x_i-T_{ij}x_j||^2 \tag{4}$
      于是，我们的工作转移到了对代价函数 $x_i-T_{ij}x_j||^2$ 求最小值上。

二、里程计的图优化

如下图所示，假设一个机器人初始起点在0处，然后机器人向前移动，通过里程计测得它向前移动了1米，到达第二个地点。接着，又向后返回，里程计测得它向后移动了0.8米。但是，~~通过闭环检测~~ ，发现它回到了初始点。可以看出，里程计误差导致计算的位姿和观测到有差异，那么，机器人这几个状态中的位姿到底是怎么样的才最好的满足这些条件呢？

在这里插入图片描述

首先构建位姿之间的关系，即图的边：
$x_0=0（初始起点在0处） \\x_1=x_0+1（向前移动了1米） \\x_2=x_1-0.8（向后移动了0.8米） \\x_2=x_0（回到了初始点） \tag{5}$

构建的图如下：

在这里插入图片描述

线性方程组中变量小于方程的个数，要计算出最优的结果，就要使用最小二乘法，需要构建残差平方和函数，那么从上面可知，残差函数是：
$f_0=x_0-0（初始起点在0处） \\f_1=x_1-x_0-1（向前移动了1米） \\f_2=x_2-x_1+0.8（向后移动了0.8米） \\f_3=x_2-x_0（回到了初始点） \tag{6}$

残差平方和函数是：
$F=\sum_{i,j} f_i^2=f_0^2+f_1^2+f_2^2+f_3^2= \\(x_0-0)^2+(x_1-x_0-1)^2+(x_2-x_1+0.8)^2+ (x_2-x_0)^2 \tag{7}$

为了使残差平方和最小，我们对上面的函数每个变量求偏导，并使得偏导数等于0。

$\frac{\partial F}{\partial x_0}= 2x_0-2(x_1-x_0-1)-2(x_2-x_0) \\=6x_0-2x_1-2x_2+2 \\=3x_0-x_1-x_2+1=0 \\\Rightarrow 3x_0-x_1-x_2=-1 \tag{8}$
$\\\frac{\partial F}{\partial x_1}= 2(x_1-x_0-1)-2(x_2-x_1+0.8) \\=-2x_0+4x_1-2x_2-3.6 \\=-x_0+2x_1-x_2-1.8=0 \\\Rightarrow -x_0+2x_1-x_2=1.8 \tag{9}$
$\\\frac{\partial F}{\partial x_2}= 2(x_2-x_1+0.8)+2(x_2-x_0) \\=-x_0-x_1+2x_2+0.8=0 \\\Rightarrow -x_0-x_1+2x_2=-0.8 \tag{10}$

      把公式（8）、（9）、（10）合成矩阵形式，可得：
$\begin{bmatrix} 3&-1&-1 \\-1&2&-1 \\-1&-1&2 \end{bmatrix} \begin{bmatrix} x_0\\x_1\\x_2 \end{bmatrix}= \begin{bmatrix} -1\\1.8\\-0.8 \end{bmatrix} \tag{11}$
      利用矩阵求解线性方程：
$\begin{bmatrix} x_0\\x_1\\x_2 \end{bmatrix}= \begin{bmatrix} 3&-1&-1 \\-1&2&-1 \\-1&-1&2 \end{bmatrix}^{-1} \begin{bmatrix} -1\\1.8\\-0.8 \end{bmatrix}= \begin{bmatrix} 0\\0.93\\0.07 \end{bmatrix} \tag{12}$
      所以调整以后为满足这些边的条件，机器人的位姿为：
$\begin{bmatrix} x_0\\x_1\\x_2 \end{bmatrix}= \begin{bmatrix} 0\\0.93\\0.07 \end{bmatrix} \tag{13}$

三、相机的图优化

上面虽然求出来最后的解，但是有个致命的问题：

机器人非常依赖闭环检测，因为只有通过闭环检测才能发现它回到了初始点，否则无法判断。而实际应用中，闭环检测的难度很大，而且也存在误差。

避开使用闭环检测这个方法，在相机图优化中，我们尝试用观测路标的方法来构建边。

如下图所示，假设一个机器人初始起点在0处，并观测到路标在其前方2m。然后机器人向前移动，通过里程计测得它向前移动了1m，这时观测到路标在其前方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） \tag{14}$
构建的图如下：

在这里插入图片描述
线性方程组中变量小于方程的个数，要计算出最优的结果，就要使用最小二乘法，需要构建残差平方和函数，那么从上面可知，残差函数是：
$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） \tag{15}$
残差平方和函数是：
$F=\sum_{i,j} 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 \tag{16}$

为了使残差平方和最小，我们对上面的函数每个变量求偏导，并使得偏导数等于0。

$\frac{\partial F}{\partial x_0}= 2x_0-2(l_0-x_0-2)-2(x_1-x_0-1) \\=6x_0-2x_1+2l_0+6 \\=3x_0-x_1+l_0+3=0 \\\Rightarrow 3x_0-x_1+l_0=-3 \tag{17}$
$\\\frac{\partial F}{\partial x_1}= 2(x_1-x_0-1)-2(l_0-x_1-0.8) \\=-2x_0+4x_1-2l_0-0.4 \\=-x_0+2x_1-l_0-0.2=0 \\\Rightarrow -x_0+2x_1-l_0=0.2 \tag{18}$
$\\\frac{\partial F}{\partial l_0}= 2(l_0-x_0-2)+2(l_0-x_1-0.8) \\=-2x_0-2x_1+4l_0-5.6=0 \\=-x_0-x_1+2l_0-2.8=0 \\\Rightarrow -x_0-x_1+2l_0=2.8 \tag{19}$
把公式（17）、（18）、（19）合成矩阵形式，可得：
$\begin{bmatrix} 3&-1&1 \\-1&2&-1 \\-1&-1&2 \end{bmatrix} \begin{bmatrix} x_0\\x_1\\l_0 \end{bmatrix}= \begin{bmatrix} -3\\0.2\\2.8 \end{bmatrix} \tag{20}$
利用矩阵求解线性方程：
$\begin{bmatrix} x_0\\x_1\\l_0 \end{bmatrix}= \begin{bmatrix} 3&-1&-1 \\-1&2&-1 \\-1&-1&2 \end{bmatrix}^{-1} \begin{bmatrix} -3\\0.2\\2.8 \end{bmatrix}= \begin{bmatrix} 0\\1.07\\1.93 \end{bmatrix} \tag{21}$

所以调整以后为满足这些边的条件，路标和机器人位姿为：
$\begin{bmatrix} x_0\\x_1\\l_0 \end{bmatrix}= \begin{bmatrix} 0\\1.07\\1.93 \end{bmatrix} \tag{22}$

四、信息阵

我们知道传感器的精度是有差别的，也就是说我们对传感器的相信程度应该不同。比如假设这里编码器信息很精确，测得的路标距离不准，我们应该赋予编码器信息更高的权重，假设是10，对应的残差是 $f_2$ ，那么重新得到残差平方和如下：

$F=\sum_{i,j} 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 \tag{23}$

为了使残差平方和最小，我们对上面的函数每个变量求偏导，并使得偏导数等于0。

$\frac{\partial F}{\partial x_0}= 2x_0-2(l_0-x_0-2)-20(x_1-x_0-1) \\=24x_0-20x_1-2l_0+24 \\=12x_0-10x_1-l_0+12=0 \\\Rightarrow 12x_0-10x_1-l_0=-12 \tag{24}$
$\\\frac{\partial F}{\partial x_1}= 20(x_1-x_0-1)-2(l_0-x_1-0.8) \\=-20x_0+22x_1-2l_0-18.4 \\=-10x_0+11x_1-l_0-9.2=0 \\\Rightarrow -10x_0+11x_1-l_0=9.2 \tag{25}$
$\\\frac{\partial F}{\partial l_0}= 2(l_0-x_0-2)+2(l_0-x_1-0.8) \\=-2x_0-2x_1+4l_0-5.6=0 \\=-x_0-x_1+2l_0-2.8=0 \\\Rightarrow -x_0-x_1+2l_0=2.8 \tag{26}$

把公式（24）、（25）、（26）合成矩阵形式，可得：
$\begin{bmatrix} 12&-10&-1 \\-10&11&-1 \\-1&-1&2 \end{bmatrix} \begin{bmatrix} x_0\\x_1\\l_0 \end{bmatrix}= \begin{bmatrix} -12\\9.2\\2.8 \end{bmatrix} \tag{27}$
利用矩阵求解线性方程：
$\begin{bmatrix} x_0\\x_1\\l_0 \end{bmatrix}= \begin{bmatrix} 12&-10&-1 \\-10&11&-1 \\-1&-1&2 \end{bmatrix}^{-1} \begin{bmatrix} -12\\9.2\\2.8 \end{bmatrix}= \begin{bmatrix} 0\\1.01\\1.9 \end{bmatrix} \tag{28}$