一、BA优化

BA（Bundle Adjustment）优化，是指从视觉重建中提炼出最优的 3D3D3D 模型和相机参数(内参数和外参数)。从每一个特征点反射出来的几束光线(bundles of light rays),在我们把相机姿态和特征点空间位置做出最优的调整 (adjustment) 之后,最后收束到相机光心的这个过程。和重投影不同的是，BA优化是对多段相机的位姿和位姿下的路标点的空间坐标进行优化。

我们的误差可以写成：
e(ξ,p)=z−1sKTPe(\xi,p)=z-\frac 1 s KTP e(ξ,p)=z−s1KTP
即：
e(ξ,p)=z−1sK⋅exp⁡(ξ∧)⋅Pe(\xi,p)=z-\frac 1 s K\cdot\exp(\xi^\land)\cdot P e(ξ,p)=z−s1K⋅exp(ξ∧)⋅P
根据我之前写的文章：

SLAM–最小化重投影误差

我们可以得出误差eee分别对位姿和路标坐标的偏导JδξJ_{\delta \xi}Jδξ和JpJ_pJp：
Jδξ=∂e∂δξ=−[fxZ′0−fxX′Z′2−fxX′Y′Z′2fx+fxX′2Z′2−fxY′Z′0fyZ′−fyY′Z′2−fy−fyY′2Z′2fyY′X′Z′2fyX′Z′]J_{\delta \xi} =\frac{\partial \pmb e}{\partial \delta \xi}=-\begin{bmatrix} \frac {f_x}{Z'} & 0 &-\frac{f_xX'}{Z'^2} &-\frac{f_xX'Y'}{Z'^2} &f_x+ \frac{f_xX'^2}{Z'^2} & -\frac{f_xY'}{Z'} & \\ ~\\ 0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2} & -f_y-\frac{f_yY'^2}{Z'^2} & \frac{f_yY'X'}{Z'^2} & \frac{f_yX'}{Z'} & \\ \end{bmatrix} Jδξ=∂δξ∂eee=−⎣⎢⎡Z′fx 00Z′fy−Z′2fxX′−Z′2fyY′−Z′2fxX′Y′−fy−Z′2fyY′2fx+Z′2fxX′2Z′2fyY′X′−Z′fxY′Z′fyX′⎦⎥⎤

Jp=∂e∂P=−[fxZ′0−fxX′Z′20fyZ′−fyY′Z′2]⋅RJ_p=\frac {\partial \pmb e}{\partial \pmb P}=-\begin{bmatrix} \frac {f_x}{Z'}&0&-\frac{f_xX'}{Z'^2} \\ ~\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}\\ \end{bmatrix}\cdot \pmb R Jp=∂PPP∂eee=−⎣⎢⎡Z′fx 00Z′fy−Z′2fxX′−Z′2fyY′⎦⎥⎤⋅RRR

我们令位姿变量为ξ\xiξ，特征点空间位置为ppp，对于m个位姿和n个特征点：
x=[ξ1ξ2...ξm∣p1p2...pn]T\pmb x=[\xi_1 \quad \xi_2 ...\xi_m| \quad p_1 \quad p_2...p_n]^T xxx=[ξ1ξ2...ξm∣p1p2...pn]T

优化的目标函数为：
12∥f(x+Δx)∥2=12∑i=1m∑j=1n∥eij+FijΔξi+EijΔpj∥2\frac 1 2 \left\| f(x+\Delta x) \right\|^2=\frac 1 2\sum_{i=1}^m \sum_{j=1}^n\left\|e_{ij}+F_{ij}\Delta \xi_i+E_{ij}\Delta p_j \right\|^2 21∥f(x+Δx)∥2=21i=1∑mj=1∑n∥eij+FijΔξi+EijΔpj∥2
其中，FijF_{ij}Fij是相对姿态的偏导数，EijE_{ij}Eij是相对与路标点的偏导；

为了简明公式，我们令：
xc=[ξ1ξ2...ξm]xp=[p1p2...pn]x_c=[\xi_1 \quad \xi_2 \space...\space\xi_m]\\ x_p= [p_1 \quad p_2 \space...\space p_n]\\ xc=[ξ1ξ2 ... ξm]xp=[p1p2 ... pn]
我们的目标函数可以写成:
12∥f(x+Δx)∥2=12∥e+FΔxc+EΔxp∥2\frac 1 2 \left\| f(x+\Delta x) \right\|^2=\frac 1 2\left\|e+F\Delta x_c+E\Delta x_p \right\|^2 21∥f(x+Δx)∥2=21∥e+FΔxc+EΔxp∥2
对于线性增量方程：HΔx=gH\Delta x=gHΔx=g，这里H=JTJH=J^TJH=JTJ；

我们可以把雅克比矩阵分块：
J=[FE],JT=[FTET]J=[F \quad E],\quad J^T=\begin{bmatrix} F^T \\ \\E^T\end{bmatrix} J=[FE],JT=⎣⎡FTET⎦⎤
则：
H=JTJ=[FTFFTEETFETE]H=J^TJ=\begin{bmatrix} F^TF &&F^TE \\ \\E^TF && E^TE\end{bmatrix} H=JTJ=⎣⎡FTFETFFTEETE⎦⎤

二、利用稀疏性求解

JijJ_{ij}Jij表示在位姿ξi\xi_iξi只观测到pjp_jpj时的雅克比矩阵，所以其他部分的导数为零。
Jij(x)=[0,0,...,(∂eij∂δξi)2×6,...,02×6，∣02×3，...,(∂eij∂pi)2×3]\large {J_{ij}(x)=\begin{bmatrix}0, &0,...,(\frac{\partial \pmb e_{ij}}{\partial \delta \xi_i}) _{2\times 6},...,0_{2 \times 6}，|0_{2\times 3}，...,(\frac{\partial \pmb e_{ij}}{\partial p_i}) _{2\times 3} \end{bmatrix}} Jij(x)=[0,0,...,(∂δξi∂eeeij)2×6,...,02×6，∣02×3，...,(∂pi∂eeeij)2×3]
所以对于整体：
H=∑ijJijTJijH=\sum_{ij}J_{ij}^TJ_{ij} H=ij∑JijTJij
由此可以把所有的JijJ_{ij}Jij放到一个JJJ表达式里：
J=[J11J12⋮Ji1Ji2⋮Jij]J=\begin{bmatrix} J_{11}\\J_{12}\\ \vdots \\J_{i1}\\J_{i2}\\ \vdots\\J_{ij} \end{bmatrix} J=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡J11J12⋮Ji1Ji2⋮Jij⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
又有稀疏性和对称性，我们把HHH分块化：
H=[BEETC]H=\begin{bmatrix} B &E\\E^T &C \end{bmatrix} H=[BETEC]
由HΔx=gH\Delta x=gHΔx=g可变为：
[BEETC][ΔxcΔxp]=[vw]\begin{bmatrix} B &E\\E^T &C \end{bmatrix} \begin{bmatrix} \Delta x_c \\\Delta x_p \end{bmatrix} = \begin{bmatrix} v\\w\end{bmatrix} [BETEC][ΔxcΔxp]=[vw]
利用稀疏性进行高斯消元：
[I−EC−10I][BEETC][ΔxcΔxp]=[I−EC−10I][vw]⟹[B−EC−1ET0ETC][ΔxcΔxp]=[v−EC−1ww]\begin{bmatrix} I & -EC^{-1}\\0 &I\end{bmatrix}\begin{bmatrix} B &E\\E^T &C \end{bmatrix} \begin{bmatrix} \Delta x_c \\\Delta x_p \end{bmatrix} = \begin{bmatrix} I & -EC^{-1}\\0 &I\end{bmatrix}\begin{bmatrix} v\\w\end{bmatrix}\\~\\~ \\ \Longrightarrow \begin{bmatrix} B-EC^{-1}E^T &0\\E^T &C \end{bmatrix} \begin{bmatrix} \Delta x_c \\\Delta x_p \end{bmatrix} = \begin{bmatrix} v-EC^{-1}w\\w\end{bmatrix} [I0−EC−1I][BETEC][ΔxcΔxp]=[I0−EC−1I][vw] ⟹[B−EC−1ETET0C][ΔxcΔxp]=[v−EC−1ww]
整理得：
(B−EC−1ET)Δxc=v−EC−1w(B-EC^{-1}E^T)\Delta x_c =v-EC^{-1}w (B−EC−1ET)Δxc=v−EC−1w
继而可以求出：
∆xp=C−1(w−ET∆xc)∆x_p = C^{−1} (w − E^T ∆x_c ) ∆xp=C−1(w−ET∆xc)

SLAM--BA优化相关推荐

SLAM中的BA优化
SLAM中的BA优化 SLAM中的BA优化,先根据相机模型和A,B图像特征匹配好的像素坐标,求出A图像上的像素坐标对应的归一化的空间点坐标,然后根据该空间点的坐标计算重投影到B图像上的像素坐标,重投影 ...
SLAM后端：BA优化（Bundle Adjustment）
BA是一种优化方法,最小化重投影误差优化PnP得到的R.t 应用场景:利用共视点的3D坐标与相机内参矩阵,根据PnP计算位姿R.t(有误差).利用R.t及相机的投影模型可以计算出这些特征点在第二帧图像 ...
SLAM图优化g2o
SLAM图优化g2o 图优化g2o框架图优化的英文是 graph optimization 或者 graph-based optimization, "图"其实是数据结构中的gr ...
SLAM后端优化之-核函数
1.核函数作用:保证每条边的误差不会大的没边,掩盖掉其他的边在SLAM后端优化中,BA优化了所有的相机姿态和所有路标点,使用的最小化误差项作的二范数平方和作为目标函数:当我们的误差来源特别大的时候: ...
(01)ORB-SLAM2源码无死角解析-(63) BA优化(g2o)→局部建图线程:Optimizer::LocalBundleAdjustment→位姿与地图点优化
讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (0 ...
如何用ceres进行两帧之间的BA优化
学习高博的书已有很长一段时间了,一直看理论,看代码,而没有自己亲自上手,最近在做BA优化,大部分SLAM是用g2o进行的,而对于ceres用的很少,由于博主根本看不懂g2o的代码风格,个人觉得很无语( ...
SfM三维点云重建：BA优化--【VS2015+OpenCV3.4+PCL1.8】
前言在SfM多视图三维点云重建–[VS2015+OpenCV3.4+PCL1.8]中实现的增量式SfM三维点云重建,会随着图片数量的增加而导致误差逐渐累积,最后可能无法完成重建.在三维重建中常使用B ...
激光SLAM后端优化——雅克比矩阵推导
激光SLAM后端优化--雅克比矩阵推导 Jacobi Matrix Jacobi Matrix In the EKF system, the maintained state quantities i ...
ORB-SLAM2学习笔记——全局BA优化
ORB-SLAM2学习笔记--全局BA优化 1.理论部分(待更新) 2.代码详解 void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIter ...
激光SLAM后端优化总结之卡尔曼滤波
激光SLAM后端优化总结之卡尔曼滤波一.贝叶斯滤波 1.1 状态估计模型 1.2 公式推导二.卡尔曼滤波(KF) 三.扩展卡尔曼滤波(EKF) 一.贝叶斯滤波 1.1 状态估计模型 1.2 公式推 ...

SLAM--BA优化

目录

一、BA优化

二、利用稀疏性求解

SLAM--BA优化相关推荐

最新文章

热门文章