132 Matrix4& transformation_matrix)
const
135 const Eigen::AngleAxis<Scalar> rotation_z(parameters(2),
136 Eigen::Matrix<Scalar, 3, 1>::UnitZ());
137 const Eigen::AngleAxis<Scalar> rotation_y(parameters(1),
138 Eigen::Matrix<Scalar, 3, 1>::UnitY());
139 const Eigen::AngleAxis<Scalar> rotation_x(parameters(0),
140 Eigen::Matrix<Scalar, 3, 1>::UnitX());
141 const Eigen::Translation<Scalar, 3> translation(
142 parameters(3), parameters(4), parameters(5));
143 const Eigen::Transform<Scalar, 3, Eigen::Affine> transform =
144 rotation_z * rotation_y * rotation_x * translation * rotation_z * rotation_y *
146 transformation_matrix = transform.matrix();
154 Matrix4& transformation_matrix)
const
156 using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
157 using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
163 auto M = ATA.template selfadjointView<Eigen::Upper>();
168 for (; source_it.
isValid() && target_it.
isValid(); ++source_it, ++target_it) {
169 const Vector3 p(source_it->x, source_it->y, source_it->z);
170 const Vector3 q(target_it->x, target_it->y, target_it->z);
171 const Vector3 n1(source_it->getNormalVector3fMap().template cast<Scalar>());
172 const Vector3 n2(target_it->getNormalVector3fMap().template cast<Scalar>());
174 if (enforce_same_direction_normals_) {
175 if (n1.dot(n2) >= 0.)
184 if (!p.array().isFinite().all() || !q.array().isFinite().all() ||
185 !n.array().isFinite().all()) {
190 v << (p + q).cross(n), n;
193 ATb += v * (q - p).dot(n);
197 const Vector6 x = M.ldlt().solve(ATb);
200 constructTransformationMatrix(x, transformation_matrix);