本文详细介绍了SLAM后端优化中的关键概念，包括投影模型和BA（Bundle Adjustment）代价模型，重点讨论了H矩阵的稀疏结构以及如何利用这种结构加速计算。此外，还探讨了鲁棒核函数在处理匹配错误时的作用，以提高算法的稳定性。最后，简要介绍了位姿图的概念，它是优化大规模定位与建图问题的一种高效方法。
1.投影模型和BA代价模型##
对于观测模型而言，我们可以简单的用以下的模型进行表示：z=h(x,y)
上图中，P点是世界坐标系的点，中间的畸变模块满足： r c 2 = u c 2 + v c 2 r_{c}^{2}=u_{c}^{2}+v_{c}^{2} rc2=uc2+vc2
把观测返程抽象出来，有：z=h(x,y) ，这里的x指代此时相机的位姿，即外参R,t，对应的李代数为 ξ \xi ξ。路边y即为三维点p，若观测数据(实际观测数据)为z，那么此次观测误差为： e = z − h ( ξ , p ) e=z-h(\xi ,p) e=z−h(ξ,p)，那么代价函数为：
1 2 ∑ i = 1 m ∑ j = 1 n ∣ ∣ e i j ∣ ∣ 2 = 1 2 ∑ i = 1 m ∑ j = 1 n ∣ ∣ z i j − h ( ξ i , p j ) ∣ ∣ 2 \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}|| e_{ij}||^{2}= \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}|| z_{ij} -h(\xi_{i},p_{j} )||^{2} 21i=1∑mj=1∑n∣∣eij∣∣2=21i=1∑mj=1∑n∣∣zij−h(ξi,pj)∣∣2
其中 z i j z_{ij} zij 为姿态i观察路标点j所产生的数据。
使用非线性优化来优化这个代价函数，当把所有的自变量定义成所有待优化的变量时，即：
x = [ ξ 1 , ⋯ , ξ m , ρ 1 , ⋯ , ρ n ] x=[\xi_{1} ,\cdots ,\xi_{m},\rho_{1} ,\cdots ,\rho_{n}] x=[ξ1,⋯,ξm,ρ1,⋯,ρn]
那么整个代价函数就可以变成：
1 2 ∣ ∣ f ( x + Δ x ) ∣ ∣ 2 ≈ 1 2 ∑ i = 1 m ∑ j = 1 n ∣ ∣ e i j + F i j Δ ξ i + E i j Δ ρ j ∣ ∣ 2 \frac{1}{2}||f(x+\Delta x)||^{2}\approx \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||e_{ij}+F_{ij}\Delta \xi _{i}+E_{ij}\Delta\rho_{j}||^{2} 21∣∣f(x+Δx)∣∣2≈2
最低0.47元/天 解锁文章
6009
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
