VIO在纯视觉的基础上添加了IMU约束，因子图如下：
VIO中，待估计的状态变量为
θ
i
=
(
R
W
B
i
,
W
p
B
i
,
W
v
B
i
,
b
i
g
,
b
i
a
)
\theta^i=(R_{WB}^i,_Wp_B^i,_Wv_B^i,b_i^g,b_i^a)
θi=(RWBi,WpBi,WvBi,big,bia)*注意不是
T
W
C
T_{WC}
TWC
观测量为
z
i
l
∈
C
i
z_{il}\in C_i
zil∈Ci,表示第i帧看到的第j个路标点,输入
(
ω
i
~
,
a
i
~
)
∈
I
i
j
(\tilde{\omega_i},\tilde{a_i})\in I_{ij}
(ωi~,ai~)∈Iij,第i、j准帧间惯导信息，其中
(
i
,
j
)
∈
K
k
(i,j)\in K_k
(i,j)∈Kk,为关键帧的集合。
θ
∗
=
arg
min
θ
∑
k
[
E
p
r
o
j
(
k
,
j
)
+
E
I
M
U
(
i
,
j
)
]
,
k
∈
K
k
\theta^*=\arg\min_\theta \sum_k[E_{proj}(k,j)+E_{IMU}(i,j)],k\in K_k
θ∗=argθmink∑[Eproj(k,j)+EIMU(i,j)],k∈Kk包括重投影误差项及IMU误差项。
重投影误差项:
E
p
r
o
j
(
k
,
j
)
=
ρ
(
(
x
k
−
π
(
X
c
k
)
)
T
Σ
k
(
x
k
−
π
(
X
c
k
)
)
)
π
(
X
c
)
=
[
f
u
X
c
Z
c
+
c
u
f
v
Y
c
Z
c
+
c
v
]
,
X
c
=
[
X
c
Y
c
Z
c
]
T
X
c
=
T
C
W
P
W
=
(
T
W
B
T
B
C
)
T
P
W
=
T
C
B
T
W
B
T
P
W
,
T
W
B
T
=
[
R
W
B
T
−
R
W
B
T
W
p
B
0
T
1
]
X
c
k
=
R
C
B
R
W
B
T
(
X
W
k
−
W
p
B
j
)
+
c
p
B
E_{proj}(k,j)=\rho((x^k-\pi(X_c^k))^T\Sigma_k(x^k-\pi(X_c^k))) \\ \pi(X_c)=\left[ \begin{matrix} f_u \frac{X_c}{Z_c}+c_u \\ f_v \frac{Y_c}{Z_c}+c_v \end{matrix} \right],X_c=\left[ \begin{matrix}X_c&Y_c&Z_c \end{matrix} \right]^T \\ Xc=T_{CW}P_W=(T_{WB}T_{BC})^TP_W=T_{CB}T_{WB}^TP_W,T_{WB}^T=\left[ \begin{matrix} R_{WB}^T &-R_{WB}^T {_Wp_B} \\ 0^T&1 \end{matrix} \right] \\ X_c^k=R_{CB}R_{WB}^T(X_W^k-_Wp_B^j)+_cp_B
Eproj(k,j)=ρ((xk−π(Xck))TΣk(xk−π(Xck)))π(Xc)=[fuZcXc+cufvZcYc+cv],Xc=[XcYcZc]TXc=TCWPW=(TWBTBC)TPW=TCBTWBTPW,TWBT=[RWBT0T−RWBTWpB1]Xck=RCBRWBT(XWk−WpBj)+cpBIMU误差
E
I
M
U
(
i
,
j
)
=
ρ
(
[
e
R
e
v
e
p
]
T
Σ
I
[
e
R
e
v
e
p
]
)
+
ρ
(
e
b
T
Σ
R
e
b
)
e
R
=
L
o
g
(
Δ
R
i
j
E
x
p
(
J
Δ
R
g
δ
b
i
g
)
R
B
W
i
R
W
B
j
)
e
v
=
R
B
W
i
(
W
v
B
j
−
W
v
B
i
−
g
W
Δ
t
i
j
)
−
(
Δ
v
i
j
+
J
Δ
v
g
δ
b
i
g
+
J
Δ
v
a
δ
b
i
a
)
e
p
=
R
B
W
i
(
W
p
B
j
−
W
p
B
i
−
W
v
B
i
Δ
t
i
j
−
1
2
g
W
Δ
t
i
j
2
)
−
(
Δ
p
i
j
+
J
Δ
p
g
δ
b
i
g
+
J
Δ
p
a
δ
b
i
a
)
e
b
=
[
b
j
g
−
b
i
g
b
j
a
−
b
i
a
]
E_{IMU}(i,j)=\rho(\left[\begin{matrix}e_R&e_v&e_p\end{matrix}\right]^T\Sigma_I\left[\begin{matrix}e_R&e_v&e_p\end{matrix} \right])+\rho(e_b^T\Sigma_Re_b) \\ e_R=Log(\Delta R_{ij}Exp(J_{\Delta R}^g\delta b_i^g)R_{BW}^iR_{WB}^j) \\ e_v=R_{BW}^i(_Wv_B^j-_Wv_B^i-g_W\Delta t_{ij})-(\Delta v_{ij}+J_{\Delta v}^g\delta b_i^g+J_{\Delta v}^a\delta b_i^a) \\ e_p=R_{BW}^i(_Wp_B^j-_Wp_B^i-_Wv_B^i\Delta t_{ij}-\frac12g_W\Delta t_{ij}^2)-(\Delta p_{ij}+J_{\Delta p}^g\delta b_i^g+J_{\Delta p}^a\delta b_i^a) \\ e_b=\left[ \begin{matrix}b_j^g-b_i^g \\ b_j^a-b_i^a \end{matrix} \right]
EIMU(i,j)=ρ([eRevep]TΣI[eRevep])+ρ(ebTΣReb)eR=Log(ΔRijExp(JΔRgδbig)RBWiRWBj)ev=RBWi(WvBj−WvBi−gWΔtij)−(Δvij+JΔvgδbig+JΔvaδbia)ep=RBWi(WpBj−WpBi−WvBiΔtij−21gWΔtij2)−(Δpij+JΔpgδbig+JΔpaδbia)eb=[bjg−bigbja−bia]
在ORB-SLAM2中，图优化存在于三个线程：Tracking、Local Mapping、Loop Closing。分别处理局部共视区域中的非线性优化，地图更新时的优化、位姿图优化检测闭环。
该过程对应的图模型
去掉了eNSPrior和只连接vPVRLast的边e
该优化函数用于陀螺仪偏置初始化
