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

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

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

问题描述

  假设存在两个点云集合 { p i } \{p_i\} {pi​}和 { p ′ } \{p'\} {p′}
  求：一个欧氏变换$R,t$使得
    $$\forall i,p_i=R{p}_i^{\prime }+t$$

求解问题

解：
假设误差项为
  $$e_i=p_i-(R{p}_i^{\prime }+t)$$
那么问题转化为优化问题：
  $$\underset{R,t}{\min }J=\frac{1}{2}\sum _{i=1}^n\Vert (p_i-(R{p}_i^{\prime }+t_{)}){\Vert }^2$$

定义质心为：
$$p=\frac{1}{n}\sum _{i=1}^n(p_i),p^{\prime }=\frac{1}{n}\sum _{i=1}^n(p_i^{\prime })$$

那么有：
$$\frac{1}{2}\sum _{i=1}^n\Vert p_i-(R{p}_i^{\prime }+t)\Vert =\frac{1}{2}\sum _{i=1}^n\Vert p_i-R{p}_i^{\prime }-t-p+R{p}^{\prime }+p-R{p}^{\prime }{\Vert }^2$$
 $$=\frac{1}{2}\sum _{i=1}^n\Vert (p_i-p-R(p_i^{\prime }-p^{\prime }))+(p-R{p}^{\prime }-t){\Vert }^2$$
$$=\frac{1}{2}\sum _{i=1}^n(\Vert p_i-p-R(p_i^{\prime }-p^{\prime }){\Vert }^2+\Vert p-R{p}^{\prime }-t{\Vert }^2+2(p_i-p-R(p_i^{\prime }-p^{\prime })(p-R{p}^{\prime }-t))$$

因为
$$\sum _{i=1}^n(p_i-p-R(p_i^{\prime }-p^{\prime })(p-R{p}^{\prime }-t)=0$$
所以问题转化为：
$$\underset{R,t}{\min }J=\frac{1}{2}\sum _{i=1}^n\Vert p_i-p-R(p_i^{\prime }-p^{\prime }){\Vert }^2+\Vert p-R{p}^{\prime }-t{\Vert }^2$$
因为左右两边都大于等于零，而且左边只和$R$相关，可以先求出R在利用R求解第二项

那么按照书里计算过程

1. 计算两组质心位置p,p’,然后计算每个点的去质心坐标：
   $$q_i=p_i-p,q_i^{\prime }=p_i^{\prime }-p^{\prime }$$
   2.根据以下优化问题计算旋转矩阵：
   $$R^{\ast }=arg\underset{R}{\min }\frac{1}{2}\sum \Vert q_i-R{q}_i^{\prime }{\Vert }^2$$
   3.根据2的结果计算t
   $$t^{\ast }=p-R{p}^{\prime }$$

展开关于R的误差项有：
$$\frac{1}{2}\sum \Vert q_i-R{q}_i^{\prime }{\Vert }^2=\frac{1}{2}\sum q_i^T{q}_i+q_i^{{}^{\prime }T}{R}^{T}R{q}_i^{\prime }-2q_i^{T}R{q}_i^{\prime }$$
因为第一项与R无管，第二项由于$R^{T}R=I$与R也无关那么问题转化为
$$\sum _{i=1}^n-q_i^{T}R{q}_i^{\prime }=\sum _{i=1}^n-tr(R{q}_i^{\prime }{q}_i^T)=-tr(R\sum _{i=1}^n{q}_i^{\prime }{q}_i^T)$$
令
$$H=\sum _{i=1}^n{q}_i^{\prime }{q}_i^T$$
因为问题是求解
$$\underset{R}{\min }.-tr(RH)$$
即：
$$\underset{R}{\max }.tr(RH)$$
假设最优解$R^{\ast }$
那么
$$tr(R^{\ast }H)\ge tr(RH)=tr(B{R}^{\ast }H)$$（因为R是正交矩阵）
对H进行SVD分解
$$H=U\Sigma V^T$$
假设
$$R^{\ast }=V{U}^T$$
那么
$$R^{\ast }H=V{U}^{T}U\Sigma V^T=V\Sigma V^T$$
令$$A=V{\Sigma }^{\frac{1}{2}}$$
因为
$$tr(R^{\ast }H)=tr(A{A}^T)\ge tr(BA{A}^T),(B{B}^T=I)$$
所以
$$R^{\ast }=V{U}^T$$是
$$\underset{R}{\max }.tr(RH)$$
最优解

现在只要证明
$$tr(A{A}^T)\ge tr(BA{A}^T),(B{B}^T=I)$$
$a_i$是A的第i列，因为$tr(AB)=tr(BA)$那么有
$$tr(BA{A}^T)=tr(A^{T}BA)=\sum a_i^t(B{a}_i)$$
根据Schwarz不等式
$$a_i^t(B{a}_i)\le \sqrt{(a_i^t{a}_i)(a_i^t{B}^{t}B{a}_i)}=a_i^t{a}_i$$
即
$$tr(BA{A}^T)=tr(A^{T}BA)\le \sum a_i^t{a}_i=tr(A{A}^T)$$

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

几个情况需要考虑

1.H是满秩， { p ′ } \{p'\} {p′}上的点非共平面
2. { p ′ } \{p'\} {p′}上的点共平面，可以对H求出的解的为0特征值的特征向量计算取反，使得$det|H|=1$
3. { p ′ } \{p'\} {p′}上的点共线，不能用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);
}