10 #ifndef EIGEN_UMEYAMA_H
11 #define EIGEN_UMEYAMA_H
21 #ifndef EIGEN_PARSED_BY_DOXYGEN
31 template<
typename MatrixType,
typename OtherMatrixType>
32 struct umeyama_transform_matrix_type
35 MinRowsAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime),
39 HomogeneousDimension = int(MinRowsAtCompileTime) ==
Dynamic ?
Dynamic : int(MinRowsAtCompileTime)+1
42 typedef Matrix<typename traits<MatrixType>::Scalar,
93 template <
typename Derived,
typename OtherDerived>
94 typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
97 typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType;
98 typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar;
102 EIGEN_STATIC_ASSERT((internal::is_same<Scalar,
typename internal::traits<OtherDerived>::Scalar>::value),
103 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
105 enum { Dimension = EIGEN_SIZE_MIN_PREFER_DYNAMIC(Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
115 const RealScalar one_over_n = RealScalar(1) /
static_cast<RealScalar
>(n);
118 const VectorType src_mean = src.
rowwise().sum() * one_over_n;
119 const VectorType dst_mean = dst.
rowwise().sum() * one_over_n;
122 const RowMajorMatrixType src_demean = src.
colwise() - src_mean;
123 const RowMajorMatrixType dst_demean = dst.
colwise() - dst_mean;
126 const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n;
129 const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose();
134 TransformationMatrixType Rt = TransformationMatrixType::Identity(m+1,m+1);
137 VectorType S = VectorType::Ones(m);
139 if ( svd.matrixU().determinant() * svd.matrixV().determinant() < 0 )
143 Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose();
148 const Scalar c = Scalar(1)/src_var * svd.singularValues().dot(S);
151 Rt.col(m).head(m) = dst_mean;
152 Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean;
153 Rt.block(0,0,m,m) *= c;
157 Rt.col(m).head(m) = dst_mean;
158 Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m,m)*src_mean;
166 #endif // EIGEN_UMEYAMA_H