爽一把手写Bundle Adjustment-程序员宅基地

技术标签： C++ SLAM

今天是传说中的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}$

优化的目标是求得最佳的 $m$ 和 $c$ 的值，使得

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

原作者删帖不实内容删帖广告或垃圾文章投诉

智能推荐

攻防世界_难度8_happy_puzzle_攻防世界困难模式攻略图文-程序员宅基地

文章浏览阅读645次。这个肯定是末尾的IDAT了，因为IDAT必须要满了才会开始一下个IDAT，这个明显就是末尾的IDAT了。，对应下面的create_head()代码。，对应下面的create_tail()代码。不要考虑爆破，我已经试了一下，太多情况了。题目来源：UNCTF。_攻防世界困难模式攻略图文

达梦数据库的导出（备份）、导入_达梦数据库导入导出-程序员宅基地

文章浏览阅读2.9k次，点赞3次，收藏10次。偶尔会用到，记录、分享。1. 数据库导出1.1 切换到dmdba用户su - dmdba1.2 进入达梦数据库安装路径的bin目录，执行导库操作　　导出语句：./dexp cwy_init/[email protected]:5236 file=cwy_init.dmp log=cwy_init_exp.log　注释：　　 cwy_init/init_123..._达梦数据库导入导出

js引入kindeditor富文本编辑器的使用_kindeditor.js-程序员宅基地

文章浏览阅读1.9k次。1. 在官网上下载KindEditor文件，可以删掉不需要要到的jsp，asp，asp.net和php文件夹。接着把文件夹放到项目文件目录下。2. 修改html文件，在页面引入js文件：<script type="text/javascript" src="./kindeditor/kindeditor-all.js"></script><script type="text/javascript" src="./kindeditor/lang/zh-CN.js"_kindeditor.js

STM32学习过程记录11——基于STM32G431CBU6硬件SPI+DMA的高效WS2812B控制方法-程序员宅基地

文章浏览阅读2.3k次，点赞6次，收藏14次。SPI的详情简介不必赘述。假设我们通过SPI发送0xAA，我们的数据线就会变为10101010，通过修改不同的内容，即可修改SPI中0和1的持续时间。比如0xF0即为前半周期为高电平，后半周期为低电平的状态。在SPI的通信模式中，CPHA配置会影响该实验，下图展示了不同采样位置的SPI时序图[1]。CPOL = 0，CPHA = 1：CLK空闲状态 = 低电平，数据在下降沿采样，并在上升沿移出CPOL = 0，CPHA = 0：CLK空闲状态 = 低电平，数据在上升沿采样，并在下降沿移出。_stm32g431cbu6

计算机网络-数据链路层_接收方收到链路层数据后,使用crc检验后,余数为0,说明链路层的传输时可靠传输-程序员宅基地

文章浏览阅读1.2k次，点赞2次，收藏8次。数据链路层习题自测问题1.数据链路(即逻辑链路)与链路(即物理链路)有何区别?“电路接通了”与”数据链路接通了”的区别何在?2.数据链路层中的链路控制包括哪些功能?试讨论数据链路层做成可靠的链路层有哪些优点和缺点。3.网络适配器的作用是什么?网络适配器工作在哪一层?4.数据链路层的三个基本问题(帧定界、透明传输和差错检测)为什么都必须加以解决？5.如果在数据链路层不进行帧定界，会发生什么问题？6.PPP协议的主要特点是什么？为什么PPP不使用帧的编号？PPP适用于什么情况？为什么PPP协议不_接收方收到链路层数据后,使用crc检验后,余数为0,说明链路层的传输时可靠传输

软件测试工程师移民加拿大_无证移民，未受过软件工程师的教育（第1部分）-程序员宅基地

文章浏览阅读587次。软件测试工程师移民加拿大无证移民，未受过软件工程师的教育(第1部分) (Undocumented Immigrant With No Education to Software Engineer(Part 1))Before I start, I want you to please bear with me on the way I write, I have very little gen...

随便推点

Thinkpad X250 secure boot failed 启动失败问题解决_安装完系统提示secureboot failure-程序员宅基地

文章浏览阅读304次。Thinkpad X250笔记本电脑，装的是FreeBSD，进入BIOS修改虚拟化配置（其后可能是误设置了安全开机），保存退出后系统无法启动，显示：secure boot failed ，把自己惊出一身冷汗，因为这台笔记本刚好还没开始做备份.....根据错误提示，到bios里面去找相关配置，在Security里面找到了Secure Boot选项，发现果然被设置为Enabled，将其修改为Disabled ，再开机，终于正常启动了。_安装完系统提示secureboot failure

C++如何做字符串分割（5种方法）_c++ 字符串分割-程序员宅基地

文章浏览阅读10w+次，点赞93次，收藏352次。1、用strtok函数进行字符串分割原型： char *strtok(char *str, const char *delim);功能：分解字符串为一组字符串。参数说明：str为要分解的字符串，delim为分隔符字符串。返回值：从str开头开始的一个个被分割的串。当没有被分割的串时则返回NULL。其它：strtok函数线程不安全，可以使用strtok_r替代。示例：//借助strtok实现split#include <string.h>#include <stdio.h&_c++ 字符串分割

2013第四届蓝桥杯 C/C++本科A组真题答案解析_2013年第四届c a组蓝桥杯省赛真题解答-程序员宅基地

文章浏览阅读2.3k次。1 .高斯日记大数学家高斯有个好习惯：无论如何都要记日记。他的日记有个与众不同的地方，他从不注明年月日，而是用一个整数代替，比如：4210后来人们知道，那个整数就是日期，它表示那一天是高斯出生后的第几天。这或许也是个好习惯，它时时刻刻提醒着主人：日子又过去一天，还有多少时光可以用于浪费呢？高斯出生于：1777年4月30日。在高斯发现的一个重要定理的日记_2013年第四届c a组蓝桥杯省赛真题解答

基于供需算法优化的核极限学习机(KELM)分类算法-程序员宅基地

文章浏览阅读851次，点赞17次，收藏22次。摘要：本文利用供需算法对核极限学习机(KELM)进行优化，并用于分类。

metasploitable2渗透测试_metasploitable2怎么进入-程序员宅基地

文章浏览阅读1.1k次。一、系统弱密码登录1、在kali上执行命令行telnet 192.168.26.1292、Login和password都输入msfadmin3、登录成功，进入系统4、测试如下：二、MySQL弱密码登录：1、在kali上执行mysql –h 192.168.26.129 –u root2、登录成功，进入MySQL系统3、测试效果：三、PostgreSQL弱密码登录1、在Kali上执行psql -h 192.168.26.129 –U post..._metasploitable2怎么进入

Python学习之路：从入门到精通的指南_python人工智能开发从入门到精通pdf-程序员宅基地

文章浏览阅读257次。本文将为初学者提供Python学习的详细指南，从Python的历史、基础语法和数据类型到面向对象编程、模块和库的使用。通过本文，您将能够掌握Python编程的核心概念，为今后的编程学习和实践打下坚实基础。_python人工智能开发从入门到精通pdf