From 12738dd2d4f3b3ff8ea66841d3048cf48ab9af12 Mon Sep 17 00:00:00 2001 From: rmensing Date: Mon, 13 Feb 2017 17:36:17 +0100 Subject: [PATCH] Fix reflection in umeyama. --- 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, 2 insertions(+), 129 deletions(-) diff --git a/common/include/pcl/common/eigen.h b/common/include/pcl/common/eigen.h index 83e0fb95f37..e8a1945cafb 100644 --- a/common/include/pcl/common/eigen.h +++ b/common/include/pcl/common/eigen.h @@ -378,40 +378,6 @@ 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 552224e6966..5ff6db1cda1 100644 --- a/common/include/pcl/common/impl/eigen.hpp +++ b/common/include/pcl/common/impl/eigen.hpp @@ -733,99 +733,6 @@ 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 1707c82ab51..39a63c4dffd 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 = pcl::umeyama (cloud_src, cloud_tgt, false); + transformation_matrix = Eigen::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 6f2de207257..ba698538d36 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 = pcl::umeyama (src, tgt, false); + Eigen::Matrix4d transformation_matrix = Eigen::umeyama (src, tgt, false); // Return the correct transformation transform.segment<4> (0).matrix () = transformation_matrix.cast ().row (0);