From eada1ee505aecc2e6dc4bb4ddf55ae8502c1d257 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 14:48:22 -0500 Subject: [PATCH] Adding adjoint and adjoint transpose functions --- gtsam/geometry/Pose2.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f03e0852e..079c649f3 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -132,6 +132,8 @@ public: * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ Matrix3 AdjointMap() const; + + /// Apply AdjointMap to twist xi inline Vector3 Adjoint(const Vector3& xi) const { return AdjointMap()*xi; } @@ -141,6 +143,20 @@ public: */ static Matrix3 adjointMap(const Vector3& v); + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + Vector3 adjoint(const Vector3& xi, const Vector3& y) { + return adjointMap(xi) * y; + } + + /** + * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. + */ + Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) { + return adjointMap(xi).transpose() * y; + } + /** * wedge for SE(2): * @param xi 3-dim twist (v,omega) where