10 #ifndef EIGEN_UMEYAMA_H
11 #define EIGEN_UMEYAMA_H
19 #include "./InternalHeaderCheck.h"
23 #ifndef EIGEN_PARSED_BY_DOXYGEN
33 template<
typename MatrixType,
typename OtherMatrixType>
34 struct umeyama_transform_matrix_type
37 MinRowsAtCompileTime = internal::min_size_prefer_dynamic(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime),
41 HomogeneousDimension = int(MinRowsAtCompileTime) ==
Dynamic ?
Dynamic : int(MinRowsAtCompileTime)+1
44 typedef Matrix<typename traits<MatrixType>::Scalar,
95 template <
typename Derived,
typename OtherDerived>
96 typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
99 typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType;
100 typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar;
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)
107 enum { Dimension = internal::min_size_prefer_dynamic(Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
117 const RealScalar one_over_n = RealScalar(1) /
static_cast<RealScalar
>(n);
120 const VectorType src_mean = src.
rowwise().
sum() * one_over_n;
121 const VectorType dst_mean = dst.
rowwise().
sum() * one_over_n;
124 const RowMajorMatrixType src_demean = src.
colwise() - src_mean;
125 const RowMajorMatrixType dst_demean = dst.
colwise() - dst_mean;
128 const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose();
133 TransformationMatrixType Rt = TransformationMatrixType::Identity(m+1,m+1);
136 VectorType S = VectorType::Ones(m);
147 const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n;
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;
159 Rt.col(m).head(m) = dst_mean;
160 Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m,m)*src_mean;
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