35 MinRowsAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime),
95umeyama(
const MatrixBase<Derived>& src,
const MatrixBase<OtherDerived>& dst,
bool with_scaling =
true)
97 typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType;
98 typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar;
99 typedef typename NumTraits<Scalar>::Real RealScalar;
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) };
107 typedef Matrix<Scalar, Dimension, 1> VectorType;
108 typedef Matrix<Scalar, Dimension, Dimension> MatrixType;
109 typedef typename internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType;
111 const Index m = src.rows();
112 const Index n = src.cols();
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;
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:95