温习ICP KD树

3维点如果知道其对应关系 是有经过证明的闭环解的，证明过程在：
https://igl.ethz.ch/projects/ARAP/svd_rot.pdf
这里：SVD分解 和特征值分解有关系 但是还不一样 矩阵的特征值分解要求是方阵 而SVD分解不必

ICP的具体过程大致：
1.先知道一个大致的初始解（否则效果不好 收敛速度慢甚至不收敛）
通过KD树找到最近的点 （为什么？ 因为用SVD计算R T 需要知道对应点 试想如果我们有3个点对，如果匹配顺序错误 RT必定错误 而且R的计算也是通过对应点对的互相关系数矩阵的分解来计算的）

2. 通过匹配点对计算闭环解

然后迭代 直到满足结束条件

KD树 也是一棵二叉数 但是每次分枝到下一层的时候 会换一个维度 所以说建树的复杂度是O(维度*log(N)) 查找的复杂度是 O(logN)