From 1d46bb42a3e547fbfda2a4d72fa65a25ed55be31 Mon Sep 17 00:00:00 2001 From: sdmiller Date: Sun, 14 Apr 2013 16:35:09 -0700 Subject: [PATCH] Fixing TransformationEstimationSVDScale --- .../impl/transformation_estimation_svd.hpp | 45 ++++++++++--------- .../transformation_estimation_svd_scale.hpp | 23 ++++++---- .../transformation_estimation_svd.h | 11 ++++- .../transformation_estimation_svd_scale.h | 13 ++++-- 4 files changed, 57 insertions(+), 35 deletions(-) diff --git a/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp b/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp index 16d68f50007..14cbf904bae 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp @@ -139,27 +139,30 @@ pcl::registration::TransformationEstimationSVD ++target_it; } - // Call Umeyama directly from Eigen (PCL patched version until Eigen is released) - transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false); - -#if 0 - source_it.reset (); target_it.reset (); - // is the source dataset - transformation_matrix.setIdentity (); - - Eigen::Matrix centroid_src, centroid_tgt; - // Estimate the centroids of source, target - compute3DCentroid (source_it, centroid_src); - compute3DCentroid (target_it, centroid_tgt); - source_it.reset (); target_it.reset (); - - // Subtract the centroids from source, target - Eigen::Matrix cloud_src_demean, cloud_tgt_demean; - demeanPointCloud (source_it, centroid_src, cloud_src_demean); - demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean); - - getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); -#endif + if (use_umeyama_) + { + // Call Umeyama directly from Eigen (PCL patched version until Eigen is released) + transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false); + } + else + { + source_it.reset (); target_it.reset (); + // is the source dataset + transformation_matrix.setIdentity (); + + Eigen::Matrix centroid_src, centroid_tgt; + // Estimate the centroids of source, target + compute3DCentroid (source_it, centroid_src); + compute3DCentroid (target_it, centroid_tgt); + source_it.reset (); target_it.reset (); + + // Subtract the centroids from source, target + Eigen::Matrix cloud_src_demean, cloud_tgt_demean; + demeanPointCloud (source_it, centroid_src, cloud_src_demean); + demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean); + + getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); + } } /////////////////////////////////////////////////////////////////////////////////////////// diff --git a/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp b/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp index 6ae68cea144..9cb35469a07 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp @@ -42,16 +42,16 @@ ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::registration::TransformationEstimationSVDScale::getTransformationFromCorrelation ( - const Eigen::MatrixXf &cloud_src_demean, - const Eigen::Vector4f ¢roid_src, - const Eigen::MatrixXf &cloud_tgt_demean, - const Eigen::Vector4f ¢roid_tgt, + const Eigen::Matrix &cloud_src_demean, + const Eigen::Matrix ¢roid_src, + const Eigen::Matrix &cloud_tgt_demean, + const Eigen::Matrix ¢roid_tgt, Matrix4 &transformation_matrix) const { transformation_matrix.setIdentity (); // Assemble the correlation matrix H = source * target' - Eigen::Matrix H = (cloud_src_demean.cast () * cloud_tgt_demean.cast ().transpose ()).topLeftCorner (3, 3); + Eigen::Matrix H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3); // Compute the Singular Value Decomposition Eigen::JacobiSVD > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); @@ -68,7 +68,14 @@ pcl::registration::TransformationEstimationSVDScale R = v * u.transpose (); // rotated cloud - Eigen::Matrix src_ = R * cloud_src_demean.cast (); + Eigen::Matrix R4; + R4.block (0, 0, 3, 3) = R; + R4 (0, 3) = 0; + R4 (1, 3) = 0; + R4 (2, 3) = 0; + R4 (3, 3) = 1; + + Eigen::Matrix src_ = R4 * cloud_src_demean; float scale1, scale2; double sum_ss = 0.0f, sum_tt = 0.0f, sum_tt_ = 0.0f; @@ -91,8 +98,8 @@ pcl::registration::TransformationEstimationSVDScale Rc (R * centroid_src.cast ().head (3)); - transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.cast (). head (3) - Rc; + const Eigen::Matrix Rc (R * centroid_src.head (3)); + transformation_matrix.block (0, 3, 3, 1) = centroid_tgt. head (3) - Rc; } //#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD; diff --git a/registration/include/pcl/registration/transformation_estimation_svd.h b/registration/include/pcl/registration/transformation_estimation_svd.h index 59032aac441..c5030686f39 100644 --- a/registration/include/pcl/registration/transformation_estimation_svd.h +++ b/registration/include/pcl/registration/transformation_estimation_svd.h @@ -63,7 +63,12 @@ namespace pcl typedef typename TransformationEstimation::Matrix4 Matrix4; - TransformationEstimationSVD () {}; + /** \brief Constructor + * \param[in] use_umeyama Toggles whether or not to use 3rd party software*/ + TransformationEstimationSVD (bool use_umeyama=true): + use_umeyama_ (use_umeyama) + {} + virtual ~TransformationEstimationSVD () {}; /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. @@ -137,13 +142,15 @@ namespace pcl * \param[in] centroid_tgt the input target cloud, in Eigen format * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix */ - void + virtual void getTransformationFromCorrelation ( const Eigen::Matrix &cloud_src_demean, const Eigen::Matrix ¢roid_src, const Eigen::Matrix &cloud_tgt_demean, const Eigen::Matrix ¢roid_tgt, Matrix4 &transformation_matrix) const; + + bool use_umeyama_; }; } diff --git a/registration/include/pcl/registration/transformation_estimation_svd_scale.h b/registration/include/pcl/registration/transformation_estimation_svd_scale.h index e51eaf448d2..045c830ea9a 100644 --- a/registration/include/pcl/registration/transformation_estimation_svd_scale.h +++ b/registration/include/pcl/registration/transformation_estimation_svd_scale.h @@ -63,6 +63,11 @@ namespace pcl typedef typename TransformationEstimationSVD::Matrix4 Matrix4; + /** \brief Inherits from TransformationEstimationSVD, but forces it to not use the Umeyama method */ + TransformationEstimationSVDScale (): + TransformationEstimationSVD (false) + {} + protected: /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src * tgt' * \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen format @@ -72,10 +77,10 @@ namespace pcl * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix */ void - getTransformationFromCorrelation (const Eigen::MatrixXf &cloud_src_demean, - const Eigen::Vector4f ¢roid_src, - const Eigen::MatrixXf &cloud_tgt_demean, - const Eigen::Vector4f ¢roid_tgt, + getTransformationFromCorrelation (const Eigen::Matrix &cloud_src_demean, + const Eigen::Matrix ¢roid_src, + const Eigen::Matrix &cloud_tgt_demean, + const Eigen::Matrix ¢roid_tgt, Matrix4 &transformation_matrix) const; };