# Visual Odometry
視覺里程計（Visual Odometry, VO）中基於匹配特徵點推導相機相對運動（旋轉 $R$ 和平移
$𝑡$）的過程，核心是利用對極幾何（Epipolar Geometry）和本質矩陣（Essential Matrix, 𝐸）以及基礎矩陣（Fundamental Matrix, 𝐹）。


## 1.基本符號
- $O_1$和$O_2$顯示的是相機在兩個位置的相機中心
- $P$為三維空間的某一個點，$P = \left[\begin{array}{c} X\\Y\\Z\\ \end{array}\right]$
- $p_1和p_2$是$P$在圖像$I_1和I_2$中的像素座標。
- 極平面（Epipolar Plane）：由 $O_1、O_2和P$構成的平面。
- 極點（Epipole）：基線$O_1O_2$與圖像平面的交點（例如$e_1$是基線在$I_1$上的投影，$e_2$是基線在$I_2$上的投影）。
- 基線（Baseline）：相機中心$O_1和O_2$之間的連線$O_1O_2$。

## 2. 相機成像模型
第一台相機($O_1$): $P$是世界座標(假設第一台相機坐標系為世界坐標系)，投影為
$$
s_1p_1 = KP
$$
第二台相機($O_2$): $P$先經由旋轉和平移($RP+t$)轉道第二台相機坐標系，再投影為
$$
s_2p_2 = K(RP+t)
$$

- $s_1, s_2$: 表示$P$在相機坐標系下的深度(Z座標)。
- $p_1, p_2$: 圖像中的像素座標(通常是齊次座標$P = \left[\begin{array}{ccc} u&v&1\\ \end{array}\right]^T$
- $K$: 相機內參矩陣。
- $R,t$: 相機從$O_1$到$O_2$的旋轉和平移。

將像素坐標($p_1和p_2$)轉換到規範化坐標（normalized coordinates）($x_1和x_2$)，即去除𝐾和𝑠的影響。
$$
x_1 = K^{-1}p_1, \space \\
x_2 = k^{-1}p_2
$$
$x_1和x_2$表示在規範化平面($Z=1$平面)上的座標，形式為$\left[\begin{array}{ccc} x&y&1\\ \end{array}\right]^T$，這樣深度$s$的影響就被消除了。

## 3.規範化座標的運動方程
$$
x_2 \approx Rx_1 + t
$$
$Rx_1 + t$表示點$P$在第一台相機坐標系中的規範化座標$x_1$，經由旋轉($R$)和平移($t$)後，轉換到第二台坐標系中的規範化座標。
這邊寫$\approx$原因是$x_2$和$Rx_1+t$可能不在同一深度(Z座標不同)，但他們的方向相同(在同一個射線上)。

## 4. 推導對極約束

兩邊同時左乘上$t$，等同和$t$做外積(叉積)，這邊要用上叉積矩陣形式$[t]_{\times}$
$$
[t]_{\times} x_2 = [t]_{\times} (Rx_1 + t) = [t]_{\times} Rx_1 + [t]_{\times} t = [t]_{\times} Rx_1 \rightarrow\\
[t]_{\times} x_2 = [t]_{\times} Rx_1
$$
Note: $[t]_{\times} t = 0$ (外積性質)

兩邊同時左乘上$x_2$，
$$
x_2^T[t]_{\times} x_2 = x_2^T[t]_{\times} Rx_1
$$
因為$[t]_{\times} x_2與x_2$是垂直的向量，所以內積等於0→$x_2^T[t]_{\times} x_2=0$
因此
$$
x_2^T[t]_{\times} Rx_1 =0
$$

## 5.本質矩陣(Essential Matrix）和基礎矩陣（Fundamental Matrix）
- 本質矩陣(Essential Matrix)$E = [t]_{\times}R$表示規範化座標系的對極約束。
- 基礎矩陣(Fundamental Matrix)$F=K^{-T}E K^{-1}$表示像素座標下的對極約束。
- $x_2^TEx_1=0$是規範化座標下的對極約束。
- $p_2^TFp_1=0$是像素座標下的對極約束。

## 6. 相機運動估計的步驟

1. 使用匹配點對（$p_1, p_2$）計算 $𝐹$（例如 8 點算法）或 $𝐸$（例如 5 點算法）。
2. 從$E$分解得到 $𝑅$和 $𝑡$（例如使用奇異值分解SVD，選擇滿足正深度約束的解）。
----------

## 完整推導

從相機投影開始:
$$
s_1p_1 = KP,\space\space\space\space\space\space\space\space s_2p_2 = K(RP+t)
$$
轉換到規範化座標:
$$
x_1 = K^{-1}p_1, \space\space x_2=K^{-1}p_2
$$
$$
s_1x_1 = P, \space\space s_2x_2=RP+t
$$
消除$P$:
$$
P=s_1x_1 \rightarrow s_2x_2=R(s_1x_1)+t
$$
$$
s_2x_2 = s_1Rx_1+t
$$
兩邊與t做外積:
$$
t \times s_2x_2 = t \times (s_1Rx_1+t) \rightarrow s_2(t \times x_2)=s_1(t \times Rx_1)+(t \times t)
$$
$$
s_2(t \times x_2) = s_1(t \times Rx_1) \rightarrow t \times x_2 = \frac{s_1}{s_2}(t \times Rx_1)
$$
兩邊左乘 $x_2^T$:
$$
x_2^T(t \times x_2) = \frac{s_1}{s_2}x_2^T(t \times Rx_1)
$$
左邊的 $x_2^T(t \times x_2)=0$ (向量垂直，內積=0)，所以
$$
x_2^T(t \times Rx_1)=0
$$
用叉積矩陣表示
$$
x_2^T[t]_{\times} Rx_1=0
$$
定義本質矩陣(Essential Matrix）:
$$
E = [t]_{\times} R
$$
則:
$$
x_2^TEx_1=0
$$
回到像素座標:

$$
x_1= K^{-1}p_1, \space \space x_2= K^{-1}p_2,
$$
$$
p_2^TK^{-T}EK^{-1}p_1 = 0
$$
定義基礎矩陣(Fundamental Matrix):
$$
F = K^{-T}EK^{-1}
$$
則:
$$
p_2^TFp_1 = 0
$$

--------------------------------
本質矩陣(Essential Matrix）為一個3*3矩陣，因此內有九個未知數，平移和旋轉各有三個自由度，所以E也共有六個自由度，但由於尺度等價性，實際上E只有五個自由度。也是因為E只有五個自由度，所以至少需要用5個點來求解E，但由於E的內在性質是非線性特質，一般用8個點來估算E，更多點的話會用隨機採樣一致性(RANSAC)來估算，求得E後，在用奇異值分解(SVD)來算出$R$和$t$。

但2D相機-2D相機還是有姿態估計的問題
1. 尺度不確定性: 因為估計的位移矢量是沒有單位的，所以相機移動只有相對值，沒有絕對值，所以會將第一幀和第二幀求得的位移矢量做歸一化處理，後面的位移都是相對該結果的變化。
2. 初始化的旋轉問題: 單目初始化必須由一定程度的評移，否則t趨近0時會導致R誤差過大。
3. 多於8個點的情形: 使用特徵點進行估算式採用RANSAC，可有效避免錯誤特徵點。

通過上述方式可以將相鄰幀之間的運動進行局部估算，這可能造成的問題有不可避免的誤差累加，因為每次估算都會有一定程度的誤差，所以誤差會因為運動時間增加，不斷累積，軌跡的漂移會用越來越厲害，所以需要通過**後端的優化和回環檢測**，避免誤差累積越來越大。

## 後端優化
主要針對前面估算的結果優化來取得最佳的姿態估算。
在求解幀之間的旋轉矩陣和位移矢量過程，就是一個系列的$(X和Y)$的參數解，使函數誤差最小→其實就是線性回歸概念。
$X$和$Y$是觀測值: $X$是機器人當下的姿態(位置和姿態)，$Y$是觀察到的路徑座標(圖像中的特徵點)，實際上前端已經推出觀測方程式，但得到的觀測方程式和實際觀察值存在一定程度的誤差，後段就需要將誤差最小化。→利用最小平方法，使殘差平方和最小。
1. 假設有$m$個觀察點: $(x_i, y_i), \forall i=1,...,m$
2. 函數為: $y=f(x, \beta)$，$\mathbf{\beta}=\left(\begin{array}{ccc} \beta_1&\beta_2&...&\beta_n\\ \end{array}\right)$，($m>=n$)
3. find $\beta$→$min\{S\}=min\{ \sum_{i=1}^nr_i^2\}$。
$r_i = y_i - f(x_i, \beta), \forall i=1,...,m$

4. $$\frac{\partial S}{\partial \beta_j}=2\sum_ir_i\frac{\partial r_i}{\partial \beta_j}=0, j=1,...,n$$

5. 非線性系統中，無close-form，利用迭代方式找解，
$$\beta_j \rightarrow \beta_j^{k+1} = \beta_j^k + \Delta\beta_j $$
6. $\beta^k$用泰勒展開式:
$$f(x_i,\beta)\rightarrow f(x_i, \beta^k)+\sum_j \frac{\partial f(x_i, \beta^k)}{\partial \beta_j}(\beta_j - \beta_j^k) \approx f(x_i, \beta^k) + \sum_j J_{ij}\Delta \beta_j$$
令$\frac{r_i}{\beta_j}=-J_{ij}$
7. residual為
$$\Delta y_i = y_i - f(x_i, \beta^k)$$
$$r_i = y_i - f(x, \beta) = (y_i-f(x_i, \beta^k))+(f(x_i, \beta^k)-f(x_i,\beta))=\Delta y_i - \sum_{s=1}^n J_{is}\beta_s
$$
8. 代入(4)
$$\frac{\partial S}{\partial \beta_j}=2\sum_ir_i\frac{\partial r_i}{\partial \beta_j}=0
\rightarrow 2\sum_i(\Delta y_i - \sum_{s=1}^n J_{is}\beta_s)\frac{\partial (\Delta y_i - \sum_{s=1}^n J_{is}\beta_s)}{\partial \beta_j}=0 \\
\rightarrow -2 \sum_{i=1}^m J_{ij} (\Delta y_i - \sum_{s=1}^nJ_{is}\Delta\beta_s) =0 $$
$$
\sum_{i=1}^m \sum_{s=1}^n J_{ij}J_{is}\Delta\beta_s = \sum_{i=1}^mJ_{ij}\Delta y_i, j=1,2,...,n
\rightarrow (\mathbf{J^T}\mathbf{J})\Delta\mathbf{\beta}=\mathbf{J^T}\Delta\mathbf{y}
$$

$\beta$為VSLAM求解得到的$R$和$t$, $\mathbf{J}$為variance對$\beta$的導數。

當$\Delta\beta$夠小的時候就停止訓練，否則將代入下一個位置點繼續優化。

後端優化部分，實際上會更多基於G2O這個圖優化庫進行最小平方法優化。
說詞是因為SLAM本身誤差項目就比較多且不知道之間的關聯性，同時本身系統也是非線性非高斯系統，所以用圖優化被大多數人採納。
圖優化將最佳化表現為圖，"頂點"表示優化變量，"邊"表示優化項目，將非線性的最小平方法問題建構一個對應的圖進行最佳化。G2O中就可以選擇使用高斯牛頓法等方式進行求且非線性最小平方問題。

## 回環檢測
主要目的是讓機器人能夠認識到自己曾經去過的地方，從而解決位置隨時間飄移的問題。
回環檢測依據兩幅圖像的相似性來確認回環檢測的關係。
圖像特徵會以Bag-of-Words(BoW)來描述和存放，每個BoW會存放單詞(word)，多個單詞組成字典，字典的單詞的存放依照k叉樹結構來存放。
回環流程:
1. 創建字典: 將各frame圖像特徵描述存放到k叉樹
2. 在創建字典的時候會計算圖像特徵描述的權重
3. 對比圖像進行相似度計算，確認圖像中的特徵點都對應哪耶單詞，計算frame差異。
回檢檢測在字典應用上需要注意:
1. 增加字典規模，否則不同圖像相似度差異會比較小。
2. 增加關鍵frame的處理，因為相鄰的frame相似度較高，回環檢測的frame應該稀疏些。

## 建圖
將機器人移動的過程周圍環境拼接起來得到完整的地圖，地圖的好處有定位、導航、避障、重建、互動等

https://blog.csdn.net/weixin_41944449/article/details/119864865

稀疏標記地圖: 只能用於定位。
稠密地圖: 可用來導航、避障、重建。
語意地圖: 用來互動→例如VR、AR等。

NOTE: 掃地機器人最起碼需要半稀疏半稠密地圖，否則無法實現導航進行路徑規劃等。

### 單目相基如何重建地圖?
單目重建地圖需要知道每一個像素的距離，在多frame圖像中得到相機運動後，三角化計算像素的距離，需要用到「極線搜索」和「塊匹配技術」找到frame之間同一的對應的像素點(不像特徵點可以直接知道frame之間特徵點的對應關係)，在這個需要對每個像素點都進行操作所以要「極線搜索」
如上圖，知道$p_1$點後，在經過運動之後，極線上哪個點才是我們在$O_1$上看到的$p_1$點。
如果是特徵點還能在特徵點匹配找到$p_2$的位置，但目前沒有圖像特徵描述，只能從極線來搜索$p_2$點。
可以從$O_2$位置的極線的某一頭走到另一頭，逐個比較每個像素和$p_1$的相似程度，但該方法可能有很多類似的點，所以在建圖中，通過「塊匹配」的方式來查找相似點，通過分塊進行匹配，取兩塊的差的平方和或是規一化互相計算相似度，得到frame之間相似的像素，然後透過三角量測得到開像素的深度。

在得到frame之間的旋轉矩陣和位移矢量以及像素點的深度之後，可以建構pointcloud。

https://blog.csdn.net/weixin_41944449/article/details/119864865

https://www.youtube.com/watch?v=AU_PXxvxBHM

https://blog.csdn.net/xiaojinger_123/article/details/118519375

https://blog.csdn.net/weixin_42164552/article/details/82184129