爽一把手写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

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

智能推荐

oracle 12c 集群安装后的检查_12c查看crs状态-程序员宅基地

文章浏览阅读1.6k次。安装配置gi、安装数据库软件、dbca建库见下：http://blog.csdn.net/kadwf123/article/details/784299611、检查集群节点及状态：[root@rac2 ~]# olsnodes -srac1 Activerac2 Activerac3 Activerac4 Active[root@rac2 ~]_12c查看crs状态

解决jupyter notebook无法找到虚拟环境的问题_jupyter没有pytorch环境-程序员宅基地

文章浏览阅读1.3w次，点赞45次，收藏99次。我个人用的是anaconda3的一个python集成环境，自带jupyter notebook，但在我打开jupyter notebook界面后，却找不到对应的虚拟环境，原来是jupyter notebook只是通用于下载anaconda时自带的环境，其他环境要想使用必须手动下载一些库：1.首先进入到自己创建的虚拟环境(pytorch是虚拟环境的名字)activate pytorch2.在该环境下下载这个库conda install ipykernelconda install nb__jupyter没有pytorch环境

国内安装scoop的保姆教程_scoop-cn-程序员宅基地

文章浏览阅读5.2k次，点赞19次，收藏28次。选择scoop纯属意外，也是无奈，因为电脑用户被锁了管理员权限，所有exe安装程序都无法安装，只可以用绿色软件，最后被我发现scoop，省去了到处下载XXX绿色版的烦恼，当然scoop里需要管理员权限的软件也跟我无缘了（譬如everything）。推荐添加dorado这个bucket镜像，里面很多中文软件，但是部分国外的软件下载地址在github，可能无法下载。以上两个是官方bucket的国内镜像，所有软件建议优先从这里下载。上面可以看到很多bucket以及软件数。如果官网登陆不了可以试一下以下方式。_scoop-cn

Element ui colorpicker在Vue中的使用_vue el-color-picker-程序员宅基地

文章浏览阅读4.5k次，点赞2次，收藏3次。首先要有一个color-picker组件 <el-color-picker v-model="headcolor"></el-color-picker>在data里面data() { return {headcolor: ’ #278add ’ //这里可以选择一个默认的颜色} }然后在你想要改变颜色的地方用v-bind绑定就好了，例如：这里的:sty..._vue el-color-picker

迅为iTOP-4412精英版之烧写内核移植后的镜像_exynos 4412 刷机-程序员宅基地

文章浏览阅读640次。基于芯片日益增长的问题，所以内核开发者们引入了新的方法，就是在内核中只保留函数，而数据则不包含，由用户（应用程序员）自己把数据按照规定的格式编写，并放在约定的地方，为了不占用过多的内存，还要求数据以根精简的方式编写。boot启动时，传参给内核，告诉内核设备树文件和kernel的位置，内核启动时根据地址去找到设备树文件，再利用专用的编译器去反编译dtb文件，将dtb还原成数据结构，以供驱动的函数去调用。firmware是三星的一个固件的设备信息，因为找不到固件，所以内核启动不成功。_exynos 4412 刷机

Linux系统配置jdk_linux配置jdk-程序员宅基地

文章浏览阅读2w次，点赞24次，收藏42次。Linux系统配置jdkLinux学习教程，Linux入门教程（超详细）_linux配置jdk

随便推点

matlab(4)：特殊符号的输入_matlab微米怎么输入-程序员宅基地

文章浏览阅读3.3k次，点赞5次，收藏19次。xlabel('\delta');ylabel('AUC');具体符号的对照表参照下图：_matlab微米怎么输入

C语言程序设计-文件(打开与关闭、顺序、二进制读写)-程序员宅基地

文章浏览阅读119次。顺序读写指的是按照文件中数据的顺序进行读取或写入。对于文本文件，可以使用fgets、fputs、fscanf、fprintf等函数进行顺序读写。在C语言中，对文件的操作通常涉及文件的打开、读写以及关闭。文件的打开使用fopen函数，而关闭则使用fclose函数。在C语言中，可以使用fread和fwrite函数进行二进制读写。‍ Biaoge 于2024-03-09 23:51发布阅读量：7 ️文章类型：【 C语言程序设计】在C语言中，用于打开文件的函数是____，用于关闭文件的函数是____。

Touchdesigner自学笔记之三_touchdesigner怎么让一个模型跟着鼠标移动-程序员宅基地

文章浏览阅读3.4k次，点赞2次，收藏13次。跟随鼠标移动的粒子以grid（SOP）为partical（SOP）的资源模板，调整后连接【Geo组合+point spirit（MAT)】，在连接【feedback组合】适当调整。影响粒子动态的节点【metaball(SOP)+force(SOP)】添加mouse in（CHOP)鼠标位置到metaball的坐标，实现鼠标影响。..._touchdesigner怎么让一个模型跟着鼠标移动

【附源码】基于java的校园停车场管理系统的设计与实现61m0e9计算机毕设SSM_基于java技术的停车场管理系统实现与设计-程序员宅基地

文章浏览阅读178次。项目运行环境配置：Jdk1.8 + Tomcat7.0 + Mysql + HBuilderX（Webstorm也行）+ Eclispe（IntelliJ IDEA,Eclispe,MyEclispe,Sts都支持）。项目技术：Springboot + mybatis + Maven +mysql5.7或8.0+html+css+js等等组成，B/S模式 + Maven管理等等。环境需要1.运行环境：最好是java jdk 1.8，我们在这个平台上运行的。其他版本理论上也可以。_基于java技术的停车场管理系统实现与设计

Android系统播放器MediaPlayer源码分析_android多媒体播放源码分析时序图-程序员宅基地

文章浏览阅读3.5k次。前言对于MediaPlayer播放器的源码分析内容相对来说比较多，会从Java-&amp;gt;Jni-&amp;gt;C/C++慢慢分析，后面会慢慢更新。另外，博客只作为自己学习记录的一种方式，对于其他的不过多的评论。MediaPlayerDemopublic class MainActivity extends AppCompatActivity implements SurfaceHolder.Cal..._android多媒体播放源码分析时序图

java 数据结构与算法 ——快速排序法-程序员宅基地

文章浏览阅读2.4k次，点赞41次，收藏13次。java 数据结构与算法 ——快速排序法_快速排序法