ICP的不同解法：SVD分解，高斯牛顿法。代码实现

ICP （Iterative Closest Point，最近迭代）是点云配准中的一个很重要的算法。假设有两个点云P和Q，想要找到一个变换T，使得P=TQ，那么就需要使用到这个算法。

1.最近邻查找

这一步是为Q中的点Qi找到Q中相对应的点。

这个要分情况讨论：比如在RGBD-slam中，特征点的匹配已经为我们找到了P、Q中点的对应关系，那么这一步其实是已经完成了。但是对于两坨点云，没有点和点的对应关系，我们只好先做这样一个假设：离Qi最近的P中的点Pi就是它对应的点，显然这个是不准确的，但是我们可以通过一次次迭代来使这个假设趋于正确。

对于在P中找到离Qi最近的点，最先想到的办法是遍历，把所有P中的点遍历一次，找最近的点，这是可行的，但是这个计算量太大了。可以将P建成一个KDtree，使得这一步的查找复杂度降下来。

2.姿态解算

通过上一步得到了两个点云中每个点的对应关系，完成了匹配。接下来通过姿态的解算，得到一个变换T，使得TQ尽可能接近P。方法是构造误差函数，最小化误差。

2.1 SVD解法

公式推导的文章太多啦，我就不写了，放一个觉得十分不错的博客ICP-SVD公式推导
我把根据这个解法的代码放上来，尽可能注释清楚：

// pts1 和pts2是两个点云，其中的pts1[i]和pts2[i]是对应好的点
void my_icp(const vector<Point3f> &pts1,
            const vector<Point3f> &pts2){
   
    int size1 = pts1.size();
    int size2 = pts2.size();
    assert(size1 == size2);
    // 计算质心，p1 、p2分别为两个点云的质心
    Point3f p1, p2;
    for(int i=0; i<size1; ++i){
   
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 /= size1;
    p2 /= size1;
    // 去质心后的点云，pts1_mean 、pts2_mean是减去了质心的点云
    vector<Point3f> pts1_mean;
    vector<Point3f> pts2_mean;
    for(int i=0; i<size1; ++i){
   
        pts1_mean.push_back(pts1[i] - p1);
        pts2_mean.push_back(pts2[i] - p2);
    }