12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Rot2::ClosestTo with SVD

release/4.3a0
Varun Agrawal 2022-04-29 18:42:18 -04:00
parent e704b40ab5
commit 9eff71ea8e
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Rot2.cpp
View File

@ -131,11 +131,14 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
/* ************************************************************************* */ /* ************************************************************************* */
Rot2 Rot2::ClosestTo(const Matrix2& M) { Rot2 Rot2::ClosestTo(const Matrix2& M) {
double c = M(0, 0); Eigen::JacobiSVD<Matrix2> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
double s = M(1, 0); const Matrix2& U = svd.matrixU();
double theta_rad = std::atan2(s, c); const Matrix2& V = svd.matrixV();
c = cos(theta_rad); const double det = (U * V.transpose()).determinant();
s = sin(theta_rad); Matrix2 M_prime = (U * Vector2(1, det).asDiagonal() * V.transpose());
double c = M_prime(0, 0);
double s = M_prime(1, 0);
return Rot2::fromCosSin(c, s); return Rot2::fromCosSin(c, s);
} }