最近在看icp演算法，該演算法主要用於獲得被測物體的完整幾何資訊，這種情況下就需要將不同視角即不同參考座標下的兩組或者多組點雲統一到統一座標系下，進行點雲的配準。在配準演算法中，研究者使用最多的是icp演算法。
icp演算法的流程概括起來也就是五步：
（1）選取初始點集：目標點雲p中選取點集pi；（其中有均勻取樣，隨機取樣，法矢取樣等方法）
（2）計算最近點集：找出源點雲q中的點集qi，使得||qi-pi||=min；（其中有點到點，點到線，點到面等方法）
（3）計算變換矩陣：計算點集pi到點集qi的旋轉矩陣r和平移矩陣t；（其中有四分量，svd奇異值分解法等方法）
（4）應用變換矩陣：將點集pi進行rpi+t的變換變成新的點集pi；
（5）誤差函式計算與閾值判斷：通過變換後的點集pi與點集qi，計算兩者誤差的平方的平均值。倘若未滿足閾值或迭代條件，則將新的pi返回至第二步，尋找新的qi繼續計算。
而其中最被人津津樂道，已經多加研究的，主要就是第二步最近點集的計算方法，其中點到點的方法被應用的最多，然而其效率相對並不是很高；點到線和點到面都是基於點到點的改善演算法，其下效率能夠明顯提高但是適用性並不是很強。
不過這裡說的主要是標準的icp演算法，即點集中的資料都是三維的情況。在用於slam的建圖時，其多為二維的運用情況，這種情況的計算方式又有所不同，具體可以參考思維導圖的第四點。
引用：[1]
[2]
[3] censi a . an icp variant using a point-to-line metric[c]// ieee international conference on robotics & automation. ieee, 2008.
最接近點對問題
一維最臨近點對 假設我們用x軸上某個點m將s劃分為2個子集s1和s2 基於平衡子問題的思想，用s中各點座標的中位數來作分割點。遞迴地在s1和s2上找出其最接近點對和，並設d min，s中的最接近點對或者是，或者是，或者是某個，其中p3 s1且q3 s2。如果s的最接近點對是，即 p3 q3 由於在s...
演算法 最接近原點的 K 個點
題目 我們有乙個由平面上的點組成的列表 points。需要從中找出 k 個距離原點 0,0 最近的點。這裡，平面上兩點之間的距離是歐幾里德距離。例如 輸入 points 1,3 2,2 k 1 輸出 2,2 分析 如題也就是說，求出前k個離 0,0 最近的點。那就要排序。距離如何排序？先用勾股定理求...
（五）迭代最近點（ICP）
icp演算法 從這裡開始，筆者便秉承恩師的教誨，用一些比較規範的寫法來介紹演算法原理。已知條件 匹配的一組三維點雲 p p dots,p q q dots,q tips 這堆點雲一般是根據兩幀影象的特徵點進行匹配，再利用匹配的畫素位置在深度圖中查詢深度值，利用相機內參以及深度值進行計算相機座標系下的...