- 本节是对上一节BA的进一步简化，旨在提高优化实时性.
- 难点在于位姿图部分的雅可比矩阵求解（涉及李代数扰动模型求导），书中的相关推导存在跳步（可能数学功底强的人认为过渡的理所当然），笔者参考了知乎Clark的推导，并以顺向思维的方式重新整理推导过程，使得该部分更加通俗易懂.
视觉SLAM学习打卡【10】-后端·滑动窗口法&位姿图
- 一、本讲缘由
- 二、滑动窗口法
- （1）某个时刻窗口的优化处理
- （2）窗口滑动，结构发生改变
- 三、位姿图
- 四、实践g2o_viewer报错解决方案
一、本讲缘由
接上节 视觉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,Σ)
其中，均值部分 μ k \mu_{k} μk为为第 k个关键帧的位姿均值，即BA迭代之后的结果；所有关键帧的协方差矩阵 Σ \Sigma Σ是对整个 BA 的 H 矩阵进行舒尔消元边缘化后的系数矩阵.
（2）窗口滑动，结构发生改变
滑动中，状态变量的更新讨论：
- 新增一个关键帧和对应观测到的路标点
类似于（1），BA （N+1） 个关键帧和对应的路标点. - 删除 / 边缘化一个旧的关键帧
删除旧关键帧 x1，将 x1 边缘化之后将导致整个问题不再稀疏，将破坏路标部分的对角块结构.
当边缘化路标点时，S= [ B − E C − 1 E T 0 E T C ] \begin{bmatrix}B-EC^{-1}E^\mathrm{T}&0\\E^\mathrm{T}&C\end{bmatrix} [B−EC−1ETET0C]，Fill-in将出现在左上角的位姿块中，右下角的路标块仍为对角阵，保持稀疏，不影响求解
当边缘化关键帧时，Fill-in将出现在右下角的路标块中，BA无法按照之前的稀疏方式迭代求解.
解决方法：边缘化关键帧的同时，边缘化它观测到的路标点，保持了右下角的对角块结构。在OKVIS中，根据要边缘化的关键帧所看到的路标点是否在最新的关键帧中能看到来考虑是否边缘化此路标点。如果不能，就直接边缘化这个路标点；如果能，就丢弃被边缘化关键帧对这个路标点的观测，从而保持BA的稀疏性。
三、位姿图
构建一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初值（对极几何 / PnP / ICP）。不同的是，一旦初始估计完成，就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系，省去了大量特征点优化的计算，只保留了关键帧的轨迹，从而构建了所谓的位姿图.
图优化中，节点表示相机位姿，以
T
1
,
⋯
,
T
n
T_1,\cdots,T_n
T1,⋯,Tn表示；边是两个位姿节点之间相对运动的估计（特征点法/直接法/GPS / IMU积分）
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)∨
其中， δ ξ i \delta\xi_{i} δξi乘左扰动到了右边，是因为逆的存在 ( exp ( δ ξ i ∧ ) T i ) − 1 = T i − 1 ⋅ ( exp ( δ ξ i ∧ ) ) − 1 = T i − 1 ⋅ exp ( − δ ξ i ∧ ) \begin{aligned}&(\exp(\delta\xi_{i}^{\wedge})T_{i})^{-1}\\&=T_{i}^{-1}\cdot(\exp(\delta\xi_{i}^{\wedge}))^{-1}\\&=T_{i}^{-1}\cdot \exp(-\delta\xi_{i}^{\wedge})\end{aligned} (exp(δξi∧)Ti)−1=Ti−1⋅(exp(δξi∧))−1=Ti−1⋅exp(−δξi∧)
根据伴随性质公式 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)∧))∨
对上述指数 exp ( ( − A d ( T j − 1 ) δ ξ i ) ∧ ) \exp\left(\left(-Ad(T_j^{-1})\delta\xi_i\right)^\wedge\right) exp((−Ad(Tj−1)δξi)∧)和 exp ( ( A d ( T j − 1 ) δ ξ j ) ∧ ) \exp\left(\left(Ad(T_j^{-1})\delta\xi_j\right)^\wedge\right) exp((Ad(Tj−1)δξj)∧)分别做泰勒一阶展开： exp ( ( − A d ( T j − 1 ) δ ξ i ) ∧ ) = I + ( − A d ( T j − 1 ) δ ξ i ) ∧ exp ( ( A d ( T j − 1 ) δ ξ j ) ∧ ) = I + ( A d ( T j − 1 ) δ ξ j ) ∧ \exp\left(\left(-Ad(T_j^{-1})\delta\xi_i\right)^\wedge\right)=I+\left(-Ad(T_j^{-1})\delta\xi_i\right)^\wedge\\\exp\left(\left(Ad(T_j^{-1})\delta\xi_j\right)^\wedge\right)=I+\left(Ad(T_j^{-1})\delta\xi_j\right)^\wedge exp((−Ad(Tj−1)δξi)∧)=I+(−Ad(Tj−1)δξi)∧exp((Ad(Tj−1)δξj)∧)=I+(Ad(Tj−1)δξj)∧忽略二次项得： exp ( ( − A d ( T j − 1 ) δ ξ i ) ∧ ) exp ( ( A d ( T j − 1 ) δ ξ j ) ∧ ) ≈ ( I + ( − A d ( T j − 1 ) δ ξ i ) ∧ ) ( I + ( A d ( T j − 1 ) δ ξ j ) ∧ ) ≈ I + ( − A d ( T j − 1 ) δ ξ i ) ∧ + ( A d ( T j − 1 ) δ ξ j ) ∧ + ( − A d ( T j − 1 ) δ ξ i ) ∧ ( A d ( T j − 1 ) δ ξ j ) ∧ ⏟ 二次项忽略不计 = I + ( − A d ( T j − 1 ) δ ξ i ) ∧ + ( A d ( T j − 1 ) δ ξ j ) ∧ \begin{aligned} \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)& \approx\left(I+\left(-Ad(T_{j}^{-1})\delta\xi_{i}\right)^{\wedge}\right)\left(I+\left(Ad(T_{j}^{-1})\delta\xi_{j}\right)^{\wedge}\right) \\ &\approx I+\left(-Ad(T_j^{-1})\delta\xi_i\right)^{\wedge}+\left(Ad(T_j^{-1})\delta\xi_j\right)^{\wedge} \\ &+\underbrace{{\left(-Ad(T_{j}^{-1})\delta\xi_{i}\right)^{\wedge}\left(Ad(T_{j}^{-1})\delta\xi_{j}\right)^{\wedge}}}_{\text{二次项忽略不计}} \\ &=I+\left(-Ad(T_j^{-1})\delta\xi_i\right)^{\wedge}+\left(Ad(T_j^{-1})\delta\xi_j\right)^{\wedge} \end{aligned} exp((−Ad(Tj−1)δξi)∧)exp((Ad(Tj−1)δξj)∧)≈(I+(−Ad(Tj−1)δξi)∧)(I+(Ad(Tj−1)δξj)∧)≈I+(−Ad(Tj−1)δξi)∧+(Ad(Tj−1)δξj)∧+二次项忽略不计 (−Ad(Tj−1)δξi)∧(Ad(Tj−1)δξj)∧=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 ) ∧ ] ) ∨ \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)∧])∨
- 令变换矩阵 T i j − 1 T i − 1 T j T_{ij}^{-1}T_i^{-1}T_j Tij−1Ti−1Tj对应的李代数为 e i j e_{ij} eij，根据李群李代数的对应关系得： T i j − 1 T i − 1 T j = exp ( e i j ∧ ) T_{ij}^{-1}T_i^{-1}T_j=\exp(e_{ij}^{\wedge}) Tij−1Ti−1Tj=exp(eij∧)
- 再令 exp ( x ∧ ) = I + ( − A d ( T j − 1 ) δ ξ i ) ∧ + ( A d ( T j − 1 ) δ ξ j ) ∧ \exp(x^\wedge)=I+\left(-Ad(T_j^{-1})\delta\xi_i\right)^\wedge+\left(Ad(T_j^{-1})\delta\xi_j\right)^\wedge exp(x∧)=I+(−Ad(Tj−1)δξi)∧+(Ad(Tj−1)δξj)∧根据对数函数的泰勒展开式： ln ( A ) = ( A − I ) − ( A − I ) 2 2 + ( A − I ) 3 3 − … \ln(A)=(A-I)-\frac{(A-I)^2}2+\frac{(A-I)^3}3-\ldots ln(A)=(A−I)−2(A−I)2+3(A−I)3−…取其中的一阶项，可得： x ∧ = ln ( I + ( − A d ( T j − 1 ) δ ξ i ) ∧ + ( A d ( T j − 1 ) δ ξ j ) ∧ ) ≈ I + ( − A d ( T j − 1 ) δ ξ i ) ∧ + ( A d ( T j − 1 ) δ ξ j ) ∧ − I = − ( A d ( T j − 1 ) δ ξ i ) ∧ + ( A d ( T j − 1 ) δ ξ j ) ∧ \begin{aligned} x^{\wedge}& =\ln\left(I+\left(-Ad(T_j^{-1})\delta\xi_i\right)^\wedge+\left(Ad(T_j^{-1})\delta\xi_j\right)^\wedge\right) \\ &\approx I+\left(-Ad(T_j^{-1})\delta\xi_i\right)^\wedge+\left(Ad(T_j^{-1})\delta\xi_j\right)^\wedge-I \\ &=-\left(Ad(T_j^{-1})\delta\xi_i\right)^{\wedge}+\left(Ad(T_j^{-1})\delta\xi_j\right)^{\wedge} \end{aligned} x∧=ln(I+(−Ad(Tj−1)δξi)∧+(Ad(Tj−1)δξj)∧)≈I+(−Ad(Tj−1)δξi)∧+(Ad(Tj−1)δξj)∧−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：
g2o_viewer: command not found
原因：
之前编译g2o库的时候因为少装了部分依赖所以没有编译出 g2o_viewer的可执行文件
解决方案：
补充下述依赖
sudo apt-get install libsuitesparse-dev qtdeclarative5-dev qt5-qmake
sudo apt-get install libqglviewer-dev-qt5
进入g2o的build文件夹下
cmake ..
make
sudo make install
问题2：
g2o_viewer: error while loading shared libraries: libg2o_viewer.so: cannot o
解决方案：
sudo ldconfig
（sudo ldconfig命令的作用是确保新安装的动态链接库能够被系统正确识别和共享）
