135 Matrix4& transformation_matrix)
const
137 const int npts =
static_cast<int>(source_it.
size());
139 transformation_matrix.setIdentity();
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) {
148 const PointSource& a = *source_it;
149 const PointTarget& b = *target_it;
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;
159 c1[0] += axbx - azbz - ayby;
160 c1[5] += ayby - azbz - axbx;
161 c1[10] += azbz - axbx - ayby;
162 c1[15] += axbx + ayby + azbz;
163 c1[1] += axby + aybx;
164 c1[2] += axbz + azbx;
165 c1[3] += aybz - azby;
166 c1[6] += azby + aybz;
167 c1[7] += azbx - axbz;
168 c1[11] += axby - aybx;
196 const Eigen::Matrix<double, 4, 4> A =
197 (0.25 /
static_cast<double>(npts)) * C2.transpose() * C2 - C1;
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();
204 const Eigen::Matrix<double, 4, 1> smat =
205 -(0.5 /
static_cast<double>(npts)) * C2 * qmat;
207 const Eigen::Quaternion<double> q(qmat(3), qmat(0), qmat(1), qmat(2));
208 const Eigen::Quaternion<double> s(smat(3), smat(0), smat(1), smat(2));
210 const Eigen::Quaternion<double> t = s * q.conjugate();
212 const Eigen::Matrix<double, 3, 3> R(q.toRotationMatrix());
214 for (
int i = 0; i < 3; ++i)
215 for (
int j = 0; j < 3; ++j)
216 transformation_matrix(i, j) = R(i, j);
218 transformation_matrix(0, 3) = -t.x();
219 transformation_matrix(1, 3) = -t.y();
220 transformation_matrix(2, 3) = -t.z();