diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 85307e322..77a635453 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -33,6 +33,8 @@ INSTANTIATE_LIE(Pose2); /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose2); +#define SLOW_BUT_CORRECT_EXPMAP + static const Matrix I3 = eye(3), Z12 = zeros(1,2); static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); @@ -117,6 +119,13 @@ Matrix Pose2::AdjointMap() const { ); } +/* ************************************************************************* */ +Vector Pose2::Adjoint(const Vector& xi) const { + assert(xi.size() == 3); + return AdjointMap()*xi; +} + + /* ************************************************************************* */ Pose2 Pose2::inverse(boost::optional H1) const { if (H1) *H1 = -AdjointMap(); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 26244877b..6cf3c91fc 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -159,10 +159,7 @@ public: * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ Matrix AdjointMap() const; - inline Vector Adjoint(const Vector& xi) const { - assert(xi.size() == 3); - return AdjointMap()*xi; - } + Vector Adjoint(const Vector& xi) const; /** * wedge for SE(2):