CPD算法源码的研究

---

引言：
因为近期在研究扫地机的匹配算法，所有用到了CPD算法，但是CPD算法在我们的项目中总会出错，但是我们对于出错的原因并不是很了解，于是便开启看源码的坑。。。

---

一. CPD代码的整个调用框架
CPD算法按照论文中的算法流程（rigid的情况）：

如上图的左边的算法流程所示，接下来记录的也是针对左边的算法流程。
整个CPD的调用是采用matlab来调用C语言的代码。主要的实现实在cpd_rigid.m这个文件里。但是因为会涉及到一些参数的设置，所有一般是采用cpd_register.m来调用cpd_rigid这个函数的。
二. 各个函数的具体实现和调用
以下是cpd_register的定义：

function [Transform, C,sigma2]=cpd_register(X,Y, opt)
%这其中省略了大部分参数的判断
%%%% Choose the method and start CPD point-set registration
switch lower(opt.method)
    case 'rigid'%
        [C, R, t, s, sigma2, iter, T]=cpd_rigid(X,Y, opt.rot, opt.scale, opt.max_it, opt.tol, opt.viz, opt.outliers, opt.fgt, opt.corresp, opt.sigma2);
   %省略了其它的case
end

cpd_register的定义，其中的Transform就是两个点集之间匹配之后的旋转和平移矩阵。C是X中Y的对应点，就是Y= X(C,:),所以C的长度应该和Y的行数相等，sigma2表示的是目前两个点集的距离的平方和。在我们的工程中使用这个函数的情况如下：

opt.method='rigid'; % use rigid registration
opt.viz=0;          % show every iteration
opt.outliers=0.6;   % use 0.6 noise weight
opt.normalize=0;    % normalize to unit variance and zero mean before registering (default)
opt.scale=1;        % estimate global scaling too (default)
opt.rot=1;          % estimate strictly rotational matrix (default)
opt.corresp=1;      % compute correspondence vector at the end of registration (not being estimated by default)
opt.max_it=100;     % max number of iterations
opt.tol=1e-8;       % tolerance
%以上是参数的设置：
[Transform, C,sigma2] = cpd_register(X,Y, opt);
%X，Y是两个二维点集，其中X是不动的点集，Y是待匹配的点集（需要移动）

因此调用之前需要先设定参数。
在cpd_register中调用了cpd_rigid这个函数。传入的参数也是X，Y点集和一些参数。cpd_rigid这个函数里面调用了C语言实现的部分代码。实现CPD的整个算法流程主要是cpd_rigid这个函数。

function [C, R, t, s, sigma2, iter, T]=cpd_rigid(X,Y, rot, scale, max_it, tol, viz, outliers, fgt, corresp, sigma2)

[N, D]=size(X);
[M, D]=size(Y);
if viz, figure; end;

% Initialization
if ~exist('sigma2','var') || isempty(sigma2) || (sigma2==0)
    sigma2=(M*trace(X'*X)+N*trace(Y'*Y)-2*sum(X)*sum(Y)')/(M*N*D);
end
%'流程开始
sigma2_init=sigma2;
T=Y; s=1; R=eye(D);

% Optimization
iter=0; ntol=tol+10; L=0;
while (iter<max_it) && (ntol > tol) && (sigma2 > 10*eps)

    L_old=L;
    % Check wheather we want to use the Fast Gauss Transform
    if (fgt==0)  % no FGT
        %E step
        [P1,Pt1, PX, L]=cpd_P(X,T, sigma2 ,outliers); st='';
    else         % FGT
        [P1, Pt1, PX, L, sigma2, st]=cpd_Pfast(X, T, sigma2, outliers, sigma2_init, fgt);
    end
    ntol=abs((L-L_old)/L);
    disp([' CPD Rigid ' st ' : dL= ' num2str(ntol) ', iter= ' num2str(iter) ' sigma2= ' num2str(sigma2) ' Lost= ' num2str(L)]);
    Np=sum(Pt1);% all pmn
    mu_x=X'*Pt1/Np;
    mu_y=Y'*P1/Np;
    % Solve for Rotation, scaling, translation and sigma^2
    A=PX'*Y-Np*(mu_x*mu_y'); % A= X'P'*Y-X'P'1*1'P'Y/Np;
    [U,S,V]=svd(A); C=eye(D);
    if rot, C(end,end)=det(U*V'); end % check if we need strictly rotation (no reflections)
    R=U*C*V';
    sigma2save=sigma2;
    scale = 0;
    if scale  % check if estimating scaling as well, otherwise s=1
        s=trace(S*C)/(sum(sum(Y.^2.*repmat(P1,1,D))) - Np*(mu_y'*mu_y));
        sigma2=abs(sum(sum(X.^2.*repmat(Pt1,1,D))) - Np*(mu_x'*mu_x) -s*trace(S*C))/(Np*D);
    else
        sigma2=abs((sum(sum(X.^2.*repmat(Pt1,1,D))) - Np*(mu_x'*mu_x)+sum(sum(Y.^2.*repmat(P1,1,D))) - Np*(mu_y'*mu_y) -2*trace(S*C))/(Np*D));
    end

    t=mu_x-s*R*mu_y;

    % Update the GMM centroids
    T=s*Y*R'+repmat(t',[M 1]);

    iter=iter+1;
     if viz, cpd_plot_iter(X, T); end; % show current iteration if viz=1
end
% Find the correspondence, such that Y corresponds to X(C,:)
%corresp = 0;
if corresp, C=cpd_Pcorrespondence(X,T,sigma2save,outliers); else C=0; end;

由以上的代码可以知道，当FGT库不使用的时候，调用的是cpd_P.c这个C语言的代码。这个cpd_P.c函数实现的就是计算算法流程中的Pmn这个概率1值（所有的概率值都是基于距离来进行计算的）。在该算法中涉及到的主要三个输出参数是Pt1，P1，Px，这三个参数分别对应着概率值P(x|m)，P(m|x)，而Px就是P1乘以X的坐标。也就是论文中对应的先验概率和后验概率之间的转换。
下面贴出cpd_P.c 的源码：里面会有一些注释
主要实现计算的是里面的cpd_comp这个函数。

void cpd_comp(
        double* x,
        double* y, 
        double* sigma2,
        double* outlier,
        double* P1,
        double* Pt1,
        double* Px,
        double* E,
        int N,
        int M,
        int D
        )

{
  int       n, m, d;
  double    ksig, diff, razn, outlier_tmp, sp;
  double    *P, *temp_x;

  P = (double*) calloc(M, sizeof(double));// get the Y memory
  temp_x = (double*) calloc(D, sizeof(double));//get a point memory

  ksig = -2.0 * *sigma2;
  outlier_tmp=(*outlier*M*pow (-ksig*3.14159265358979,0.5*D))/((1-*outlier)*N); 
 /* printf ("ksig = %lf\n", *sigma2);*/
  /* outlier_tmp=*outlier*N/(1- *outlier)/M*(-ksig*3.14159265358979); */
  //pow(x,y) =  x^y

  for (n=0; n < N; n++) {//the number of X

      sp=0;
      for (m=0; m < M; m++) {//the number of Y
          razn=0;
          for (d=0; d < D; d++) {//the dimi of point 
             diff=*(x+n+d*N)-*(y+m+d*M);  diff=diff*diff;
             razn+=diff;
          }

          *(P+m)=exp(razn/ksig);//第p+m个点存储的就是第n个点到第m个点的e的距离，每次n变化的时候都会被更改，razn越大，sp越小
          sp+=*(P+m);
      }//sp计算一个x点到所有y点的欧式距离的指数之和

      sp+=outlier_tmp;//此刻的sp中存储的是第n点到Y中所有的点的距离之和，就是计算出这个两个点集之间的噪声点
      *(Pt1+n)=1-outlier_tmp/ sp;
      //Pt1表示的是每个x点距离y中所有点的距离的一个转换，距离越远（sp越小），这个Pt1的值越小
      for (d=0; d < D; d++) {
       *(temp_x+d)=*(x+n+d*N)/ sp;
      }

      for (m=0; m < M; m++) {

          *(P1+m)+=*(P+m)/ sp;//P1里面存储的是X中第n个点到Y中第m个点的距离。其实P1中存储的就是y中每个点到x中每个点的距离之后除以
          //上式表示的是当前的Y点到x点的距离在所有x点到Y点中的距离的比重。就是求出这个高斯模型中心的概率。这个概率会累加
          for (d=0; d < D; d++) {
          *(Px+m+d*M)+= *(temp_x+d)**(P+m);//这里表示对于一个x点(x1,x2),分别乘以x到y1，y2,y3的距离的负指数，如果距离远，那么得到的值就越小
          //其实px就是存储的是Y中的一个点到X中的所有点的距离的负指数之和，也就是对应着GMM中是由这个中心点生成这些数据的概率
          }

      }

   *E +=  -log(sp);     
  }
  *E +=D*N*log(*sigma2)/2;


  free((void*)P);
  free((void*)temp_x);

  return;
}

上面这个图比较乱，不过先贴上，有时间再来重新整理一下。这个图里面是拿Y等于3个点，X等于4个点的情况来进行考虑。然后一一走一遍cpd_P.c这个代码流程，得出Pt1，P1，Px的值。

由于自己的知识范围有限，可能一些知识理解的不是很清楚，希望大家帮忙更好的解答！先谢谢啦！

关于CPD，个人的理解就是考虑全部的数据点，不仅仅是考虑单独的某个点距离最近就可以了，而是考虑那些远距离的点对整体的一个影响。通过求correspondence这个信息，得出了一些以下的实验结果来验证CPD是和距离相关的：

如上图所示，对于上一排的三幅图片，分别是匹配之前，匹配之后，求出correspondence之后的效果图。上面的两个点集分布比较均匀，在那个特殊形状的上下分布的数据点比较均匀。求出的correspondence也是比较准确的，但是对于X中的第一个点，都会指向Y中的某个点，虽然这是多对一的情况，但是这个问题一直没想通为啥？（求解释–）
下面一排的三幅图就是数据点分布不均匀的情况。这种情况下匹配效果较差，但是找到的correspondence还是整体距离之和较小的。