SLAM基礎問題總結（3）

1、推導直接法BA，直接法的分類，三個假設及優劣
$\boldsymbol{p}_{1}=\left[\begin{array}{c}{u} \\ {v} \\ {1}\end{array}\right]=\frac{1}{Z_{1}} \boldsymbol{K} \boldsymbol{P}$.$\boldsymbol{p}_{2}=\left[\begin{array}{c}{u} \\ {v} \\ {1}\end{array}\right]=\frac{1}{Z_{2}} \boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t})=\frac{1}{Z_{2}} \boldsymbol{K}\left(\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}\right)_{1 : 3}$
優化相機的位姿，來尋找與 p1 更相似的 p2 。這同樣可以通過解一個優化問題，但此時最小化的不是重投影誤差，而是光度誤差（Photometric Error），也就是 P 的兩個像素的亮度誤差：$e=\boldsymbol{I}_{1}\left(\boldsymbol{p}_{1}\right)-\boldsymbol{I}_{2}\left(\boldsymbol{p}_{2}\right)$在直接法中，假設一個空間點在各個視角下，成像的灰度是不變的。有許多個（比如 N 個）空間點 Pi ，那麼，整個相機位姿估計問題變爲：$\min _{\xi} J(\boldsymbol{\xi})=\sum_{i=1}^{N} e_{i}^{T} e_{i}, \quad e_{i}=\boldsymbol{I}_{1}\left(\boldsymbol{p}_{1, i}\right)-\boldsymbol{I}_{2}\left(\boldsymbol{p}_{2, i}\right)$給 exp(ξ) 左乘一個小擾動 exp(δξ)，得：$\begin{aligned} e(\boldsymbol{\xi} \oplus \delta \boldsymbol{\xi}) &=\boldsymbol{I}_{1}\left(\frac{1}{Z_{1}} \boldsymbol{K} \boldsymbol{P}\right)-\boldsymbol{I}_{2}\left(\frac{1}{Z_{2}} \boldsymbol{K} \exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}\right) \\ & \approx \boldsymbol{I}_{1}\left(\frac{1}{Z_{1}} \boldsymbol{K} \boldsymbol{P}\right)-\boldsymbol{I}_{2}\left(\frac{1}{Z_{2}} \boldsymbol{K}\left(1+\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}\right) \\ &=\boldsymbol{I}_{1}\left(\frac{1}{Z_{1}} \boldsymbol{K} \boldsymbol{P}\right)-\boldsymbol{I}_{2}\left(\frac{1}{Z_{2}} \boldsymbol{K} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}+\frac{1}{Z_{2}} \boldsymbol{K} \delta \boldsymbol{\xi}^{\wedge} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}\right) \end{aligned}$記：$\begin{aligned} \boldsymbol{q} &=\delta \boldsymbol{\xi}^{\wedge} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P} \\ \boldsymbol{u} &=\frac{1}{Z_{2}} \boldsymbol{K} \boldsymbol{q} \end{aligned}$利用一階泰勒展開，有：$\begin{aligned} e(\boldsymbol{\xi} \oplus \delta \boldsymbol{\xi}) &=\boldsymbol{I}_{1}\left(\frac{1}{Z_{1}} \boldsymbol{K} \boldsymbol{P}\right)-\boldsymbol{I}_{2}\left(\frac{1}{Z_{2}} \boldsymbol{K} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}+\boldsymbol{u}\right) \\ & \approx \boldsymbol{I}_{1}\left(\frac{1}{Z_{1}} \boldsymbol{K} \boldsymbol{P}\right)-\boldsymbol{I}_{2}\left(\frac{1}{Z_{2}} \boldsymbol{K} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}\right)-\frac{\partial \boldsymbol{I}_{2}}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}} \frac{\partial \boldsymbol{q}}{\partial \delta \xi} \delta \boldsymbol{\xi} \\ &=e(\boldsymbol{\xi})-\frac{\partial \boldsymbol{I}_{2}}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}} \frac{\partial \boldsymbol{q}}{\partial \delta \boldsymbol{\xi}} \delta \boldsymbol{\xi} \end{aligned}$ ∂I2 /∂u 爲 u 處的像素梯度；
∂u/∂q 爲投影方程關於相機座標系下的三維點的導數。$\frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}}=\left[\begin{array}{ccc}{\frac{\partial u}{\partial X}} & {\frac{\partial u}{\partial Y}} & {\frac{\partial u}{\partial Z}} \\ {\frac{\partial v}{\partial X}} & {\frac{\partial v}{\partial Y}} & {\frac{\partial v}{\partial Z}}\end{array}\right]=\left[\begin{array}{ccc}{\frac{f_{x}}{Z}} & {0} & {-\frac{f_{x} X}{Z^{2}}} \\ {0} & {\frac{f_{y}}{Z}} & {-\frac{f_{y} Y}{Z^{2}}}\end{array}\right]$∂q/∂δξ 爲變換後的三維點對變換的導數：$\frac{\partial \boldsymbol{q}}{\partial \delta \boldsymbol{\xi}}=\left[\boldsymbol{I},-\boldsymbol{q}^{\wedge}\right]$推導了誤差相對於李代數的雅可比矩陣：$\boldsymbol{J}=-\frac{\partial \boldsymbol{I}_{2}}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \boldsymbol{\xi}}$其中：$\frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}}=\left[\begin{array}{cccccc}{\frac{f_{x}}{Z}} & {0} & {-\frac{f_{x} X}{Z^{2}}} & {-\frac{f_{x} X Y}{Z^{2}}} & {f_{x}+\frac{f_{x} X^{2}}{Z^{2}}} & {-\frac{f_{x} Y}{Z}} \\ {0} & {\frac{f_{y}}{Z}} & {-\frac{f_{y} Y}{Z^{2}}} & {-f_{y}-\frac{f_{y} Y^{2}}{Z^{2}}} & {\frac{f_{y} X Y}{Z^{2}}} & {\frac{f_{y} X}{Z}}\end{array}\right]$然後使用 G-N 或L-M 計算增量，迭代求解。

直接法分類：

• 稀疏直接法通常我們使用數百個至上千個關鍵點，並且像 L-K 光流那樣，假設它周圍像素也是不變的。這種稀疏直接法不必計算描述子，並且只使用數百個像素，因此速度最快，但只能計算稀疏的重構。
• 考慮只使用帶有梯度的像素點，捨棄像素梯度不明顯的地方。這稱之爲半稠密（Semi-Dense）的直接法，可以重構一個半稠密結構。
• P 爲所有像素，稱爲稠密直接法。稠密重構需要計算所有像素（一般幾十萬至幾百個），因此多數不能在現有的 CPU 上實時計算，需要GPU 的加速。

直接法完全依靠優化來求解相機位姿。像素梯度引導着優化的方向。如果我們想要得到正確的優化結果，就必須保證大部分像素梯度能夠把優化引導到正確的方向。

優點：

• 可以省去計算特徵點、描述子的時間。
• 只要求有像素梯度即可，無須特徵點。因此，直接法可以在特徵缺失的場合下使用。比較極端的例子是隻有漸變的一張圖像。它可能無法提取角點類特徵，但可以用直接法估計它的運動。
• 可以構建半稠密乃至稠密的地圖，這是特徵點法無法做到的。

缺點：

• 非凸性——直接法完全依靠梯度搜索，降低目標函數來計算相機位姿。其目標函數中需要取像素點的灰度值，而圖像是強烈非凸的函數。這使得優化算法容易進入極小，只在運動很小時直接法才能成功。
• 單個像素沒有區分度。找一個和他像的實在太多了！——於是我們要麼計算圖像塊，要麼計算複雜的相關性。由於每個像素對改變相機運動的“意見”不一致。只能少數服從多數，以數量代替質量。
• 灰度值不變是很強的假設。如果相機是自動曝光的，當它調整曝光參數時，會使得圖像整體變亮或變暗。光照變化時亦會出現這種情況。特徵點法對光照具有一定的容忍性，而直接法由於計算灰度間的差異，整體灰度變化會破壞灰度不變假設，使算法失敗。針對這一點，目前的直接法開始使用更細緻的光度模型標定相機，以便在曝光時間變化時也能讓直接法工作。

2、什麼是閉環檢測？常用的方法有哪些？有沒有創新？
閉環檢測：

• 相機經過了同一個地方，採集到了相似的數據。迴環檢測的關鍵，就是如何有效地檢測出相機經過同一個地方這件事。可以爲後端的 Pose
  Graph 提供更多的有效數據，使之得到更好的估計，特別是得到一個全局一致（GlobalConsistent）的估計。還可以利用迴環檢測進行重定位。

方法：

• 對任意兩張圖像都做一遍特徵匹配，根據正確匹配的數量確定哪兩個圖像存在關聯。
• Bag-of-Words（BoW）----k-means++、K 叉樹字典

創新：

• 是否能對機器學習的圖像特徵進行聚類，而不是 SURF、ORB 這樣的人工設計特徵 進行聚類？
• 是否有更好的方式進行聚類，而不是用樹結構加上 K-means 這些較樸素的方式？結合目前機器學習的發展，二進制描述子的學習和無監督的聚類，都是很有望在深度學習框架中得以解決的問題。
• HBST：https://gitlab.com/srrg-software/srrg_hbst

3、簡單描述Ceres優化庫
Ceres 庫面向通用的最小二乘問題的求解，Ceres 求解的最小二乘問題最一般的形式如下（帶邊界的核函數最小二乘）：$\begin{array}{ll}{\min _{x}} & {\frac{1}{2} \sum_{i} \rho_{i}\left(\left\|f_{i}\left(x_{i_{1}}, \ldots x_{i_{n}}\right)\right\|^{2}\right)} \\ {\text { s.t. }} & {l_{j} \leq x_{j} \leq u_{j}}\end{array}$核函數 ρ(·)，優化變量爲 x1 ,…,xn ，fi 稱爲代價函數（Cost function），在 SLAM 中亦可理解爲誤差項。lj 和 uj 爲第 j 個優化變量的上限和下限。在 Ceres 中，將定義優化變量 x 和每個代價函數 fi ，再調用 Ceres 進行求解。可以選擇使用 G-N 或者 L-M 進行梯度下降，並設定梯度下降的條件，Ceres 會在優化之後，將最優估計值返回

4、描述（擴展）卡爾曼濾波與粒子濾波。
KF：主要參考這篇博客https://blog.csdn.net/u010720661/article/details/63253509

EKF：

• KF主要針對的是線性系統，SLAM 中的運動方程和觀測方程通常是非線性函數，尤其是視覺 SLAM中的相機模型，需要使用相機內參模型以及李代數表示的位姿，更不可能是一個線性系統。
• 一個高斯分佈，經過非線性變換後，往往不再是高斯分佈，所以在非線性系統中，必須取一定的近似，將一個非高斯的分佈近似成一個高斯分佈。

把卡爾曼濾波器的結果拓展到非線性系統中來，稱爲擴展卡爾曼濾波器（Ex-tended Kalman Filter，EKF）。通常的做法是，在某個點附近考慮運動方程以及觀測方程的一階泰勒展開，只保留一階項，即線性的部分，然後按照線性系統進行推導。令 k−1 時刻的均值與協方差矩陣爲：

$\hat{\boldsymbol{x}}_{k-1}, \hat{\boldsymbol{P}}_{k-1}$卡爾曼增益$\boldsymbol{K}_{k}$$\boldsymbol{K}_{k}=\overline{\boldsymbol{P}}_{k} \boldsymbol{H}^{\mathrm{T}}\left(\boldsymbol{H} \overline{\boldsymbol{P}}_{k} \boldsymbol{H}^{\mathrm{T}}+\boldsymbol{Q}_{k}\right)^{-1}$預測和更新方程：$\hat{\boldsymbol{x}}_{k}=\overline{\boldsymbol{x}}_{k}+\boldsymbol{K}_{k}\left(\boldsymbol{z}_{k}-h\left(\overline{\boldsymbol{x}}_{k}\right)\right), \hat{\boldsymbol{P}}_{k}=\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}\right) \overline{\boldsymbol{P}}_{k}$
EKF的侷限：

• 濾波器方法在一定程度上假設了馬爾可夫性，也就是 k 時刻的狀態只與 k −1時刻相關，而與 k − 1 之前的狀態和觀測都無關。如果當前幀確實與很久之前的數據有關（例如迴環），那麼濾波器就會難以處理這種情況。
• EKF 濾波器僅在 $\hat{\boldsymbol{x}}_{k-1}$處做了一次線性化，但一階泰勒展開並不一定能夠近似整個函數。如果系統有強烈的非線性，那線性近似就只在很小範圍內成立，不能認爲在很遠的地方仍能用線性來近似。這就是 EKF 的非線性誤差，是它的主要問題所在。
• 從程序實現上來說，EKF 需要存儲狀態量的均值和方差，並對它們進行維護和更新。如果把路標也放進狀態的話，由於視覺 SLAM 中路標數量很大，這個存儲量是相當可觀的，且與狀態量呈平方增長（因爲要存儲協方差矩陣）。因此，EKF SLAM 普遍被認爲不可適用於大型場景。

通常認爲，在同等計算量的情況下，非線性優化能取得更好的效果。
粒子濾波：
主要參考博客：https://blog.csdn.net/piaoxuezhong/article/details/78619150

未完待續。。。。