爽一把手写Bundle Adjustment

最新推荐文章于 2024-08-22 08:22:44 发布

Bruce梦博

最新推荐文章于 2024-08-22 08:22:44 发布

阅读量2.2k

点赞数 6

分类专栏： SLAM C++

本文链接：https://blog.csdn.net/bdl295/article/details/90373529

版权

本文介绍了在视觉SLAM中的重要技术Bundle Adjustment（BA），它是一个大规模最小二乘优化问题，用于优化相机位姿和路标点的估计。通过Levenberg-Marquardt优化算法，详细讲解了如何构建和求解增量方程，最后提供了使用Ceres优化库的示例和完整的BA代码链接。

摘要由CSDN通过智能技术生成

今天是传说中的5.20，同时也是本人最近特别钟爱的神剧《权力的游戏》大结局的日子，作为一名伪技术宅，这么重大的日子当然要写一篇文来纪念一下。所以就有了这篇手写Bundle Adjustment的记录文。

Bundle Adjustment（后面简写成BA）在视觉SLAM里面的作用不用多说，自然是非常重要的。BA主要求解的是一个关于相机位姿以及路标点的空间坐标的大规模最小二乘优化问题，每一个相机位姿都与多个路标点关联，而每一个路标点也同时和多个相机位姿相关联，由此形成复杂的网状的相互约束，通过调整相机位姿和路标点的位置估计，使得所有路标点在所有相机中的重投影误差最小。下面我们就来亲自动手尝试一下求解Bundle Adjustment。相关的理论可以参考《视觉SLAM十四讲》第六章和第十章，这里仅简单略过。

1 Levenberg-Marquardt优化

考虑简单的最小二乘优化问题：

$\min_{\textbf{x}}{\frac{1}{2}\left \| f(\textbf{x}) \right \|_2^2}$

将 $f(\textbf{x})$ 进行一阶泰勒展开：

$f(\textbf{x})\approx f(\textbf{x})+J(\textbf{x})\Delta \textbf{x}$

同时通过拉格朗日乘子添加对增量的约束，得到以下形式：

$\min_{\textbf{x}}{\frac{1}{2}\left \| f(\textbf{x})+J(\textbf{x})\Delta \textbf{x} \right \|_2^2+\frac{\lambda}{2}\left \| D\Delta \textbf{x} \right \|^2}$

对 $\Delta \textbf{x}$ 求导并令导数等于0，可得以下方程

$(J(\textbf{x})^TJ(\textbf{x})+\lambda D^TD)\Delta \textbf{x}=-J(\textbf{x})^Tf(\textbf{x})$

求解以上增量方程，可得 $\Delta \textbf{x}$ 的值，进而令 $\textbf{x}=\textbf{x}+\Delta \textbf{x}$ 更新对优化变量 $\textbf{x}$ 的估计。

在实际应用中通常取 D=I 或者 $D^TD=diag(J(\textbf{x})^TJ(\textbf{x}))$ ，同时还涉及到对 $\lambda$ 大小的调整。

在应用Levenberg-Marquardt优化算法时，最主要的步骤就是求函数值 $f(\textbf{x})$ 和雅可比 $J(\textbf{x})$ ，以及求解增量方程得出 $\Delta \textbf{x}$ 。

下面尝试一下Ceres优化库的一个曲线拟合的实例 $y=e^{mx+c}$ 。拟合用的数据点通过对真实曲线 $y=e^{0.3x+0.1}$ 进行采样并附加标准差 $\sigma=0.2$ 的高斯噪声生成。

#include<random>
#include<vector>

//用来生成高斯噪声
std::default_random_engine e;
std::normal_distribution<double> n(0, 0.2);
//用来存储数据点
std::vector<std::pair<double, double>> data;
//生成数据点，假设x的范围是0-5
for(double x = 0; x < 5; x += 0.05)
{
	double y = exp(0.3*x + 0.1)+n(e);
	data.push_back(std::make_pair(x, y));
}

根据以上数据点拟合曲线的最小二乘优化问题为

$\min_{m,c}{\frac{1}{2}\sum_k (e^{mx_k+c}-y_k)^2}=\min_{m,c}{\frac{1}{2}\left \| \begin{bmatrix}e^{mx_1+c}-y_1\\e^{mx_2+c}-y_2\\\dots\\e^{mx_N+c}-y_N \end{bmatrix} \right \|^2_2}=\min_{m,c}{\frac{1}{2}\left \| F(m,c) \right \|^2_2}$