在前面介绍了 :
slam中图优化的基本概念 SLAM中的图优化—-基本概念及Rviz显示位姿图code
非线性最小二乘的理论 SLAM中的图优化—-本质理论-非线性最小二乘

本篇介绍如何在SLAM中应用非线性最小二乘实现图优化

非线性最小二乘在 SLAM领域和机器人状态估计领域应用的非常广泛
流程都是差不多的.像g2o ceres gtsam 实际上就是通用的库,本质都是求非线性最小二乘

构建线性系统

构建误差向量

还是按上面这个图:
1 观测值为匹配计算得到的节点i和节点j的相对位姿,就是红色的那个边

2 预测值为里程积分得到的当前节点i和节点j的相对位姿,就是绿色的那个形成的边.
每个节点都有一个相对于世界坐标系的位姿,位姿由上一节点积分来,所以可以直接用i和j的位姿求相对位姿:

3 误差函数的定义:
相对观测值有个相对位姿,预测值有个相对位姿,那么这两个相对位姿之间的偏差是多少呢?
就是求两个相对位姿的相对位姿,也就是:

上面这个是误差函数的抽象表达式,那么精确表达式则是下面这种

首先已知每个点的位姿矩阵


则预测值的相对位姿为:

测量值的相对位姿是由Rij和tij构成,按照上面的推论,则误差函数的向量形式

构建Jacobian矩阵

然后就是求 Jacobian(雅克比矩阵):
就是对误差函数的向量分别求xi和xj的偏导
求xi的偏导:
xi由(ti,θi)构成,所有
1 误差向量的第一行对ti求导, 结果:

放到矩阵的1,1位置

2 误差向量的第一行对θi求导, 结果:

放到矩阵的1,2位置

3 误差向量的第二行对ti求导, 结果:
0
放到矩阵的2,1位置

4 误差向量的第二行对θi求导, 结果:
-1
放到矩阵的2,2位置

所以误差向量对xi求偏导的结果是:

同理误差向量对xj求偏导的结果是:

现在有了误差函数和雅克比矩阵
(上面的误差函数是通过变换矩阵计算相对位姿然后再转为向量形式,也可以直接用向量形式相减 gtsam是这样的)

上面的操作构建了误差函数并线性化,然后求得了雅克比矩阵:

这里要注意一个性质:

误差函数只跟xi 和xj 有关,x是一个多维的,可以是很大的比如1000维,但是求完偏导后除了xi和xj其它位置都为0

Jacobian矩阵的形式,图形化就是下面这样的:

这样的形式,可以说是一个稀疏的向量.
推导非线性最小二乘的时候构建H矩阵的公式是:

所以H也是一个稀疏的矩阵

向量里仅有两项非0,那么矩阵里就只有4项非0

构建b向量

同理bij也是稀疏的

H和b最终都是和的形式,
b最后是稠密的,形式如下:

求解线性系统

H最后是稀疏的,形式如下:

为什么要特意强调H矩阵是稀疏的呢?
H 矩阵为稀疏矩阵,可以利用此特征进行快速求解。这因为如此,才有了开源库,使得非线性最小二乘在图优化里面有了成熟的应用.
在算∆x的时候是要求H的逆的

如果H不是稀疏的话,1000维的H,求其逆耗费的运算量是非常大的

有了H矩阵和b矩阵,那么非线性最小二乘就可以解了,然后就是不断迭代了.

总结

以上整个过程的推导总结下使用方法:
构建线性系统:

• 已知误差项eij和Jacobian矩阵Aij 和Bij
• 向量b更新
  
• 矩阵H更新
  
  求解线性系统:
• 已知矩阵H和向量b
• 求解线性方程组
  
• 不断进行迭代,直至收敛
  
  x即为优化后的结果