开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > 3D图形编程 > 查看源码
absorient.cc
资源名称：scanalyze-1.0.3_source_code.rar　[点击查看]
上传用户：kellyonhid
上传日期：2013-10-12
资源大小：932k
文件大小：18k
源码类别：

3D图形编程

开发平台：

Visual C++

absorient.cc：源码内容
  1. //############################################################
  2. // absorient.cc
  3. // Kari Pulli
  4. // 06/06/97
  5. // 
  6. // Two methods for solving the rotation and translation for
  7. // registering two sets of 3D points.
  8. //
  9. // Horn's method takes as input two lists of corresponding
  10. // 3D points. It calculates in one step the optimum motion
  11. // to align the sets. The starting point does not have to
  12. // be close to the solution.
  13. //
  14. // Chen & Medioni's method takes as input a list of points
  15. // and a list of planes, one plane for each point. The 
  16. // algorithm tries to move the sets so that the points get
  17. // close to their corresponding planes. It is assumed that
  18. // only small rotations are needed, i.e., the sets are 
  19. // in approximate registration.
  20. //############################################################
  22. #include <math.h>
  23. #include <limits.h>
  24. #include <iostream.h>
  25. #include "Pnt3.h"
  26. #include "Xform.h"
  27. #include "tnt.h"
  28. #include "vec.h"
  29. #include "cmat.h"
  30. #include "trislv.h"
  31. #include "transv.h"         /* transpose views */
  32. #include "lu.h"
  33. #include <float.h>
  35. #ifdef WIN32
  36. #define cbrt(r)  (pow((r), 1./3))
  37. #endif
  40. ////////////////////////////////////////////////////////
  41. ////////////////////////////////////////////////////////
  42. ////              Horn's method starts              ////
  43. ////////////////////////////////////////////////////////
  44. ////////////////////////////////////////////////////////
  47. // transform a quaternion to a rotation matrix
  48. static void 
  49. quaternion2matrix(double *q, double m[3][3])
  50. {
  51.   double q00 = q[0]*q[0];
  52.   double q11 = q[1]*q[1];
  53.   double q22 = q[2]*q[2];
  54.   double q33 = q[3]*q[3];
  55.   double q03 = q[0]*q[3];
  56.   double q13 = q[1]*q[3];
  57.   double q23 = q[2]*q[3];
  58.   double q02 = q[0]*q[2];
  59.   double q12 = q[1]*q[2];
  60.   double q01 = q[0]*q[1];
  61.   m[0][0] = q00 + q11 - q22 - q33;
  62.   m[1][1] = q00 - q11 + q22 - q33;
  63.   m[2][2] = q00 - q11 - q22 + q33;
  64.   m[0][1] = 2.0*(q12-q03);
  65.   m[1][0] = 2.0*(q12+q03);
  66.   m[0][2] = 2.0*(q13+q02);
  67.   m[2][0] = 2.0*(q13-q02);
  68.   m[1][2] = 2.0*(q23-q01);
  69.   m[2][1] = 2.0*(q23+q01);
  70. }
  73. // find the coefficients of the characteristic eqn.
  74. // l^4 + a l^3 + b l^2 + c l + d = 0
  75. // for a symmetric 4x4 matrix
  76. static void
  77. characteristicPol(double Q[4][4], double c[4])
  78. {
  79.   // squares
  80.   double q01_2 = Q[0][1] * Q[0][1];
  81.   double q02_2 = Q[0][2] * Q[0][2];
  82.   double q03_2 = Q[0][3] * Q[0][3];
  83.   double q12_2 = Q[1][2] * Q[1][2];
  84.   double q13_2 = Q[1][3] * Q[1][3];
  85.   double q23_2 = Q[2][3] * Q[2][3];
  87.   // other factors
  88.   double q0011 = Q[0][0] * Q[1][1];
  89.   double q0022 = Q[0][0] * Q[2][2];
  90.   double q0033 = Q[0][0] * Q[3][3];
  91.   double q0102 = Q[0][1] * Q[0][2];
  92.   double q0103 = Q[0][1] * Q[0][3];
  93.   double q0223 = Q[0][2] * Q[2][3];
  94.   double q1122 = Q[1][1] * Q[2][2];
  95.   double q1133 = Q[1][1] * Q[3][3];
  96.   double q1223 = Q[1][2] * Q[2][3];
  97.   double q2233 = Q[2][2] * Q[3][3];
  99.   // a
  100.   c[0] = -Q[0][0] - Q[1][1] - Q[2][2] - Q[3][3];
  102.   // b
  103.   c[1] = - q01_2 - q02_2 - q03_2 + q0011 - q12_2 - 
  104.     q13_2 + q0022 + q1122 - q23_2 + q0033 + q1133 + 
  105.     q2233;
  107.   // c
  108.   c[2] = (q02_2 + q03_2 + q23_2)*Q[1][1] - 2*q0102*Q[1][2] + 
  109.     (q12_2 + q13_2 + q23_2)*Q[0][0] +
  110.     (q01_2 + q03_2 - q0011 + q13_2 - q1133)*Q[2][2] - 
  111.     2*Q[0][3]*q0223 - 2*(q0103 + q1223)*Q[1][3] + 
  112.     (q01_2 + q02_2 - q0011 + q12_2 - q0022)*Q[3][3];
  113.     
  114.   // d
  115.   c[3] = 2*(-Q[0][2]*Q[0][3]*Q[1][2] + q0103*Q[2][2] -
  116.     Q[0][1]*q0223 + Q[0][0]*q1223)*Q[1][3] +
  117.     q02_2*q13_2 - q03_2*q1122 - q13_2*q0022 + 
  118.     2*Q[0][3]*Q[1][1]*q0223 - 2*q0103*q1223 + q01_2*q23_2 - 
  119.     q0011*q23_2 - q02_2*q1133 + q03_2*q12_2 +
  120.     2*q0102*Q[1][2]*Q[3][3] - q12_2*q0033 - q01_2*q2233 + 
  121.     q0011*q2233;
  122. }
  126. static int ferrari(double a, double b, double c, double d,
  127.   double rts[4]);
  129. // calculate the maximum eigenvector of a symmetric
  130. // 4x4 matrix
  131. // from B. Horn 1987 Closed-form solution of absolute
  132. // orientation using unit quaternions (J.Opt.Soc.Am.A)
  133. void
  134. maxEigenVector(double Q[4][4], double ev[4])
  135. {
  136.   static C_matrix<double> N(4,4);
  137.   double rts[4];
  138.   double c[4];
  139.   // find the coeffs for the characteristic eqn.
  140.   characteristicPol(Q, c);
  141.   // find roots
  142.   ferrari(c[0], c[1], c[2], c[3], rts);
  143.   // find maximum root = maximum eigenvalue
  144.   double l = rts[0];
  145.   if (rts[1] > l) l = rts[1];
  146.   if (rts[2] > l) l = rts[2];
  147.   if (rts[3] > l) l = rts[3];
  149.   /*
  150.   SHOW(l);
  151.   SHOW(rts[0]);
  152.   SHOW(rts[1]);
  153.   SHOW(rts[2]);
  154.   SHOW(rts[3]);
  155.   */
  157.   // create the Q - l*I matrix
  158.   N[0][0]=Q[0][0]-l;N[0][1]=Q[0][1] ;N[0][2]=Q[0][2]; N[0][3]=Q[0][3];
  159.   N[1][0]=Q[1][0]; N[1][1]=Q[1][1]-l;N[1][2]=Q[1][2]; N[1][3]=Q[1][3];
  160.   N[2][0]=Q[2][0]; N[2][1]=Q[2][1] ;N[2][2]=Q[2][2]-l;N[2][3]=Q[2][3];
  161.   N[3][0]=Q[3][0]; N[3][1]=Q[3][1] ;N[3][2]=Q[3][2];N[3][3]=Q[3][3]-l;
  162.   // the columns of the inverted matrix should be multiples of
  163.   // the eigenvector, pick the largest
  164.   static Vector<int> ipiv(4);
  165.   static Vector<double> best(4),curr(4);
  166.   //C_matrix<double> Ntmp = N;
  167.   if (LU_factor(N, ipiv)) {
  168.     //SHOW(Q[0][0]);
  169.     cerr << "maxEigenVector():" << endl;
  170.     cerr << "LU_factor failed!" << endl;
  171.     cerr << "return identity quaternion" << endl;
  172.     //cerr << N << endl;
  173.     ev[0] = 1.0;
  174.     ev[1] = ev[2] = ev[3] = 0.0;
  175.     return;
  176.   }
  177.   best = 0; best[0] = 1;
  178.   LU_solve(N, ipiv, best);
  179.   double len = 
  180.     best[0]*best[0] + best[1]*best[1] + 
  181.     best[2]*best[2] + best[3]*best[3];
  182.   for (int i=1; i<4; i++) {
  183.     curr = 0; curr[i] = 1;
  184.     LU_solve(N, ipiv, curr);
  185.     double tlen = 
  186.       curr[0]*curr[0] + curr[1]*curr[1] + 
  187.       curr[2]*curr[2] + curr[3]*curr[3];
  188.     if (tlen > len) { len = tlen; best = curr; }
  189.   }
  190.   // normalize the result
  191.   len = 1.0/sqrt(len);
  192.   ev[0] = best[0]*len;
  193.   ev[1] = best[1]*len;
  194.   ev[2] = best[2]*len;
  195.   ev[3] = best[3]*len;
  196. }
  198. // find the transformation that aligns measurements to
  199. // model
  200. void
  201. horn_align(Pnt3 *p,      // the model points  (source)
  202.    Pnt3 *x,      // the measured points (destination)
  203.    int n,        // how many pairs
  204.    double q[7])  // the reg. vector 0..3 rot, 4..6 trans
  205. {
  206.   if (n<3) {
  207.     cerr << "horn_align() was given " << n << " pairs," 
  208.  << " while at least 3 are required" << endl;
  209.     return;
  210.   }
  212.   Pnt3 p_mean, x_mean;
  213.   double S[3][3];
  214.   double Q[4][4];
  215.   int i,j;
  217.   // calculate the centers of mass
  218.   for (i=0; i<n; i++) {
  219.     p_mean += p[i];
  220.     x_mean += x[i];
  221.   }
  222.   p_mean /= n;
  223.   x_mean /= n;
  224.   // calculate the cross covariance matrix
  225.   for (i=0; i<3; i++)
  226.     for (j=0; j<3; j++)
  227.       S[i][j] = 0;
  228.   for (i=0; i<n; i++) {
  229.     S[0][0] += p[i][0]*x[i][0];
  230.     S[0][1] += p[i][0]*x[i][1];
  231.     S[0][2] += p[i][0]*x[i][2];
  232.     S[1][0] += p[i][1]*x[i][0];
  233.     S[1][1] += p[i][1]*x[i][1];
  234.     S[1][2] += p[i][1]*x[i][2];
  235.     S[2][0] += p[i][2]*x[i][0];
  236.     S[2][1] += p[i][2]*x[i][1];
  237.     S[2][2] += p[i][2]*x[i][2];
  238.   }
  239.   double fact = 1/double(n);
  240.   for (i=0; i<3; i++)
  241.     for (j=0; j<3; j++)
  242.       S[i][j] *= fact;
  243.   S[0][0] -= p_mean[0]*x_mean[0];
  244.   S[0][1] -= p_mean[0]*x_mean[1];
  245.   S[0][2] -= p_mean[0]*x_mean[2];
  246.   S[1][0] -= p_mean[1]*x_mean[0];
  247.   S[1][1] -= p_mean[1]*x_mean[1];
  248.   S[1][2] -= p_mean[1]*x_mean[2];
  249.   S[2][0] -= p_mean[2]*x_mean[0];
  250.   S[2][1] -= p_mean[2]*x_mean[1];
  251.   S[2][2] -= p_mean[2]*x_mean[2];
  252.   // calculate the 4x4 symmetric matrix Q
  253.   double trace = S[0][0] + S[1][1] + S[2][2];
  254.   double A23 = S[1][2] - S[2][1];
  255.   double A31 = S[2][0] - S[0][2];
  256.   double A12 = S[0][1] - S[1][0];
  258.   Q[0][0] = trace;
  259.   Q[0][1] = Q[1][0] = A23;
  260.   Q[0][2] = Q[2][0] = A31;
  261.   Q[0][3] = Q[3][0] = A12;
  262.   for (i=0; i<3; i++)
  263.     for (j=0; j<3; j++)
  264.       Q[i+1][j+1] = S[i][j]+S[j][i]-(i==j ? trace : 0);
  265.   
  266.   maxEigenVector(Q, q);
  268.   // calculate the rotation matrix
  269.   double m[3][3]; // rot matrix
  270.   quaternion2matrix(q, m);
  271.   // calculate the translation vector, put it into q[4..6]
  272.   q[4] = x_mean[0] - m[0][0]*p_mean[0] - 
  273.     m[0][1]*p_mean[1] - m[0][2]*p_mean[2];
  274.   q[5] = x_mean[1] - m[1][0]*p_mean[0] - 
  275.     m[1][1]*p_mean[1] - m[1][2]*p_mean[2];
  276.   q[6] = x_mean[2] - m[2][0]*p_mean[0] - 
  277.     m[2][1]*p_mean[1] - m[2][2]*p_mean[2];
  278. }
  281. /**************************************************/
  282. static int 
  283. qudrtc(double b, double c, double rts[4])
  284. /* 
  285.      solve the quadratic equation - 
  287.          x**2+b*x+c = 0 
  289. */
  290. {
  291.   int nquad;
  292.   double rtdis;
  293.   double dis = b*b-4.0*c;
  294.   
  295.   if (dis >= 0.0) {
  296.     nquad = 2;
  297.     rtdis = sqrt(dis);
  298.     if (b > 0.0) rts[0] = ( -b - rtdis)*.5;
  299.     else         rts[0] = ( -b + rtdis)*.5;
  300.     if (rts[0] == 0.0) rts[1] = -b;
  301.     else               rts[1] = c/rts[0];
  302.   } else {
  303.     nquad = 0;
  304.     rts[0] = 0.0;
  305.     rts[1] = 0.0;
  306.   }
  307.   return nquad;
  308. } /* qudrtc */
  309. /**************************************************/
  311. static double 
  312. cubic(double p, double q, double r)
  313. /* 
  314.      find the lowest real root of the cubic - 
  315.        x**3 + p*x**2 + q*x + r = 0 
  317.    input parameters - 
  318.      p,q,r - coeffs of cubic equation. 
  320.    output- 
  321.      cubic - a real root. 
  323.    method - 
  324.      see D.E. Littlewood, "A University Algebra" pp.173 - 6 
  326.      Charles Prineas   April 1981 
  328. */
  329. {
  330.   //int nrts;
  331.   double po3,po3sq,qo3;
  332.   double uo3,u2o3,uo3sq4,uo3cu4;
  333.   double v,vsq,wsq;
  334.   double m,mcube,n;
  335.   double muo3,s,scube,t,cosk,sinsqk;
  336.   double root;
  337.   
  338.   double doubmax = sqrt(DBL_MAX);
  339.   
  340.   m = 0.0;
  341.   //nrts =0;
  342.   if        ((p > doubmax) || (p <  -doubmax)) {
  343.     root = -p;
  344.   } else if ((q > doubmax) || (q <  -doubmax)) {
  345.     if (q > 0.0) root = -r/q;
  346.     else         root = -sqrt(-q);
  347.   } else if ((r > doubmax)|| (r <  -doubmax)) {
  348.     root =  -cbrt(r);
  349.   } else {
  350.     po3 = p/3.0;
  351.     po3sq = po3*po3;
  352.     if (po3sq > doubmax) root =  -p;
  353.     else {
  354.       v = r + po3*(po3sq + po3sq - q);
  355.       if ((v > doubmax) || (v < -doubmax)) 
  356. root = -p;
  357.       else {
  358. vsq = v*v;
  359. qo3 = q/3.0;
  360. uo3 = qo3 - po3sq;
  361. u2o3 = uo3 + uo3;
  362. if ((u2o3 > doubmax) || (u2o3 < -doubmax)) {
  363.   if (p == 0.0) {
  364.     if (q > 0.0) root =  -r/q;
  365.     else         root =  -sqrt(-q);
  366.   } else         root =  -q/p;
  367. }
  368. uo3sq4 = u2o3*u2o3;
  369. if (uo3sq4 > doubmax) {
  370.   if (p == 0.0) {
  371.     if (q > 0.0) root = -r/q;
  372.     else         root = -sqrt(fabs(q));
  373.   } else         root = -q/p;
  374. }
  375. uo3cu4 = uo3sq4*uo3;
  376. wsq = uo3cu4 + vsq;
  377. if (wsq >= 0.0) {
  378.   //
  379.   // cubic has one real root 
  380.   //
  381.   //nrts = 1;
  382.   if (v <= 0.0) mcube = ( -v + sqrt(wsq))*.5;
  383.   if (v  > 0.0) mcube = ( -v - sqrt(wsq))*.5;
  384.   m = cbrt(mcube);
  385.   if (m != 0.0) n = -uo3/m;
  386.   else          n = 0.0;
  387.   root = m + n - po3;
  388. } else {
  389.   //nrts = 3;
  390.   //
  391.   // cubic has three real roots 
  392.   //
  393.   if (uo3 < 0.0) {
  394.     muo3 = -uo3;
  395.     s = sqrt(muo3);
  396.     scube = s*muo3;
  397.     t =  -v/(scube+scube);
  398.     cosk = cos(acos(t)/3.0);
  399.     if (po3 < 0.0)
  400.       root = (s+s)*cosk - po3;
  401.     else {
  402.       sinsqk = 1.0 - cosk*cosk;
  403.       if (sinsqk < 0.0) sinsqk = 0.0;
  404.       root = s*( -cosk - sqrt(3*sinsqk)) - po3;
  405.     }
  406.   } else
  407.     //
  408.     // cubic has multiple root -  
  409.     //
  410.     root = cbrt(v) - po3;
  411. }
  412.       }
  413.     }
  414.   }
  415.   return root;
  416. } /* cubic */
  417. /***************************************/
  420. static int 
  421. ferrari(double a, double b, double c, double d, double rts[4])
  422. /* 
  423.      solve the quartic equation - 
  425.    x**4 + a*x**3 + b*x**2 + c*x + d = 0 
  427.      input - 
  428.    a,b,c,e - coeffs of equation. 
  430.      output - 
  431.    nquar - number of real roots. 
  432.    rts - array of root values. 
  434.      method :  Ferrari - Lagrange
  435.      Theory of Equations, H.W. Turnbull p. 140 (1947)
  437.      calls  cubic, qudrtc 
  438. */
  439. {
  440.   int nquar,n1,n2;
  441.   double asq,ainv2;
  442.   double v1[4],v2[4];
  443.   double p,q,r;
  444.   double y;
  445.   double e,f,esq,fsq,ef;
  446.   double g,gg,h,hh;
  448.   asq = a*a;
  450.   p = b;
  451.   q = a*c-4.0*d;
  452.   r = (asq - 4.0*b)*d + c*c;
  453.   y = cubic(p,q,r);
  455.   esq = .25*asq - b - y;
  456.   if (esq < 0.0) return(0);
  457.   else {
  458.     fsq = .25*y*y - d;
  459.     if (fsq < 0.0) return(0);
  460.     else {
  461.       ef = -(.25*a*y + .5*c);
  462.       if ( ((a > 0.0)&&(y > 0.0)&&(c > 0.0))
  463.    || ((a > 0.0)&&(y < 0.0)&&(c < 0.0))
  464.    || ((a < 0.0)&&(y > 0.0)&&(c < 0.0))
  465.    || ((a < 0.0)&&(y < 0.0)&&(c > 0.0))
  466.    ||  (a == 0.0)||(y == 0.0)||(c == 0.0)
  467.    ) {
  468. /* use ef - */
  469. if ((b < 0.0)&&(y < 0.0)&&(esq > 0.0)) {
  470.   e = sqrt(esq);
  471.   f = ef/e;
  472. } else if ((d < 0.0) && (fsq > 0.0)) {
  473.   f = sqrt(fsq);
  474.   e = ef/f;
  475. } else {
  476.   e = sqrt(esq);
  477.   f = sqrt(fsq);
  478.   if (ef < 0.0) f = -f;
  479. }
  480.       } else {
  481. e = sqrt(esq);
  482. f = sqrt(fsq);
  483. if (ef < 0.0) f = -f;
  484.       }
  485.       /* note that e >= 0.0 */
  486.       ainv2 = a*.5;
  487.       g = ainv2 - e;
  488.       gg = ainv2 + e;
  489.       if ( ((b > 0.0)&&(y > 0.0))
  490.    || ((b < 0.0)&&(y < 0.0)) ) {
  491. if (( a > 0.0) && (e != 0.0)) g = (b + y)/gg;
  492. else if (e != 0.0) gg = (b + y)/g;
  493.       }
  494.       if ((y == 0.0)&&(f == 0.0)) {
  495. h = 0.0;
  496. hh = 0.0;
  497.       } else if ( ((f > 0.0)&&(y < 0.0))
  498.   || ((f < 0.0)&&(y > 0.0)) ) {
  499. hh = -.5*y + f;
  500. h = d/hh;
  501.       } else {
  502. h = -.5*y - f;
  503. hh = d/h;
  504.       }
  505.       n1 = qudrtc(gg,hh,v1);
  506.       n2 = qudrtc(g,h,v2);
  507.       nquar = n1+n2;
  508.       rts[0] = v1[0];
  509.       rts[1] = v1[1];
  510.       rts[n1+0] = v2[0];
  511.       rts[n1+1] = v2[1];
  512.       return nquar;
  513.     }
  514.   }
  515. } /* ferrari */
  518. ////////////////////////////////////////////////////////
  519. ////////////////////////////////////////////////////////
  520. ////        Chen & Medioni's method starts          ////
  521. ////////////////////////////////////////////////////////
  522. ////////////////////////////////////////////////////////
  524. static void
  525. get_transform(double correction[6],
  526.       double m[3][3], double t[3])
  527. {
  528.   // produces a premult matrix: p' = M . p + t
  529.   double sa = sin(correction[0]); 
  530.   double ca = sqrt(1-sa*sa);
  531.   double sb = sin(correction[1]); 
  532.   double cb = sqrt(1-sb*sb);
  533.   double sr = sin(correction[2]);
  534.   double cr = sqrt(1-sr*sr);
  536.   t[0] = correction[3];
  537.   t[1] = correction[4];
  538.   t[2] = correction[5];
  540.   m[0][0] = cb*cr;
  541.   m[0][1] = -cb*sr;
  542.   m[0][2] = sb;
  544.   m[1][0] = sa*sb*cr + ca*sr;
  545.   m[1][1] = -sa*sb*sr + ca*cr;
  546.   m[1][2] = -sa*cb;
  548.   m[2][0] = -ca*sb*cr + sa*sr;
  549.   m[2][1] = ca*sb*sr + sa*cr;
  550.   m[2][2] = ca*cb;
  551. }
  553. // Solve x from Ax=b using Cholesky decomposition.
  554. // This function changes the contents of A in the process.
  555. static bool
  556. cholesky_solve(double A[6][6], double b[6], double x[6])
  557. {
  558.   int i, j, k;
  559.   double sum;
  561.   for (i=0; i<6; i++) {
  563.     for (sum=A[i][i], k=0; k<i; k++)
  564.       sum -= A[i][k]*A[i][k];
  566.       if (sum < 0.0) {
  567.         cerr << "Cholesky: matrix not pos.semidef." << endl;
  568.         SHOW(sum);
  569.         return false;
  570.       } else if (sum < 1.0e-7) {
  571.         cerr << "Cholesky: matrix not pos.def." << endl;
  572.         return false;
  573.       } else {
  574.         A[i][i] = sqrt(sum);
  575.     }
  577.     for (j=i+1; j<6; j++) {
  579.       for (sum=A[i][j], k=0; k<i; k++)
  580.         sum -= A[i][k]*A[j][k];
  582.       A[j][i] = sum / A[i][i];
  583.     }
  584.   }
  586.   for (i=0; i<6; i++) {               // Forward elimination;
  587.     for (sum=b[i], j=0; j<i; j++)     // solve Ly=b, store y in x
  588.       sum -= A[i][j]*x[j];
  589.     x[i] = sum / A[i][i];
  590.   }
  592.   for (i=5; i>=0; i--) {              // Backward elimination;
  593.     for (sum=x[i], j=i+1; j<6; j++)   // solve L'x = y
  594.       sum -= A[j][i]*x[j];
  595.     x[i] = sum / A[i][i];
  596.   }
  598.   return true;
  599. }
  601. // Calculate the rotation and translation that moves points in 
  602. // array ctr toward their pairs.
  603. void
  604. chen_medioni(Pnt3 *ctr,     // control points (source)
  605.      Pnt3 *srf,     // points on the tangent plane (target)
  606.      Pnt3 *nrm,     // the normals at the pairs
  607.      int n,         // how many pairs
  608.      double q[7])   // registration quaternion
  609. {
  610.   // The least-squares solutions for the translation and 
  611.   // rotation are found independently.
  612.   // Therefore it is much better to first move the control points
  613.   // around origin, and fix the result in the end.
  614.   // cm = Sum{ctr[i]}/n
  615.   // R(p-cm) + t + cm == Rp + { t + cm - Rcm }
  616.   
  617.   Pnt3 cm;
  618.   for (int i=0; i<n; i++) cm += ctr[i];
  619.   cm /= float(n);
  621.   double HtH[6][6];
  622.   double HtP[6], Hi[6];
  624.   for (i=0; i<6; i++) {
  625.     HtH[i][0] =
  626.     HtH[i][1] =
  627.     HtH[i][2] =
  628.     HtH[i][3] =
  629.     HtH[i][4] =
  630.     HtH[i][5] =
  631.     HtP[i] = 0;
  632.   }
  634.   double Pi;
  635.   Pnt3 PxS;
  636.   double sum = 0;
  637.   for (i=0; i<n; i++) {
  638.     Pi = dot(srf[i]-ctr[i], nrm[i]);
  639.     PxS = cross(ctr[i]-cm, nrm[i]);
  640.     Hi[0] = PxS[0];
  641.     Hi[1] = PxS[1];
  642.     Hi[2] = PxS[2];
  643.     Hi[3] = nrm[i][0];
  644.     Hi[4] = nrm[i][1];
  645.     Hi[5] = nrm[i][2];
  646.     HtP[0] += Pi * Hi[0];
  647.     HtP[1] += Pi * Hi[1];
  648.     HtP[2] += Pi * Hi[2];
  649.     HtP[3] += Pi * Hi[3];
  650.     HtP[4] += Pi * Hi[4];
  651.     HtP[5] += Pi * Hi[5];
  652.     HtH[0][0] += Hi[0]*Hi[0];
  653.     HtH[0][1] += Hi[0]*Hi[1];
  654.     HtH[0][2] += Hi[0]*Hi[2];
  655.     HtH[0][3] += Hi[0]*Hi[3];
  656.     HtH[0][4] += Hi[0]*Hi[4];
  657.     HtH[0][5] += Hi[0]*Hi[5];
  658.     HtH[1][1] += Hi[1]*Hi[1];
  659.     HtH[1][2] += Hi[1]*Hi[2];
  660.     HtH[1][3] += Hi[1]*Hi[3];
  661.     HtH[1][4] += Hi[1]*Hi[4];
  662.     HtH[1][5] += Hi[1]*Hi[5];
  663.     HtH[2][2] += Hi[2]*Hi[2];
  664.     HtH[2][3] += Hi[2]*Hi[3];
  665.     HtH[2][4] += Hi[2]*Hi[4];
  666.     HtH[2][5] += Hi[2]*Hi[5];
  667.     HtH[3][3] += Hi[3]*Hi[3];
  668.     HtH[3][4] += Hi[3]*Hi[4];
  669.     HtH[3][5] += Hi[3]*Hi[5];
  670.     HtH[4][4] += Hi[4]*Hi[4];
  671.     HtH[4][5] += Hi[4]*Hi[5];
  672.     HtH[5][5] += Hi[5]*Hi[5];
  673.     sum += Pi*Pi;
  674.   }
  676.   cout << "Sqrt of average squared error before transform " 
  677.        << sqrtf(sum/n) << endl;
  678.   
  679.   // solve Ax=b using Cholesky decomposition
  680.   double d[6];
  681.   Xform<double> xf;
  682.   if( cholesky_solve(HtH,HtP,d) ) {
  683.     double m[3][3];
  684.     double t[3];
  685.     get_transform(d, m, t);
  686.     
  687.     // fix the translation, see comments above
  688.     float *c = &cm[0];
  689.     t[0] += c[0] - (m[0][0]*c[0]+m[0][1]*c[1]+m[0][2]*c[2]);
  690.     t[1] += c[1] - (m[1][0]*c[0]+m[1][1]*c[1]+m[1][2]*c[2]);
  691.     t[2] += c[2] - (m[2][0]*c[0]+m[2][1]*c[1]+m[2][2]*c[2]);
  693.     // output as quaternion
  694.     xf.fromRotTrans (m, t);
  695.   } else {
  696.     cerr << "Warning: Cholesky failed" << endl;
  697.   }
  699.   xf.toQuaternion (q);
  700.   xf.getTranslation (q+4);
  701. }
  703. #ifdef TEST_ABSORIENT
  705. #include "Pnt3.h"
  706. #include "Random.h"
  707. #include "Xform.h"
  709. void
  710. main(void)
  711. {
  712.   Random rand;
  714.   Pnt3 x[1000];
  715.   Pnt3 p[1000];
  716.   double qd[7];
  718.   // random points
  719.   for (int i=0; i<1000; i++) {
  720.     x[i] = Pnt3(rand(), rand(), rand())*200.0 - Pnt3(100,100,100);
  721.   }
  723.   // random rotation
  724.   double axis[3] = {1,1,1};
  725.   axis[0] = rand();
  726.   axis[1] = rand();
  727.   axis[2] = rand();
  728.   Xform<double> xf;
  729.   xf.rot(rand(), axis[0], axis[1], axis[2]);
  731.   float sum = 0.0;
  732.   for (i=0; i<1000; i++) {
  733.     p[i] = x[i];
  734.     xf(p[i]);
  735.     sum += dist2(p[i],x[i]);
  736.   }
  737.   SHOW(sum);
  739.   horn_align(&p[0], &x[0], 1000, qd);
  740.   xf.fromQuaternion(qd, qd[4], qd[5], qd[6]);
  742.   sum = 0.0;
  743.   for (i=0; i<1000; i++) {
  744.     xf(p[i]);
  745.     sum += dist2(p[i],x[i]);
  746.   }
  747.   SHOW(sum);
  748. }
  749. #endif