icp演算法
從這裡開始，筆者便秉承恩師的教誨，用一些比較規範的寫法來介紹演算法原理。
已知條件
匹配的一組三維點雲:
$$p = \, p_, \dots, p_ \}, q = \, q_, \dots, q_ \}$$
tips: (這堆點雲一般是根據兩幀影象的特徵點進行匹配，再利用匹配的畫素位置在深度圖中查詢深度值，利用相機內參以及深度值進行計算相機座標系下的空間點)。
問題
在上述已知條件下，求解兩坨點雲的相對位姿(旋轉矩陣 $r$ 和位移向量 $t$)。
方法
求解相對位姿的方法，我們可以分解成如下步驟進行：
步驟一：假設兩坨匹配點雲的相對位姿是 $(r, t)$，則約束關係可以描述成如下形式：
$$p_ = rq_ + t, \forall \in [1, n]$$
步驟二：由於實際上我們提供的匹配對數目通常是大於未知數個數的，因此該方程是乙個超定方程，這是其一；其二便是我們採集影象深度值的裝置存在一定的雜訊，無法使得匹配對能完全對準，因此也有一定的誤差。因此我們需要用乙個誤差的形式來表示步驟1中的問題，並將我們的問題轉換為最小化誤差：
$$e = \sum_^ || p_ - (rq_ + t) ||^$$
步驟三：利用點雲的均值可以幫助我們簡化問題。(筆者理解，這是為了減少點雲偏置的影響，讓整個點雲不會因為整體偏差太大而和目標點雲匹配不上。這有點像ncc基於畫素的匹配方法，ncc會要求先減掉整體增加的部分，即取均值，再進行歸一化操作，這個目標是為了消除比例光照誤差。如果有其他理解的讀者，歡迎交流)。假設兩坨點雲的中心為 $\hat, \hat$。則有：
$$\hat = \frac\sum_^p_, \hat = \frac\sum_^q_$$
我們將步驟2中的問題進行簡化：
$$\begin e =& \sum_^ || p_ - (rq_ + t) - \hat + \hat - r\hat + r\hat||^ \\ =& \sum_^ || [(p_ - \hat) - r(q_ - \hat)] + (\hat - r\hat - t)||^ \\ =& \sum_^ ||(p_ - \hat) - r(q_ - \hat)||^ + \sum_^|| \hat - (r\hat + t)||^ + 2\sum_^[(p_ - \hat) - r(q_ - \hat)][\hat - (r\hat + t)] \end $$
步驟四：仔細研究一下步驟3，我們說要把步驟2的公式進行簡化，可是看起來結果更加凌亂，並且複雜了？別急，我們來仔細分析一下。比如步驟3公式裡的最後一部分可以進行簡化：
$$ \begin l =& 2\sum_^[(p_ - \hat) - r(q_ - \hat)][\hat - (r\hat + t)] \\ =& 2[(p_+p_+ \dots + p_ - n\hat)-r(q_+q_+\dots+q_ - n\hat)][\hat - (r\hat + t)] \\ =& 0 \end $$
oh man？沒了？是的，最後一項相加以後結果為0。可以丟掉了。我們把步驟3的公式簡化為如下形式：
$$ \begin e =& \sum_^ ||(p_ - \hat) - r(q_ - \hat)||^ + \sum_^|| \hat - (r\hat + t)||^ \end $$
上述公式中第一項只和旋轉矩陣 $r$ 相關，而第二項中跟旋轉矩陣 $r, t$ 都相關，但是只與點雲中心座標相關。於是，我們可以先根據第一部分計算旋轉矩陣，再令第二部分為0,利用旋轉矩陣計算位移向量。ok，fine，非常完美的方案。接下來，我們的問題就簡化成了兩個部分：
$$ e_ = \sum_^ ||(p_ - \hat) - r(q_ - \hat)||^ $$
$$ e_ = \hat - (r\hat + t) = 0 $$
步驟五：接下來是優化 $e_$，我們令 $p_^ = p_-\hat$，$q_^ = q_-\hat$。展開式子：
$$ \begin e_ =& \sum_^ ||(p_ - \hat) - r(q_ - \hat)||^ \\ =& \sum_^(p_^ - rq_^)^ \\ =& \sum_^((p_^)^p_^ - 2\sum_^(p_^)^rq_^ + \sum_^(q_^)^r^rq_^ \end $$
上述式子第一項與旋轉矩陣無關，刪掉。第三項中由於 $r^r = i$，因此也與旋轉矩陣無關。則優化問題便簡化為：
$$ \begin e_ =& -2\sum_^(p_^)^rq_^ \\ =& -2\sum_^tr(rq_^(p_^)^) \\ =& -2tr(r\sum_^q_^(p_^)^) \end $$
步驟六：優化 $e_$，參考高博提供的參考文獻(arun, k somani and huang, thomas s and blostein, steven d, least-squares fitting of two 3-d point sets, pami, 1987.).令 $w = \sum_^q_^(p_^)^$，利用svd分解可以解得旋轉矩陣：
$$r = uv^$$
步驟七：將解得的旋轉矩陣代入 $e_$ 中，則可以直接計算出位移向量：
$$t^ = \hat - r\hat$$
步驟八：以svd求解的結果進一步進行bundle adjustment優化，使位姿更精確。
總結
ok，至此，基於svd分解計算匹配icp的方法步驟我們已經介紹清楚了，主要是利用點雲中心來簡化問題，通過先計算旋轉矩陣，再利用旋轉矩陣計算位移向量的方法完成相對位姿的估計。根據每乙個步驟的公式，我們可以很輕鬆地實現**，筆者大致實現了乙個
icp。筆者博文主要參考
高博的知乎，但是在寫法上嘗試了筆者導師一直引導的描述問題和求解問題的方法。
參考資料
1. 相機位姿求解問題？
2. 點雲匹配介紹與icp演算法
3. icp**
icp點雲匹配迭代最近鄰演算法
一 含義 1.icp演算法能夠使兩個不同座標系下的點集匹配到乙個座標系中，這個過程就是配準，配準的操作就是找到從座標系1變換到座標系2的剛性變換。2.icp的本質就是配準，但有不同的配準方案，icp演算法本質是基於最小二乘的最優配準方法。該方法重複進行選擇對應關係對，計算最優剛體變換，直到滿足正確配...
迭代最接近點（ICP）演算法概述
最近在看icp演算法，該演算法主要用於獲得被測物體的完整幾何資訊，這種情況下就需要將不同視角即不同參考座標下的兩組或者多組點雲統一到統一座標系下，進行點雲的配準。在配準演算法中，研究者使用最多的是icp演算法。icp演算法的流程概括起來也就是五步 1 選取初始點集 目標點雲p中選取點集pi 其中有均...
點雲（剛性）配準 icp
二 解讀 提到配準演算法，icp認第二，沒哪種演算法敢認第一，可見，icp這道坎是繞不過去的，在本文中，會重點介紹icp的原理與實現方案，同時，也會結合pcl中的 進行詳細介紹。從本質上講，icp類演算法的基本原理是 對於兩組點雲p和q，計算旋轉矩陣r與旋轉矩陣t，使目標函式e最小 e 1 2 fr...