diff options
Diffstat (limited to 'eigen/Eigen/src/Geometry/Umeyama.h')
| -rw-r--r-- | eigen/Eigen/src/Geometry/Umeyama.h | 19 |
1 files changed, 4 insertions, 15 deletions
diff --git a/eigen/Eigen/src/Geometry/Umeyama.h b/eigen/Eigen/src/Geometry/Umeyama.h index 5e20662..7e933fc 100644 --- a/eigen/Eigen/src/Geometry/Umeyama.h +++ b/eigen/Eigen/src/Geometry/Umeyama.h @@ -97,7 +97,6 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType; typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar; typedef typename NumTraits<Scalar>::Real RealScalar; - typedef typename Derived::Index Index; EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL) EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename internal::traits<OtherDerived>::Scalar>::value), @@ -136,22 +135,12 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo // Eq. (39) VectorType S = VectorType::Ones(m); - if (sigma.determinant()<Scalar(0)) S(m-1) = Scalar(-1); + + if ( svd.matrixU().determinant() * svd.matrixV().determinant() < 0 ) + S(m-1) = -1; // Eq. (40) and (43) - const VectorType& d = svd.singularValues(); - Index rank = 0; for (Index i=0; i<m; ++i) if (!internal::isMuchSmallerThan(d.coeff(i),d.coeff(0))) ++rank; - if (rank == m-1) { - if ( svd.matrixU().determinant() * svd.matrixV().determinant() > Scalar(0) ) { - Rt.block(0,0,m,m).noalias() = svd.matrixU()*svd.matrixV().transpose(); - } else { - const Scalar s = S(m-1); S(m-1) = Scalar(-1); - Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); - S(m-1) = s; - } - } else { - Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); - } + Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); if (with_scaling) { |