libfovis: absolute_orientation_horn.hpp Source File

Go to the documentation of this file.
00001 // Albert Huang <albert@csail.mit.edu>
00002 //
00003 // Implementation of 
00004 //
00005 //    Berthold K. P. Horn, 
00006 //    "Closed-form solution of absolute orientation using unit quaternions",
00007 //    Journal of the Optical society of America A, Vol. 4, April 1987
00008 
00009 #ifndef __absolute_orientation_horn__
00010 #define __absolute_orientation_horn__
00011 
00012 #include <assert.h>
00013 #include <Eigen/Geometry>
00014 #include <Eigen/Eigenvalues>
00015 
00030 template <typename DerivedA, typename DerivedB>
00031 int absolute_orientation_horn(const Eigen::MatrixBase<DerivedA>& P1, 
00032         const Eigen::MatrixBase<DerivedB>& P2,
00033         Eigen::Isometry3d* result)
00034 {
00035     int num_points = P1.cols();
00036     assert(P1.cols() == P2.cols());
00037     assert(P1.rows() == 3 && P2.rows() == 3);
00038     if(num_points < 3)
00039         return -1;
00040 
00041     // compute centroids of point sets
00042     Eigen::Vector3d P1_centroid = P1.rowwise().sum() / num_points;
00043     Eigen::Vector3d P2_centroid = P2.rowwise().sum() / num_points;
00044 
00045     Eigen::MatrixXd R1 = P1;
00046     R1.colwise() -= P1_centroid;
00047     Eigen::MatrixXd R2 = P2;
00048     R2.colwise() -= P2_centroid;
00049 
00050     // compute matrix M
00051     double Sxx = R1.row(0).dot(R2.row(0));
00052     double Sxy = R1.row(0).dot(R2.row(1));
00053     double Sxz = R1.row(0).dot(R2.row(2));
00054     double Syx = R1.row(1).dot(R2.row(0));
00055     double Syy = R1.row(1).dot(R2.row(1));
00056     double Syz = R1.row(1).dot(R2.row(2));
00057     double Szx = R1.row(2).dot(R2.row(0));
00058     double Szy = R1.row(2).dot(R2.row(1));
00059     double Szz = R1.row(2).dot(R2.row(2));
00060 
00061     double A00 = Sxx + Syy + Szz;
00062     double A01 = Syz - Szy;
00063     double A02 = Szx - Sxz;
00064     double A03 = Sxy - Syx;
00065     double A11 = Sxx - Syy - Szz;
00066     double A12 = Sxy + Syx;
00067     double A13 = Szx + Sxz;
00068     double A22 = -Sxx + Syy - Szz;
00069     double A23 = Syz + Szy;
00070     double A33 = -Sxx - Syy + Szz;
00071 
00072     // prepare matrix for eigen analysis
00073     Eigen::Matrix4d N;
00074     N << A00, A01, A02, A03,
00075         A01, A11, A12, A13,
00076         A02, A12, A22, A23,
00077         A03, A13, A23, A33;
00078 
00079     Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(N);
00080 
00081     // rotation quaternion is the eigenvector with greatest eigenvalue
00082     Eigen::Vector4d eigvals = eigensolver.eigenvalues();
00083     int max_eigen_ind = 0;
00084     double max_eigen_val = eigvals(0);
00085     for(int i=1; i<4; i++) {
00086         if(eigvals(i) > max_eigen_val) {
00087             max_eigen_val = eigvals(i);
00088             max_eigen_ind = i;
00089         }
00090     }
00091     Eigen::Vector4d quat = eigensolver.eigenvectors().col(max_eigen_ind);
00092     Eigen::Quaterniond rotation(quat[0], quat[1], quat[2], quat[3]);
00093  
00094     // now compute the resulting isometry
00095     result->setIdentity();
00096     result->translate(P2_centroid - rotation * P1_centroid);
00097     result->rotate(rotation);
00098 
00099     return 0;
00100 }
00101 #endif