Merge pull request #1820 from delftrobotics-forks/fix-reflection-umeyama

Fix reflection in umeyama.

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 <code>false</code> 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 Derived, typename OtherDerived> 
-  typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
-  umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& 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

common/include/pcl/common/impl/eigen.hpp

@@ -733,99 +733,6 @@ pcl::loadBinary (Eigen::MatrixBase<Derived> const & matrix_, std::istream& file)
     }
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////
-template <typename Derived, typename OtherDerived> 
-typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
-pcl::umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling)
-{
-  typedef typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType;
-  typedef typename Eigen::internal::traits<TransformationMatrixType>::Scalar Scalar;
-  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
-  typedef typename Derived::Index Index;
-
-  EIGEN_STATIC_ASSERT (!Eigen::NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL)
-  EIGEN_STATIC_ASSERT ((Eigen::internal::is_same<Scalar, typename Eigen::internal::traits<OtherDerived>::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<Scalar, Dimension, 1> VectorType;
-  typedef Eigen::Matrix<Scalar, Dimension, Dimension> MatrixType;
-  typedef typename Eigen::internal::plain_matrix_type_row_major<Derived>::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<RealScalar> (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<MatrixType> 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 <typename Scalar> bool
 pcl::transformLine (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_in,

registration/include/pcl/registration/impl/transformation_estimation_svd.hpp

@@ -144,7 +144,7 @@ pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>
     }
 
     // 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
   {

sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp

@@ -289,7 +289,7 @@ pcl::SampleConsensusModelRegistration<PointT>::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<float> ().row (0);