Sprase Bundle Adjustment
推导
假设空间有
n
n
n个三维点在,在
m
m
m张图片(
m
m
m个相机位姿),且用
i
i
i表示空间点序号,用
j
j
j表示相机序号.令
x
i
j
\textbf{x}_{ij}
xij为第
i
i
i个点在第
j
j
j张图像上的投影.BA旨在预测精确的相机位姿和空间点坐标.每个相机
j
j
j位姿用
a
j
\textbf{a}_j
aj表示,每个三维点坐标
i
i
i用
b
i
\textbf{b}_i
bi表示.最小化重投影误差:
a
i
∗
,
b
j
∗
=
arg
min
a
j
,
b
i
∑
j
=
1
m
∑
i
=
1
n
d
(
Q
(
a
j
,
b
i
)
,
x
i
j
)
2
a_i^*,b_j^*=\mathop{\arg\min}_{a_j,b_i} \sum_{j=1}^m\sum_{i=1}^nd(Q(a_j,b_i),x_{ij})^2
ai∗,bj∗=argminaj,bij=1∑mi=1∑nd(Q(aj,bi),xij)2其中
Q
(
a
j
,
b
i
)
Q(a_j,b_i)
Q(aj,bi)表示路标点
b
i
b_i
bi在相机
a
j
a_j
aj下的投影(即为重投影),
d
(
x
,
y
)
d(x,y)
d(x,y)表示二者欧式距离.
向量
P
∈
R
M
P\in\Bbb{R}^M
P∈RM,代表所有
m
m
m个投影矩阵和
n
n
n个三维空间点的所有待优化参数:
P
=
(
a
1
T
,
.
.
.
,
a
m
T
,
b
1
T
,
.
.
.
,
b
n
T
)
T
,
P
∈
R
M
P=(a_1^T,...,a_m^T,b_1^T,...,b_n^T)^T,P\in\Bbb{R}^M
P=(a1T,...,amT,b1T,...,bnT)T,P∈RM向量
X
∈
R
N
X\in\Bbb{R}^N
X∈RN是所有照片内所有路标点的观测坐标
X
=
(
x
11
T
,
.
.
.
,
x
1
m
T
,
x
21
T
,
.
.
.
,
x
2
m
T
,
.
.
.
,
x
n
1
T
,
.
.
.
,
x
n
m
T
)
T
,
X
∈
R
N
X=(x_{11}^T,...,x_{1m}^T,x_{21}^T,...,x_{2m}^T,...,x_{n1}^T,...,x_{nm}^T)^T,X\in\Bbb{R}^N
X=(x11T,...,x1mT,x21T,...,x2mT,...,xn1T,...,xnmT)T,X∈RN其中
x
i
j
x_{ij}
xij是第i个空间点在相机j上的投影坐标,我们这里假设了所有目标点能在任何相机中都观测到,即
x
i
j
x_{ij}
xij都不为0,若没有该假设,
X
X
X向量存在全0行.
P
0
P_0
P0是初始参数估计值(给定初始三维空间点和相机内外参数(前端估计得到)),
∑
X
\sum_X
∑X表示观测向量
X
X
X的协方差矩阵(没有先验知识下,默认是单位矩阵,表示各个观测向量之间不相关),重投影
X
′
=
f
(
P
)
X'=f(P)
X′=f(P)表示为
X
′
=
(
x
′
11
T
,
.
.
.
,
x
′
1
m
T
,
x
′
21
T
,
.
.
.
,
x
′
2
m
T
,
.
.
.
,
x
′
n
1
T
,
.
.
.
,
x
′
n
m
T
)
T
,
X
′
∈
R
N
X'=({x'}_{11}^{T},...,{x'}_{1m}^T,{x'}_{21}^T,...,{x'}_{2m}^T,...,{x'}_{n1}^T,...,{x'}_{nm}^T)^T,X'\in\Bbb{R}^N
X′=(x′11T,...,x′1mT,x′21T,...,x′2mT,...,x′n1T,...,x′nmT)T,X′∈RN其中
x
′
i
j
=
Q
(
a
j
,
b
i
)
{x'}_{ij}=Q(a_j,b_i)
x′ij=Q(aj,bi)为
b
i
b_i
bi经过
a
j
a_j
aj的重投影坐标.
BA变为最小化
ϵ
T
∑
X
−
1
ϵ
\epsilon^T\sum_X^{-1}\epsilon
ϵT∑X−1ϵ,其中
ϵ
=
X
′
−
X
\epsilon=X'-X
ϵ=X′−X是关于
P
P
P的函数.使用LM非线性最小二乘算法迭代求解,在
μ
\mu
μ较大时转换为最速下降法,采用负梯度方向;在
μ
\mu
μ较小时,LM方法退化为GN方法,即求解下列方程:
J
T
Σ
X
−
1
J
δ
=
J
T
Σ
X
−
1
ϵ
J^T\Sigma_X^{-1}J\delta=J^T\Sigma_X^{-1}\epsilon
JTΣX−1Jδ=JTΣX−1ϵ
J
J
J是
f
(
⋅
)
f(·)
f(⋅)的雅克比矩阵,
δ
\delta
δ是步长,用于更新
P
P
P.
由于并非每个空间点和每个相机之间都具有联系(即每个空间点不一定在每一个图像中都出现),所以方程系数矩阵具有稀疏结构.
我们以一个较小的
m
m
m和
n
n
n举例说明,结果很容易推导至任意数量的目标和相机.
假设
n
=
4
n=4
n=4个空间点和
m
=
3
m=3
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
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
X=(x11T,x12T,,x13T,x21T,x22T,x23T,x31T,x32T,x33T,x41T,x42T,x43T)T
P
=
(
a
1
T
,
a
2
T
,
a
3
T
,
b
1
T
,
b
2
T
,
b
3
T
,
b
4
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
P=(a1T,a2T,a3T,b1T,b2T,b3T,b4T)T对非当前图像对应的相机位姿偏导数为0:
δ
x
′
i
j
δ
a
k
=
0
,
∀
j
̸
=
k
\frac{\delta {x'}_{ij}}{\delta a_k}=0,\forall j\not=k
δakδx′ij=0,∀j̸=k对投影不是当前二维坐标的目标点偏导数为0:
δ
x
′
i
j
δ
b
k
=
0
,
∀
i
̸
=
k
\frac{\delta {x'}_{ij}}{\delta b_k}=0,\forall i\not=k
δbkδx′ij=0,∀i̸=k并令
A
i
j
=
δ
x
′
i
j
δ
a
j
A_{ij}=\frac{\delta {x'}_{ij}}{\delta a_j}
Aij=δajδx′ij,
B
i
j
=
δ
x
′
i
j
δ
b
i
B_{ij}=\frac{\delta {x'}_{ij}}{\delta b_i}
Bij=δbiδx′ij.
迭代步长可以写为
δ
T
=
(
δ
a
T
,
δ
b
T
)
T
=
(
δ
a
1
T
,
δ
a
2
T
,
δ
a
3
T
,
δ
b
1
T
,
δ
b
2
T
,
δ
b
3
T
,
δ
b
4
T
)
T
\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
δT=(δaT,δbT)T=(δa1T,δa2T,δa3T,δb1T,δb2T,δb3T,δb4T)T其中
δ
b
\delta_b
δb可以直接相加,
δ
a
\delta_a
δa需要做乘
下面叙述如何充分利用这样的稀疏结构求解正规方程.
可以得到雅克比矩阵
J
J
J
∂
X
∂
P
=
(
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
)
=
(
A
∣
B
)
\frac{\partial X}{\partial P}=\begin{pmatrix} 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{pmatrix}=\begin{pmatrix} A|B \end{pmatrix}
∂P∂X=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛A1100A2100A3100A41000A1200A2200A3200A42000A1300A2300A3300A43B11B12B13000000000000B21B22B23000000000000B31B32B33000000000000B41B42B43⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞=(A∣B)
观测向量X的协方差矩阵是对角矩阵
Σ
X
=
d
i
a
g
(
σ
x
11
,
σ
x
12
,
σ
x
13
,
σ
x
21
,
σ
x
22
,
σ
x
23
,
σ
x
31
,
σ
x
32
,
σ
x
33
,
σ
x
41
,
σ
x
42
,
σ
x
43
)
\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})
ΣX=diag(σx11,σx12,σx13,σx21,σx22,σx23,σx31,σx32,σx33,σx41,σx42,σx43)带入迭代方程,方程左边为:
定义:
U
j
=
∑
i
=
1
4
A
i
j
T
Σ
x
i
j
−
1
A
i
j
U_j=\sum_{i=1}^4A_{ij}^T\Sigma_{x_{ij}}^{-1}A_{ij}
Uj=i=1∑4AijTΣxij−1Aij
V
i
=
∑
j
=
1
3
B
i
j
T
Σ
x
i
j
−
1
B
i
j
V_i=\sum_{j=1}^3B_{ij}^T\Sigma_{x_{ij}}^{-1}B_{ij}
Vi=j=1∑3BijTΣxij−1Bij
W
i
j
=
A
i
j
T
Σ
x
i
j
−
1
B
i
j
W_{ij}=A_{ij}^T\Sigma_{x_{ij}}^{-1}B_{ij}
Wij=AijTΣxij−1Bij上述矩阵变为
(
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
)
=
(
U
W
W
T
V
)
\begin{pmatrix} 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{pmatrix}=\begin{pmatrix} U&W\\ W^T&V \end{pmatrix}
⎝⎜⎜⎜⎜⎜⎜⎜⎜⎛U100W11TW21TW31TW41T0U20W12TW22TW32TW42T00U3W13TW23TW33TW43TW11W12W13V1000W21W22W230V200W31W32W3300V30W41W42W43000V4⎠⎟⎟⎟⎟⎟⎟⎟⎟⎞=(UWTWV)迭代公式右边,定义:
ϵ
a
j
=
∑
i
=
1
4
A
i
j
T
Σ
x
i
j
−
1
ϵ
i
j
\epsilon_{a_j}=\sum_{i=1}^4A_{ij}^T\Sigma_{x_{ij}}^{-1}\epsilon_{ij}
ϵaj=i=1∑4AijTΣxij−1ϵij
ϵ
b
i
=
∑
i
=
1
4
B
i
j
T
Σ
x
i
j
−
1
ϵ
i
j
\epsilon_{b_i}=\sum_{i=1}^4B_{ij}^T\Sigma_{x_{ij}}^{-1}\epsilon_{ij}
ϵbi=i=1∑4BijTΣxij−1ϵij
迭代方程简化为
(
U
W
W
T
V
)
(
δ
a
δ
b
)
=
(
ϵ
a
ϵ
b
)
\begin{pmatrix} U&W\\ W^T&V \end{pmatrix}\begin{pmatrix} \delta_a\\\delta_b \end{pmatrix}=\begin{pmatrix} \epsilon_a\\ \epsilon_b \end{pmatrix}
(UWTWV)(δaδb)=(ϵaϵb)两边左乘
(
I
−
W
V
−
1
0
I
)
\begin{pmatrix} I&-WV^{-1}\\ 0&I \end{pmatrix}
(I0−WV−1I)
结果为
(
U
−
W
V
−
1
W
T
0
W
T
V
)
(
δ
a
δ
b
)
=
(
ϵ
a
−
W
V
−
1
ϵ
b
ϵ
b
)
\begin{pmatrix} U-WV^{-1}W^T&0\\ W^T&V \end{pmatrix}\begin{pmatrix} \delta_a\\\delta_b \end{pmatrix}=\begin{pmatrix} \epsilon_a-WV^{-1}\epsilon_b\\ \epsilon_b \end{pmatrix}
(U−WV−1WTWT0V)(δaδb)=(ϵa−WV−1ϵbϵb)
即:
{
(
U
−
W
V
−
1
W
T
)
δ
a
=
ϵ
a
−
W
V
−
1
ϵ
b
W
T
δ
a
+
V
δ
b
=
ϵ
b
\begin{cases} (U-WV^{-1}W^T) \delta_a= \epsilon_a-WV^{-1}\epsilon_b\\ W^T\delta_a+V\delta_b=\epsilon_b \end{cases}
{(U−WV−1WT)δa=ϵa−WV−1ϵbWTδa+Vδb=ϵb其中第一式的
U
−
W
V
−
1
W
T
U-WV^{-1}W^T
U−WV−1WT 被称为舒尔补(Schur complement),通常会选用 Sparse Cholesky 进行求解该式。由第一式可计算
δ
a
\delta_a
δa,代入二式,可以求得
δ
b
\delta_b
δb:
δ
b
=
V
−
1
(
ϵ
b
−
W
T
δ
a
)
\delta_b=V^{-1}(\epsilon_b-W^T\delta_a)
δb=V−1(ϵb−WTδa)
所以,对于任意数量的目标点n和相机n,我们均可以求解LM的迭代步长.前面我们假设的是所有目标点在每一个图像中都出现了,如果点
k
k
k没有在照片
l
l
l中出现,则
x
k
l
=
0
x_{kl}=0
xkl=0,相应的
A
k
l
=
0
,
B
k
l
=
0
A_{kl}=0,B_{kl}=0
Akl=0,Bkl=0.这样,对某个固定的
j
j
j,其包含的求和项中出现的所有
i
i
i表示相机
j
j
j能观测到的所有路标点
i
i
i;在某个固定
i
i
i对应的求和项中出现的所有
j
j
j是能观测到路标点
i
i
i的所有相机
j
j
j.
总结:基于LM的BA算法流程
输入:
m
m
m个初始相机参数
a
j
,
j
=
1
,
…
,
m
a_j,j=1,…,m
aj,j=1,…,m;
n
n
n个初始三维物点坐标
b
i
,
i
=
1
,
…
,
n
b_i,i=1,…,n
bi,i=1,…,n,观测的特征点坐标
x
i
j
x_{ij}
xij(第
j
j
j张图第
i
i
i个点),LM算法的阻尼因子
μ
μ
μ
输出: 使用LM-BA得到的迭代步长
δ
\delta
δ
算法:(仅求解
δ
\delta
δ部分,需嵌入LM算法)
- 计算偏导数矩阵和残差向量:
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} - 计算辅助变量
U j : = ∑ i A i j T Σ x i j − 1 A i j , V i : = ∑ j B i j T Σ x i j − 1 B i j , W i j : = A i j T Σ x i j − 1 B i j 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} Uj:=i∑AijTΣxij−1Aij,Vi:=j∑BijTΣxij−1Bij,Wij:=AijTΣxij−1Bij ϵ a j : = ∑ i A i j T Σ x i j − 1 ϵ i j , ϵ b i : = ∑ j B i j T Σ x i j − 1 ϵ i j \epsilon_{a_j}:=\sum_{i}A_{ij}^T\Sigma_{x_{ij}}^{-1}\epsilon_{ij},\epsilon_{b_i}:=\sum_{j}B_{ij}^T\Sigma_{x_{ij}}^{-1}\epsilon_{ij} ϵaj:=i∑AijTΣxij−1ϵij,ϵbi:=j∑BijTΣxij−1ϵij - 将 U U U和 V V V的主对角线元素上加上阻尼因子 μ μ μ,得到 U ∗ U^* U∗和 V ∗ V^* V∗
- 计算 δ a , δ b \delta_a,\delta_b δa,δ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算法