对于窗口中得连续两帧
b
k
b_{k}
bk 和
b
k
+
1
b_{k+1}
bk+1 ，已经从视觉SFM中得到了旋转
q
b
k
c
0
q_{b_{k}}^{c_{0}}
qbkc0 和
q
b
k
+
1
c
0
q_{b_{k+1}}^{c_{0}}
qbk+1c0 ，从IMU预积分中得到了相邻帧旋转
γ
^
b
k
+
1
b
k
\hat{\gamma}^{b_{k}}_{b_{k+1}}
γ^bk+1bk 。 根据约束方程，联立所有相邻帧，最小化代价函数（论文式(7)）：
min
δ
b
w
∑
k
∈
B
∥
q
b
k
+
1
c
0
−
1
⊗
q
b
k
c
0
⊗
γ
b
k
+
1
b
k
∥
2
\min _{\delta b_{w}} \sum_{k \in \mathcal{B}}\left\|q_{b_{k+1}}^{c_{0}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}} \otimes \gamma_{b_{k+1}}^{b_{k}}\right\|^{2}
δbwmink∈B∑qbk+1c0−1⊗qbkc0⊗γbk+1bk2 其中对陀螺仪偏置求IMU预积分项线性化，有：
γ
b
k
+
1
b
k
≈
γ
^
b
k
+
1
b
k
⊗
[
1
1
2
J
b
w
γ
δ
b
w
]
\gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} J_{b_{w}}^{\gamma} \delta b_{w} \end{array}\right]
γbk+1bk≈γ^bk+1bk⊗[121Jbwγδbw] 在具体实现的时候，因为上述约束方程为：
q
b
k
+
1
c
0
−
1
⊗
q
b
k
c
0
⊗
γ
b
k
+
1
b
k
=
[
1
0
]
q_{b_{k+1}}^{c_{0}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}} \otimes \gamma_{b_{k+1}}^{b_{k}}=\left[\begin{array}{l} 1 \\ 0 \end{array}\right]
qbk+1c0−1⊗qbkc0⊗γbk+1bk=[10] 有：
γ
b
k
+
1
b
k
=
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
⊗
[
1
0
]
\gamma_{b_{k+1}}^{b_{k}}=q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}} \otimes\left[\begin{array}{l} 1 \\ 0 \end{array}\right]
γbk+1bk=qbkc0−1⊗qbk+1c0⊗[10] 代入
δ
b
w
\delta b_{w}
δbw 得 :
γ
^
b
k
+
1
b
k
⊗
[
1
1
2
J
b
w
γ
δ
b
w
]
=
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
⊗
[
1
0
]
\hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} J_{b_{w}}^{\gamma} \delta b_{w} \end{array}\right]=q_{b_{k}}^{c 0-1} \otimes q_{b_{k+1}}^{c 0} \otimes\left[\begin{array}{l} 1 \\ 0 \end{array}\right]
γ^bk+1bk⊗[121Jbwγδbw]=qbkc0−1⊗qbk+1c0⊗[10]
只考虑虚部，有 :
J
b
w
γ
δ
b
w
=
2
(
γ
^
b
k
+
1
b
k
−
1
⊗
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
)
v
e
c
J_{b_{w}}^{\gamma} \delta b_{w}=2\left(\hat{\gamma}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\right)_{v e c}
Jbwγδbw=2(γ^bk+1bk−1⊗qbkc0−1⊗qbk+1c0)vec
两侧乘以
J
b
w
γ
T
\mathbf{J_{b_{w}}^{\gamma}}^{T}
JbwγT ，用LDLT分解求得
δ
b
w
\delta b_{w}
δbw 。 在代码中，
q
i
,
j
\mathrm{q}_\mathrm{i,j}
qi,j 即
q
b
k
+
1
b
k
=
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
q_{b_{k+1}}^{b_{k}}=q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}
qbk+1bk=qbkc0−1⊗qbk+1c0
t
m
p
−
A
\mathrm{tmp}_{-} \mathrm{A}
tmp−A 即
J
b
w
γ
J_{b_{w}}^{\gamma}
Jbwγ
t
m
p
−
B
\mathrm{tmp}_{-} \mathrm{B}
tmp−B 即
2
(
r
^
b
k
+
1
b
k
−
1
⊗
q
b
k
+
1
b
k
)
v
e
c
=
2
(
r
^
b
k
+
1
b
k
−
1
⊗
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
)
v
e
c
2\left(\hat{r}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k+1}}^{b_{k}}\right)_{v e c}=2\left(\hat{r}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\right)_{v e c}
2(r^bk+1bk−1⊗qbk+1bk)vec=2(r^bk+1bk−1⊗qbkc0−1⊗qbk+1c0)vec 根据上面的代价函数构造
A
x
=
B
\mathrm{Ax}=\mathrm{B}
Ax=B 即
J
b
w
γ
T
J
b
w
γ
δ
b
w
=
2
J
b
w
γ
T
(
r
^
b
k
+
1
b
k
−
1
⊗
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
)
vec
J_{b_{w}}^{\gamma T} J_{b_{w}}^{\gamma} \delta b_{w}=2 J_{b_{w}}^{\gamma T}\left(\hat{r}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\right)_{\text {vec }}
JbwγTJbwγδbw=2JbwγT(r^bk+1bk−1⊗qbkc0−1⊗qbk+1c0)vec 然后采用LDLT分解求得
δ
b
w
\delta b_{w}
δbw 。
