libfovis
absolute_orientation_horn.hpp
1 // Albert Huang <albert@csail.mit.edu>
2 //
3 // Implementation of
4 //
5 // Berthold K. P. Horn,
6 // "Closed-form solution of absolute orientation using unit quaternions",
7 // Journal of the Optical society of America A, Vol. 4, April 1987
8 
9 #ifndef __absolute_orientation_horn__
10 #define __absolute_orientation_horn__
11 
12 #include <assert.h>
13 #include <Eigen/Geometry>
14 #include <Eigen/Eigenvalues>
15 
30 template <typename DerivedA, typename DerivedB>
31 int absolute_orientation_horn(const Eigen::MatrixBase<DerivedA>& P1,
32  const Eigen::MatrixBase<DerivedB>& P2,
33  Eigen::Isometry3d* result)
34 {
35  int num_points = P1.cols();
36  assert(P1.cols() == P2.cols());
37  assert(P1.rows() == 3 && P2.rows() == 3);
38  if(num_points < 3)
39  return -1;
40 
41  // compute centroids of point sets
42  Eigen::Vector3d P1_centroid = P1.rowwise().sum() / num_points;
43  Eigen::Vector3d P2_centroid = P2.rowwise().sum() / num_points;
44 
45  Eigen::MatrixXd R1 = P1;
46  R1.colwise() -= P1_centroid;
47  Eigen::MatrixXd R2 = P2;
48  R2.colwise() -= P2_centroid;
49 
50  // compute matrix M
51  double Sxx = R1.row(0).dot(R2.row(0));
52  double Sxy = R1.row(0).dot(R2.row(1));
53  double Sxz = R1.row(0).dot(R2.row(2));
54  double Syx = R1.row(1).dot(R2.row(0));
55  double Syy = R1.row(1).dot(R2.row(1));
56  double Syz = R1.row(1).dot(R2.row(2));
57  double Szx = R1.row(2).dot(R2.row(0));
58  double Szy = R1.row(2).dot(R2.row(1));
59  double Szz = R1.row(2).dot(R2.row(2));
60 
61  double A00 = Sxx + Syy + Szz;
62  double A01 = Syz - Szy;
63  double A02 = Szx - Sxz;
64  double A03 = Sxy - Syx;
65  double A11 = Sxx - Syy - Szz;
66  double A12 = Sxy + Syx;
67  double A13 = Szx + Sxz;
68  double A22 = -Sxx + Syy - Szz;
69  double A23 = Syz + Szy;
70  double A33 = -Sxx - Syy + Szz;
71 
72  // prepare matrix for eigen analysis
73  Eigen::Matrix4d N;
74  N << A00, A01, A02, A03,
75  A01, A11, A12, A13,
76  A02, A12, A22, A23,
77  A03, A13, A23, A33;
78 
79  Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(N);
80 
81  // rotation quaternion is the eigenvector with greatest eigenvalue
82  Eigen::Vector4d eigvals = eigensolver.eigenvalues();
83  int max_eigen_ind = 0;
84  double max_eigen_val = eigvals(0);
85  for(int i=1; i<4; i++) {
86  if(eigvals(i) > max_eigen_val) {
87  max_eigen_val = eigvals(i);
88  max_eigen_ind = i;
89  }
90  }
91  Eigen::Vector4d quat = eigensolver.eigenvectors().col(max_eigen_ind);
92  Eigen::Quaterniond rotation(quat[0], quat[1], quat[2], quat[3]);
93 
94  // now compute the resulting isometry
95  result->setIdentity();
96  result->translate(P2_centroid - rotation * P1_centroid);
97  result->rotate(rotation);
98 
99  return 0;
100 }
101 #endif
Generated on Thu Aug 6 2015 22:22:06 for libfovis by   doxygen 1.8.9.1