Eigen  3.4.90 (git rev 67eeba6e720c5745abc77ae6c92ce0a44aa7b7ae)
Umeyama.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 #ifndef EIGEN_UMEYAMA_H
11 #define EIGEN_UMEYAMA_H
12 
13 // This file requires the user to include
14 // * Eigen/Core
15 // * Eigen/LU
16 // * Eigen/SVD
17 // * Eigen/Array
18 
19 #include "./InternalHeaderCheck.h"
20 
21 namespace Eigen {
22 
23 #ifndef EIGEN_PARSED_BY_DOXYGEN
24 
25 // These helpers are required since it allows to use mixed types as parameters
26 // for the Umeyama. The problem with mixed parameters is that the return type
27 // cannot trivially be deduced when float and double types are mixed.
28 namespace internal {
29 
30 // Compile time return type deduction for different MatrixBase types.
31 // Different means here different alignment and parameters but the same underlying
32 // real scalar type.
33 template<typename MatrixType, typename OtherMatrixType>
34 struct umeyama_transform_matrix_type
35 {
36  enum {
37  MinRowsAtCompileTime = internal::min_size_prefer_dynamic(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime),
38 
39  // When possible we want to choose some small fixed size value since the result
40  // is likely to fit on the stack. So here, min_size_prefer_dynamic is not what we want.
41  HomogeneousDimension = int(MinRowsAtCompileTime) == Dynamic ? Dynamic : int(MinRowsAtCompileTime)+1
42  };
43 
44  typedef Matrix<typename traits<MatrixType>::Scalar,
45  HomogeneousDimension,
46  HomogeneousDimension,
47  AutoAlign | (traits<MatrixType>::Flags & RowMajorBit ? RowMajor : ColMajor),
48  HomogeneousDimension,
49  HomogeneousDimension
50  > type;
51 };
52 
53 }
54 
55 #endif
56 
95 template <typename Derived, typename OtherDerived>
96 typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
97 umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, bool with_scaling = true)
98 {
99  typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType;
100  typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar;
101  typedef typename NumTraits<Scalar>::Real RealScalar;
102 
103  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL)
104  EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename internal::traits<OtherDerived>::Scalar>::value),
105  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
106 
107  enum { Dimension = internal::min_size_prefer_dynamic(Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
108 
109  typedef Matrix<Scalar, Dimension, 1> VectorType;
110  typedef Matrix<Scalar, Dimension, Dimension> MatrixType;
111  typedef typename internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType;
112 
113  const Index m = src.rows(); // dimension
114  const Index n = src.cols(); // number of measurements
115 
116  // required for demeaning ...
117  const RealScalar one_over_n = RealScalar(1) / static_cast<RealScalar>(n);
118 
119  // computation of mean
120  const VectorType src_mean = src.rowwise().sum() * one_over_n;
121  const VectorType dst_mean = dst.rowwise().sum() * one_over_n;
122 
123  // demeaning of src and dst points
124  const RowMajorMatrixType src_demean = src.colwise() - src_mean;
125  const RowMajorMatrixType dst_demean = dst.colwise() - dst_mean;
126 
127  // Eq. (38)
128  const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose();
129 
131 
132  // Initialize the resulting transformation with an identity matrix...
133  TransformationMatrixType Rt = TransformationMatrixType::Identity(m+1,m+1);
134 
135  // Eq. (39)
136  VectorType S = VectorType::Ones(m);
137 
138  if ( svd.matrixU().determinant() * svd.matrixV().determinant() < 0 )
139  S(m-1) = -1;
140 
141  // Eq. (40) and (43)
142  Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose();
143 
144  if (with_scaling)
145  {
146  // Eq. (36)-(37)
147  const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n;
148 
149  // Eq. (42)
150  const Scalar c = Scalar(1)/src_var * svd.singularValues().dot(S);
151 
152  // Eq. (41)
153  Rt.col(m).head(m) = dst_mean;
154  Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean;
155  Rt.block(0,0,m,m) *= c;
156  }
157  else
158  {
159  Rt.col(m).head(m) = dst_mean;
160  Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m,m)*src_mean;
161  }
162 
163  return Rt;
164 }
165 
166 } // end namespace Eigen
167 
168 #endif // EIGEN_UMEYAMA_H
TransposeReturnType transpose()
Definition: Transpose.h:184
ConstColwiseReturnType colwise() const
Definition: DenseBase.h:551
ConstRowwiseReturnType rowwise() const
Definition: DenseBase.h:539
EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
Definition: EigenBase.h:65
EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
Definition: EigenBase.h:62
Two-sided Jacobi SVD decomposition of a rectangular matrix.
Definition: JacobiSVD.h:514
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:52
const DiagonalWrapper< const Derived > asDiagonal() const
Definition: DiagonalMatrix.h:327
Scalar determinant() const
Definition: Determinant.h:110
The matrix class, also used for vectors and row-vectors.
Definition: Matrix.h:182
const MatrixVType & matrixV() const
Definition: SVDBase.h:191
const SingularValuesType & singularValues() const
Definition: SVDBase.h:203
const MatrixUType & matrixU() const
Definition: SVDBase.h:175
const SumReturnType sum() const
Definition: VectorwiseOp.h:480
internal::umeyama_transform_matrix_type< Derived, OtherDerived >::type umeyama(const MatrixBase< Derived > &src, const MatrixBase< OtherDerived > &dst, bool with_scaling=true)
Returns the transformation between two point sets.
Definition: Umeyama.h:97
@ ColMajor
Definition: Constants.h:321
@ RowMajor
Definition: Constants.h:323
@ AutoAlign
Definition: Constants.h:325
const unsigned int RowMajorBit
Definition: Constants.h:68
Namespace containing all symbols from the Eigen library.
Definition: Core:139
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:59
const int Dynamic
Definition: Constants.h:24
Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
Definition: NumTraits.h:231