首先,明确在系统初始化阶段可以利用的已知量,以及待求解的未知量。
已知:1.各帧相对于第1帧的位姿
q
c
1
c
i
,
p
c
1
c
i
q_{c_1c_i},p_{c_1c_i}
qc1ci,pc1ci;(根据SfM求得)
2.各相邻帧之间的位姿变化
q
b
k
b
k
+
1
,
p
b
k
b
k
+
1
q_{b_kb_{k+1}},p_{b_kb_{k+1}}
qbkbk+1,pbkbk+1;(根据IMU的预积分求得)
待求解:1.相机和IMU之间的外参
q
b
c
,
p
b
c
q_{bc},p_{bc}
qbc,pbc;
2.传感器的偏置
b
b
b;
3.初始速度
v
v
v,尺度
s
s
s,重力矢量
g
c
1
g^{c_1}
gc1;
4.计算初始时刻IMU系相对于世界坐标系的位姿
q
w
b
1
q_{wb_1}
qwb1,。
1.相机和IMU之间的外参
几何约束:
对于任意相邻两个时刻,
t
k
+
1
t_{k+1}
tk+1时刻的相机坐标系在
t
k
t_k
tk时刻的IMU系下的表示,都有两种表示路径(如图蓝线和绿线)即:
q
b
k
c
k
+
1
=
q
b
k
c
k
⊗
q
c
k
c
k
+
1
=
q
b
k
b
k
+
1
⊗
q
b
k
+
1
c
k
+
1
q_{b_kc_{k+1}}=q_{b_kc_{k}}\otimes q_{c_{k}c_{k+1}}=q_{b_kb_{k+1}}\otimes q_{b_{k+1}c_{k+1}}
qbkck+1=qbkck⊗qckck+1=qbkbk+1⊗qbk+1ck+1
【注意】相机和IMU之间的外参
q
b
c
q_{bc}
qbc 可以认为是不变化的!有:
q
b
c
⊗
q
c
k
c
k
+
1
=
q
b
k
b
k
+
1
⊗
q
b
c
[
q
c
k
c
k
+
1
]
R
q
b
c
=
[
q
b
k
b
k
+
1
]
L
q
b
c
(
[
q
c
k
c
k
+
1
]
R
−
[
q
b
k
b
k
+
1
]
L
)
q
b
c
=
0
Q
k
+
1
k
q
b
c
=
0
\begin{aligned} q_{bc}\otimes q_{c_{k}c_{k+1}} &= q_{b_kb_{k+1}}\otimes q_{bc} \\ [ q_{c_{k}c_{k+1}}]_R q_{bc} &= [q_{b_kb_{k+1}}]_L q_{bc} \\ ([ q_{c_{k}c_{k+1}}]_R - [q_{b_kb_{k+1}}]_L) q_{bc} &= 0 \\ Q^k_{k+1} q_{bc} &= 0 \\ \end{aligned}
qbc⊗qckck+1[qckck+1]Rqbc([qckck+1]R−[qbkbk+1]L)qbcQk+1kqbc=qbkbk+1⊗qbc=[qbkbk+1]Lqbc=0=0
q c k c k + 1 , q b k b k + 1 q_{c_{k}c_{k+1}}, q_{b_kb_{k+1}} qckck+1,qbkbk+1是已知量,则 Q k + 1 k Q^k_{k+1} Qk+1k可计算得到。
将多个时刻的线性方程累积起来,得到:
[
Q
1
0
Q
2
1
⋮
Q
N
N
−
1
]
q
b
c
=
Q
N
⋅
q
b
c
=
0
\begin{bmatrix} Q^0_1 \\ Q^1_2\\ \vdots \\Q^{N-1}_{N} \end{bmatrix} q_{bc} =Q_N\cdot q_{bc}= 0
⎣⎢⎢⎢⎡Q10Q21⋮QNN−1⎦⎥⎥⎥⎤qbc=QN⋅qbc=0
如果加上鲁棒权重则有:
[
w
1
0
⋅
Q
1
0
w
2
1
⋅
Q
2
1
⋮
w
N
N
−
1
⋅
Q
N
N
−
1
]
q
b
c
=
Q
N
⋅
q
b
c
=
0
\begin{bmatrix} w^0_1\cdot Q^0_1 \\ w^1_2\cdot Q^1_2\\ \vdots \\ w^{N-1}_N\cdot Q^{N-1}_{N} \end{bmatrix} q_{bc} =Q_N\cdot q_{bc}= 0
⎣⎢⎢⎢⎡w10⋅Q10w21⋅Q21⋮wNN−1⋅QNN−1⎦⎥⎥⎥⎤qbc=QN⋅qbc=0
而权重计算的依据为两条路径的接近程度:即
Δ
R
≜
R
^
b
c
−
1
R
b
k
b
k
+
1
−
1
R
^
b
c
R
c
k
c
k
+
1
\Delta R \triangleq \hat{R}^{-1}_{bc}R^{-1}_{b_kb_{k+1}}\hat{R}_{bc}R_{c_kc_{k+1}}
ΔR≜R^bc−1Rbkbk+1−1R^bcRckck+1与单位阵
I
I
I的接近程度。
这里使用旋转轴与旋转角来衡量(单位阵对应的旋转角为0,如果
Δ
R
\Delta R
ΔR对应的旋转角越大,认为越不可靠,将其对应的项非常小的权重),即:
r
k
+
1
k
=
arccos
(
t
r
(
Δ
R
)
−
1
)
/
2
w
k
+
1
k
=
{
1
,
r
k
+
1
k
<
t
h
r
e
s
h
o
l
d
t
h
r
e
s
h
o
l
d
r
k
+
1
k
,
o
t
h
e
r
w
i
s
e
\begin{aligned} r^k_{k+1} &= \arccos (tr(\Delta R)-1)/2 \\ w^k_{k+1} &= \begin{cases} 1 &, r^k_{k+1}<threshold \\ \frac{threshold}{r^k_{k+1}} &, otherwise \end{cases} \end{aligned}
rk+1kwk+1k=arccos(tr(ΔR)−1)/2={1rk+1kthreshold,rk+1k<threshold,otherwise
至此,求解 Q N Q_N QN进行SVD分解后最小奇异值对应的奇异向量,即可求出 q b c q_{bc} qbc。
2.传感器的偏置 b b b
几何约束:
当
q
b
c
q_{bc}
qbc已知,
q
b
k
b
k
+
1
q_{b_kb_{k+1}}
qbkbk+1既可以由IMU预积分得到,又可以通过相邻两帧相机位姿结合(IMU及相机之间的外参)得到,即:
q
b
k
b
k
+
1
=
q
c
1
b
k
−
1
⊗
q
c
1
b
k
+
1
q_{b_kb_{k+1}}=q^{-1}_{c_1b_k}\otimes q_{c_1b_{k+1}}
qbkbk+1=qc1bk−1⊗qc1bk+1
但是,由于传感器偏置的存在,二者不会完全相等!优化如下目标函数:
min
δ
b
g
∥
⌊
q
c
1
b
k
+
1
−
1
⊗
q
c
1
b
k
⊗
q
b
k
b
k
+
1
⌋
x
y
z
∥
2
\min_{\delta b_g}\| \lfloor q^{-1}_{c_1b_{k+1}}\otimes q_{c_1b_k} \otimes q_{b_kb_{k+1}} \rfloor _{xyz}\|^2
δbgmin∥⌊qc1bk+1−1⊗qc1bk⊗qbkbk+1⌋xyz∥2
迭代过程中需要求残差项关于
δ
b
g
\delta b_g
δbg的雅可比矩阵(实际上仅
q
b
k
b
k
+
1
q_{b_kb_{k+1}}
qbkbk+1与之有关),即:
q
b
k
b
k
+
1
≈
q
^
b
k
b
k
+
1
⊗
[
1
1
2
J
b
g
q
δ
b
g
]
q_{b_kb_{k+1}} \approx \hat q_{b_kb_{k+1}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}J^{q}_{b_g} \delta b_g \end{bmatrix}
qbkbk+1≈q^bkbk+1⊗[121Jbgqδbg]
求解最小二乘问题,只需完成两件事!1.明确残差函数的雅可比;2.构造Hessian矩阵。
3.初始速度 v v v,尺度 s s s,重力矢量 g c 1 g^{c_1} gc1
几何约束:位移矢量将满足
(1)
s
p
ˉ
c
1
b
k
=
s
p
ˉ
c
1
c
k
−
R
c
1
b
k
p
b
c
s\bar p_{{c_1}b_k}=s\bar p_{{c_1}c_k}-R_{c_1b_k}p_{bc} \tag{1}
spˉc1bk=spˉc1ck−Rc1bkpbc(1)
IMU预积分的约束(只需将之前的世界坐标系换作此时的
c
1
c_1
c1系):
(2)
α
b
k
b
k
+
1
=
R
b
k
c
1
(
p
c
1
b
k
+
1
−
p
c
1
b
k
−
v
k
c
1
+
1
2
g
c
1
Δ
t
2
)
β
b
k
b
k
+
1
=
R
b
k
c
1
(
v
b
k
+
1
c
1
−
v
b
k
c
1
+
g
c
1
Δ
t
)
\begin{aligned} \alpha_{b_kb_{k+1}} &= R_{b_kc_1}(p_{c_1b_{k+1}}-p_{c_1b_{k}}-v^{c_1}_{k}+\frac{1}{2}g^{c_1}\Delta t^2) \\ \beta_{b_kb_{k+1}} &= R_{b_kc_1}(v^{c_1}_{b_{k+1}}-v^{c_1}_{b_{k}}+g^{c_1}\Delta t) \end{aligned} \tag{2}
αbkbk+1βbkbk+1=Rbkc1(pc1bk+1−pc1bk−vkc1+21gc1Δt2)=Rbkc1(vbk+1c1−vbkc1+gc1Δt)(2)
将公式(1)代入公式(2)有:
(2)
α
b
k
b
k
+
1
=
R
b
k
c
1
(
s
(
p
ˉ
c
1
b
k
+
1
−
p
ˉ
c
1
b
k
)
−
R
c
1
b
k
v
k
b
k
Δ
t
+
1
2
g
c
1
Δ
t
2
)
=
R
b
k
c
1
(
s
p
ˉ
c
1
b
k
+
1
−
s
p
ˉ
c
1
b
k
−
R
c
1
b
k
v
k
b
k
Δ
t
+
1
2
g
c
1
Δ
t
2
)
=
R
b
k
c
1
[
(
s
p
ˉ
c
1
c
k
+
1
−
R
c
1
b
k
+
1
p
b
c
)
−
(
s
p
ˉ
c
1
c
k
−
R
c
1
b
k
p
b
c
)
−
R
c
1
b
k
v
k
b
k
Δ
t
+
1
2
g
c
1
Δ
t
2
]
=
s
R
b
k
c
1
(
p
ˉ
c
1
c
k
+
1
−
p
ˉ
c
1
c
k
)
−
R
b
k
c
1
R
c
1
b
k
+
1
p
b
c
+
p
b
c
−
v
k
b
k
Δ
t
+
1
2
R
b
k
c
1
g
c
1
Δ
t
2
β
b
k
b
k
+
1
=
R
b
k
c
1
(
R
c
1
b
k
+
1
v
k
+
1
b
k
+
1
−
R
c
1
b
k
v
k
b
k
+
g
c
1
Δ
t
)
=
R
b
k
c
1
R
c
1
b
k
+
1
v
k
+
1
b
k
+
1
−
v
k
b
k
+
R
b
k
c
1
g
c
1
Δ
t
\begin{aligned} \alpha_{b_kb_{k+1}} &= R_{b_kc_1}(s(\bar p_{c_1b_{k+1}}-\bar p_{c_1b_{k}})- R_{c_1b_k}v^{b_k}_{k} \Delta t+\frac{1}{2}g^{c_1}\Delta t^2) \\ &= R_{b_kc_1}(s\bar p_{c_1b_{k+1}}-s\bar p_{c_1b_{k}}- R_{c_1b_k}v^{b_k}_{k} \Delta t+\frac{1}{2}g^{c_1}\Delta t^2) \\ &= R_{b_kc_1}[(s\bar p_{{c_1}c_{k+1}}-R_{c_1b_{k+1}}p_{bc} )-(s\bar p_{{c_1}c_k}-R_{c_1b_k}p_{bc} )- R_{c_1b_k}v^{b_k}_{k} \Delta t+\frac{1}{2}g^{c_1}\Delta t^2] \\ &= sR_{b_kc_1}(\bar p_{{c_1}c_{k+1}}-\bar p_{{c_1}c_k})-R_{b_kc_1}R_{c_1b_{k+1}}p_{bc} +p_{bc} - v^{b_k}_{k} \Delta t+\frac{1}{2}R_{b_kc_1}g^{c_1}\Delta t^2 \\ \beta_{b_kb_{k+1}} &= R_{b_kc_1}(R_{{c_1}b_{k+1}}v^{b_{k+1}}_{{k+1}}-R_{{c_1}b_{k}}v^{b_{k}}_{{k}}+g^{c_1}\Delta t) \\ &= R_{b_kc_1}R_{{c_1}b_{k+1}}v^{b_{k+1}}_{{k+1}}-v^{b_{k}}_{{k}}+R_{b_kc_1}g^{c_1}\Delta t \end{aligned} \tag{2}
αbkbk+1βbkbk+1=Rbkc1(s(pˉc1bk+1−pˉc1bk)−Rc1bkvkbkΔt+21gc1Δt2)=Rbkc1(spˉc1bk+1−spˉc1bk−Rc1bkvkbkΔt+21gc1Δt2)=Rbkc1[(spˉc1ck+1−Rc1bk+1pbc)−(spˉc1ck−Rc1bkpbc)−Rc1bkvkbkΔt+21gc1Δt2]=sRbkc1(pˉc1ck+1−pˉc1ck)−Rbkc1Rc1bk+1pbc+pbc−vkbkΔt+21Rbkc1gc1Δt2=Rbkc1(Rc1bk+1vk+1bk+1−Rc1bkvkbk+gc1Δt)=Rbkc1Rc1bk+1vk+1bk+1−vkbk+Rbkc1gc1Δt(2)
和待估计量
X
I
k
=
[
v
k
b
k
,
v
k
+
1
b
k
+
1
,
g
c
1
,
s
]
\Chi_I^{k}=[v^{b_k}_k,v^{b_{k+1}}_{k+1},g^{c_1},s]
XIk=[vkbk,vk+1bk+1,gc1,s]相关的放到右边,整理得:
(3)
α
b
k
b
k
+
1
+
R
b
k
c
1
R
c
1
b
k
+
1
p
b
c
−
p
b
c
=
−
v
k
b
k
Δ
t
+
1
2
R
b
k
c
1
g
c
1
Δ
t
2
+
s
R
b
k
c
1
(
p
ˉ
c
1
c
k
+
1
−
p
ˉ
c
1
c
k
)
β
b
k
b
k
+
1
=
−
v
k
b
k
+
R
b
k
c
1
R
c
1
b
k
+
1
v
k
+
1
b
k
+
1
+
R
b
k
c
1
g
c
1
Δ
t
\begin{aligned} \alpha_{b_kb_{k+1}} +R_{b_kc_1}R_{c_1b_{k+1}}p_{bc} - p_{bc} &= -v^{b_k}_{k} \Delta t +\frac{1}{2}R_{b_kc_1}g^{c_1}\Delta t^2 + sR_{b_kc_1}(\bar p_{{c_1}c_{k+1}}-\bar p_{{c_1}c_k}) \\ \beta_{b_kb_{k+1}} &= -v^{b_{k}}_{{k}}+R_{b_kc_1}R_{{c_1}b_{k+1}}v^{b_{k+1}}_{{k+1}}+R_{b_kc_1}g^{c_1}\Delta t \end{aligned} \tag{3}
αbkbk+1+Rbkc1Rc1bk+1pbc−pbcβbkbk+1=−vkbkΔt+21Rbkc1gc1Δt2+sRbkc1(pˉc1ck+1−pˉc1ck)=−vkbk+Rbkc1Rc1bk+1vk+1bk+1+Rbkc1gc1Δt(3)
写成矩阵形式:
[
α
^
b
k
b
k
+
1
+
R
b
k
c
1
R
c
1
b
k
+
1
p
b
c
−
p
b
c
β
^
b
k
b
k
+
1
]
=
[
−
I
Δ
t
0
1
2
R
b
k
c
1
Δ
t
2
R
b
k
c
1
(
p
ˉ
c
1
c
k
+
1
−
p
ˉ
c
1
c
k
)
−
I
R
b
k
c
1
R
c
1
b
k
+
1
R
b
k
c
1
Δ
t
0
]
[
v
k
b
k
v
k
+
1
b
k
+
1
g
c
1
s
]
Z
^
b
k
+
1
b
k
=
H
b
k
+
1
b
k
X
I
k
\begin{aligned} \begin{bmatrix} \hat \alpha_{b_kb_{k+1}} +R_{b_kc_1}R_{c_1b_{k+1}}p_{bc} - p_{bc} \\ \hat \beta_{b_kb_{k+1}} \end{bmatrix} &= \begin{bmatrix} -I\Delta t & 0 & \frac{1}{2}R_{b_kc_1}\Delta t^2 & R_{b_kc_1}(\bar p_{{c_1}c_{k+1}}-\bar p_{{c_1}c_k}) \\ -I & R_{b_kc_1}R_{{c_1}b_{k+1}} & R_{b_kc_1}\Delta t & 0 \end{bmatrix} \begin{bmatrix}v^{b_k}_k\\ v^{b_{k+1}}_{k+1} \\ g^{c_1} \\ s \end{bmatrix} \\ \hat Z^{b_k}_{b_{k+1}} &= H^{b_k}_{b_{k+1}}\Chi^{k}_I \end{aligned}
[α^bkbk+1+Rbkc1Rc1bk+1pbc−pbcβ^bkbk+1]Z^bk+1bk=[−IΔt−I0Rbkc1Rc1bk+121Rbkc1Δt2Rbkc1ΔtRbkc1(pˉc1ck+1−pˉc1ck)0]⎣⎢⎢⎡vkbkvk+1bk+1gc1s⎦⎥⎥⎤=Hbk+1bkXIk
即可转化为线性最小二乘问题求解:
min
X
I
k
∥
Z
^
b
k
+
1
b
k
−
H
b
k
+
1
b
k
X
I
k
∥
2
\min_{\Chi^k_I}\| \hat Z^{b_k}_{b_{k+1}} - H^{b_k}_{b_{k+1}}\Chi^{k}_I \|^2
XIkmin∥Z^bk+1bk−Hbk+1bkXIk∥2