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

finish Rot2.ClosestTo()

release/4.3a0
John Lambert 2021-12-07 11:59:28 -07:00 committed by GitHub
parent dfb9497d81
commit 37af8cfbec
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/Rot2.cpp
View File

@ -129,6 +129,14 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
} }
} }
/* ************************************************************************* */
static Rot2 ClosestTo(const Matrix2& M) {
double theta_rad = atan2(M(0,0), M(1,0));
double c = cos(theta_rad);
double s = sin(theta_rad);
return Rot2::fromCosSin(c, s);
}
/* ************************************************************************* */ /* ************************************************************************* */
} // gtsam } // gtsam