162 Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
170 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean,
187 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
188 const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
189 const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
190 const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
191 Matrix4& transformation_matrix)
const
193 transformation_matrix.setIdentity();
196 Eigen::Matrix<Scalar, 3, 3> H =
197 (cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3);
200 Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3>> svd(
201 H, Eigen::ComputeFullU | Eigen::ComputeFullV);
202 Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU();
203 Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV();
206 if (u.determinant() * v.determinant() < 0) {
207 for (
int x = 0; x < 3; ++x)
211 Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose();
214 transformation_matrix.topLeftCorner(3, 3) = R;
215 const Eigen::Matrix<Scalar, 3, 1> Rc(R * centroid_src.head(3));
216 transformation_matrix.block(0, 3, 3, 1) = centroid_tgt.head(3) - Rc;
219 size_t N = cloud_src_demean.cols();
220 PCL_DEBUG(
"[pcl::registration::TransformationEstimationSVD::"
221 "getTransformationFromCorrelation] Loss: %.10e\n",
222 (cloud_tgt_demean - R * cloud_src_demean).squaredNorm() / N);