非线性最小二乘， BA(Bundle Adjustment)

在看高翔博士的《视觉slam十四讲》，把看到的BA相关的东西记录一下，方便自己之后复习，有理解不对的地方，还请指正！

一、非线性最小二乘

1.1 总体思路，一阶法和二阶法

最小二乘问题，我们称F(x)是代价函数， f(x)是误差函数；

直接求再比较各个极值点/鞍点的大小一般是比较难的。因此，一般采取迭代的方法：即从某个初值x0开始，每一次寻找一个可以使F(x)取得极小值的增量delta_x，当增量delta_x比较小的时候，我们就认为认为达到了极值。

一阶法寻找delta_x：将F(x)做一阶泰勒展开：，并取；这样一定能够保证代价函数值下降，一阶法也叫最速下降法；

二阶法寻找delta_x：将F(x)做二阶泰勒展开：且令，可得, 求解这个线性方程，就可以得到每一步的delta_x，二阶法也叫做牛顿法； H矩阵不好求，所以经常用高斯牛顿法等近似方法；

1.2 高斯牛顿法

对误差f(x)做泰勒展开：, 每一次迭代时，优化问题变为：

此时，求增量delta_x，可以直接令，得到，求解这个线性方程，就可以得到每一步的delta_x。这里可以认为，高斯牛顿法用误差函数的Jacobian相乘，近似了牛顿法中的代价函数的Hessian

高斯法的不一定收敛，主要原因是：1）不一定有逆矩阵， delta_x不太稳定；2）求出的delta_x太大，导致一阶泰勒近似失效；

1.3 LM法（Levenberg-Marquart）

LM法在一定程度的缓解了高斯牛顿法的问题，能够提供更加稳定和准确的增量delta_x：1）给增量delta_x增加一个信赖区间，变成一个带约束的优化问题；2）定义一个指标 p 刻画误差函数一阶近似的好坏程度，并据此动态改变信赖区间的大小。LM具体算法是：

a. 给定初始值x0, 初始优化半径u;

b. 对于每一次迭代，在高斯牛顿法的基础上增加信赖区间，求解：

c. 计算误差函数一阶近似的好坏程度：

d. 如果p太小，说明当前位置梯度太大，信赖区间需要保守一些，则设置： u = 0.5u

e. 如果p太大，说明当前位置梯度比较小，信赖区间可以激进一些，则设置： u = 2u

f. 如果p大于某个阈值，则可以认为近似可行，可以进行下一次迭代: x_k+1 = x_k+delta_x

g. 判断算法是否收敛；如果没有收敛，则用 f 中计算得到的x_k+1作为初值，返回b开始新一轮迭代

b中带约束的优化问题可以用拉格朗日乘数法求解上述 (学过SVM的同学请举手~~)：

其核心仍然是解线性方程：，当λ较小时，说明二阶近似比较好，基本等效于高斯牛顿法，当λ比较大的时候，说明二阶近似不好，基本等效于一阶法（最速下降法）

二、特征点法的BA(Bundle Adjustment，光束法平差)

2.1 误差函数和代价函数

特征点法认为路标点在图片中投影的像素坐标已知，因此可以最小化重投影误差，来求解相机位姿和路标点位置，具体是：

定义自变量x包含了所有带求解的多帧位姿和多个路标点，位姿T在前，路标点p在后；

定义第j个路标在第i帧的误差函数是：; 其中代表重投影误差，代表第i帧相机位姿；代表第j个路标的位置；代表第j个路标在第i帧中投影的像素坐标；

代价函数可以定义为多个路标在多帧中的重投影误差的平方: 。我们要求解的最优化问题定义为：，即求取一组位姿和路标点，使得代价函数最小；

我们一般用迭代法求解上述最优化问题，因此我们实际关心的是求得每一步迭代时的增量delta_x, 即：，其中，是位姿的增量，是地图点的增量；

2.2 获得每次迭代增量delta_x的线性方程

我们对误差函数进行一阶近似：

其中: , 是每个误差函数对于x的jacobian;

令 , 得到关于增量delta_x的线性方程：

取就是高斯法，取就是LM法；

J_ij只与第i帧和第j个路标点有关，只在ii, ij, ji, jj四块处取值；累加而成的H矩阵因此具有如下的稀疏结构：

B块只与相机位姿有关，是对角阵； C块只与路标点有关，是对角阵； E块非零的地方表示对应的相机位置有对对应路标点的观测；

2.3 边缘化（Schur消元）

增量方程可利用H矩阵的特点快速求解：

对增量方程进行划分：，并消去地图点增量：

, 整理得到：

可以得到只有位姿的方程：，C是对角矩阵，C的逆比较好求，这个方程比较好解；

再解出路标增量：，同样利用了C的逆比较好求；

以上消元的方法，先固定了路标点增量，求出位姿增量，再求出路标点的增量，从概率论的角度说先求了固定路标点下的条件概率，再求路标点的边缘概率，因此称为边缘化；

SLAM中的marginalization 和 Schur complement_白巧克力亦唯心的博客-CSDN博客_schur消元

非线性最小二乘， BA(Bundle Adjustment)相关推荐

Bundle Adjustment (BA) in vSLAM or SFM
文章目录 Overview BA as a NonLinear Least Squares Problem Solved with LM JTJ\mathbf{J}^T \mathbf{J}JTJ s ...
SLAM专题（10）- 最小化重投影误差与Bundle Adjustment （BA）
在SFM(structure from motion)的计算中BA(Bundle Adjustment)作为最后一步优化具有很重要的作用,在近几年兴起的基于图的SLAM(simultaneous lo ...
Bundle Adjustment （BA）简述
笔者到底想讲些啥? 在SFM(structure from motion)的计算中BA(Bundle Adjustment)作为最后一步优化具有很重要的作用,在近几年兴起的基于图的SLAM(simul ...
关于sba（sparse bundle adjustment）的30个常见问题
Q1 -- 什么是sba? sba是一个C/C++软件包对广义稀疏光束平差,在GNU公共许可证下分发.sba是通用的,提供关于定义涉及光束法平差的图像投影的参数选择和函数关系增强的灵活性. Q2 -- ...
ORB_SLAM2代码阅读(5)——Bundle Adjustment
ORB_SLAM2代码阅读(5)--Bundle Adjustment 1. 说明 2. Bundle Adjustment(BA)的物理意义 3. BA的数学表达 4. BA的求解方法 4.1 最速 ...
SLAM笔记（五）光束平差法(Bundle Adjustment)
1.什么是光束平差法前边的八点法,五点法等可以求出闭式解的前提是已经知道确切的点对.但实际情况中往往存在大量的噪声,点与点不是精确地对应甚至出现一些错误匹配. 光束平差法由Bundle Adjus ...
SLAM算法＆技术之Gauss-Newton非线性最小二乘算法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达编辑丨点云PCL 前言很多问题最终归结为一个最小二乘问题,如SLAM算法中的Bundle Adjus ...
矩阵求逆c语言实现_[V-SLAM] Bundle Adjustment 实现
SLAM问题的后端有主要有滤波和优化两种方案.目前,普遍认为优化的方法在精度上要超过滤波方法,因为它可以进行多次的线性化.近年来出现的SLAM算法也大都是基于优化的算法(如ORB-SLAM.DSO等) ...
OpenCV实现SfM（四）：Bundle Adjustment
http://blog.csdn.net/AIchipmunk/article/details/52433884 OpenCV实现SfM(四):Bundle Adjustment 标签: opencv ...

非线性最小二乘， BA(Bundle Adjustment)

非线性最小二乘， BA(Bundle Adjustment)相关推荐

最新文章

热门文章