[vtkusers] (Help) how to test vtk IterativeClosestPoint(ICP) arithmetic with JacobiN

[老猫]

I'm using vtk to solve a problem which can use IterativeClosestPoint(ICP) arithmetic to match the source points with the target points.
 What I can get is a Matrix4x4 from the functions in vtk. Now I want to check if the result is correct. So I try to use JacobiN(,,,) in vtk to get the 
 quaternion and then use the quaternion to get the rotation matrix. At last , I can get the translation vector.
 But when I compare the translation vector I get and the translation vector I set, I find they are not similar at all.
  
 I can give you the part of the code to show my mean:
  
     //the sourcedata and target data have been set.Both of them have 100 points.And I set the translation vector(No rotation matrix)
  
     vtkIterativeClosestPointTransform * icp = vtkIterativeClosestPointTransform::New();
    icp->SetSource(SourcePolydata);
    icp->SetTarget(TargetPolydata);
    icp->GetLandmarkTransform()->SetModeToRigidBody(); 
    icp->SetMaximumNumberOfIterations(20);
    icp->StartByMatchingCentroidsOn();
    icp->Modified();
    icp->Update();
  
     vtkMatrix4x4* M = icp->GetMatrix();
    cout << "Get the Matrix4X4 " << endl;
  
     double ** a;
    a = (double **)malloc(sizeof(double *)*4);
    for( i = 0; i < 4; i++ )
    *(a+i)=(double *)malloc(sizeof(double)*4);
     double ** R;
    R = (double **)malloc(sizeof(double *)*4);
    for( i = 0; i < 4; i++ )
    *(R+i)=(double *)malloc(sizeof(double)*4);
    double * T;
    T = (double *)malloc(sizeof(double)*4);
     for( i = 0; i <= 3; i++ )
    {
        printf("\n");
        for( int j = 0; j <= 3; j++ )
        {
            printf("%e\t",M->Element[i][j]);
            a[i][j] = M->Element[i][j]; 
        }
    }
    cout << endl;
 
     //Get the quaternion
     vtkMath::JacobiN( (double **)a, 4, (double *)T, (double **)R );
     
     //Get the rotation matrix
     double Rr[3][3];
     Rr[0][0] = R[0][0]*R[0][0]+R[0][1]*R[0][1]-R[0][2]*R[0][2]-R[0][3]*R[0][3];
    Rr[0][1] = 2*(R[0][1]*R[0][2]-R[0][0]*R[0][3]);
    Rr[0][2] = 2*(R[0][1]*R[0][3]+R[0][0]*R[0][2]);
    Rr[1][0] = 2*(R[0][1]*R[0][2]+R[0][0]*R[0][3]);
    Rr[1][1] = -(R[0][0]*R[0][0]-R[0][1]*R[0][1]+R[0][2]*R[0][2]-R[0][3]*R[0][3]);
    Rr[1][2] = 2*(R[0][2]*R[0][3]-R[0][0]*R[0][1]);
    Rr[2][0] = 2*(R[0][1]*R[0][3]-R[0][0]*R[0][2]);
    Rr[2][1] = 2*(R[0][2]*R[0][3]+R[0][0]*R[0][1]);
    Rr[2][2] = -(R[0][0]*R[0][0]-R[0][1]*R[0][1]-R[0][2]*R[0][2]+R[0][3]*R[0][3]);
  
     double C3[3];
     // C2[] is the centre point of target points,C1[] is the centre point of source points  
    C3[0] = C2[0] * Rr[0][0] + C2[1] * Rr[1][0] + C2[2] * Rr[2][0] ;
    C3[1] = C2[0] * Rr[0][1] + C2[1] * Rr[1][1] + C2[2] * Rr[2][1] ;
    C3[2] = C2[0] * Rr[0][2] + C2[1] * Rr[1][2] + C2[2] * Rr[2][2] ;
     
     for( i = 0; i < 3; i++ )
        cout << C1[i] - C3[i] << " ";
    cout << endl;
  
 Maybe my way to test was wrong.But I want to get the rotation matrix and translation vector. Could you please solve the problem or give me another way to get what I want.
 Thank you.
        Tranks
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://www.vtk.org/pipermail/vtkusers/attachments/20110302/31e809f3/attachment.htm>