From 2b82ff65e7ea4ed4488c9b1a361e6da30940e2e5 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 9 Jan 2010 23:15:06 +0000 Subject: [PATCH] Deriving Lie objects from a base class Lie, which provides member functions to access global functions, for use in MATLAB. --- cpp/Lie-inl.h | 32 ++++++++++++++++++++++++++++++++ cpp/Lie.h | 23 +++++++++++++++++++++++ cpp/Makefile.am | 2 +- cpp/Point2.cpp | 4 ++++ cpp/Point2.h | 2 +- cpp/Point3.cpp | 3 +++ cpp/Point3.h | 2 +- cpp/Pose2.cpp | 4 ++++ cpp/Pose2.h | 19 ++++++++++++------- cpp/Pose3.cpp | 4 ++++ cpp/Pose3.h | 2 +- cpp/Rot2.cpp | 4 ++++ cpp/Rot2.h | 2 +- cpp/Rot3.cpp | 4 ++++ cpp/Rot3.h | 2 +- cpp/testPose2.cpp | 7 +++++++ 16 files changed, 103 insertions(+), 13 deletions(-) create mode 100644 cpp/Lie-inl.h diff --git a/cpp/Lie-inl.h b/cpp/Lie-inl.h new file mode 100644 index 000000000..b617595a3 --- /dev/null +++ b/cpp/Lie-inl.h @@ -0,0 +1,32 @@ +/* + * LieBaseImplementations.h + * + * Created on: Jan 9, 2010 + * Author: richard + */ + +#include "Lie.h" + +namespace gtsam { + template + size_t Lie::dim() const { + return gtsam::dim(*((T*)this)); + } + + /** + * Returns Exponential mapy + */ + template + T Lie::expmap(const Vector& v) const { + return gtsam::expmap(*((T*)this),v); + } + + /** + * Returns Log map + */ + template + Vector Lie::logmap(const T& lp) const { + return gtsam::logmap(*((T*)this),lp); + } + +} diff --git a/cpp/Lie.h b/cpp/Lie.h index 9429131a0..c6d5cd9d9 100644 --- a/cpp/Lie.h +++ b/cpp/Lie.h @@ -38,6 +38,29 @@ namespace gtsam { template inline T expmap(const T& l0, const Vector& v) { return expmap(v)*l0; } + /** + * Base class for Lie group type + */ + template + class Lie { + public: + + /** + * Returns dimensionality of the tangent space + */ + size_t dim() const; + + /** + * Returns Exponential mapy + */ + T expmap(const Vector& v) const; + + /** + * Returns Log map + */ + Vector logmap(const T& lp) const; + + }; } diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 0b6e14ad6..e907b0325 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -157,7 +157,7 @@ testSQPOptimizer_SOURCES = $(example) testSQPOptimizer.cpp testSQPOptimizer_LDADD = libgtsam.la # geometry -headers += Lie.h +headers += Lie.h Lie-inl.h sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 testPoint2_SOURCES = testPoint2.cpp diff --git a/cpp/Point2.cpp b/cpp/Point2.cpp index 88ef85c22..63a256fc0 100644 --- a/cpp/Point2.cpp +++ b/cpp/Point2.cpp @@ -5,11 +5,15 @@ */ #include "Point2.h" +#include "Lie-inl.h" using namespace std; namespace gtsam { + /** Explicit instantiation of base class to export members */ + template class Lie; + /* ************************************************************************* */ void Point2::print(const string& s) const { cout << s << "(" << x_ << ", " << y_ << ")" << endl; diff --git a/cpp/Point2.h b/cpp/Point2.h index a4266ce1a..55431c08b 100644 --- a/cpp/Point2.h +++ b/cpp/Point2.h @@ -18,7 +18,7 @@ namespace gtsam { * Derived from testable so has standard print and equals, and assert_equals works * Functional, so no set functions: once created, a point is constant. */ - class Point2: Testable { + class Point2: Testable, public Lie { private: double x_, y_; diff --git a/cpp/Point3.cpp b/cpp/Point3.cpp index 1642c9be7..ac7c5b7a3 100644 --- a/cpp/Point3.cpp +++ b/cpp/Point3.cpp @@ -4,10 +4,13 @@ */ #include "Point3.h" +#include "Lie-inl.h" namespace gtsam { + /** Explicit instantiation of base class to export members */ + template class Lie; /* ************************************************************************* */ bool Point3::equals(const Point3 & q, double tol) const { diff --git a/cpp/Point3.h b/cpp/Point3.h index 1c2e02d5d..febf0fa95 100644 --- a/cpp/Point3.h +++ b/cpp/Point3.h @@ -19,7 +19,7 @@ namespace gtsam { /** A 3D point */ - class Point3: Testable { + class Point3: Testable, public Lie { private: double x_, y_, z_; diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index f3afa31d4..d48074d21 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -4,11 +4,15 @@ */ #include "Pose2.h" +#include "Lie-inl.h" using namespace std; namespace gtsam { + /** Explicit instantiation of base class to export members */ + template class Lie; + /* ************************************************************************* */ void Pose2::print(const string& s) const { cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl; diff --git a/cpp/Pose2.h b/cpp/Pose2.h index a58c307b1..6e07cd21e 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -9,18 +9,18 @@ #pragma once -#include "Point2.h" -#include "Rot2.h" #include "Matrix.h" #include "Testable.h" #include "Lie.h" +#include "Point2.h" +#include "Rot2.h" namespace gtsam { /** * A 2D pose (Point2,Rot2) */ - class Pose2: Testable { + class Pose2: Testable, public Lie { private: Point2 t_; Rot2 r_; @@ -73,10 +73,12 @@ namespace gtsam { inline Pose2 compose(const Pose2& p1, const Pose2& p0) { return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); } - /* exponential and log maps around identity */ - // Create an incremental pose from x,y,theta + /** exponential and log maps around identity */ + + /** Create an incremental pose from x,y,theta */ template<> inline Pose2 expmap(const Vector& v) { return Pose2(v[0], v[1], v[2]); } - // Return the x,y,theta of this pose + + /** Return the x,y,theta of this pose */ inline Vector logmap(const Pose2& p) { return Vector_(3, p.x(), p.y(), p.theta()); } @@ -91,7 +93,7 @@ namespace gtsam { return rotate(pose.r(), point)+pose.t(); } /** Return relative pose between p1 and p2, in p1 coordinate frame */ - // todo: make sure compiler finds this version of between. + /** todo: make sure compiler finds this version of between. */ //inline Pose2 between(const Pose2& p0, const Pose2& p2) { // return Pose2(p0.r().invcompose(p2.r()), p0.r().unrotate(p2.t()-p0.t())); } Matrix Dbetween1(const Pose2& p0, const Pose2& p2); @@ -105,4 +107,7 @@ namespace gtsam { inline Point2 operator*(const Pose2& pose, const Point2& point) { return transform_from(pose, point); } + + } // namespace gtsam + diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index 1e7252b31..1f6511d8c 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -5,12 +5,16 @@ #include #include "Pose3.h" +#include "Lie-inl.h" using namespace std; using namespace boost::numeric::ublas; namespace gtsam { + /** Explicit instantiation of base class to export members */ + template class Lie; + /* ************************************************************************* */ void Pose3::print(const string& s) const { R_.print(s + ".R"); diff --git a/cpp/Pose3.h b/cpp/Pose3.h index 633fed8b9..05408216a 100644 --- a/cpp/Pose3.h +++ b/cpp/Pose3.h @@ -17,7 +17,7 @@ namespace gtsam { /** A 3D pose (R,t) : (Rot3,Point3) */ - class Pose3 : Testable { + class Pose3 : Testable, public Lie { private: Rot3 R_; Point3 t_; diff --git a/cpp/Rot2.cpp b/cpp/Rot2.cpp index 9151fc6d7..a9dc8431f 100644 --- a/cpp/Rot2.cpp +++ b/cpp/Rot2.cpp @@ -6,11 +6,15 @@ */ #include "Rot2.h" +#include "Lie-inl.h" using namespace std; namespace gtsam { + /** Explicit instantiation of base class to export members */ + template class Lie; + /* ************************************************************************* */ void Rot2::print(const string& s) const { cout << s << ":" << theta() << endl; diff --git a/cpp/Rot2.h b/cpp/Rot2.h index a0a4d139d..076b762eb 100644 --- a/cpp/Rot2.h +++ b/cpp/Rot2.h @@ -16,7 +16,7 @@ namespace gtsam { /* Rotation matrix */ - class Rot2: Testable { + class Rot2: Testable, public Lie { private: /** we store cos(theta) and sin(theta) */ double c_, s_; diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index 81af053d8..bd4728d66 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -7,11 +7,15 @@ */ #include "Rot3.h" +#include "Lie-inl.h" using namespace std; namespace gtsam { + /** Explicit instantiation of base class to export members */ + template class Lie; + /* ************************************************************************* */ // static member functions to construct rotations diff --git a/cpp/Rot3.h b/cpp/Rot3.h index 4675b4187..723e8194a 100644 --- a/cpp/Rot3.h +++ b/cpp/Rot3.h @@ -17,7 +17,7 @@ namespace gtsam { /* 3D Rotation */ - class Rot3: Testable { + class Rot3: Testable, public Lie { private: /** we store columns ! */ Point3 r1_, r2_, r3_; diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index 1cda7a18d..ec0a22f72 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -186,6 +186,13 @@ TEST( Pose2, between ) CHECK(assert_equal(numericalH2,actualH2)); } +/* ************************************************************************* */ +TEST(Pose2, members) +{ + Pose2 pose; + CHECK(pose.dim() == 3); +} + /* ************************************************************************* */ int main() { TestResult tr;