From 4894327a9912ea4f2e9bd8a1c4ffff57a04aab75 Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Thu, 8 Jun 2017 20:34:31 +0200 Subject: [PATCH 1/3] Revert "Fix reflection in umeyama." This reverts commit 12738dd2d4f3b3ff8ea66841d3048cf48ab9af12 following the discussion in #1820. --- common/include/pcl/common/eigen.h | 34 +++++++ common/include/pcl/common/impl/eigen.hpp | 93 +++++++++++++++++++ .../impl/transformation_estimation_svd.hpp | 2 +- .../impl/sac_model_registration.hpp | 2 +- 4 files changed, 129 insertions(+), 2 deletions(-) diff --git a/common/include/pcl/common/eigen.h b/common/include/pcl/common/eigen.h index e8a1945cafb..83e0fb95f37 100644 --- a/common/include/pcl/common/eigen.h +++ b/common/include/pcl/common/eigen.h @@ -378,6 +378,40 @@ namespace pcl : (int (a) == Eigen::Dynamic || int (b) == Eigen::Dynamic) ? Eigen::Dynamic \ : (int (a) <= int (b)) ? int (a) : int (b)) + /** \brief Returns the transformation between two point sets. + * The algorithm is based on: + * "Least-squares estimation of transformation parameters between two point patterns", + * Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573 + * + * It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that + * \f{align*} + * \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2 + * \f} + * is minimized. + * + * The algorithm is based on the analysis of the covariance matrix + * \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$ + * of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where + * \f$d\f$ is corresponding to the dimension (which is typically small). + * The analysis is involving the SVD having a complexity of \f$O(d^3)\f$ + * though the actual computational effort lies in the covariance + * matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when + * the input point sets have dimension \f$d \times m\f$. + * + * \param[in] src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$ + * \param[in] dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$. + * \param[in] with_scaling Sets \f$ c=1 \f$ when false is passed. (default: false) + * \return The homogeneous transformation + * \f{align*} + * T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix} + * \f} + * minimizing the resudiual above. This transformation is always returned as an + * Eigen::Matrix. + */ + template + typename Eigen::internal::umeyama_transform_matrix_type::type + umeyama (const Eigen::MatrixBase& src, const Eigen::MatrixBase& dst, bool with_scaling = false); + /** \brief Transform a point using an affine matrix * \param[in] point_in the vector to be transformed * \param[out] point_out the transformed vector diff --git a/common/include/pcl/common/impl/eigen.hpp b/common/include/pcl/common/impl/eigen.hpp index 5ff6db1cda1..552224e6966 100644 --- a/common/include/pcl/common/impl/eigen.hpp +++ b/common/include/pcl/common/impl/eigen.hpp @@ -733,6 +733,99 @@ pcl::loadBinary (Eigen::MatrixBase const & matrix_, std::istream& file) } } +////////////////////////////////////////////////////////////////////////////////////////// +template +typename Eigen::internal::umeyama_transform_matrix_type::type +pcl::umeyama (const Eigen::MatrixBase& src, const Eigen::MatrixBase& dst, bool with_scaling) +{ + typedef typename Eigen::internal::umeyama_transform_matrix_type::type TransformationMatrixType; + typedef typename Eigen::internal::traits::Scalar Scalar; + typedef typename Eigen::NumTraits::Real RealScalar; + typedef typename Derived::Index Index; + + EIGEN_STATIC_ASSERT (!Eigen::NumTraits::IsComplex, NUMERIC_TYPE_MUST_BE_REAL) + EIGEN_STATIC_ASSERT ((Eigen::internal::is_same::Scalar>::value), + YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) + + enum { Dimension = PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC (Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) }; + + typedef Eigen::Matrix VectorType; + typedef Eigen::Matrix MatrixType; + typedef typename Eigen::internal::plain_matrix_type_row_major::type RowMajorMatrixType; + + const Index m = src.rows (); // dimension + const Index n = src.cols (); // number of measurements + + // required for demeaning ... + const RealScalar one_over_n = 1 / static_cast (n); + + // computation of mean + const VectorType src_mean = src.rowwise ().sum () * one_over_n; + const VectorType dst_mean = dst.rowwise ().sum () * one_over_n; + + // demeaning of src and dst points + const RowMajorMatrixType src_demean = src.colwise () - src_mean; + const RowMajorMatrixType dst_demean = dst.colwise () - dst_mean; + + // Eq. (36)-(37) + const Scalar src_var = src_demean.rowwise ().squaredNorm ().sum () * one_over_n; + + // Eq. (38) + const MatrixType sigma (one_over_n * dst_demean * src_demean.transpose ()); + + Eigen::JacobiSVD svd (sigma, Eigen::ComputeFullU | Eigen::ComputeFullV); + + // Initialize the resulting transformation with an identity matrix... + TransformationMatrixType Rt = TransformationMatrixType::Identity (m + 1, m + 1); + + // Eq. (39) + VectorType S = VectorType::Ones (m); + if (sigma.determinant () < 0) + S (m - 1) = -1; + + // Eq. (40) and (43) + const VectorType& d = svd.singularValues (); + Index rank = 0; + for (Index i = 0; i < m; ++i) + if (!Eigen::internal::isMuchSmallerThan (d.coeff (i), d.coeff (0))) + ++rank; + if (rank == m - 1) + { + if (svd.matrixU ().determinant () * svd.matrixV ().determinant () > 0) + Rt.block (0, 0, m, m).noalias () = svd.matrixU () * svd.matrixV ().transpose (); + else + { + const Scalar s = S (m - 1); + S (m - 1) = -1; + Rt.block (0, 0, m, m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose (); + S (m - 1) = s; + } + } + else + { + Rt.block (0, 0, m, m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose (); + } + + // Eq. (42) + if (with_scaling) + { + // Eq. (42) + const Scalar c = 1 / src_var * svd.singularValues ().dot (S); + + // Eq. (41) + Rt.col (m).head (m) = dst_mean; + Rt.col (m).head (m).noalias () -= c * Rt.topLeftCorner (m, m) * src_mean; + Rt.block (0, 0, m, m) *= c; + } + else + { + Rt.col (m).head (m) = dst_mean; + Rt.col (m).head (m).noalias () -= Rt.topLeftCorner (m, m) * src_mean; + } + + return (Rt); +} + ////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::transformLine (const Eigen::Matrix &line_in, diff --git a/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp b/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp index 39a63c4dffd..1707c82ab51 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp @@ -144,7 +144,7 @@ pcl::registration::TransformationEstimationSVD } // Call Umeyama directly from Eigen (PCL patched version until Eigen is released) - transformation_matrix = Eigen::umeyama (cloud_src, cloud_tgt, false); + transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false); } else { diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp index ba698538d36..6f2de207257 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp @@ -289,7 +289,7 @@ pcl::SampleConsensusModelRegistration::estimateRigidTransformationSVD ( } // Call Umeyama directly from Eigen - Eigen::Matrix4d transformation_matrix = Eigen::umeyama (src, tgt, false); + Eigen::Matrix4d transformation_matrix = pcl::umeyama (src, tgt, false); // Return the correct transformation transform.segment<4> (0).matrix () = transformation_matrix.cast ().row (0); From c79d00f2609f0758dcf60ba1db55bf6c34f191e2 Mon Sep 17 00:00:00 2001 From: Jakub Olesinski Date: Mon, 5 Sep 2016 19:42:50 +0200 Subject: [PATCH 2/3] Adapting umeyama from Eigen-3.3-beta2 to pcl. This fixes Eigen bug 774 for umeyama reflections. --- common/include/pcl/common/impl/eigen.hpp | 28 ++++-------------------- 1 file changed, 4 insertions(+), 24 deletions(-) diff --git a/common/include/pcl/common/impl/eigen.hpp b/common/include/pcl/common/impl/eigen.hpp index 552224e6966..f9c76dbd352 100644 --- a/common/include/pcl/common/impl/eigen.hpp +++ b/common/include/pcl/common/impl/eigen.hpp @@ -780,37 +780,17 @@ pcl::umeyama (const Eigen::MatrixBase& src, const Eigen::MatrixBase 0) - Rt.block (0, 0, m, m).noalias () = svd.matrixU () * svd.matrixV ().transpose (); - else - { - const Scalar s = S (m - 1); - S (m - 1) = -1; - Rt.block (0, 0, m, m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose (); - S (m - 1) = s; - } - } - else - { - Rt.block (0, 0, m, m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose (); - } + Rt.block (0,0,m,m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose (); - // Eq. (42) if (with_scaling) { // Eq. (42) - const Scalar c = 1 / src_var * svd.singularValues ().dot (S); + const Scalar c = Scalar (1)/ src_var * svd.singularValues ().dot (S); // Eq. (41) Rt.col (m).head (m) = dst_mean; From 7f76a71e1ed6076575942d8e108935881b99543c Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Thu, 8 Jun 2017 20:47:45 +0200 Subject: [PATCH 3/3] Use Eigen::umeyama() if Eigen 3.3.0 or above is available --- common/include/pcl/common/impl/eigen.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/common/include/pcl/common/impl/eigen.hpp b/common/include/pcl/common/impl/eigen.hpp index f9c76dbd352..3f3c00ebdf4 100644 --- a/common/include/pcl/common/impl/eigen.hpp +++ b/common/include/pcl/common/impl/eigen.hpp @@ -738,6 +738,9 @@ template typename Eigen::internal::umeyama_transform_matrix_type::type pcl::umeyama (const Eigen::MatrixBase& src, const Eigen::MatrixBase& dst, bool with_scaling) { +#if EIGEN_VERSION_AT_LEAST (3, 3, 0) + return Eigen::umeyama (src, dst, with_scaling); +#else typedef typename Eigen::internal::umeyama_transform_matrix_type::type TransformationMatrixType; typedef typename Eigen::internal::traits::Scalar Scalar; typedef typename Eigen::NumTraits::Real RealScalar; @@ -804,6 +807,7 @@ pcl::umeyama (const Eigen::MatrixBase& src, const Eigen::MatrixBase