51 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>&
cloud_src_demean,
53 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>&
cloud_tgt_demean,
60 Eigen::Matrix<Scalar, 3, 3> H =
64 Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3>>
svd(
65 H, Eigen::ComputeFullU | Eigen::ComputeFullV);
66 Eigen::Matrix<Scalar, 3, 3> u =
svd.matrixU();
67 Eigen::Matrix<Scalar, 3, 3> v =
svd.matrixV();
70 if (u.determinant() * v.determinant() < 0) {
71 for (
int x = 0; x < 3; ++x)
75 Eigen::Matrix<Scalar, 3, 3>
R = v * u.transpose();
78 Eigen::Matrix<Scalar, 4, 4>
R4;
79 R4.block(0, 0, 3, 3) =
R;
100 const Eigen::Matrix<Scalar, 3, 1>
Rc(scale *
R *
centroid_src.head(3));