diff --git a/.cproject b/.cproject index 7eaf0065f..d0e73a8bb 100644 --- a/.cproject +++ b/.cproject @@ -1,4 +1,4 @@ - + @@ -298,22 +298,6 @@ - -make --j2 -install -true -true -true - - -make --j2 -check -true -true -true - make -k @@ -468,7 +452,7 @@ make - + testBayesTree.run true false @@ -483,7 +467,7 @@ make - + testSymbolicFactorGraph.run true false @@ -719,18 +703,10 @@ true true - + make -j2 -testPose3Config.run -true -true -true - - -make --j2 -testPose3Graph.run +testLieConfig.run true true true @@ -759,6 +735,22 @@ true true + +make +-j2 +install +true +true +true + + +make +-j2 +check +true +true +true + diff --git a/cpp/Lie.h b/cpp/Lie.h index 593feabc9..11385f485 100644 --- a/cpp/Lie.h +++ b/cpp/Lie.h @@ -75,7 +75,7 @@ namespace gtsam { /** Call equal on the object without tolerance (use default tolerance) */ template inline bool equal(const T& obj1, const T& obj2) { - return obj1.equal(obj2); + return obj1.equals(obj2); } // Vector Group operations diff --git a/cpp/Point2.h b/cpp/Point2.h index 931dc2dbf..07a6fe9e6 100644 --- a/cpp/Point2.h +++ b/cpp/Point2.h @@ -62,18 +62,22 @@ namespace gtsam { } }; - // Lie group functions + /** Lie group functions */ - // Dimensionality of the tangent space + /** Global print calls member function */ + inline void print(const Point2& p, std::string& s) { p.print(s); } + inline void print(const Point2& p) { p.print(); } + + /** Dimensionality of the tangent space */ inline size_t dim(const Point2& obj) { return 2; } - // Exponential map around identity - just create a Point2 from a vector + /** Exponential map around identity - just create a Point2 from a vector */ template<> inline Point2 expmap(const Vector& dp) { return Point2(dp); } - // Log map around identity - just return the Point2 as a vector + /** Log map around identity - just return the Point2 as a vector */ inline Vector logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } - // "Compose", just adds the coordinates of two points. + /** "Compose", just adds the coordinates of two points. */ inline Point2 compose(const Point2& p1, const Point2& p0) { return p0+p1; } inline Matrix Dcompose1(const Point2& p1, const Point2& p0) { return Matrix_(2,2, @@ -84,7 +88,7 @@ namespace gtsam { 1.0, 0.0, 0.0, 1.0); } - // "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() + /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ inline Point2 inverse(const Point2& p) { return Point2(-p.x(), -p.y()); } } diff --git a/cpp/Point3.h b/cpp/Point3.h index febf0fa95..a1f873f83 100644 --- a/cpp/Point3.h +++ b/cpp/Point3.h @@ -78,18 +78,20 @@ namespace gtsam { }; - /** return DOF, dimensionality of tangent space */ + /** Global print calls member function */ + inline void print(const Point3& p, std::string& s) { p.print(s); } + inline void print(const Point3& p) { p.print(); } - // Dimensionality of the tangent space + /** return DOF, dimensionality of tangent space */ inline size_t dim(const Point3&) { return 3; } - // Exponential map at identity - just create a Point3 from x,y,z + /** Exponential map at identity - just create a Point3 from x,y,z */ template<> inline Point3 expmap(const Vector& dp) { return Point3(dp); } - // Log map at identity - return the x,y,z of this point + /** Log map at identity - return the x,y,z of this point */ inline Vector logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } - // "Compose" - just adds coordinates of two points + /** "Compose" - just adds coordinates of two points */ inline Point3 compose(const Point3& p1, const Point3& p0) { return p0+p1; } inline Matrix Dcompose1(const Point3& p1, const Point3& p0) { return Matrix_(3,3, @@ -104,11 +106,11 @@ namespace gtsam { 0.0, 0.0, 1.0); } - // "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() + /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */ inline Point3 inverse(const Point3& p) { return Point3(-p.x(), -p.y(), -p.z()); } - // Syntactic sugar for multiplying coordinates by a scalar s*p + /** Syntactic sugar for multiplying coordinates by a scalar s*p */ inline Point3 operator*(double s, const Point3& p) { return p*s;} /** add two points, add(p,q) is same as p+q */ diff --git a/cpp/Pose3.h b/cpp/Pose3.h index 7cc61bd1a..af05b5205 100644 --- a/cpp/Pose3.h +++ b/cpp/Pose3.h @@ -68,42 +68,42 @@ namespace gtsam { } }; // Pose3 class - // global print + /** global print */ inline void print(const Pose3& p, const std::string& s = "") { p.print(s);} - // Dimensionality of the tangent space + /** Dimensionality of the tangent space */ inline size_t dim(const Pose3&) { return 6; } - // Compose two poses + /** Compose two poses */ inline Pose3 compose(const Pose3& p0, const Pose3& p1) { return Pose3(p0.rotation()*p1.rotation(), p0.translation() + p0.rotation()*p1.translation()); } - // Find the inverse pose s.t. inverse(p)*p = Pose3() + /** Find the inverse pose s.t. inverse(p)*p = Pose3() */ inline Pose3 inverse(const Pose3& p) { Rot3 Rt = inverse(p.rotation()); return Pose3(Rt, Rt*(-p.translation())); } - // Exponential map at identity - create a pose with a translation and - // rotation (in canonical coordinates) + /** Exponential map at identity - create a pose with a translation and + * rotation (in canonical coordinates). */ template<> Pose3 expmap(const Vector& d); - // Log map at identity - return the translation and canonical rotation - // coordinates of a pose. + /** Log map at identity - return the translation and canonical rotation + * coordinates of a pose. */ Vector logmap(const Pose3& p); - // todo: these are the "old-style" expmap and logmap about the specified - // pose. - // Increments the offset and rotation independently given a translation and - // canonical rotation coordinates + /** todo: these are the "old-style" expmap and logmap about the specified + * pose. + * Increments the offset and rotation independently given a translation and + * canonical rotation coordinates */ template<> inline Pose3 expmap(const Pose3& p0, const Vector& d) { return Pose3(expmap(p0.rotation(), sub(d, 0, 3)), expmap(p0.translation(), sub(d, 3, 6))); } - // Independently computes the logmap of the translation and rotation. + /** Independently computes the logmap of the translation and rotation. */ template<> inline Vector logmap(const Pose3& p0, const Pose3& pp) { const Vector r(logmap(p0.rotation(), pp.rotation())), t(logmap(p0.translation(), pp.translation())); diff --git a/cpp/Rot2.h b/cpp/Rot2.h index 076b762eb..603e2bfdc 100644 --- a/cpp/Rot2.h +++ b/cpp/Rot2.h @@ -77,36 +77,40 @@ namespace gtsam { // Lie group functions - // Dimensionality of the tangent space + /** Global print calls member function */ + inline void print(const Rot2& r, std::string& s) { r.print(s); } + inline void print(const Rot2& r) { r.print(); } + + /** Dimensionality of the tangent space */ inline size_t dim(const Rot2&) { return 1; } - // Expmap around identity - create a rotation from an angle + /** Expmap around identity - create a rotation from an angle */ template<> inline Rot2 expmap(const Vector& v) { if (zero(v)) return (Rot2()); else return Rot2(v(0)); } - // Logmap around identity - return the angle of the rotation + /** Logmap around identity - return the angle of the rotation */ inline Vector logmap(const Rot2& r) { return Vector_(1, r.theta()); } - // Compose - make a new rotation by adding angles + /** Compose - make a new rotation by adding angles */ inline Rot2 compose(const Rot2& r0, const Rot2& r1) { return Rot2( r0.c() * r1.c() - r0.s() * r1.s(), r0.s() * r1.c() + r0.c() * r1.s()); } - // Syntactic sugar R1*R2 = compose(R1,R2) + /** Syntactic sugar R1*R2 = compose(R1,R2) */ inline Rot2 operator*(const Rot2& r0, const Rot2& r1) { return compose(r0, r1); } - // The inverse rotation - negative angle + /** The inverse rotation - negative angle */ inline Rot2 inverse(const Rot2& r) { return Rot2(r.c(), -r.s());} - // Shortcut to compose the inverse: invcompose(R0,R1) = inverse(R0)*R1 + /** Shortcut to compose the inverse: invcompose(R0,R1) = inverse(R0)*R1 */ inline Rot2 invcompose(const Rot2& r0, const Rot2& r1) { return Rot2( r0.c() * r1.c() + r0.s() * r1.s(), diff --git a/cpp/Rot3.h b/cpp/Rot3.h index bd28d633d..9c235749a 100644 --- a/cpp/Rot3.h +++ b/cpp/Rot3.h @@ -117,6 +117,9 @@ namespace gtsam { } }; + /** Global print calls member function */ + inline void print(const Rot3& r, std::string& s) { r.print(s); } + inline void print(const Rot3& r) { r.print(); } /** * Rodriguez' formula to compute an incremental rotation matrix diff --git a/cpp/testLieConfig.cpp b/cpp/testLieConfig.cpp index c273bf9b2..6d28d9398 100644 --- a/cpp/testLieConfig.cpp +++ b/cpp/testLieConfig.cpp @@ -110,11 +110,15 @@ TEST(LieConfig, expmap_d) config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); //config0.print("config0"); + CHECK(equal(config0, config0)); + CHECK(config0.equals(config0)); LieConfig poseconfig; poseconfig.insert("p1", Pose2(1,2,3)); poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); //poseconfig.print("poseconfig"); + CHECK(equal(config0, config0)); + CHECK(config0.equals(config0)); } /* ************************************************************************* */