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

minor fixes to Pose2 and Rot2

release/4.3a0
Varun Agrawal 2022-02-06 00:08:31 -05:00
parent 7ebbe1869e
commit 784bdc64c5
3 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Pose2.h
View File

@ -324,8 +324,8 @@ GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
// Convenience typedef
using Pose2Pair = std::pair<Pose2, Pose2>;
using Pose2Pairs = std::vector<std::pair<Pose2, Pose2> >;
using Pose2Pairs = std::vector<Pose2Pair>;
template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {};

16
gtsam/geometry/Rot2.cpp
View File

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

2
gtsam/geometry/Rot2.h
View File

@ -209,7 +209,7 @@ namespace gtsam {
/** return 2*2 transpose (inverse) rotation matrix */
Matrix2 transpose() const;
/** Find closest valid rotation matrix, given a 2x2 matrix */
static Rot2 ClosestTo(const Matrix2& M);