Sprase Bundle-Adjustment

Sprase Bundle Adjustment

推导

假设空间有$n$个三维点在,在$m$张图片($m$个相机位姿),且用$i$表示空间点序号,用$j$表示相机序号.令${\text{x}}_{ij}$为第$i$个点在第$j$张图像上的投影.BA旨在预测精确的相机位姿和空间点坐标.每个相机$j$位姿用${\text{a}}_j$表示,每个三维点坐标$i$用${\text{b}}_i$表示.最小化重投影误差:
$$a_i^{\ast },b_j^{\ast }={\mathrm{argmin}}_{a_j,b_i}\sum _{j=1}^m\sum _{i=1}^{n}d(Q(a_j,b_i),x_{ij}{)}^2$$其中$Q(a_j,b_i)$表示路标点$b_i$在相机$a_j$下的投影(即为重投影),$d(x,y)$表示二者欧式距离.
向量$P\in {\mathbb{R}}^M$,代表所有$m$个投影矩阵和$n$个三维空间点的所有待优化参数:
$$P=(a_1^T,...,a_m^T,b_1^T,...,b_n^T{)}^T,P\in {\mathbb{R}}^M$$向量$X\in {\mathbb{R}}^N$是所有照片内所有路标点的观测坐标
$$X=(x_{11}^T,...,x_{1m}^T,x_{21}^T,...,x_{2m}^T,...,x_{n1}^T,...,x_{nm}^T{)}^T,X\in {\mathbb{R}}^N$$其中$x_{ij}$是第i个空间点在相机j上的投影坐标,我们这里假设了所有目标点能在任何相机中都观测到,即$x_{ij}$都不为0,若没有该假设,$X$向量存在全0行.$P_0$是初始参数估计值(给定初始三维空间点和相机内外参数(前端估计得到)),${\sum }_X$表示观测向量$X$的协方差矩阵(没有先验知识下,默认是单位矩阵,表示各个观测向量之间不相关),重投影$X^{\prime }=f(P)$表示为
$$X^{\prime }=({x^{\prime }}_{11}^T,...,{x^{\prime }}_{1m}^T,{x^{\prime }}_{21}^T,...,{x^{\prime }}_{2m}^T,...,{x^{\prime }}_{n1}^T,...,{x^{\prime }}_{nm}^T{)}^T,X^{\prime }\in {\mathbb{R}}^N$$其中${x^{\prime }}_{ij}=Q(a_j,b_i)$为$b_i$经过$a_j$的重投影坐标.
BA变为最小化${ϵ}^T{\sum }_X^{-1}ϵ$,其中$ϵ=X^{\prime }-X$是关于$P$的函数.使用LM非线性最小二乘算法迭代求解,在$\mu$较大时转换为最速下降法,采用负梯度方向;在$\mu$较小时,LM方法退化为GN方法,即求解下列方程:
$$J^T{\Sigma }_X^{-1}J\delta =J^T{\Sigma }_X^{-1}ϵ$$$J$是$f(\cdot )$的雅克比矩阵,$\delta$是步长,用于更新$P$.
由于并非每个空间点和每个相机之间都具有联系(即每个空间点不一定在每一个图像中都出现),所以方程系数矩阵具有稀疏结构.
我们以一个较小的$m$和$n$举例说明,结果很容易推导至任意数量的目标和相机.
假设$n=4$个空间点和$m=3$个相机视角.观测值向量变成$$X=(x_{11}^T,x_{12}^T,,x_{13}^T,x_{21}^T,x_{22}^T,x_{23}^T,x_{31}^T,x_{32}^T,x_{33}^T,x_{41}^T,x_{42}^T,x_{43}^T{)}^T$$$$P=(a_1^T,a_2^T,a_3^T,b_1^T,b_2^T,b_3^T,b_4^T{)}^T$$对非当前图像对应的相机位姿偏导数为0:
$$\frac{\delta {x^{\prime }}_{ij}}{\delta a_k}=0,\forall j̸=k$$对投影不是当前二维坐标的目标点偏导数为0:
$$\frac{\delta {x^{\prime }}_{ij}}{\delta b_k}=0,\forall i̸=k$$并令$A_{ij}=\frac{\delta {x^{\prime }}_{ij}}{\delta a_j}$,$B_{ij}=\frac{\delta {x^{\prime }}_{ij}}{\delta b_i}$.
迭代步长可以写为$${\delta }^T=({\delta }_a^T,{\delta }_b^T{)}^T=({\delta }_{a1}^T,{\delta }_{a2}^T,{\delta }_{a3}^T,{\delta }_{b1}^T,{\delta }_{b2}^T,{\delta }_{b3}^T,{\delta }_{b4}^T{)}^T$$其中${\delta }_b$可以直接相加,${\delta }_a$需要做乘
下面叙述如何充分利用这样的稀疏结构求解正规方程.
可以得到雅克比矩阵$J$
$$\frac{\partial X}{\partial P}=\left(\begin{array}{ccccccc}{A}_{11}& 0& 0& B_{11}& 0& 0& 0\\ 0& A_{12}& 0& B_{12}& 0& 0& 0\\ 0& 0& A_{13}& B_{13}& 0& 0& 0\\ A_{21}& 0& 0& 0& B_{21}& 0& 0\\ 0& A_{22}& 0& 0& B_{22}& 0& 0\\ 0& 0& A_{23}& 0& B_{23}& 0& 0\\ A_{31}& 0& 0& 0& 0& B_{31}& 0\\ 0& A_{32}& 0& 0& 0& B_{32}& 0\\ 0& 0& A_{33}& 0& 0& B_{33}& 0\\ A_{41}& 0& 0& 0& 0& 0& B_{41}\\ 0& A_{42}& 0& 0& 0& 0& B_{42}\\ 0& 0& A_{43}& 0& 0& 0& B_{43}\end{array}\right)=\left(\begin{array}{c}A|B\end{array}\right)$$

观测向量X的协方差矩阵是对角矩阵
$${\Sigma }_X=diag({\sigma }_{x11},{\sigma }_{x12},{\sigma }_{x13},{\sigma }_{x21},{\sigma }_{x22},{\sigma }_{x23},{\sigma }_{x31},{\sigma }_{x32},{\sigma }_{x33},{\sigma }_{x41},{\sigma }_{x42},{\sigma }_{x43})$$带入迭代方程,方程左边为:

定义:$$U_j=\sum _{i=1}^4{A}_{ij}^T{\Sigma }_{x_{ij}}^{-1}{A}_{ij}$$$$V_i=\sum _{j=1}^3{B}_{ij}^T{\Sigma }_{x_{ij}}^{-1}{B}_{ij}$$$$W_{ij}=A_{ij}^T{\Sigma }_{x_{ij}}^{-1}{B}_{ij}$$上述矩阵变为
$$\left(\begin{array}{ccccccc}{U}_1& 0& 0& W_{11}& W_{21}& W_{31}& W_{41}\\ 0& U_2& 0& W_{12}& W_{22}& W_{32}& W_{42}\\ 0& 0& U_3& W_{13}& W_{23}& W_{33}& W_{43}\\ W_{11}^T& W_{12}^T& W_{13}^T& V_1& 0& 0& 0\\ W_{21}^T& W_{22}^T& W_{23}^T& 0& V_2& 0& 0\\ W_{31}^T& W_{32}^T& W_{33}^T& 0& 0& V_3& 0\\ W_{41}^T& W_{42}^T& W_{43}^T& 0& 0& 0& V_4\end{array}\right)=\left(\begin{array}{cc}U& W\\ W^T& V\end{array}\right)$$迭代公式右边,定义:
$${ϵ}_{a_j}=\sum _{i=1}^4{A}_{ij}^T{\Sigma }_{x_{ij}}^{-1}{ϵ}_{ij}$$$${ϵ}_{b_i}=\sum _{i=1}^4{B}_{ij}^T{\Sigma }_{x_{ij}}^{-1}{ϵ}_{ij}$$
迭代方程简化为
$$\left(\begin{array}{cc}U& W\\ W^T& V\end{array}\right)\left(\begin{array}{c}{\delta }_a\\ {\delta }_b\end{array}\right)=\left(\begin{array}{c}{ϵ}_a\\ {ϵ}_b\end{array}\right)$$两边左乘
$$\left(\begin{array}{cc}I& -W{V}^{-1}\\ 0& I\end{array}\right)$$
结果为
$$\left(\begin{array}{cc}U-W{V}^{-1}{W}^T& 0\\ W^T& V\end{array}\right)\left(\begin{array}{c}{\delta }_a\\ {\delta }_b\end{array}\right)=\left(\begin{array}{c}{ϵ}_a-W{V}^{-1}{ϵ}_b\\ {ϵ}_b\end{array}\right)$$
即:
$$\{\begin{array}{l}(U-W{V}^{-1}{W}^T){\delta }_a={ϵ}_a-W{V}^{-1}{ϵ}_b\\ W^T{\delta }_a+V{\delta }_b={ϵ}_b\end{array}$$其中第一式的$U-W{V}^{-1}{W}^T$ 被称为舒尔补（Schur complement）,通常会选用 Sparse Cholesky 进行求解该式。由第一式可计算${\delta }_a$,代入二式,可以求得${\delta }_b$:
$${\delta }_b=V^{-1}({ϵ}_b-W^T{\delta }_a)$$
所以,对于任意数量的目标点n和相机n,我们均可以求解LM的迭代步长.前面我们假设的是所有目标点在每一个图像中都出现了,如果点$k$没有在照片$l$中出现,则$x_{kl}=0$,相应的$A_{kl}=0,B_{kl}=0$.这样,对某个固定的$j$,其包含的求和项中出现的所有$i$表示相机$j$能观测到的所有路标点$i$;在某个固定$i$对应的求和项中出现的所有$j$是能观测到路标点$i$的所有相机$j$.

总结:基于LM的BA算法流程

输入: $m$个初始相机参数$a_j，j=1,\dots ,m$；$n$个初始三维物点坐标$b_i，i=1,\dots ,n$，观测的特征点坐标$x_{ij}$(第$j$张图第$i$个点)，LM算法的阻尼因子$\mu$
输出: 使用LM-BA得到的迭代步长$\delta$
算法:(仅求解$\delta$部分,需嵌入LM算法)

1. 计算偏导数矩阵和残差向量:
   A i j : = ∂ x ′ i j ∂ a j = ∂ Q ( a j , b i ) ∂ a j , B i j : = ∂ x ′ i j ∂ b i = ∂ Q ( a j , b i ) ∂ b i , ϵ i j = x i j − x i j ′ . j ∈ { 1 , … , m } , i ∈ { 1 , . . . , n } A_{ij}:=\frac{\partial {x'}_{ij}}{\partial a_j}=\frac{\partial Q(a_j,b_i)}{\partial a_j},B_{ij}:=\frac{\partial {x'}_{ij}}{\partial b_i}=\frac{\partial Q(a_j,b_i)}{\partial b_i},\epsilon_{ij}=x_{ij}-x'_{ij}. j\in\{1,…,m\},i\in\{1,...,n\} Aij​:=∂aj​∂x′ij​​=∂aj​∂Q(aj​,bi​)​,Bij​:=∂bi​∂x′ij​​=∂bi​∂Q(aj​,bi​)​,ϵij​=xij​−xij′​.j∈{1,…,m},i∈{1,...,n}
2. 计算辅助变量
   $$U_j:=\sum _i{A}_{ij}^T{\Sigma }_{x_{ij}}^{-1}{A}_{ij},V_i:=\sum _j{B}_{ij}^T{\Sigma }_{x_{ij}}^{-1}{B}_{ij},W_{ij}:=A_{ij}^T{\Sigma }_{x_{ij}}^{-1}{B}_{ij}$$$${ϵ}_{a_j}:=\sum _i{A}_{ij}^T{\Sigma }_{x_{ij}}^{-1}{ϵ}_{ij},{ϵ}_{b_i}:=\sum _j{B}_{ij}^T{\Sigma }_{x_{ij}}^{-1}{ϵ}_{ij}$$
3. 将$U$和$V$的主对角线元素上加上阻尼因子$\mu$，得到$U^{\ast }$和$V^{\ast }$
4. 计算${\delta }_a,{\delta }_b$,构成$\delta$

抽空自己实现一遍!!!

参考资料

译自
The Design and Implementation of a Generic Sparse Bundle Adjustment Software Package Based on the Levenberg-Marquardt Algorithm
Bundle Adjustment简述
Eigen实战:BA
Bundle Adjustment算法学习
LM算法