SLAM基礎問題總結（1）

1、如何對匹配好的點做進一步的處理，更好保證匹配效果
（1）確定匹配最大距離，漢明距離小於最小距離的兩倍
（2）使用KNN-matching算法，令K=2。則每個match得到兩個最接近的descriptor，然後計算最接近距離和次接近距離之間的比值，當比值大於既定值時，才作爲最終match。
（3）RANSAC（使用RANSAC找到最佳單應性矩陣。由於這個函數使用的特徵點同時包含正確和錯誤匹配點，因此計算的單應性矩陣依賴於二次投影的準確性）
以上方法均爲傳統的特徵匹配篩選算法：https://www.cnblogs.com/Jessica-jie/p/8622449.html
（4）在ORB-SLAM中，根據圖像的連續性，使用網格化與圖像金字塔的方式，實現ORB特徵點之間的的特徵匹配。雖然匹配效果較好，但是速度較慢，且匹配的特徵點數量較少。最後還結合和RANSAC算法，估計H或F矩陣，實現單目的初始化。
（5）GMS（https://github.com/JiawangBian/GMS-Feature-Matcher）是目前速度最快，效果最好的一種特徵匹配算法。通過匹配的特徵點周圍特徵匹配的數量，構建概率統計模型，獲得正確的特徵匹配。

2、單目相機，F和H矩陣有何不同，E和F矩陣有何不同，只旋轉不平移能不能求F，只旋轉不平移能不能求H
（1）F矩陣描述的是點與線之間的關係，即空間點的極限約束。H矩陣描述的是圖像中同一平面的點的對應關係。
（2）$\mathrm{E}=\mathrm{t}^{\wedge} \mathrm{R}$， $F=K^{-T} E K^{-1}$ ，$\mathrm{H}=\mathrm{R}-\mathrm{t}^{\star} \mathrm{n} \mathrm{T} / \mathrm{d}$
（3）在相機只有旋轉而沒有平移的情況，此時t爲0，E也將爲0，導致無法求解R，這時可以使用單應矩陣H求旋轉，但僅有旋轉，無法三角化求深度。

3、什麼是BA?推導相機投影模型的雅克比矩陣J，以及J的每一項代表的含義（視覺SLAM14講）
BA的本質是一個優化模型，其目的是最小化重投影/光度誤差，用於優化相機位姿和世界點。如圖 7-12 所示，我們通過特徵匹配，知道了 p1和 p2 是同一個空間點 P 的投影，但是我們不知道相機的位姿。在初始值中， P 的投影 p^2與實際的 p2 之間有一定的距離。於是我們調整相機的位姿，使得這個距離變小。不過，由於這個調整需要考慮很多個點，所以最後每個點的誤差通常都不會精確爲零。

局部BA用於優化局部的相機位姿，提高跟蹤的精確度；全局BA用於全局過程中的相機位姿，使相機經過長時間、長距離的移動之後，相機位姿還比較準確。BA是一個圖優化模型，一般選擇LM(Levenberg-Marquardt)算法並在此基礎上利用BA模型的稀疏性進行計算；可以直接計算，也可以使用g2o或者Ceres等優化庫進行計算。
BA優化的推導過程：
考慮 n 個三維空間點 P 和它們的投影 p，我們希望計算相機的位姿$\boldsymbol{R}, \boldsymbol{t}$，它的李代數表示爲 ξ。假設某空間點座標爲$\boldsymbol{P}_{i}=\left[X_{i}, Y_{i}, Z_{i}\right]^{T}$，其投影的像素座標爲$\boldsymbol{u}_{i}=\left[u_{i}, v_{i}\right]^{T}$。像素位置與空間點位置的關係如下：
$s_{i}\left[\begin{array}{c}{u_{i}} \\ {v_{i}} \\ {1}\end{array}\right]=\boldsymbol{K} \exp \left(\boldsymbol{\xi}^{\wedge}\right)\left[\begin{array}{c}{X_{i}} \\ {Y_{i}} \\ {Z_{i}} \\ {1}\end{array}\right]$寫成矩陣形式就是（K爲相機內部參數）：
$s_{i} \boldsymbol{u}_{i}=\boldsymbol{K} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}_{i}$由於相機位姿未知以及觀測點的噪聲，該等式存在一個誤差。因此，我們把誤差求和，構建最小二乘問題，然後尋找最好的相機位姿，使它最小化：
$\xi^{*}=\arg \min _{\xi} \frac{1}{2} \sum_{i=1}^{n}\left\|u_{i}-\frac{1}{s_{i}} K \exp \left(\xi^{\wedge}\right) P_{i}\right\|_{2}^{2}$以上是一個最小二乘問題，使用李代數，可以構建無約束的優化，很方便地通過 G-N, L-M 等優化算法進行求解。在使用 G-N 和 L-M 之前，我們需要知道每個誤差項關於優化變量的導數，也就是線性化：
$e(\boldsymbol{x}+\Delta \boldsymbol{x}) \approx \boldsymbol{e}(\boldsymbol{x})+\boldsymbol{J} \Delta \boldsymbol{x}$當 e 爲像素座標誤差（2 維）， x 爲相機位姿（6 維）時， J 將是一個 2 × 6 的矩陣。
首先，記變換到相機座標系下的空間點座標爲 P ′，並且把它前三維取出來：
$\left.\boldsymbol{P}^{\prime}=\left(\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}\right)_{1 : 3}=\left[X^{\prime}, Y^{\prime}\right], Z^{\prime}\right]^{T}$那麼，相機投影模型相對於 P ′ 則爲：
$s \boldsymbol{u}=\boldsymbol{K} \boldsymbol{P}^{\prime}$展開有：$\left[\begin{array}{c}{s u} \\ {s v} \\ {s}\end{array}\right]=\left[\begin{array}{ccc}{f_{x}} & {0} & {c_{x}} \\ {0} & {f_{y}} & {c_{y}} \\ {0} & {0} & {1}\end{array}\right]\left[\begin{array}{c}{X^{\prime}} \\ {Y^{\prime}} \\ {Z^{\prime}}\end{array}\right]$化簡：$u=f_{x} \frac{X^{\prime}}{Z^{\prime}}+c_{x}, \quad v=f_{y} \frac{Y^{\prime}}{Z^{\prime}}+c_{y}$求誤差時，可以把這裏的 u，v 與實際的測量值比較，求差。在定義了中間變量後，對 ξ^ 左乘擾動量 δξ，然後考慮 e 的變化關於擾動量的導數。利用鏈式法則，可以列寫如下：
$\frac{\partial e}{\partial \delta \xi}=\lim _{\delta \xi \rightarrow 0} \frac{e(\delta \xi \oplus \xi)}{\delta \xi}=\frac{\partial e}{\partial P^{\prime}} \frac{\partial P^{\prime}}{\partial \delta \xi}$上式第一項是誤差關於投影點的導數：
$\frac{\partial e}{\partial \boldsymbol{P}^{\prime}}=-\left[\begin{array}{ccc}{\frac{\partial u}{\partial X^{\prime}}} & {\frac{\partial u}{\partial Y^{\prime}}} & {\frac{\partial u}{\partial Z^{\prime}}} \\ {\frac{\partial v}{\partial X^{\prime}}} & {\frac{\partial v}{\partial Y^{\prime}}} & {\frac{\partial v}{\partial Z^{\prime}}}\end{array}\right]=-\left[\begin{array}{ccc}{\frac{f_{x}}{Z^{\prime}}} & {0} & {-\frac{f_{x} X^{\prime}}{Z^{\prime 2}}} \\ {0} & {\frac{f_{y}}{Z^{\prime}}} & {-\frac{f_{y} Y^{\prime}}{Z^{\prime 2}}}\end{array}\right]$而第二項爲變換後的點關於李代數的導數：$\frac{\partial(\boldsymbol{T} \boldsymbol{P})}{\partial \delta \boldsymbol{\xi}}=(\boldsymbol{T} \boldsymbol{P})^{\odot}=\left[\begin{array}{cc}{\boldsymbol{I}} & {-\boldsymbol{P}^{\prime \wedge}} \\ {\boldsymbol{0}^{T}} & {\boldsymbol{0}^{T}}\end{array}\right]$取出了前三維：
$\frac{\partial P^{\prime}}{\partial \delta \xi}=\left[I,-P^{\prime \prime}\right]$將這兩項相乘，就得到了 2 × 6 的雅可比矩陣J:
$\frac{\partial e}{\partial \delta \xi}=-\left[\begin{array}{cccccc}{\frac{f_{x}}{Z^{\prime}}} & {0} & {-\frac{f_{x} X^{\prime}}{Z^{\prime 2}}} & {-\frac{f_{x} X^{\prime} Y^{\prime}}{Z^{2}}} & {f_{x}+\frac{f_{x} X^{2}}{Z^{\prime 2}}} & {-\frac{f_{x} Y^{\prime}}{Z^{\prime}}} \\ {0} & {\frac{f_{y}}{Z^{\prime}}} & {-\frac{f_{y} Y^{\prime}}{Z^{\prime 2}}} & {-f_{y}-\frac{f_{y} Y^{\prime 2}}{Z^{\prime 2}}} & {\frac{f_{y} X^{\prime} Y^{\prime}}{Z^{\prime 2}}} & {\frac{f_{y} X^{\prime}}{Z^{\prime}}}\end{array}\right]$這個雅可比矩陣描述了重投影誤差關於相機位姿李代數的一階變化關係。我們保留了前面的負號，因爲這是由於誤差是由觀測值減預測值定義的。另一方面，除了優化位姿，我們還希望優化特徵點的空間位置。因此，需要討論 e 關於空間點 P 的導數。所幸這個導數矩陣相對來說容易一些。仍利用鏈式法則，有：$\frac{\partial e}{\partial \boldsymbol{P}}=\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}^{\prime}} \frac{\partial \boldsymbol{P}^{\prime}}{\partial \boldsymbol{P}}$第一項已在前面推導了，第二項，按照定義：$\boldsymbol{P}^{\prime}=\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}=\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t}$我們發現 P ′ 對 P 求導後只剩下 R。於是：$\frac{\partial e}{\partial \boldsymbol{P}}=-\left[\begin{array}{ccc}{\frac{f_{x}}{Z^{\prime}}} & {0} & {-\frac{f_{x} X^{\prime}}{Z^{\prime 2}}} \\ {0} & {\frac{f_{y}}{Z^{\prime}}} & {-\frac{f_{y} Y^{\prime}}{Z^{\prime 2}}}\end{array}\right] \boldsymbol{R}$於是，我們推導了觀測相機方程關於相機位姿與特徵點的兩個導數矩陣。它們十分重要，能夠在優化過程中提供重要的梯度方向，指導優化的迭代。
4、單目初始化的目的及兩種方法
單目初始化的目的：構建單目系統的歸一化尺度，獲得初始的三維地圖點。

在單目視覺中，我們對兩張圖像的 t 歸一化，相當於固定了尺度。雖然我們不知道它的實際長度爲多少，但我們以這時的 t 爲單位 1，計算相機運動和特徵點的 3D 位置。這被稱爲單目 SLAM 的初始化。
在初始化之後，就可以用 3D-2D 來計算相機運動了。初始化之後的軌跡和地圖的單位，就是初始化時固定的尺度。因此，單目 SLAM 有一步不可避免的初始化。初始化的兩張圖像必須有一定程度的平移，而後的軌跡和地圖都將以此步的平移爲單位。

單目初始化的兩種方法：使用基本矩陣F（極限約束），或是單應性矩陣H（平面結構）進行初始化。

從 E 分解到 R; t 的過程中，如果相機發生的是純旋轉，導致 t 爲零，那麼，得到的E 也將爲零，這將導致我們無從求解 R。不過，此時我們可以依靠 H 求取旋轉，但僅有旋轉時，我們無法用三角測量估計特徵點的空間位置（這將在下文提到），於是，另一個結論是， 單目初始化不能只有純旋轉，必須要有一定程度的平移。如果沒有平移，單目將無法初始化。
5、三角測量的過程及代碼實現，有哪些不確定性及如何提高三角測量的精度

按照對極幾何中的定義，設 x 1 ,x 2 爲兩個特徵點p1,p2的歸一化座標，那麼它們滿足：
$s_{1} \boldsymbol{x}_{1}=s_{2} \boldsymbol{R} \boldsymbol{x}_{2}+\boldsymbol{t}$如果我要算 s2，那麼先對上式兩側左乘一個$\boldsymbol{x}_{1}^{\wedge}$ ，得：$s_{1} \boldsymbol{x}_{1}^{\wedge} \boldsymbol{x}_{1}=0=s_{2} \boldsymbol{x}_{1}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{2}+\boldsymbol{x}_{1}^{\wedge} \boldsymbol{t}$該式左側爲零，右側可看成 s2 的一個方程，可以根據它直接求得 s2 。有了 s2，s1 也非常容易求出。於是，我們就得到了兩個幀下的點的深度，確定了它們的空間座標。當然，由於噪聲的存在，我們估得的 R,t，不一定精確使式子爲零，所以更常見做法求最小二乘解而不是零解。
在視覺SLAM14講中有一個關於三角測量的代碼實現，使用的是opencv中的cv::triangulatePoints函數。在ORB-SLAM中也有關於三角測量的代碼，使用的是SVD奇異值分解，代碼如下：

// Trianularization: 已知匹配特徵點對{x x'} 和 各自相機矩陣{P P'}, 估計三維點 X
// x' = P'X  x = PX
// 它們都屬於 x = aPX模型
//                         |X|
// |x|     |p1 p2  p3  p4 ||Y|     |x|    |--p0--||.|
// |y| = a |p5 p6  p7  p8 ||Z| ===>|y| = a|--p1--||X|
// |z|     |p9 p10 p11 p12||1|     |z|    |--p2--||.|
// 採用DLT的方法：x叉乘PX = 0
// |yp2 -  p1|     |0|
// |p0 -  xp2| X = |0|
// |xp1 - yp0|     |0|
// 兩個點:
// |yp2   -  p1  |     |0|
// |p0    -  xp2 | X = |0| ===> AX = 0
// |y'p2' -  p1' |     |0|
// |p0'   - x'p2'|     |0|
// 變成程序中的形式：
// |xp2  - p0 |     |0|
// |yp2  - p1 | X = |0| ===> AX = 0
// |x'p2'- p0'|     |0|
// |y'p2'- p1'|     |0|

/**
 * @brief 給定投影矩陣P1,P2和圖像上的點kp1,kp2，從而恢復3D座標
 *  * @param kp1 特徵點, in reference frame
 * @param kp2 特徵點, in current frame
 * @param P1  投影矩陣P1
 * @param P2  投影矩陣P２
 * @param x3D 三維點
 * @see       Multiple View Geometry in Computer Vision - 12.2 Linear triangulation methods p312
 */
void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{
    // 在DecomposeE函數和ReconstructH函數中對t有歸一化
    // 這裏三角化過程中恢復的3D點深度取決於 t 的尺度，
    // 但是這裏恢復的3D點並沒有決定單目整個SLAM過程的尺度
    // 因爲CreateInitialMapMonocular函數對3D點深度會縮放，然後反過來對 t 有改變

    cv::Mat A(4,4,CV_32F);

    A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
    A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
    A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);    
    A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);

    cv::Mat u,w,vt;
    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
    x3D = vt.row(3).t();   //奇異值最小的爲恢復的3D點
    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);   //齊次座標
}

三角測量的精度提高：

• 當平移很小時，像素上的不確定性將導致較大的深度不確定性。也就是說，如果特徵點運動一個像素 δx，使得視線角變化了一個角度 δθ，那麼測量到深度值將有 δd 的變化。從幾何關係可以看到，當 t 較大時，δd將明顯變小，這說明平移較大時，在同樣的相機分辨率下，三角化測量將更精確。但是，在增大平移，會導致匹配失效；而平移太小，則三角化精度不夠。

在SVO中，使用深度濾波器對估計的三維點進行深度估計。假設特徵點服從高斯分佈，並且對它不斷地進行觀測，在信息正確的情況下，我們就能夠期望它的方差會不斷減小乃至收斂。這就得到了一個濾波器，稱爲深度濾波器（Depth Filter）。
6、描述PnP
PnP（Perspective-n-Point）是求解 3D 到 2D 點對運動的方法。它描述了當我們知道n 個 3D 空間點以及它們的投影位置時，如何估計相機所在的位姿。目前遇到的場景主要有兩個，其一是求解相機相對於某2維圖像/3維物體的位姿；其二就是SLAM算法中估計相機位姿時通常需要PnP給出相機初始位姿。

• 在場景1中，我們通常輸入的是物體在世界座標系下的3D點以及這些3D點在圖像上投影的2D點，因此求得的是相機座標系相對於世界座標系(Twc)的位姿

• 在場景2中，通常輸入的是上一幀中的3D點（在上一幀的相機座標系下表示的點）和這些3D點在當前幀中的投影得到的2D點，所以它求得的是當前幀相對於上一幀的位姿變換

兩種情況本質上是相同的，都是基於已知3D點和對應的圖像2D點求解相機運動的過程。
PnP 問題有很多種求解方法，例如用三對點估計位姿的 P3P[45]，直接線性變換（DLT），EPnP（Efficient PnP），UPnP等等）。此外，還能用非線性優化的方式，構建最小二乘問題並迭代求解，也就是萬金油式的 Bundle Adjustment。
7、相機標定原理及實現過程，棋盤格有哪些好處
相機標定分爲單目相機標定和雙目相機標定，很多博客對此進行了詳細的介紹：
https://www.cnblogs.com/zyly/p/9366080.html
https://www.cnblogs.com/rainbow70626/p/5514598.html
https://www.cnblogs.com/wangguchangqing/p/8335131.html
這些博客寫的非常詳細了，因此不再進行過多的描述。棋盤格標定板具有製作簡單，精度高的好處。