利用SVD的方法求解ICP（详细推导）

引用资料（理论部分其实就是把第一个的不详细和错误的地方说了一下，翻译了一下第二个文献以及不明了的地方说一下O(∩_∩)O哈哈~）：

高翔《视觉SLAM十四讲》
K. S. ARUN, T. S. HUANG, AND S. D. BLOSTEIN
Least-Squares Fitting of Two 3-D Point Sets

问题描述

  假设存在两个点云集合和
  求：一个欧氏变换使得
    

求解问题

解：
假设误差项为
  
那么问题转化为优化问题：
  

定义质心为：

那么有：

 

因为

所以问题转化为：

因为左右两边都大于等于零，而且左边只和相关，可以先求出R在利用R求解第二项

那么按照书里计算过程

1. 计算两组质心位置p,p',然后计算每个点的去质心坐标：
   
   2.根据以下优化问题计算旋转矩阵：
   
   3.根据2的结果计算t
   

展开关于R的误差项有：

因为第一项与R无管，第二项由于与R也无关那么问题转化为

令

因为问题是求解

即：

假设最优解
那么
（因为R是正交矩阵）
对H进行SVD分解

可以得到

那么

令
因为

所以
是

最优解

现在只要证明

是A的第i列，因为那么有

根据Schwarz不等式

即

注意这个计算需要H是满秩，

几个情况需要考虑

1.H是满秩，上的点非共平面
2.上的点共平面，可以对H求出的解的为0特征值的特征向量计算取反，使得
3.上的点共线，不能用SVD求解

代码

//这个是将点云dstPoint利用RT配到srcPoint上的  srcPoint=dstPoint*R+T
void registrateNPoint(std::vector<cv::Point3d>& srcPoints,std::vector<cv::Point3d>& dstPoints,cv::Mat&R,cv::Mat&T){
    if(srcPoints.size()!=dstPoints.size()||srcPoints.size()<3||dstPoints.size()<3)
    {
        std::cout<<"srcPoints.size():\t"<<srcPoints.size();
        std::cout<<"dstPoints.size():\t"<<dstPoints.size();
        std::cout<<"registrateNPoint points size donot match!";

    }
    double srcSumX = 0.0f;
    double srcSumY = 0.0f;
    double srcSumZ = 0.0f;

    double dstSumX = 0.0f;
    double dstSumY = 0.0f;
    double dstSumZ = 0.0f;

    size_t pointsNum=srcPoints.size();
    for(size_t i=0;i<pointsNum;i++){
        srcSumX+=srcPoints[i].x;
        srcSumY+=srcPoints[i].y;
        srcSumZ+=srcPoints[i].z;

        dstSumX+=dstPoints[i].x;
        dstSumY+=dstPoints[i].y;
        dstSumZ+=dstPoints[i].z;
    }
    cv::Point3d srcCentricPt(srcSumX / pointsNum,srcSumY / pointsNum,srcSumZ / pointsNum);
    cv::Point3d dstCentricPt(dstSumX / pointsNum,dstSumY / pointsNum,dstSumZ / pointsNum);
    cv::Mat srcMat;
    srcMat=cv::Mat::zeros(3, pointsNum, CV_64F);
    cv::Mat dstMat;
    dstMat=cv::Mat::zeros(3, pointsNum, CV_64F);
    for (size_t i = 0; i < pointsNum; ++ i)
    {

        srcMat.at<double>(0,i)=srcPoints[i].x - srcCentricPt.x;
        srcMat.at<double>(1,i)=srcPoints[i].y - srcCentricPt.y;
        srcMat.at<double>(2,i)=srcPoints[i].z - srcCentricPt.z;

        dstMat.at<double>(0,i)=dstPoints[i].x - dstCentricPt.x;
        dstMat.at<double>(1,i)=dstPoints[i].y - dstCentricPt.y;
        dstMat.at<double>(2,i)=dstPoints[i].z - dstCentricPt.z;
    }

    cv::Mat matS = srcMat * dstMat.t();

    cv::Mat matU, matW, matV;
    cv::SVDecomp(matS, matW, matU, matV);

    cv::Mat matTemp = matU * matV;
    double det = cv::determinant(matTemp);

    double datM[] = {1, 0, 0, 0, 1, 0, 0, 0, det};
    cv::Mat matM(3, 3, CV_64FC1, datM);

    cv::Mat matR = matV.t() * matM * matU.t();
    double tx,ty,tz;
    tx = dstCentricPt.x- (srcCentricPt.x* matR.at<double>(0,0) + srcCentricPt.y* matR.at<double>(0,1) + srcCentricPt.z* matR.at<double>(0,2));
    ty = dstCentricPt.y- (srcCentricPt.x* matR.at<double>(1,0) + srcCentricPt.y* matR.at<double>(1,1) + srcCentricPt.z * matR.at<double>(1,2));
    tz = dstCentricPt.z- (srcCentricPt.x* matR.at<double>(2,0) + srcCentricPt.y* matR.at<double>(2,1) + srcCentricPt.z * matR.at<double>(2,2));
    double datT[]={tx,ty,tz};
    cv::Mat matT(3, 1, CV_64F,datT);
    matR.copyTo(R);
    matT.copyTo(T);
}