三维点集拟合：平面拟合、RANSAC、ICP算法

ACM算法分类：http://www.kuqin.com/algorithm/20080229/4071.html

一: 拟合一个平面：使用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）：空间向量的旋转：

2-D绕原点旋转变换矩阵是：

[cosA  sinA]      [cosA -sinA]
[-sinA cosA] 或者 [sinA cosA]

2-D绕任意一点旋转变换矩阵是ÿ