三维点集拟合：平面拟合、RANSAC、ICP算法
? ? ? ??ACM算法分類：http://www.kuqin.com/algorithm/20080229/4071.html;CSDN容易吞圖,不過編輯器里面圖片還是顯示的.....
一、 擬合一個平面
? ? ? ? 使用SVD分解，代碼里面去找吧
????? ? 空間平面方程的一般表達式為：?????? ? Ax+By+Cz+D=0;
???? ?? 則有：平面法向量為n=（A,B,C）.
第一種方法： 對于空間中n個點（n3）
????? ?? 空間中的離散點得到擬合平面，其實這就是一個最優化的過程。即求這些點到某個平面距離最小和的問題。由此，我們知道一個先驗消息，那就是該平面一定會過眾散點的平均值。接著我們需要做的工作就是求這個平面的法向量。
????? ?? 根據協方差矩陣的SVD變換，最小奇異值對應的奇異向量就是平面的方向。
注意：這個方法是直接的計算方法，沒辦法解決數值計算遇到的病態矩陣問題.在公式轉化代碼之前必須對空間點坐標進行近似歸一化！
?
第二種方法：使用法線方法， 對于空間中n個點（n3），若已獲得點云法線
????? ? 使用合適的方法剔除離群點，計算點云的形心P；
?
???? ? ? 若在已經獲得法線的點云中，可以對法線進行剔除離散點之后，求取最小方差的均值，直接求得法線方向N( alpha, beta, theta )；
? ? ???? 使用點法式描述三維平面；或者根據形心P和法線方向，計算出平面方程的一般式。
?
? ?使用法線多次聚類：完成場景平面提取
??????? 使用法線兩次聚類：第一次根據法線方向進行聚類，使用一個歐式距離約束，找出方向接近的簇S(1)，這樣得到的S(1)內的集合，每一類指向了大致相同的方向，但距離上并不一定接近；第二次，再次根據點云的空間位置進行聚類，對S(1)的每一簇內再次進行基于距離的聚類，找出每一簇內位置接近的類別，這樣再次對集合進行劃分，得到的每一類方向大致相同，而位置較近，可以假設為一個平面的點。
?????? 此外，若考慮到平面密度要求，還可以再根據密度進行一次聚類，把密度較低的平面從集合中踢出去。
?
二：空間向量的旋轉
2-D繞原點旋轉變換矩陣是：
[cosA sinA] [cosA -sinA] [-sinA cosA] 或者 [sinA cosA]2-D繞任意一點旋轉變換矩陣是：
[x y 1] [1 0 0] [cosA sinA 0] [1 0 0] [x' y' -] [0 1 0] x [0 1 0] x [-sinA cosA 0] x [0 1 0] = [- - -] [0 0 1] [rtx rty 1] [0 0 1] [-rtx -rty 1] [- - -]?
二、 利用Ransac算法進行擬合
????????
作者：王先榮 原文鏈接:http://www.cnblogs.com/xrwang/archive/2011/03/09/ransac-1.html
?? ? ??? 本文翻譯自維基百科，英文原文地址是：http://en.wikipedia.org/wiki/ransac，如果您英語不錯，建議您直接查看原文。
? ? ? ?? RANSAC是“RANdom SAmple Consensus（隨機抽樣一致）”的縮寫。它可以從一組包含“局外點”的觀測數據集中，通過迭代方式估計數學模型的參數。它是一種不確定的算法——它有一定的概率得出一個合理的結果；為了提高概率必須提高迭代次數。該算法最早由Fischler和Bolles于1981年提出。
?? ? ? ? RANSAC的基本假設是：
（1）數據由“局內點”組成，例如：數據的分布可以用一些模型參數來解釋；（2）“局外點”是不能適應該模型的數據；（3）除此之外的數據屬于噪聲。
? ? ?? ? 局外點產生的原因有：噪聲的極值；錯誤的測量方法；對數據的錯誤假設。
????? ?? RANSAC也做了以下假設：給定一組（通常很小的）局內點，存在一個可以估計模型參數的過程；而該模型能夠解釋或者適用于局內點。
本文內容
1 示例2 概述3 算法4 參數5 優點與缺點6 應用7 參考文獻8 外部鏈接
一、示例
??????? 一個簡單的例子是從一組觀測數據中找出合適的2維直線。假設觀測數據中包含局內點和局外點，其中局內點近似的被直線所通過，而局外點遠離于直線。簡單的最小二乘法不能找到適應于局內點的直線，原因是最小二乘法盡量去適應包括局外點在內的所有點。相反，RANSAC能得出一個僅僅用局內點計算出模型，并且概率還足夠高。但是，RANSAC并不能保證結果一定正確，為了保證算法有足夠高的合理概率，我們必須小心的選擇算法的參數。
?????????????? 左圖：包含很多局外點的數據集?????? 右圖：RANSAC找到的直線（局外點并不影響結果）
????????
二、概述
??? RANSAC算法的輸入是一組觀測數據，一個可以解釋或者適應于觀測數據的參數化模型，一些可信的參數。
??? RANSAC通過反復選擇數據中的一組隨機子集來達成目標。被選取的子集被假設為局內點，并用下述方法進行驗證：
??? 1.有一個模型適應于假設的局內點，即所有的未知參數都能從假設的局內點計算得出。
??? 2.用1中得到的模型去測試所有的其它數據，如果某個點適用于估計的模型，認為它也是局內點。
??? 3.如果有足夠多的點被歸類為假設的局內點，那么估計的模型就足夠合理。
??? 4.然后，用所有假設的局內點去重新估計模型，因為它僅僅被初始的假設局內點估計過。
??? 5.最后，通過估計局內點與模型的錯誤率來評估模型。
??? 這個過程被重復執行固定的次數，每次產生的模型要么因為局內點太少而被舍棄，要么因為比現有的模型更好而被選用。
三、算法
偽碼形式的算法如下所示：
輸入：
data —— 一組觀測數據
model —— 適應于數據的模型
n —— 適用于模型的最少數據個數
k —— 算法的迭代次數
t —— 用于決定數據是否適應于模型的閥值
d —— 判定模型是否適用于數據集的數據數目
輸出：
best_model —— 跟數據最匹配的模型參數（如果沒有找到好的模型，返回null）
best_consensus_set —— 估計出模型的數據點
best_error —— 跟數據相關的估計出的模型錯誤
?RANSAC算法的可能變化包括以下幾種：
??? （1）如果發現了一種足夠好的模型（該模型有足夠小的錯誤率），則跳出主循環。這樣可能會節約計算額外參數的時間。
??? （2）直接從maybe_model計算this_error，而不從consensus_set重新估計模型。這樣可能會節約比較兩種模型錯誤的時間，但可能會對噪聲更敏感。
四、參數
?? ? ?? 我們不得不根據特定的問題和數據集通過實驗來確定參數t和d。然而參數k（迭代次數）可以從理論結果推斷。當我們從估計模型參數時，用p表示一些迭代過程中從數據集內隨機選取出的點均為局內點的概率；此時，結果模型很可能有用，因此p也表征了算法產生有用結果的概率。用w表示每次從數據集中選取一個局內點的概率，如下式所示：
?
????? w = 局內點的數目 / 數據集的數目
?? ? ?? 通常情況下，我們事先并不知道w的值，但是可以給出一些魯棒的值。假設估計模型需要選定n個點，wn是所有n個點均為局內點的概率；1 ?wn是n個點中至少有一個點為局外點的概率，此時表明我們從數據集中估計出了一個不好的模型。 (1 ?wn)k表示算法永遠都不會選擇到n個點均為局內點的概率，它和1-p相同。因此，?1 ?p = (1 ? wn)k
??? 我們對上式的兩邊取對數，得出
???
?? ? ?? 值得注意的是，這個結果假設n個點都是獨立選擇的；也就是說，某個點被選定之后，它可能會被后續的迭代過程重復選定到。這種方法通常都不合理，由此推導出的k值被看作是選取不重復點的上限。例如，要從上圖中的數據集尋找適合的直線，RANSAC算法通常在每次迭代時選取2個點，計算通過這兩點的直線maybe_model，要求這兩點必須唯一。
??????? 為了得到更可信的參數，標準偏差或它的乘積可以被加到k上。k的標準偏差定義為：
???
?
五、優點與缺點
?? ?? ? RANSAC的優點是它能魯棒的估計模型參數。例如，它能從包含大量局外點的數據集中估計出高精度的參數。
?? ?? ? RANSAC的缺點是它計算參數的迭代次數沒有上限；如果設置迭代次數的上限，得到的結果可能不是最優的結果，甚至可能得到錯誤的結果。RANSAC只有一定的概率得到可信的模型，概率與迭代次數成正比。
? ?? ?? RANSAC的另一個缺點是它要求設置跟問題相關的閥值。
?? ?? ? RANSAC只能從特定的數據集中估計出一個模型，如果存在兩個（或多個）模型，RANSAC不能找到別的模型。
六、應用
?? ? ?? RANSAC算法經常用于計算機視覺，例如同時求解相關問題與估計立體攝像機的基礎矩陣。
?
三、 點云匹配：ICP算法（Iterative Closest Point迭代最近點）
??????
參考鏈接：機器視覺之 ICP算法和RANSAC算法：http://www.cnblogs.com/yin52133/archive/2012/07/21/2602562.html
???? ? ICP（Iterative Closest Point迭代最近點）算法是一種點集對點集配準方法，
?????? 如下圖，假設PR（紅色塊）和RB（藍色塊）是兩個點集，該算法就是計算怎么把PB平移旋轉，使PB和PR盡量重疊，建立模型的Alignment模型。
? ? ? （圖1）
?????? ICP是改進自對應點集配準算法的對應點集配準算法是假設一個理想狀況，將一個模型點云數據X（如上圖的PB）利用四元數旋轉，并平移得到點云P（類似于上圖的PR）。而對應點集配準算法主要就是怎么計算出qR和qT的，知道這兩個就可以匹配點云了。
?????? 但是對應點集配準算法的前提條件是計算中的點云數據PB和PR的元素一一對應，這個條件在現實里因誤差等問題，不太可能實線，所以就有了ICP算法（我們無從知道兩個點集之間的匹配關系，只能認為距離最近的點為同一個，所以此方法為迭代最近點）。
?????? ICP算法是從源點云上的（PB）每個點 先計算出目標點云（PR）的每個點的距離，使每個點和目標云的最近點匹配。
?????? 這樣滿足了對應點集配準算法的前提條件、每個點都有了對應的映射點，則可以按照對應點集配準算法計算，但因為這個是假設，所以需要重復迭代運行上述過程，直到均方差誤差小于某個閥值。
?????? 也就是說 每次迭代，整個模型是靠近一點，每次都重新找最近點，然后再根據對應點集批準算法算一次，比較均方差誤差，如果不滿足就繼續迭代。
?
ICP的求解方法：
? ? ? ?? 把ICP方法看做一個點云位姿變換的過程，可以使用代數方法和非線性優化方法。
???????? 假設有兩堆點云，分別記為兩個集合X=x1,x2,...,xm和Y=y1,y2,...,ym（m并不總是等于n）。
?????????? ICP公式為：??
?????? ?
1. SVD等代數方法
???????? 先構建誤差矩陣，構建最小二乘問題，求使得誤差平方和最小的點云旋轉和位移R，T。
???????? 初始化估計：ICP發展了多年之后，當然有很多的方法來估計初始的R和t，PCL自己的函數 SampleConsensusInitalAlignment 函數以及TransformationEstimationSVD函數 都可以得到較好的初始估計。
???????? 優化：得到初始化估計之后仍然存在誤差問題，RANSAC之后，若已存在完全正確匹配，則可以再次求取旋轉的essential矩陣，通過SVD分解得到最終旋轉R和平移t。
?
2.非線性優化方法
???????? RANSAC算法之后，去除掉 外點之后。
???????? 使用位姿的代數變化轉換構建一個誤差項，在非線性優化過程中不停地迭代，一般能找到極小值。
后記：
??????? 在去除外點，匹配已知的情況下，ICP的最小二乘問題總會得到一個最優解，即ICP的SVD方法總會有一個解析解。
?
二：RANSAC算法與ICP算法對比
???????? 它可以從一組包含“局外點”的觀測數據集中，通過迭代方式估計數學模型的參數。它是一種不確定的算法——它有一定的概率得出一個合理的結果；為了提高概率必須提高迭代次數。該算法最早由Fischler和Bolles于1981年提出。
??????? 光看文字還是太抽象了，我們再用圖描述
??????? RANSAC的基本假設是：（1）數據由“局內點”組成，例如：數據的分布可以用一些模型參數來解釋；（2）“局外點”是不能適應該模型的數據；（3）除此之外的數據屬于噪聲。
而下圖二里面、藍色部分為局內點，而紅色部分就是局外點，而這個算法要算出的就是藍色部分那個模型的參數
? ? ? ? （圖二）
????? RANSAC算法的輸入是一組觀測數據，一個可以解釋或者適應于觀測數據的參數化模型，一些可信的參數。
?????? 在上圖二中 ?左半部分灰色的點為觀測數據，一個可以解釋或者適應于觀測數據的參數化模型 我們可以在這個圖定義為一條直線，如y=kx + b；
???? 一些可信的參數指的就是指定的局內點范圍。而k，和b就是我們需要用RANSAC算法求出來的
RANSAC通過反復選擇數據中的一組隨機子集來達成目標。被選取的子集被假設為局內點，并用下述方法進行驗證：
? 1.有一個模型適應于假設的局內點，即所有的未知參數都能從假設的局內點計算得出。
? 2.用1中得到的模型去測試所有的其它數據，如果某個點適用于估計的模型，認為它也是局內點。
? ??? 3.如果有足夠多的點被歸類為假設的局內點，那么估計的模型就足夠合理。
? ? ? 4.然后，用所有假設的局內點去重新估計模型，因為它僅僅被初始的假設局內點估計過。
? ? ? 5.最后，通過估計局內點與模型的錯誤率來評估模型。
這個過程被重復執行固定的次數，每次產生的模型要么因為局內點太少而被舍棄，要么因為比現有的模型更好而被選用。
這個算法用圖二的例子說明就是先隨機找到內點，計算k1和b1，再用這個模型算其他內點是不是也滿足y=k1x+b2，評估模型
再跟后面的兩個隨機的內點算出來的k2和b2比較模型評估值，不停迭代最后找到最優點
?
我再用圖一的模型說明一下ICP算法
? ? ? ? ?（圖1）
?????? ICP算法的輸入是一組觀測數據，一個可以解釋或者適應于觀測數據的參數化模型，一些可信的參數。
?????? 模型對應的是空間中一個點云數據到另外一個點云數據的旋轉以及平移。第一步隨機得到的是一個點云中的點對作 ，利用其不變特征（兩點距離，兩點法向量夾角）作為哈希表的索引值搜索另一個點云中的一對對應點對，然后計算得到旋轉及平移的參數值。
然后適用變換，找到其他局內點，并在找到局內點之后重新計算旋轉及平移為下一個狀態。
然后迭代上述過程，找到最終的位置
??? ? ? 其中觀測數據就是PB，一個可以解釋或者適應于觀測數據的參數化模型是?四元數旋轉，并平移
可信的參數是兩個點對的不變特征（兩點距離，兩點法向量夾角）
區別：
???? ?? ICP算法在迭代的時候，點對是已經匹配的,只是一個求取剛性變化的過程；RANSAC算法在迭代的時候，點匹配對是隨著優化函數改變的。
?
總結
以上是生活随笔為你收集整理的三维点集拟合：平面拟合、RANSAC、ICP算法的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： Hamburgers
- 下一篇： -1.#IND000 图像类型转换