???專欄匯總

視覺SLAM十四講學(xué)習(xí)筆記-第一講_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第二講-初識SLAM_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第二講-開發(fā)環(huán)境搭建_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第三講-旋轉(zhuǎn)矩陣和Eigen庫_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第三講-旋轉(zhuǎn)向量、歐拉角、四元數(shù)_goldqiu的博客-CSDN博客

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

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

視覺SLAM十四講學(xué)習(xí)筆記-第四講-李代數(shù)求導(dǎo)與擾動模型_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第四講-Sophus實踐、相似變換群與李代數(shù)_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第五講-相機(jī)模型_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第五講-圖像和實踐_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第四講---第五講學(xué)習(xí)筆記總結(jié)---李群和李代數(shù)、相機(jī)_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第六講-非線性優(yōu)化的非線性最小二乘問題_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第六講-非線性優(yōu)化的實踐-高斯牛頓法和曲線擬合_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第六講學(xué)習(xí)筆記總結(jié)（1）---非線性優(yōu)化原理_goldqiu的博客-CSDN博客

視覺SLAM十四講學(xué)習(xí)筆記-第六講學(xué)習(xí)筆記總結(jié)（2）---非線性優(yōu)化應(yīng)用_goldqiu的博客-CSDN博客

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

視覺SLAM十四講學(xué)習(xí)筆記-第七講-視覺里程計-對極幾何和對極約束、本質(zhì)矩陣、基礎(chǔ)矩陣

視覺SLAM十四講學(xué)習(xí)筆記-第七講-視覺里程計-單應(yīng)矩陣和實踐_goldqiu的博客-CSDN博客

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

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

7.9 3D?3D：ICP

假設(shè)有一組配對好的 3D 點（比如對兩幅 RGB-D 圖像進(jìn)行了匹配）：

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

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

7.9.1 SVD 方法

定義第 i 對點的誤差項：

然后構(gòu)建最小二乘問題，求使誤差平方和達(dá)到極小的R, t：

求解:

首先，定義兩組點的質(zhì)心：

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

交叉項部分中

在求和之后為零，因此優(yōu)化目標(biāo)函數(shù)可以簡化為

左邊只和旋轉(zhuǎn)矩陣 R 相關(guān)，而右邊既有 R 也有 t，但只和質(zhì)心相關(guān)。只要獲得了R，令第二項為零就能得到 t。

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

計算兩組點的質(zhì)心位置 p, p′，然后計算每個點的去質(zhì)心坐標(biāo)：

2. 根據(jù)以下優(yōu)化問題計算旋轉(zhuǎn)矩陣：

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

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

展開關(guān)于 R 的誤差項，得：

第一項和 R 無關(guān)，第二項由于 RTR = I，也與 R 無關(guān)。因此實際上優(yōu)化目標(biāo)函數(shù)變?yōu)?/p>

下面通過 SVD 解出上述問題中最優(yōu)的 R：

先定義矩陣：

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

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

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

7.9.2 非線性優(yōu)化方法

以李代數(shù)表達(dá)位姿時，目標(biāo)函數(shù)可以寫成

使用李代數(shù)擾動模型：

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

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

7.10 實踐：求解 ICP

7.10.1 SVD 方法

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

代碼：slambook/ch7/pose_estimation_3d3d.cpp

調(diào)用 Eigen 進(jìn)行 SVD，然后計算 R, t 矩陣。輸出匹配后的結(jié)果。

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

7.10.2 非線性優(yōu)化方法

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

代碼：slambook/ch7/pose_estimation_3d3d.cpp

這是一個一元邊，寫法類似于g2o::EdgeSE3ProjectXYZ，不過觀測量從 2 維變成了 3 維，內(nèi)部沒有相機(jī)模型，并且只關(guān)聯(lián)到一個節(jié)點。雅可比矩陣給出了關(guān)于相機(jī)位姿的導(dǎo)數(shù)，是一個 3 × 6 的矩陣。 調(diào)用 g2o 進(jìn)行優(yōu)化的代碼是相似的，設(shè)定好圖優(yōu)化的節(jié)點和邊即可。

優(yōu)化的結(jié)果：

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

7.11 小結(jié)

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

特征點如何提取并匹配

如何通過 2D?2D 的特征點估計相機(jī)運(yùn)動。

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

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

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

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

在工程實現(xiàn)中，這些情況很少出現(xiàn)。

習(xí)題

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

設(shè)計程序調(diào)用 OpenCV 中的其他種類特征點。統(tǒng)計在提取 1000 個特征點時在機(jī)器上所用的時間。

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

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

在 PnP 優(yōu)化中，將第一個相機(jī)的觀測也考慮進(jìn)來，程序應(yīng)如何書寫？最后結(jié)果會有何變化？

在 ICP 程序中，將空間點也作為優(yōu)化變量考慮進(jìn)來，程序應(yīng)如何書寫？最后結(jié)果會有何變化？

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

使用 Sophus 的 SE3 類，自己設(shè)計 g2o 的節(jié)點與邊，實現(xiàn) PnP 和 ICP 的優(yōu)化。

在 Ceres 中實現(xiàn) PnP 和 ICP 的優(yōu)化。

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。