当前位置:   article > 正文

激光slam:如何在SLAM中应用非线性最小二乘实现图优化_slam面试题最小二乘解法编程

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即为优化后的结果
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/我家小花儿/article/detail/292594
推荐阅读
相关标签
  

闽ICP备14008679号