视觉SLAM学习打卡【10】-后端·滑动窗口法&位姿图
一、本讲缘由
接上节 视觉SLAM学习打卡【9】-后端·卡尔曼滤波器&光束法平差，BA能同时优化位姿与空间点。然而，大量的路标点/特征点，使得计算量增大，实时性降低。本讲致力于控制BA的规模。
- 滑动窗口法（Sliding Window）：将BA控制在一个时间窗口中，离开窗口的关键帧被丢弃.
- 共视图（Covisibility graph）：与当前相机存在共同观测的关键帧构成的图像即为共视图。仅优化与当前帧有20个以上共视路标的关键帧，其余固定不变.
- 位姿图（Pose Graph）：不管路标，只管轨迹，构建一个只有轨迹的图优化.
二、滑动窗口法
仅保留离当前时刻最近的 N个关键帧（从连续视频中抽出的一部分图像），去掉时间上最早的关键帧。于是BA被固定在一个时间窗口内，离开这个窗口则被丢弃，这种方法称为滑动窗口法。
（1）某个时刻窗口的优化处理
假设此时这个窗口内有 N 个关键帧和M个路标点.
- 关键帧位姿表达为： x 1 , ⋯ , x N x_1,\cdots,x_N x1,⋯,xN
- 路标点为： y 1 , ⋯ , y M y_1,\cdots,y_M y1,⋯,yM
用上一讲BA方法处理这个滑动窗口，包括建立最小二乘问题，构建整体的Hessian海森矩阵，然后边缘化所有路标点来加速求解。
最后，优化结果为： [ x 1 , ⋯ , x N ] T ∼ N ( [ μ 1 , ⋯ , μ N ] T , Σ ) [x_1,\cdots,x_N]^T\sim N([\mu_1,\cdots,\mu_N]^T,\Sigma) [x1,⋯,xN]T∼N([μ1,⋯,μN]T,Σ)
（2）窗口滑动，结构发生改变
滑动中，状态变量的更新讨论：
- 新增一个关键帧和对应观测到的路标点
类似于（1），BA （N+1） 个关键帧和对应的路标点. - 删除 / 边缘化一个旧的关键帧
删除旧关键帧 x1，将 x1 边缘化之后将导致整个问题不再稀疏，将破坏路标部分的对角块结构.
三、位姿图
构建一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初值（对极几何 / PnP / ICP）。不同的是，一旦初始估计完成，就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系，省去了大量特征点优化的计算，只保留了关键帧的轨迹，从而构建了所谓的位姿图.
图优化中，节点表示相机位姿，以
T
1
,
⋯
,
T
n
T_1,\cdots,T_n
T1,⋯,Tn表示；边是两个位姿节点之间相对运动的估计（特征点法/直接法/GPS / IMU积分）
估计 T i T_{i} Ti和 T j T_{j} Tj之间的运动 Δ T i j \Delta T_{ij} ΔTij，李群写法： Δ T i j = T i − 1 T j ΔT_{ij}=T_i^{-1}T_j ΔTij=Ti−1Tj李代数写法： Δ ξ i j = ξ i − 1 ∘ ξ j = ln ( T i − 1 T j ) ∨ \Delta\xi_{ij}=\xi_i^{-1}\circ\xi_j=\ln(T_i^{-1}T_j)^\vee Δξij=ξi−1∘ξj=ln(Ti−1Tj)∨构建误差 e i j e_{ij} eij: e i j = ln ( T i j − 1 T i − 1 T j ) ∨ = l n ( I ) = 0 e_{ij}=\ln(T_{ij}^{-1}T_i^{-1}T_j)^\vee= ln(I)=0 eij=ln(Tij−1Ti−1Tj)∨=ln(I)=0优化变量有两个 ξ i \xi_{i} ξi和 ξ j \xi_{j} ξj，求 e i j e_{ij} eij关于这两个变量的导数（利用扰动模型，给 ξ i \xi_{i} ξi和 ξ j \xi_{j} ξj各乘一个左扰动 δ ξ i \delta\xi_{i} δξi和 δ ξ j \delta\xi_{j} δξj）： e ^ i j = ln ( T i j − 1 T i − 1 exp ( ( − δ ξ i ) ∧ ) exp ( δ ξ j ∧ ) T j ) ∨ \hat{e}_{ij}=\ln(T_{ij}^{-1}T_i^{-1}\exp((-\delta\xi_i)^{\wedge})\exp(\delta\xi_j^{\wedge})T_j)^{\vee} e^ij=ln(Tij−1Ti−1exp((−δξi)∧)exp(δξj∧)Tj)∨
根据伴随性质公式 exp ( ( A d ( T ) ξ ) ∧ ) = T exp ( ξ ∧ ) T − 1 \exp((Ad(T)\xi)^{\wedge})=T\exp(\xi^{\wedge})T^{-1} exp((Ad(T)ξ)∧)=Texp(ξ∧)T−1 （其中， A d ( T ) = [ R t ∧ R 0 R ] Ad(T)=\begin{bmatrix}R&t^\wedge R\\\mathbf{0}&R\end{bmatrix} Ad(T)=[R0t∧RR]）的变形 exp ( ξ ∧ ) T = T exp ( ( A d ( T − 1 ) ξ ) ∧ ) \exp(\xi^\wedge)T=T\exp((Ad(T^{-1})\xi)^\wedge) exp(ξ∧)T=Texp((Ad(T−1)ξ)∧) ，把扰动项挪到最右边： e ^ i j = ln ( T i j − 1 T i − 1 exp ( ( − δ ξ i ) ∧ ) exp ( δ ξ j ∧ ) T j ⏟ 伴随性质 ) ∨ = ln ( T i j − 1 T i − 1 exp ( ( − δ ξ i ) ∧ ) T j exp ( ( A d ( T j − 1 ) δ ξ j ) ∧ ) ⏞ ) ∨ = ln ( T i j − 1 T i − 1 exp ( ( − δ ξ i ) ∧ ) T j ⏟ 伴随性质 exp ( ( A d ( T j − 1 ) δ ξ j ) ∧ ) ∨ = ln ( T i j − 1 T i − 1 T j exp ( ( − A d ( T j − 1 ) δ ξ i ) ∧ ) ⏞ exp ( ( A d ( T j − 1 ) δ ξ j ) ∧ ) ) ∨ \begin{aligned} \hat{e}_{ij}& =\ln\left(T_{ij}^{-1}T_{i}^{-1}\exp\left((-\delta\xi_{i})^{\wedge}\right)\underbrace{\exp(\delta\xi_{j}^{\wedge})T_{j}}_{\text{伴随性质}}\right)^{\vee} \\ &=\ln\left(T_{ij}^{-1}T_i^{-1}\exp((-\delta\xi_i)^\wedge)\overbrace{T_j\exp((Ad(T_j^{-1})\delta\xi_j)^\wedge)}\right)^\vee \\ &=\ln\left(T_{ij}^{-1}T_i^{-1}\underbrace{\exp\left((-\delta\xi_i)^\wedge\right)T_j}_\text{伴随性质}\exp\left((Ad(T_j^{-1})\delta\xi_j\right)^\wedge\right)^\vee \\ &=\ln\left(T_{ij}^{-1}T_i^{-1}\overbrace{T_j\exp\left(\left(-Ad(T_j^{-1})\delta\xi_i\right)^{\wedge}\right)}\exp\left(\left(Ad(T_j^{-1})\delta\xi_j\right)^{\wedge}\right)\right)^{\vee} \end{aligned} e^ij=ln Tij−1Ti−1exp((−δξi)∧)伴随性质 exp(δξj∧)Tj ∨=ln(Tij−1Ti−1exp((−δξi)∧)Tjexp((Ad(Tj−1)δξj)∧) )∨=ln Tij−1Ti−1伴随性质 exp((−δξi)∧)Tjexp((Ad(Tj−1)δξj)∧ ∨=ln(Tij−1Ti−1Tjexp((−Ad(Tj−1)δξi)∧) exp((Ad(Tj−1)δξj)∧))∨
e ^ i j ≈ ln ( T i j − 1 T i − 1 T j [ I + ( − A d ( T j − 1 ) δ ξ i ) ∧ + ( A d ( T j − 1 ) δ ξ j ) ∧ ] ) ∨ \hat{e}_{ij}\approx\ln\left(T_{ij}^{-1}T_i^{-1}T_j\left[I+\left(-Ad(T_j^{-1})\delta\xi_i\right)^\wedge+\left(Ad(T_j^{-1})\delta\xi_j\right)^\wedge\right]\right)^\vee e^ij≈ln(Tij−1Ti−1Tj[I+(−Ad(Tj−1)δξi)∧+(Ad(Tj−1)δξj)∧])∨
e
^
i
j
=
ln
(
T
i
j
−
1
T
i
−
1
T
j
[
I
+
(
−
A
d
(
T
j
−
1
)
δ
ξ
i
)
∧
+
(
A
d
(
T
j
−
1
)
δ
ξ
j
)
∧
]
)
∨
=
ln
(
exp
(
e
i
j
∧
)
exp
(
x
∧
)
)
∨
\begin{aligned} \hat{e}_{ij}& =\ln\left(T_{ij}^{-1}T_i^{-1}T_j\left[I+\left(-Ad(T_j^{-1})\delta\xi_i\right)^\wedge+\left(Ad(T_j^{-1})\delta\xi_j\right)^\wedge\right]\right)^\vee \\ &=\ln\left(\exp(e_{ij}^{\wedge})\exp(x^{\wedge})\right)^{\vee} \end{aligned}
e^ij=ln(Tij−1Ti−1Tj[I+(−Ad(Tj−1)δξi)∧+(Ad(Tj−1)δξj)∧])∨=ln(exp(eij∧)exp(x∧))∨因此根据BCH的右乘近似公式可得（x为小量）：
e
^
i
j
=
ln
(
exp
(
e
i
j
∧
)
exp
(
x
∧
)
)
∨
=
J
r
−
1
(
e
i
j
)
x
+
e
i
j
\hat{e}_{ij}=\ln\left(\exp(e_{ij}^{\wedge})\exp(x^{\wedge})\right)^{\vee}=\mathcal{J}_r^{-1}(e_{ij})x+e_{ij}
e^ij=ln(exp(eij∧)exp(x∧))∨=Jr−1(eij)x+eij
≈
J
r
−
1
(
e
i
j
)
(
−
A
d
(
T
j
−
1
)
δ
ξ
i
+
A
d
(
T
j
−
1
)
δ
ξ
j
)
+
e
i
j
=
e
i
j
+
(
−
J
r
−
1
(
e
i
j
)
A
d
(
T
j
−
1
)
)
⏟
∂
e
i
j
∂
δ
ξ
i
δ
ξ
i
+
J
r
−
1
(
e
i
j
)
A
d
(
T
j
−
1
)
⏟
∂
e
i
j
∂
δ
ξ
j
δ
ξ
j
\begin{aligned} &\approx\mathcal{J}_r^{-1}(e_{ij})\left(-Ad(T_j^{-1})\delta\xi_i+Ad(T_j^{-1})\delta\xi_j\right)+e_{ij} \\ &=e_{ij}+\underbrace{\left(-\mathcal{J}_r^{-1}(e_{ij})Ad(T_j^{-1})\right)}_{\frac{\partial e_{ij}}{\partial\delta\xi_i}}\delta\xi_i+\underbrace{\mathcal{J}_r^{-1}(e_{ij})Ad(T_j^{-1})}_{\frac{\partial e_{ij}}{\partial\delta\xi_j}}\delta\xi_j \end{aligned}
≈Jr−1(eij)(−Ad(Tj−1)δξi+Ad(Tj−1)δξj)+eij=eij+∂δξi∂eij
(−Jr−1(eij)Ad(Tj−1))δξi+∂δξj∂eij
Jr−1(eij)Ad(Tj−1)δξj（其中，为方便计算，
J
r
−
1
(
e
i
j
)
≈
I
+
1
2
[
ϕ
e
∧
ρ
e
∧
0
ϕ
e
∧
]
\left.\mathcal{J}_r^{-1}(e_{ij})\approx I+\frac{1}{2}\left[\begin{array}{cc}\phi_e^\wedge&\rho_e^\wedge\\\mathbf{0}&\phi_e^\wedge\end{array}\right.\right]
Jr−1(eij)≈I+21[ϕe∧0ρe∧ϕe∧]或者
≈
I
\approx I
≈I）
通过
∂
e
i
j
∂
δ
ξ
i
=
0
\frac{\partial e_{ij}}{\partial\delta\xi_i}=0
∂δξi∂eij=0 和
∂
e
i
j
∂
δ
ξ
j
=
0
\frac{\partial e_{ij}}{\partial\delta\xi_j}=0
∂δξj∂eij=0 求得
δ
ξ
i
\delta\xi_{i}
δξi和
δ
ξ
j
\delta\xi_{j}
δξj不断迭代
ξ
i
\xi_{i}
ξi 和
ξ
j
\xi_{j}
ξj。求得目标函数
min
1
2
∑
i
,
j
∈
E
e
i
j
T
Σ
i
j
−
1
e
i
j
\min\frac12\sum_{i,j\in\mathcal{E}}e_{ij}^T\Sigma_{ij}^{-1}e_{ij}
min21∑i,j∈EeijTΣij−1eij的最小值。
四、实践g2o_viewer报错解决方案
问题1：
原因：
解决方案：
sudo apt-get install libsuitesparse-dev qtdeclarative5-dev qt5-qmake
sudo apt-get install libqglviewer-dev-qt5
cmake ..
make
sudo make install
问题2：
解决方案：
sudo ldconfig
（sudo ldconfig命令的作用是确保新安装的动态链接库能够被系统正确识别和共享）
