赞
踩
在前面介绍了
slam中图优化的基本概念 链接
非线性最小二乘的理论 链接
非线性最小二乘在 SLAM领域和机器人状态估计领域应用的非常广泛
流程都是差不多的.像g2o ceres gtsam 实际上就是通用的库,本质都是求非线性最小二乘
还是按上面这个图:
1 观测值为匹配计算得到的节点i和节点j的相对位姿,就是红色的那个边
2 预测值为里程积分得到的当前节点i和节点j的相对位姿,就是绿色的那个形成的边.
每个节点都有一个相对于世界坐标系的位姿,位姿由上一节点积分来,所以可以直接用i和j的位姿求相对位姿:
3 误差函数的定义:
相对观测值有个相对位姿,预测值有个相对位姿,那么这两个相对位姿之间的偏差是多少呢?
就是求两个相对位姿的相对位姿,也就是:
上面这个是误差函数的抽象表达式,那么精确表达式则是下面这种
首先已知每个点的位姿矩阵
则预测值的相对位姿为:
测量值的相对位姿是由Rij和tij构成,按照上面的推论,则误差函数的向量形式
然后就是求 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
同理bij也是稀疏的
H和b最终都是和的形式,
b最后是稠密的,形式如下:
H最后是稀疏的,形式如下:
为什么要特意强调H矩阵是稀疏的呢?
H 矩阵为稀疏矩阵,可以利用此特征进行快速求解。这因为如此,才有了开源库,使得非线性最小二乘在图优化里面有了成熟的应用.
在算∆x的时候是要求H的逆的
如果H不是稀疏的话,1000维的H,求其逆耗费的运算量是非常大的
有了H矩阵和b矩阵,那么非线性最小二乘就可以解了,然后就是不断迭代了.
以上整个过程的推导总结下使用方法:
构建线性系统:
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。