diff options
Diffstat (limited to 'extern/Eigen3/Eigen/src/Geometry/Umeyama.h')
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/Umeyama.h | 25 |
1 files changed, 15 insertions, 10 deletions
diff --git a/extern/Eigen3/Eigen/src/Geometry/Umeyama.h b/extern/Eigen3/Eigen/src/Geometry/Umeyama.h index ac0939cde52..345b47e0c37 100644 --- a/extern/Eigen3/Eigen/src/Geometry/Umeyama.h +++ b/extern/Eigen3/Eigen/src/Geometry/Umeyama.h @@ -153,16 +153,21 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); } - // Eq. (42) - const Scalar c = 1/src_var * svd.singularValues().dot(S); - - // Eq. (41) - // Note that we first assign dst_mean to the destination so that there no need - // for a temporary. - Rt.col(m).head(m) = dst_mean; - Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean; - - if (with_scaling) Rt.block(0,0,m,m) *= c; + if (with_scaling) + { + // Eq. (42) + const Scalar c = 1/src_var * svd.singularValues().dot(S); + + // Eq. (41) + Rt.col(m).head(m) = dst_mean; + Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean; + Rt.block(0,0,m,m) *= c; + } + else + { + Rt.col(m).head(m) = dst_mean; + Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m,m)*src_mean; + } return Rt; } |