???專欄匯總

視覺SLAM十四講學習筆記-第一講_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第二講-初識SLAM_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第二講-開發環境搭建_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第三講-旋轉矩陣和Eigen庫_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第三講-旋轉向量、歐拉角、四元數_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第三講-相似、仿射、射影變換和eigen程序、可視化演示_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記---前三講學習筆記總結之SLAM的作用、變換和位姿表示_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第四講-李代數求導與擾動模型_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第四講-Sophus實踐、相似變換群與李代數_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第五講-相機模型_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第五講-圖像和實踐_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第四講---第五講學習筆記總結---李群和李代數、相機_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第六講-非線性優化的非線性最小二乘問題_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第六講-非線性優化的實踐-高斯牛頓法和曲線擬合_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第六講學習筆記總結（1）---非線性優化原理_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第六講學習筆記總結（2）---非線性優化應用_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第七講-視覺里程計-特征點發和特征提取和匹配實踐_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第七講-視覺里程計-對極幾何和對極約束、本質矩陣、基礎矩陣

視覺SLAM十四講學習筆記-第七講-視覺里程計-單應矩陣和實踐_goldqiu的博客-CSDN博客

????????????視覺SLAM十四講學習筆記-第七講-視覺里程計-三角測量和實踐_goldqiu的博客-CSDN博客

視覺SLAM十四講學習筆記-第七講-視覺里程計-PnP和實踐_goldqiu的博客-CSDN博客

7.9 3D?3D：ICP

假設有一組配對好的 3D 點（比如對兩幅 RGB-D 圖像進行了匹配）：

想要找一個歐氏變換 R, t，使得：

這個問題可以用迭代最近點（Iterative Closest Point，ICP）求解。3D?3D 位姿估計問題和相機模型并沒有關系。在激光 SLAM 中也會碰到 ICP，不過由于激光數據特征不夠豐富，無從知道兩個點集之間的匹配關系，只能認為距離最近的兩個點為同一個，所以這個方法稱為迭代最近點。而在視覺中，特征點提供了較好的匹配關系，所以整個問題就變得更簡單了。在 RGB-D SLAM 中， 可以用這種方式估計相機位姿。ICP 的求解也分為兩種方式：利用線性代數的求解（主要是 SVD），以及利用非線性優化方式的求解（類似于 Bundle Adjustment）。

7.9.1 SVD 方法

定義第 i 對點的誤差項：

然后構建最小二乘問題，求使誤差平方和達到極小的R, t：

求解:

首先，定義兩組點的質心：

隨后，在誤差函數中做如下的處理：

交叉項部分中

在求和之后為零，因此優化目標函數可以簡化為

左邊只和旋轉矩陣 R 相關，而右邊既有 R 也有 t，但只和質心相關。只要獲得了R，令第二項為零就能得到 t。

于是ICP可以分為以下三個步驟求解：

計算兩組點的質心位置 p, p′，然后計算每個點的去質心坐標：

2. 根據以下優化問題計算旋轉矩陣：

3. 根據第 2 步的 R 計算 t： t? = p ? Rp′

求出了兩組點之間的旋轉，平移量很容易得到。

展開關于 R 的誤差項，得：

第一項和 R 無關，第二項由于 RTR = I，也與 R 無關。因此實際上優化目標函數變為

下面通過 SVD 解出上述問題中最優的 R：

先定義矩陣：

W 是一個 3 × 3 的矩陣，對 W 進行 SVD 分解，得： W = UΣVT

其中，Σ 為奇異值組成的對角矩陣，對角線元素從大到小排列，而 U 和 V 為對角矩陣。當 W 滿秩時，R = UVT

然后按t? = p ? Rp′ 求解 t。

7.9.2 非線性優化方法

以李代數表達位姿時，目標函數可以寫成

使用李代數擾動模型：

在非線性優化中只需不斷迭代，就能找到極小值。ICP 問題存在唯一 解或無窮多解的情況。在唯一解的情況下，只要能找到極小值解，那么這個極小值就是全局最優值，不會遇到局部極小的情況。這意味著已匹配點時求解 ICP可以任意選定初始值。

這里的ICP是指已由圖像特征給定了匹配的情況下進行位姿估計的問題。 在匹配已知的情況下，這個最小二乘問題具有解析解，所以并沒有必要進行迭代優化。對于深度已知的特征點，建模它們的 3D?3D 誤差；對于深度未知的特征點，則建模 3D?2D 的重投影誤差。可以將所有的誤差放在同一個問題中考慮，使得求解更加方便。

7.10 實踐：求解 ICP

7.10.1 SVD 方法

使用兩幅 RGB-D 圖像，通過特征匹配獲取兩組 3D 點，用 ICP 計算它們的位姿變換。

代碼：slambook/ch7/pose_estimation_3d3d.cpp

調用 Eigen 進行 SVD，然后計算 R, t 矩陣。輸出匹配后的結果。

對比ICP 與 PnP、對極幾何的運動估計結果之間的差異，在這個過程中使用了越來越多的信息，從沒有深度到有一個圖的深度到有兩個圖的深度。因此在深度準確的情況下，得到的估計也將越來越準確。但是由于 Kinect 的深度圖存在噪聲，而且有可能存在數據丟失的情況，使得不得不丟棄一些沒有深度數據的特征點。這可能導致 ICP 的估計不夠準確，并且，如果特征點丟棄得太多，可能引起由于特征點太少，無法進行運動估計的情況，即退化的情況。

7.10.2 非線性優化方法

使用李代數來表達相機位姿，與 SVD 思路不同的地方在于，在優化中不僅考慮相機的位姿，同時會優化 3D 點的空間位置。RGB-D 相機每次可以觀測到路標點的三維位置，從而產生一個 3D 觀測數據。g2o/sba 中沒有提 供 3D 到 3D 的邊，所以自定義一 種這樣的邊，并向 g2o 提供解析求導方式。

代碼：slambook/ch7/pose_estimation_3d3d.cpp

這是一個一元邊，寫法類似于g2o::EdgeSE3ProjectXYZ，不過觀測量從 2 維變成了 3 維，內部沒有相機模型，并且只關聯到一個節點。雅可比矩陣給出了關于相機位姿的導數，是一個 3 × 6 的矩陣。 調用 g2o 進行優化的代碼是相似的，設定好圖優化的節點和邊即可。

優化的結果：

只迭代一次后總體誤差穩定不變，說明僅在一次迭代之后算法即已收斂。從位姿求解的結果可以看出，它和前面 SVD 給出的位姿結果幾乎一模一樣，這說明 SVD 已經給出了優化問題的解析解。所以可以認為 SVD 給出的結果是相機位姿的最優值。 在這個ICP 中，使用了在兩個圖都有深度讀數的特征點。然而事實上只要其中一個圖深度確定，就能用類似于 PnP 的誤差方式，把它們也加到優化中來。同時，除了相機位姿之外，將空間點也作為優化變量考慮，也是一種解決問題的方式。實際的求解是非常靈活的，不必拘泥于某種固定的形式。如果同時考慮點和相機，整個問題就變得更自由了，可能會得到其他的解。比如可以讓相機少轉一些角度，而把點多移動一些。這從另一側面反映出，在 Bundle Adjustment 里面，會希望有盡可能多的約束，因為多次觀測會帶來更多的信息，更準確地估計每個變量。

7.11 小結

基于特征點的視覺里程計中的幾個重要的問題，包括：

特征點如何提取并匹配

如何通過 2D?2D 的特征點估計相機運動。

如何從 2D?2D 的匹配估計一個點的空間位置。

3D?2D 的 PnP 問題，其線性解法和 Bundle Adjustment 解法。

3D?3D 的 ICP 問題，其線性解法和 Bundle Adjustment 解法。

這省略了大量關于某些特殊情況的討論。例如，如果在對極幾何求解過程中給定的特征點共面，會發生什么情況？共線又會發生什么情況？在 PnP 和 ICP 中若給定這樣的解，又會導致什么情況？求解算法能否識別這些特殊的情況，并報告所得的解可能不可靠？

在工程實現中，這些情況很少出現。

習題

除了ORB 特征點，SIFT 或 SURF 的原理，并對比它們與 ORB 之間的優劣。

設計程序調用 OpenCV 中的其他種類特征點。統計在提取 1000 個特征點時在機器上所用的時間。

OpenCV 提供的 ORB 特征點在圖像當中分布不夠均勻。有沒讓特征點分布更加均勻的方法？

研究 FLANN 為何能夠快速處理匹配問題。除了 FLANN 之外，還有哪些可以加速匹配的手段？

在 PnP 優化中，將第一個相機的觀測也考慮進來，程序應如何書寫？最后結果會有何變化？

在 ICP 程序中，將空間點也作為優化變量考慮進來，程序應如何書寫？最后結果會有何變化？

在特征點匹配過程中，不可避免地會遇到誤匹配的情況。如果把錯誤匹配輸入到 PnP 或 ICP 中，會發生怎樣的情況？能想到哪些避免誤匹配的方法？

使用 Sophus 的 SE3 類，自己設計 g2o 的節點與邊，實現 PnP 和 ICP 的優化。

在 Ceres 中實現 PnP 和 ICP 的優化。

總結

以上是生活随笔為你收集整理的视觉SLAM十四讲学习笔记-第七讲-视觉里程计-ICP和实践的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。