diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index ccd65c5ea..af8092c60 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -183,4 +183,25 @@ namespace gtsam { return expm(xhat,K); } + /** + * function wrappers for full versions of expmap/logmap + * these will default simple types to using the existing expmap/logmap, + * but more complex ones can be specialized to use improved versions + * + * TODO: replace this approach with a naming scheme that doesn't call + * non-expmap operations "expmap" - use same approach, but with "update" + */ + + /** unary versions */ + template + T ExpmapFull(const Vector& xi) { return T::Expmap(xi); } + template + Vector LogmapFull(const T& p) { return T::Logmap(p); } + + /** binary versions */ + template + T expmapFull(const T& t, const Vector& v) { return t.expmap(v); } + template + Vector logmapFull(const T& t, const T& p2) { return t.logmap(p2); } + } // namespace gtsam diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f594fb5ac..e68f43800 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -222,5 +222,23 @@ namespace gtsam { typedef std::pair Point2Pair; boost::optional align(const std::vector& pairs); + /** + * Specializations for access to full expmap/logmap in templated functions + * + * NOTE: apparently, these *must* be indicated as inline to prevent compile error + */ + + /** unary versions */ + template<> + inline Pose2 ExpmapFull(const Vector& xi) { return Pose2::ExpmapFull(xi); } + template<> + inline Vector LogmapFull(const Pose2& p) { return Pose2::LogmapFull(p); } + + /** binary versions */ + template<> + inline Pose2 expmapFull(const Pose2& t, const Vector& v) { return t.expmapFull(v); } + template<> + inline Vector logmapFull(const Pose2& t, const Pose2& p2) { return t.logmapFull(p2); } + } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 637af42eb..56063d59a 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -195,4 +195,22 @@ namespace gtsam { return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); } + /** + * Specializations for access to full expmap/logmap in templated functions + * + * NOTE: apparently, these *must* be indicated as inline to prevent compile error + */ + + /** unary versions */ + template<> + inline Pose3 ExpmapFull(const Vector& xi) { return Pose3::ExpmapFull(xi); } + template<> + inline Vector LogmapFull(const Pose3& p) { return Pose3::LogmapFull(p); } + + /** binary versions */ + template<> + inline Pose3 expmapFull(const Pose3& t, const Vector& v) { return t.expmapFull(v); } + template<> + inline Vector logmapFull(const Pose3& t, const Pose3& p2) { return t.logmapFull(p2); } + } // namespace gtsam diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index ed3628343..9d283dd86 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -77,6 +77,14 @@ TEST(Pose2, expmap_full) { EXPECT(assert_equal(expected, actual, 1e-5)); } +/* ************************************************************************* */ +TEST(Pose2, expmap_full2) { + Pose2 pose(M_PI_2, Point2(1, 2)); + Pose2 expected(1.00811, 2.01528, 2.5608); + Pose2 actual = expmapFull(pose, Vector_(3, 0.01, -0.015, 0.99)); + EXPECT(assert_equal(expected, actual, 1e-5)); +} + /* ************************************************************************* */ TEST(Pose2, expmap2) { // do an actual series exponential map @@ -113,6 +121,14 @@ TEST(Pose2, expmap0_full) { EXPECT(assert_equal(expected, actual, 1e-5)); } +/* ************************************************************************* */ +TEST(Pose2, expmap0_full2) { + Pose2 pose(M_PI_2, Point2(1, 2)); + Pose2 expected(1.01491, 2.01013, 1.5888); + Pose2 actual = pose * ExpmapFull(Vector_(3, 0.01, -0.015, 0.018)); + EXPECT(assert_equal(expected, actual, 1e-5)); +} + #ifdef SLOW_BUT_CORRECT_EXPMAP /* ************************************************************************* */ // test case for screw motion in the plane diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 7ed84f25d..e4f36e797 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -63,11 +63,23 @@ TEST( Pose3, expmap_a_full) Vector v(6); fill(v.begin(), v.end(), 0); v(0) = 0.3; - CHECK(assert_equal(id.expmap(v), Pose3(R, Point3()))); + CHECK(assert_equal(id.expmapFull(v), Pose3(R, Point3()))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; CHECK(assert_equal(Pose3(R, P),id.expmapFull(v),1e-5)); } +/* ************************************************************************* */ +TEST( Pose3, expmap_a_full2) +{ + Pose3 id; + Vector v(6); + fill(v.begin(), v.end(), 0); + v(0) = 0.3; + CHECK(assert_equal(expmapFull(id,v), Pose3(R, Point3()))); + v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; + CHECK(assert_equal(Pose3(R, P),expmapFull(id,v),1e-5)); +} + /* ************************************************************************* */ TEST(Pose3, expmap_b) {