142 Eigen::Matrix<double, 4, 4>
C1 = Eigen::Matrix<double, 4, 4>::Zero();
143 Eigen::Matrix<double, 4, 4>
C2 = Eigen::Matrix<double, 4, 4>::Zero();
144 double* c1 =
C1.data();
145 double* c2 =
C2.data();
147 for (
int i = 0; i <
npts; ++i) {
150 const double axbx = a.x * b.x;
151 const double ayby = a.y * b.y;
152 const double azbz = a.z * b.z;
153 const double axby = a.x * b.y;
154 const double aybx = a.y * b.x;
155 const double axbz = a.x * b.z;
156 const double azbx = a.z * b.x;
157 const double aybz = a.y * b.z;
158 const double azby = a.z * b.y;
196 const Eigen::Matrix<double, 4, 4>
A =
199 const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>
es(
A);
202 es.eigenvalues().real().maxCoeff(&i);
203 const Eigen::Matrix<double, 4, 1>
qmat =
es.eigenvectors().col(i).real();
209 const Eigen::Quaternion<double>
t = s *
q.conjugate();
211 const Eigen::Matrix<double, 3, 3>
R(
q.toRotationMatrix());
213 for (
int i = 0; i < 3; ++i)
214 for (
int j = 0; j < 3; ++j)