From a12b49de40aebb61966407f24411cf9b4d237478 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 8 Jul 2021 19:43:09 -0400 Subject: [PATCH 1/2] add Pose3 expmap to wrapper --- gtsam/gtsam.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 94d10953b..049a0110c 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -690,6 +690,7 @@ class Pose3 { // Lie Group static gtsam::Pose3 Expmap(Vector v); static Vector Logmap(const gtsam::Pose3& pose); + gtsam::Pose3 expmap(Vector v); Vector logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; From d7d9ac0f0623f4d5d07bfa692df600a3729fc05b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 8 Jul 2021 19:43:25 -0400 Subject: [PATCH 2/2] typo fix --- gtsam/geometry/Pose3.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 318baab3d..d76e1b41a 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -142,7 +142,7 @@ public: static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none); /** - * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame + * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) */ Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect