SLAM基礎問題總結（2）

1、什麼是對極約束？基礎矩陣F的推導過程。

簡單來講：就是圖像 $I_{1}$ 中特徵點P1，其對應的特徵匹配點在圖像 $I_{2}$ 的極線L2上。
但是，由於我們通過特徵點匹配，確定了p2的像素位置，所以能夠推斷P的空間位置，以及相機的運動。這都是多虧了正確的特徵匹配。如果沒有特徵匹配，我們就沒法確定 p2到底在極線的哪個位置了，那時，就必須在極線上搜索以獲得正確的匹配，非常耗時。

基本矩陣的推導：
在第一幀的座標系下，設 P 的空間位置爲： $\boldsymbol{P}=[X, Y, Z]^{T}$
根據針孔相機模型，我們知道兩個像素點 p1 ,p2 的像素位置爲： $s_{1} \boldsymbol{p}_{1}=\boldsymbol{K} \boldsymbol{P}, \quad s_{2} \boldsymbol{p}_{2}=\boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t})$

取： $\boldsymbol{x}_{1}=\boldsymbol{K}^{-1} \boldsymbol{p}_{1}, \quad \boldsymbol{x}_{2}=\boldsymbol{K}^{-1} \boldsymbol{p}_{2}$ 這裏的 x1 ,x2 是兩個像素點的歸一化平面上的座標。歸一化尺度代入上式，得： $\boldsymbol{x}_{2}=\boldsymbol{R} \boldsymbol{x}_{1}+\boldsymbol{t}$ 兩邊同時左乘 $\boldsymbol{t}^{\wedge}$ 。然後，兩側同時左乘 $\boldsymbol{x}_{2}^{T}$
$\boldsymbol{x}_{2}^{T} \boldsymbol{t}^{\wedge} \boldsymbol{x}_{2}=\boldsymbol{x}_{2}^{T} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}$ $\boldsymbol{t}^{\wedge} \boldsymbol{x}_{2}$ 是一個與 t 和 x2 都垂直的向量。把它再和 x2 做內積時，將得到 0。
$\boldsymbol{x}_{2}^{T} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}=0$ 重新代入 p1 ,p2 ，有： $\boldsymbol{p}_{2}^{T} \boldsymbol{K}^{-T} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{K}^{-1} \boldsymbol{p}_{1}=0$ 這兩個式子都稱爲對極約束。中間部分記作兩個矩陣：基礎矩陣（Fun-damental Matrix）F 和本質矩陣（Essential Matrix）E，可以進一步簡化對極約束： $\boldsymbol{E}=\boldsymbol{t}^{\wedge} \boldsymbol{R}, \quad \boldsymbol{F}=\boldsymbol{K}^{-T} \boldsymbol{E} \boldsymbol{K}^{-1}, \quad \boldsymbol{x}_{2}^{T} \boldsymbol{E} \boldsymbol{x}_{1}=\boldsymbol{p}_{2}^{T} \boldsymbol{F} \boldsymbol{p}_{1}=0$ 對極約束簡潔地給出了兩個匹配點的空間位置關係。
2、RANSAC的過程及在基礎矩陣F求解上的應用，代碼實現一個例子
RANSAC：擬合直線，減少誤匹配。
RANSAC算法的輸入是一組觀測數據（往往含有較大的噪聲或無效點），一個用於解釋觀測數據的參數化模型以及一些可信的參數。RANSAC通過反覆選擇數據中的一組隨機子集來達成目標。被選取的子集被假設爲局內點，並用下述方法進行驗證：

有一個模型適應於假設的局內點，即所有的未知參數都能從假設的局內點計算得出。
用1中得到的模型去測試所有的其它數據，如果某個點適用於估計的模型，認爲它也是局內點。
如果有足夠多的點被歸類爲假設的局內點，那麼估計的模型就足夠合理。
然後，用所有假設的局內點去重新估計模型（譬如使用最小二乘法），因爲它僅僅被初始的假設局內點估計過。
最後，通過估計局內點與模型的錯誤率來評估模型。

上述過程被重複執行固定的次數，每次產生的模型要麼因爲局內點太少而被捨棄，要麼因爲比現有的模型更好而被選用。
整個過程可參考下圖：
在模型確定以及最大迭代次數允許的情況下，RANSAC總是能找到最優解。
RANSAC很好的例子是在ORB-SLAM的單目初始化中：

......
    // 匹配上的特徵點的個數
    const int N = mvMatches12.size();

    // Indices for minimum set selection
    // 新建一個容器vAllIndices，生成0到N-1的數作爲特徵點的索引
    vector<size_t> vAllIndices;
    vAllIndices.reserve(N);
    vector<size_t> vAvailableIndices;

    for(int i=0; i<N; i++)
    {
        vAllIndices.push_back(i);
    }

    // Generate sets of 8 points for each RANSAC iteration
    // 步驟2：在所有匹配特徵點對中隨機選擇8對匹配特徵點爲一組，共選擇mMaxIterations組
    // 用於FindHomography和FindFundamental求解
    // mMaxIterations:200

    ///< 二維容器，外層容器的大小爲迭代次數，內層容器大小爲每次迭代算H或F矩陣需要的點
    mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));

    DUtils::Random::SeedRandOnce(0);

    for(int it=0; it<mMaxIterations; it++)
    {
        vAvailableIndices = vAllIndices;

        // Select a minimum set
        for(size_t j=0; j<8; j++)
        {
            // 產生0到N-1的隨機數
            int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
            // idx表示哪一個索引對應的特徵點被選中
            int idx = vAvailableIndices[randi];

            mvSets[it][j] = idx;

            // randi對應的索引已經被選過了，從容器中刪除
            // randi對應的索引用最後一個元素替換，並刪掉最後一個元素
            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }
    }

    // Launch threads to compute in parallel a fundamental matrix and a homography
    // 步驟3：調用多線程分別用於計算fundamental matrix和homography
    vector<bool> vbMatchesInliersH, vbMatchesInliersF;   //經過重投影誤差篩選之後的匹配點
    float SH, SF; // score for H and F
    cv::Mat H, F; // H and F

    // ref是引用的功能:http://en.cppreference.com/w/cpp/utility/functional/ref
    // 計算homograpy並打分
    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    // 計算fundamental matrix並打分
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

    // Wait until both threads have finished
    threadH.join();
    threadF.join();

    // Compute ratio of scores
    // 步驟4：計算得分比例，選取某個模型
    float RH = SH/(SH+SF);

    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    // 步驟5：從H矩陣或F矩陣中恢復R,t
    if(RH>0.40)
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    else //if(pF_HF>0.6)
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

    return false;

3、求解BA問題用的最多的G2o，說明G2o的頂點和邊代表的含義以及類型

圖優化，是把優化問題表現成圖（Graph）的一種方式。這裏的圖是圖論意義上的圖。一個圖由若干個頂點（Vertex），以及連接着這些節點的邊（Edge）組成。進而，用頂點表示優化變量，用邊表示誤差項，g2o 爲 SLAM 提供了圖優化所需的內容，節點爲優化變量，邊爲誤差項。**
4、GN和LM的迭代過程及優劣
Gauss-Newton迭代方程：
$\boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}=-\boldsymbol{J}(\boldsymbol{x})^{T} f(\boldsymbol{x})$ 注意，我們要求解的變量是 ∆x，因此這是一個線性方程組，我們稱它爲增量方程，也可以稱爲高斯牛頓方程 (Gauss Newton equations) 或者正規方程 (Normal equations)。我們把左邊的係數定義爲 H，右邊定義爲 g，那麼上式變爲：
$\boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g}$ Gauss-Newton 的算法步驟可以寫成：

給定初始值 $\boldsymbol{x}_{0}$ 。
對於第 k 次迭代，求出當前的雅可比矩陣 $\boldsymbol{J}\left(\boldsymbol{x}_{k}\right)$ 和誤差。
求解增量方程： $\boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g}$ .
若 $\Delta x_{k}$ 足夠小，則停止。否則，令 $\boldsymbol{x}_{k+1}=\boldsymbol{x}_{k}+\Delta \boldsymbol{x}_{k}$ ，返回 2.

Gauss-Newton缺點：在使用 Gauss Newton 方法時，可能出現 $\boldsymbol{J}^{T} \boldsymbol{J}$ 爲奇異矩陣或者病態 (ill-condition) 的情況，此時增量的穩定性較差，導致算法不收斂。

Levenberg-Marquadt：
由於 Gauss-Newton 方法中採用的近似二階泰勒展開只能在展開點附近有較好的近似效果，所以我們很自然地想到應該給 ∆x 添加一個信賴區域（Trust Region），不能讓它太大而使得近似不準確。非線性優化種有一系列這類方法，這類方法也被稱之爲信賴區域方法 (Trust Region Method)。
$\rho=\frac{f(\boldsymbol{x}+\Delta \boldsymbol{x})-f(\boldsymbol{x})}{\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}}$ ρ 的分子是實際函數下降的值，分母是近似模型下降的值。如果 ρ 接近於 1，則近似是好的。如果 ρ 太小，說明實際減小的值遠少於近似減小的值，則認爲近似比較差，需要縮小近似範圍。反之，如果 ρ 比較大，則說明實際下降的比預計的更大，我們可以放大近似範圍。

Levenberg-Marquadt迭代過程：

給定初始值 $\boldsymbol{x}_{0}$ ，以及初始優化半徑 µ。
對於第 k 次迭代，求解：
$\min _{\Delta \boldsymbol{x}_{k}} \frac{1}{2}\left\|f\left(\boldsymbol{x}_{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}_{k}\right) \Delta \boldsymbol{x}_{k}\right\|^{2}, \quad \text { s.t. }\left\|\boldsymbol{D} \Delta \boldsymbol{x}_{k}\right\|^{2} \leq \mu$ 這裏 µ 是信賴區域的半徑。
計算 ρ。
若 ρ >3/4 則 µ = 2µ；
若 ρ <1/4 則 µ = 0.5µ；
如果 ρ 大於某閾值，認爲近似可行。令 $\boldsymbol{x}_{k+1}=\boldsymbol{x}_{k}+\Delta \boldsymbol{x}_{k}$ 。
判斷算法是否收斂。如不收斂則返回 2，否則結束。

在 Levenberg 提出的優化方法中，把 D 取成單位陣 I，相當於直接把 ∆x 約束在一個球中。隨後，Marqaurdt 提出將 D 取成非負數對角陣——實際中通常用 $\boldsymbol{J}^{T} \boldsymbol{J}$ 的對角元素平方根，使得在梯度小的維度上約束範圍更大一些。用 Lagrange 乘子將LM優化轉化爲一個無約束優化問題：
$\min _{\Delta \boldsymbol{x}_{k}} \frac{1}{2}\left\|f\left(\boldsymbol{x}_{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}_{k}\right) \Delta \boldsymbol{x}_{k}\right\|^{2}+\frac{\lambda}{2}\|\boldsymbol{D} \Delta \boldsymbol{x}\|^{2}$ 這裏 λ 爲 Lagrange 乘子。把它展開後，我們發現該問題的核心仍是計算增量的線性方程：
$\left(\boldsymbol{H}+\lambda \boldsymbol{D}^{T} \boldsymbol{D}\right) \Delta \boldsymbol{x}=\boldsymbol{g}$ 如果考慮它的簡化形式，即 D = I，那麼相當於求解：
$(\boldsymbol{H}+\lambda \boldsymbol{I}) \Delta \boldsymbol{x}=\boldsymbol{g}$ Levenberg-Marquadt優點：
當參數 λ 比較小時，H 佔主要地位，這說明二次近似模型在該範圍內是比較好的，L-M 方法更接近於 G-N 法。另一方面，當 λ 比較大時，λI 佔據主要地位，L-M更接近於一階梯度下降法（即最速下降），這說明附近的二次近似不夠好。L-M 的求解方式，可在一定程度上避免線性方程組的係數矩陣的非奇異和病態問題，提供更穩定更準確的增量 ∆x。
5、解釋圖優化中圖的稀疏性，邊緣化過程及可能存在的問題，解釋魯棒核函數。
稀疏性：
在以圖優化框架的視覺 SLAM 算法裏，BA 起到了核心作用。它類似於求解只有觀測方程的 SLAM 問題。
在整體 BA 目標函數上，把自變量定義成所有待優化的變量：
$\boldsymbol{x}=\left[\boldsymbol{\xi}_{1}, \ldots, \boldsymbol{\xi}_{m}, \boldsymbol{p}_{1}, \ldots, \boldsymbol{p}_{n}\right]^{T}$ 相應的，增量方程中的 ∆x 則是對整體自變量的增量。在這個意義下，當我們給自變量一個增量時，目標函數變爲：
$\frac{1}{2}\|f(\boldsymbol{x}+\Delta \boldsymbol{x})\|^{2} \approx \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|\boldsymbol{e}_{i j}+\boldsymbol{F}_{i j} \Delta \boldsymbol{\xi}_{i}+\boldsymbol{E}_{i j} \Delta \boldsymbol{p}_{j}\right\|^{2}$ 其中表示整個兒代價函數在當前狀態下對相機姿態的偏導數，而表示該函數對路標點位置的偏導。
把相機位姿變量放在一起：
$\boldsymbol{x}_{c}=\left[\boldsymbol{\xi}_{1}, \boldsymbol{\xi}_{2}, \ldots, \boldsymbol{\xi}_{m}\right]^{T} \in \mathbb{R}^{6 m}$ 並把空間點的變量也放在一起：
$\boldsymbol{x}_{p}=\left[\boldsymbol{p}_{1}, \boldsymbol{p}_{2}, \ldots, \boldsymbol{p}_{n}\right]^{T} \in \mathbb{R}^{3 n}$
因此有： $\frac{1}{2}\|f(\boldsymbol{x}+\Delta \boldsymbol{x})\|^{2}=\frac{1}{2}\left\|\boldsymbol{e}+\boldsymbol{F} \Delta \boldsymbol{x}_{c}+\boldsymbol{E} \Delta \boldsymbol{x}_{p}\right\|^{2}$ 無論使用 G-N 還是 L-M 方法，最後都將面對增量線性方程：
$\boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g}$ 以 G-N 爲例，則 H 矩陣爲：
$\boldsymbol{H}=\boldsymbol{J}^{T} \boldsymbol{J}=\left[\begin{array}{cc}{\boldsymbol{F}^{T} \boldsymbol{F}} & {\boldsymbol{F}^{T} \boldsymbol{E}} \\ {\boldsymbol{E}^{T} \boldsymbol{F}} & {\boldsymbol{E}^{T} \boldsymbol{E}}\end{array}\right]$ SLAM 的一個重要進展是認識到了矩陣 H 的稀疏結構，並發現該結構可以自然、顯式地用圖優化來表示。假設一個場景內有 2 個相機位姿 (C 1 ,C 2 ) 和 6 個路標 (P 1 ,P 2 ,P 3 ,P 4 ,P 5 ,P 6 )。

可以推出該場景下的 BA 目標函數應該是：
$\frac{1}{2}\left(\left\|e_{11}\right\|^{2}+\left\|e_{12}\right\|^{2}+\left\|e_{13}\right\|^{2}+\left\|e_{14}\right\|^{2}+\left\|e_{23}\right\|^{2}+\left\|e_{24}\right\|^{2}+\left\|e_{25}\right\|^{2}+\left\|e_{26}\right\|^{2}\right)$ 這裏的eij 使用之前定義過的代價函數。以 e11 爲例，它描述了在C1 看到了 P1 這件事，與其他的相機位姿和路標無關。不難看出 e11對相機變量 ξ2 和路標點 p2 ,…,p6 的偏導都爲 0。
$\boldsymbol{J}_{11}=\frac{\partial \boldsymbol{e}_{11}}{\partial \boldsymbol{x}}=\left(\frac{\partial \boldsymbol{e}_{11}}{\partial \boldsymbol{\xi}_{1}}, \mathbf{0}_{2 \times 6}, \frac{\partial \boldsymbol{e}_{11}}{\partial \boldsymbol{p}_{1}}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}\right)$ 爲了方便表示稀疏性，我們用帶有顏色的方塊表示矩陣在該方塊內有數值，其餘沒有顏色的區域表示矩陣在該處數值都爲 0。那麼上面的 J11 則可以表示成圖 10-5 的圖案。同理，其他的雅可比矩陣也會有類似的稀疏圖案。

爲了得到該目標函數對應的雅可比矩陣，我們可以將這些 J ij 按照一定順序列爲向量，那麼整體雅可比矩陣以及相應的 H 矩陣的稀疏情況就是圖 10-6 中所展示的那樣。

對於 H 矩陣當中處於非對角線的矩陣塊來說，如果該矩陣塊非零，則其位置所對應的變量之間會在圖中存在一條邊，我們可以從圖 10-7 中清晰地看到這一點。所以，H 矩陣當中的非對角部分的非零矩陣塊可以理解爲它對應的兩個變量之間存在聯繫，或者可以稱之爲約束。

有 m 個相機位姿，n 個路標點。由於通常路標數量遠遠會比相機多，於是有 n ≫ m。由上面推理可知，實際當中的 H 矩陣會像圖 10-8 所示的那樣。它的左上角塊顯得非常小，而右下角的對角塊佔據了大量地方。除此之外，非對角部分則分佈着散亂的觀測數據。由於它的形狀很像箭頭，又稱爲箭頭形（Arrow-like）

邊緣化：
對於具有這種稀疏結構的 H，利用 H 的稀疏性加速計算：Schur 消元 (Schur trick)。在 SLAM 研究中亦稱爲 Marginalization（邊緣化）。
對應的線性方程組也可以由 H∆x = g 變爲如下形式：
$\left[\begin{array}{ll}{\boldsymbol{B}} & {\boldsymbol{E}} \\ {\boldsymbol{E}^{\boldsymbol{T}}} & {\boldsymbol{C}}\end{array}\right]\left[\begin{array}{l}{\Delta \boldsymbol{x}_{c}} \\ {\Delta \boldsymbol{x}_{p}}\end{array}\right]=\left[\begin{array}{l}{\boldsymbol{v}} \\ {\boldsymbol{w}}\end{array}\right]$ 消去右上角的非對角部分 E，得：
$\left[\begin{array}{cc}{B-E C^{-1} E^{T}} & {0} \\ {E^{T}} & {C}\end{array}\right]\left[\begin{array}{c}{\Delta x_{c}} \\ {\Delta x_{p}}\end{array}\right]=\left[\begin{array}{c}{v-E C^{-1} w} \\ {w}\end{array}\right]$ 經過消元之後，第一行方程組變成和 ∆xp 無關的項。單獨把它拿出來，得到關於位姿部分的增量方程：
$\left[\boldsymbol{B}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{E}^{T}\right] \Delta \boldsymbol{x}_{c}=\boldsymbol{v}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{w}$ 這個線性方程組的維度和 B 矩陣一樣。我們的做法是先求解這個方程，然後把解得的 ∆xc 代入到原方程，然後求解 ∆xp 。這個過程稱爲 Marginalization，或者 Schur消元。我們將此方程的係數記爲S：

S 矩陣的非對角線上的非零矩陣塊，表示了該處對應的兩個相機變量之間存在着共同觀測的路標點，有時候稱爲共視（Co-visibility）。反之，如果該塊爲零，則表示這兩個相機沒有共同觀測
核函數：
在前面的 BA 問題中，我們最小化誤差項的二範數平方和，作爲目標函數。這種做法雖然很直觀，但存在一個嚴重的問題：如果出於誤匹配等原因，某個誤差項給的數據是錯誤的，把一條原本不應該加到圖中的邊給加進去了，然而優化算法並不能辨別出這是個錯誤數據。這時，算法會看到一條誤差很大的邊，它的梯度也很大，意味着調整與它相關的變量會使目標函數下降更多。所以，算法將試圖調整這條邊所連接的節點的估計值，使它們順應這條邊的無理要求。往往會抹平了其他正確邊的影響，使優化算法專注於調整一個錯誤的值。
於是就有了核函數的存在。核函數保證每條邊的誤差不會大的沒邊，掩蓋掉其他的邊。魯棒核函數有許多種，例如最常用的 Huber 核：
$H(e)=\left\{\begin{array}{ll}{\frac{1}{2} e^{2}} & {\text { if }|e| \leq \delta} \\ {\delta\left(|e|-\frac{1}{2} \delta\right)} & {\text { otherwise }}\end{array}\right.$ 當誤差 e 大於某個閾值 δ 後，函數增長由二次形式變成了一次形式，相當於限制了梯度的最大值。同時，Huber 核函數又是光滑的，可以很方便地求導。

除了 Huber 核之外，還有 Cauchy 核，Tukey 核等等。