SLAM学习——后端（二）

最新推荐文章于 2024-12-09 09:46:25 发布

原创

最新推荐文章于 2024-12-09 09:46:25 发布 · 2.9k 阅读

2 ·

CC 4.0 BY-SA版权

文章标签：

#SLAM #后端

本文详细介绍了SLAM后端优化中的关键概念，包括投影模型和BA（Bundle Adjustment）代价模型，重点讨论了H矩阵的稀疏结构以及如何利用这种结构加速计算。此外，还探讨了鲁棒核函数在处理匹配错误时的作用，以提高算法的稳定性。最后，简要介绍了位姿图的概念，它是优化大规模定位与建图问题的一种高效方法。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

1.投影模型和BA代价模型##

对于观测模型而言，我们可以简单的用以下的模型进行表示：z=h(x,y)

这里写图片描述

上图中，P点是世界坐标系的点，中间的畸变模块满足： $r_{c}^{2}=u_{c}^{2}+v_{c}^{2}$

把观测返程抽象出来，有：z=h(x,y) ，这里的x指代此时相机的位姿，即外参R,t，对应的李代数为 $ξ\xi$ 。路边y即为三维点p，若观测数据(实际观测数据)为z，那么此次观测误差为： $e=z−h(ξ,p)e=z-h(\xi ,p)$ ，那么代价函数为：

$12∑i=1m∑j=1n∣∣eij∣∣2=12∑i=1m∑j=1n∣∣zij−h(ξi,pj)∣∣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}$

其中 $z_{ij}$ 为姿态i观察路标点j所产生的数据。

使用非线性优化来优化这个代价函数，当把所有的自变量定义成所有待优化的变量时，即：

$,ρn]x=[\xi_{1} ,\cdots ,\xi_{m},\rho_{1} ,\cdots ,\rho_{n}]$

那么整个代价函数就可以变成：

$12∣∣f(x+Δx)∣∣2≈12∑i=1m∑j=1n∣∣eij+FijΔξi+EijΔρ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}$