Icp基本思想參考資料：http://www.cnblogs.com/jian-li/articles/4945676.html ，包括點-點，點-面的各種icp變種
Icp算法就是兩個點雲X、Y之間的匹配，最小化均方誤差
其中R是旋轉矩陣，t是平移矩陣。
方法：
搜索策略
找到最近點，使用kd-tree，參考資料
http://www.cnblogs.com/xy123001/p/5831116.html
http://blog.sina.com.cn/s/blog_6f611c300101bysf.html
誤差函數求解
常見的：基於奇異值分解的方法、四元數方法。
其他參考：
mrpt的icp
http://www.mrpt.org/Iterative_Closest_Point_%28ICP%29_and_other_matching_algorithms
用kd-tree來找最近點，是整個icp算法中最耗時的部分。所以，如果點雲密集，就要下采樣，下采樣默認5
通過ini配置文件，還可以選擇icp算法的種類，是經典算法還是LM算法，
經典算法和LM算法的區別就是 經典算法使用的最小二乘法，替換成了LM算法中的非線性最小二乘。
LM加大了計算量。
ICPfastSlam算法
Icp+粒子濾波
祝繼華, 鄭南寧, 袁澤劍,等. 基於ICP算法和粒子濾波的未知環境地圖創建[J]. 自動化學報, 2009, 35(8):1107-1113.
點-平面
下圖是點-切平面距離的示意圖
與點到點的ICP算法相比，運用點到平面的距離的方法大大減少了計算量以及迭代次數，但是該方法的魯棒性並不是太好。
由icp迭代后的點求機器人位姿
舉個例子。
下圖中有兩個坐標系A 和 B
B是由A經過平移和旋轉得到的。平移矩陣為R，旋轉矩陣T
A旋轉sita= - 90度（逆時針為正），沿A的x軸平移3之后，得到了B坐標系。
R=[0,1; -1,0] T=[3, 0]
M=[R T;0 1]
一個點在A下為[1;5],在B下為[-5;-2]
M*[-5;2] = [1;5]
Inv(M)*[1 ;5] = [-5;-2]
由此可知，激光掃描到的點，經過icp匹配之后，R 、t就已經得到了，sita tx ty也就知道了，這就相當於 機器人就是坐標原點。