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()).
template 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.template topLeftCorner<3, 3>() = R;
215 const Eigen::Matrix<Scalar, 3, 1> Rc(R * centroid_src.template head<3>());
216 transformation_matrix.template block<3, 1>(0, 3) =
217 centroid_tgt.template head<3>() - Rc;
220 size_t N = cloud_src_demean.cols();
221 PCL_DEBUG(
"[pcl::registration::TransformationEstimationSVD::"
222 "getTransformationFromCorrelation] Loss: %.10e\n",
223 (cloud_tgt_demean - R * cloud_src_demean).squaredNorm() / N);