1 Mat shape_model::procrustes(const Mat &X,const int itol,const float ftol)
 2 {
 3   /* X.cols:特征点个数，X.rows: 图像数量*2 */
 4   int N = X.cols,n = X.rows/2;
 5
 6   //remove centre of mass
 7   Mat P = X.clone();
 8   for(int i = 0; i < N; i++)
 9   {
10     /*取X第i个列向量*/
11     Mat p = P.col(i);
12     float mx = 0,my = 0;
13     for(int j = 0; j < n; j++)
14     {
15         mx += p.fl(2*j);
16         my += p.fl(2*j+1);
17     }
18     /*分别求图像集，2维空间坐标x和y的平均值*/
19     mx /= n;
20     my /= n;
21     /*对x,y坐标去中心化*/
22     for(int j = 0; j < n; j++)
23     {
24         p.fl(2*j) -= mx;
25         p.fl(2*j+1) -= my;
26     }
27   }
28   //optimise scale and rotation
29   Mat C_old;
30   for(int iter = 0; iter < itol; iter++)
31   {
32     /*计算（含n个形状）的重心*/
33     Mat C = P*Mat::ones(N,1,CV_32F)/N;
34     /*C为2n*1维矩阵，含n个重心，对n个重心归一化处理*/
35     normalize(C,C);
36     if(iter > 0)
37     {
38       if(norm(C,C_old) < ftol)//norm:求绝对范数，小于阈值，则退出循环
39         break;
40     }
41     C_old = C.clone();
42     for(int i = 0; i < N; i++)
43     {
44       //求当前形状与归一化重心之间的旋转角度，即上式a和b
45       Mat R = this->rot_scale_align(P.col(i),C);
46       for(int j = 0; j < n; j++)
47       {
48         float x = P.fl(2*j,i),y = P.fl(2*j+1,i);
49         /*仿射变化*/
50         P.fl(2*j  ,i) = R.fl(0,0)*x + R.fl(0,1)*y;
51         P.fl(2*j+1,i) = R.fl(1,0)*x + R.fl(1,1)*y;
52       }
53     }
54   }
55 return P;
56 }

http://blog.csdn.net/jinshengtao/article/details/43376049