1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for linear algebra. 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com> 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This Source Code Form is subject to the terms of the Mozilla 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Public License v. 2.0. If a copy of the MPL was not distributed 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#ifndef EIGEN_UMEYAMA_H 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#define EIGEN_UMEYAMA_H 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file requires the user to include 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// * Eigen/Core 15c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// * Eigen/LU 16c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// * Eigen/SVD 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// * Eigen/Array 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace Eigen { 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#ifndef EIGEN_PARSED_BY_DOXYGEN 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// These helpers are required since it allows to use mixed types as parameters 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for the Umeyama. The problem with mixed parameters is that the return type 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// cannot trivially be deduced when float and double types are mixed. 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace internal { 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Compile time return type deduction for different MatrixBase types. 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Different means here different alignment and parameters but the same underlying 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// real scalar type. 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename MatrixType, typename OtherMatrixType> 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathstruct umeyama_transform_matrix_type 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum { 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath MinRowsAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime), 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // When possible we want to choose some small fixed size value since the result 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // is likely to fit on the stack. So here, EIGEN_SIZE_MIN_PREFER_DYNAMIC is not what we want. 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath HomogeneousDimension = int(MinRowsAtCompileTime) == Dynamic ? Dynamic : int(MinRowsAtCompileTime)+1 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<typename traits<MatrixType>::Scalar, 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath HomogeneousDimension, 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath HomogeneousDimension, 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AutoAlign | (traits<MatrixType>::Flags & RowMajorBit ? RowMajor : ColMajor), 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath HomogeneousDimension, 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath HomogeneousDimension 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath > type; 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \geometry_module \ingroup Geometry_Module 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \brief Returns the transformation between two point sets. 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* The algorithm is based on: 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* "Least-squares estimation of transformation parameters between two point patterns", 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \f{align*} 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \f} 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* is minimized. 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* The algorithm is based on the analysis of the covariance matrix 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$ 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \f$d\f$ is corresponding to the dimension (which is typically small). 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* The analysis is involving the SVD having a complexity of \f$O(d^3)\f$ 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* though the actual computational effort lies in the covariance 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* the input point sets have dimension \f$d \times m\f$. 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* Currently the method is working only for floating point matrices. 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \todo Should the return type of umeyama() become a Transform? 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \param src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$. 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \param dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$. 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \param with_scaling Sets \f$ c=1 \f$ when <code>false</code> is passed. 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \return The homogeneous transformation 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \f{align*} 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix} 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* \f} 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* minimizing the resudiual above. This transformation is always returned as an 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath* Eigen::Matrix. 92c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath*/ 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate <typename Derived, typename OtherDerived> 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathumeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, bool with_scaling = true) 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType; 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar; 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename NumTraits<Scalar>::Real RealScalar; 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename Derived::Index Index; 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL) 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename internal::traits<OtherDerived>::Scalar>::value), 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum { Dimension = EIGEN_SIZE_MIN_PREFER_DYNAMIC(Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) }; 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar, Dimension, 1> VectorType; 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar, Dimension, Dimension> MatrixType; 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType; 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index m = src.rows(); // dimension 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Index n = src.cols(); // number of measurements 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 115c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // required for demeaning ... 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const RealScalar one_over_n = 1 / static_cast<RealScalar>(n); 117c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 118c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // computation of mean 119c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const VectorType src_mean = src.rowwise().sum() * one_over_n; 120c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const VectorType dst_mean = dst.rowwise().sum() * one_over_n; 121c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 122c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // demeaning of src and dst points 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const RowMajorMatrixType src_demean = src.colwise() - src_mean; 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const RowMajorMatrixType dst_demean = dst.colwise() - dst_mean; 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Eq. (36)-(37) 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n; 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Eq. (38) 130c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose(); 131c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath JacobiSVD<MatrixType> svd(sigma, ComputeFullU | ComputeFullV); 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 134c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Initialize the resulting transformation with an identity matrix... 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath TransformationMatrixType Rt = TransformationMatrixType::Identity(m+1,m+1); 136c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 137c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Eq. (39) 138c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VectorType S = VectorType::Ones(m); 139c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (sigma.determinant()<0) S(m-1) = -1; 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Eq. (40) and (43) 142c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const VectorType& d = svd.singularValues(); 143c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Index rank = 0; for (Index i=0; i<m; ++i) if (!internal::isMuchSmallerThan(d.coeff(i),d.coeff(0))) ++rank; 144c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (rank == m-1) { 145c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if ( svd.matrixU().determinant() * svd.matrixV().determinant() > 0 ) { 146c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Rt.block(0,0,m,m).noalias() = svd.matrixU()*svd.matrixV().transpose(); 147c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } else { 148c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar s = S(m-1); S(m-1) = -1; 149c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); 150c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath S(m-1) = s; 151c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 152c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } else { 153c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); 154c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 155c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 156c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Eq. (42) 157c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Scalar c = 1/src_var * svd.singularValues().dot(S); 158c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 159c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Eq. (41) 160c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Note that we first assign dst_mean to the destination so that there no need 161c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // for a temporary. 162c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Rt.col(m).head(m) = dst_mean; 163c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean; 164c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 165c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (with_scaling) Rt.block(0,0,m,m) *= c; 166c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 167c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return Rt; 168c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 169c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 170c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace Eigen 171c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 172c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif // EIGEN_UMEYAMA_H 173