25 #ifndef EIGEN_UMEYAMA_H
26 #define EIGEN_UMEYAMA_H
36 #ifndef EIGEN_PARSED_BY_DOXYGEN
46 template<
typename MatrixType,
typename OtherMatrixType>
47 struct umeyama_transform_matrix_type
54 HomogeneousDimension =
int(MinRowsAtCompileTime) ==
Dynamic ?
Dynamic :
int(MinRowsAtCompileTime)+1
57 typedef Matrix<typename traits<MatrixType>::Scalar,
108 template <
typename Derived,
typename OtherDerived>
109 typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
112 typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType;
113 typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar;
115 typedef typename Derived::Index Index;
118 EIGEN_STATIC_ASSERT((internal::is_same<Scalar,
typename internal::traits<OtherDerived>::Scalar>::value),
119 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
127 const Index m = src.rows();
128 const Index n = src.cols();
131 const RealScalar one_over_n = 1 /
static_cast<RealScalar
>(n);
134 const VectorType src_mean = src.
rowwise().sum() * one_over_n;
135 const VectorType dst_mean = dst.
rowwise().sum() * one_over_n;
138 const RowMajorMatrixType src_demean = src.
colwise() - src_mean;
139 const RowMajorMatrixType dst_demean = dst.
colwise() - dst_mean;
142 const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n;
145 const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose();
150 TransformationMatrixType Rt = TransformationMatrixType::Identity(m+1,m+1);
153 VectorType S = VectorType::Ones(m);
154 if (sigma.determinant()<0) S(m-1) = -1;
160 if ( svd.
matrixU().determinant() * svd.
matrixV().determinant() > 0 ) {
161 Rt.block(0,0,m,m).noalias() = svd.
matrixU()*svd.
matrixV().transpose();
163 const Scalar s = S(m-1); S(m-1) = -1;
164 Rt.block(0,0,m,m).noalias() = svd.
matrixU() * S.asDiagonal() * svd.
matrixV().transpose();
168 Rt.block(0,0,m,m).noalias() = svd.
matrixU() * S.asDiagonal() * svd.
matrixV().transpose();
177 Rt.col(m).head(m) = dst_mean;
178 Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean;
180 if (with_scaling) Rt.block(0,0,m,m) *= c;
187 #endif // EIGEN_UMEYAMA_H