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) {
double c = M(0, 0);
double s = M(1, 0);
double theta_rad = std::atan2(s, c);
c = cos(theta_rad);
s = sin(theta_rad);
Eigen::JacobiSVD<Matrix2> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
const Matrix2& U = svd.matrixU();
const Matrix2& V = svd.matrixV();
const double det = (U * V.transpose()).determinant();
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);
}