From eeacef81b624c5ddc924ba2b39ee753de0143307 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 15 Oct 2011 18:38:49 +0000 Subject: [PATCH] Merge branch 'master' into measurement_concepts Conflicts: gtsam/slam/BearingRangeFactor.h --- gtsam/base/LieScalar.h | 3 +-- gtsam/base/LieVector.h | 3 +-- gtsam/base/Testable.h | 27 ++++++++++++++++++++++++ gtsam/base/tests/testLieScalar.cpp | 3 +++ gtsam/base/tests/testLieVector.cpp | 3 +++ gtsam/geometry/Cal3Bundler.h | 2 +- gtsam/geometry/Cal3DS2.h | 2 +- gtsam/geometry/Cal3_S2.h | 2 +- gtsam/geometry/Point2.h | 5 ++--- gtsam/geometry/Pose2.h | 2 +- gtsam/geometry/Rot2.h | 2 +- gtsam/geometry/StereoPoint2.h | 2 +- gtsam/geometry/tests/testCal3Bundler.cpp | 1 + gtsam/geometry/tests/testCal3DS2.cpp | 1 + gtsam/geometry/tests/testCal3_S2.cpp | 3 +++ gtsam/geometry/tests/testPoint2.cpp | 1 + gtsam/geometry/tests/testPose2.cpp | 7 ++++++ gtsam/geometry/tests/testRot2.cpp | 1 + gtsam/slam/BearingFactor.h | 5 +++++ gtsam/slam/BearingRangeFactor.h | 4 +++- gtsam/slam/BetweenConstraint.h | 4 ++++ gtsam/slam/BetweenFactor.h | 6 +++++- gtsam/slam/BoundingConstraint.h | 4 ++++ gtsam/slam/PriorFactor.h | 4 ++++ 24 files changed, 82 insertions(+), 15 deletions(-) diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index fc9004930..7d73255b7 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -17,7 +17,6 @@ #pragma once -#include #include namespace gtsam { @@ -25,7 +24,7 @@ namespace gtsam { /** * LieScalar is a wrapper around double to allow it to be a Lie type */ - struct LieScalar : public Lie, Testable { + struct LieScalar : public Lie { /** default constructor - should be unnecessary */ LieScalar() {} diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 640870634..bec42f775 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -17,7 +17,6 @@ #pragma once -#include #include #include @@ -26,7 +25,7 @@ namespace gtsam { /** * LieVector is a wrapper around vector to allow it to be a Lie type */ -struct LieVector : public Vector, public Lie, Testable { +struct LieVector : public Vector, public Lie { /** default constructor - should be unnecessary */ LieVector() {} diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index b09591ae6..51945f64f 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -65,6 +65,25 @@ namespace gtsam { }; // Testable class + /** + * A testable concept check to be placed in unit tests, rather than subclassing + * + */ + template + class TestableConcept { + static bool checkTestableConcept(const T& d) { + // check print function, with optional string + d.print(std::string()); + d.print(); + + // check print, with optional threshold + double tol = 1.0; + bool r1 = d.equals(d, tol); + bool r2 = d.equals(d); + return r1 && r2; + } + }; // Testable class + /** * This template works for any type with equals */ @@ -104,3 +123,11 @@ namespace gtsam { }; } // gtsam + +/** + * Macros for using the TestableConcept + * - An instantiation for use inside unit tests + * - A typedef for use inside generic algorithms + */ +#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::TestableConcept; +#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept _gtsam_TestableConcept_##T; diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index a5428f803..795a39688 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -16,10 +16,13 @@ #include +#include #include using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(LieScalar) + const double tol=1e-9; /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 02d0c2f07..e8dfae741 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -16,10 +16,13 @@ #include +#include #include using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(LieVector) + /* ************************************************************************* */ TEST( testLieVector, construction ) { Vector v = Vector_(3, 1.0, 2.0, 3.0); diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index cb508af15..620dabad6 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -26,7 +26,7 @@ namespace gtsam { * @brief Calibration used by Bundler * @ingroup geometry */ -class Cal3Bundler: public Testable { +class Cal3Bundler { private: double f_, k1_, k2_ ; diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 90ed76aca..595247249 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -26,7 +26,7 @@ namespace gtsam { * @brief Calibration of a camera with radial distortion * @ingroup geometry */ - class Cal3DS2 : Testable { + class Cal3DS2 { private: double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double k1_, k2_ ; // radial 2nd-order and 4th-order diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 4c265f86a..d2235e699 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -29,7 +29,7 @@ namespace gtsam { * @brief The most common 5DOF 3D->2D calibration * @ingroup geometry */ - class Cal3_S2: Testable { + class Cal3_S2 { private: double fx_, fy_, s_, u0_, v0_; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 8a1a42ea2..0e9acdbf4 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -19,18 +19,17 @@ #include #include -#include #include namespace gtsam { /** * A 2D point - * Derived from testable so has standard print and equals, and assert_equals works + * Complies with the Testable Concept * Functional, so no set functions: once created, a point is constant. * @ingroup geometry */ - class Point2: Testable, public Lie { + class Point2: public Lie { public: /// dimension of the variable - used to autodetect sizes static const size_t dimension = 2; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 67e712810..783220db5 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -31,7 +31,7 @@ namespace gtsam { * A 2D pose (Point2,Rot2) * @ingroup geometry */ - class Pose2: Testable, public Lie { + class Pose2: public Lie { public: static const size_t dimension = 3; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 916bdf6d1..7304ddcaf 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -28,7 +28,7 @@ namespace gtsam { * NOTE: the angle theta is in radians unless explicitly stated * @ingroup geometry */ - class Rot2: Testable, public Lie { + class Rot2: public Lie { public: /** get the dimension by the type */ diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 551b6ecdc..d6ad8f352 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -26,7 +26,7 @@ namespace gtsam { * A 2D stereo point, v will be same for rectified images * @ingroup geometry */ - class StereoPoint2: Testable, Lie { + class StereoPoint2: Lie { public: static const size_t dimension = 3; private: diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 096450cf1..ef00c6d00 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -16,6 +16,7 @@ #include +#include #include #include diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index 4cb14c30b..22bd9c8a1 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -16,6 +16,7 @@ #include +#include #include #include diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 99525ec1f..c341f7b54 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -15,11 +15,14 @@ */ #include +#include #include #include using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2) + Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2); Point2 p(1, -2); diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index ab3201495..2aac1703b 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -16,6 +16,7 @@ **/ #include +#include #include using namespace std; diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 66b0584ce..95603fce1 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -23,6 +23,7 @@ using namespace boost::assign; #include +#include #include #include #include @@ -34,6 +35,12 @@ using namespace std; // #define SLOW_BUT_CORRECT_EXPMAP +// concept checks for testable +GTSAM_CONCEPT_TESTABLE_INST(Pose2) +GTSAM_CONCEPT_TESTABLE_INST(Point2) +GTSAM_CONCEPT_TESTABLE_INST(Rot2) +GTSAM_CONCEPT_TESTABLE_INST(LieVector) + /* ************************************************************************* */ TEST(Pose2, constructors) { Point2 p; diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 3edc53e3a..02695c5ab 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 768ca419b..418e03350 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -16,6 +16,7 @@ #pragma once +#include #include namespace gtsam { @@ -35,6 +36,10 @@ namespace gtsam { typedef NonlinearFactor2 Base; Rot z_; /** measurement */ + + /** concept check by type */ + GTSAM_CONCEPT_TESTABLE_TYPE(Rot) + public: /** default constructor for serialization/testing only */ diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 287c6661c..69f849391 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include namespace gtsam { @@ -41,7 +42,8 @@ namespace gtsam { Rot bearing_; double range_; - // Concept requirements for this factor + /** concept check by type */ + GTSAM_CONCEPT_TESTABLE_TYPE(Rot) GTSAM_CONCEPT_RANGE_MEASUREMENT(Pose, Point) public: diff --git a/gtsam/slam/BetweenConstraint.h b/gtsam/slam/BetweenConstraint.h index 327fd465d..0d4ae632c 100644 --- a/gtsam/slam/BetweenConstraint.h +++ b/gtsam/slam/BetweenConstraint.h @@ -17,6 +17,7 @@ #pragma once +#include #include namespace gtsam { @@ -35,6 +36,9 @@ namespace gtsam { X measured_; /// fixed between value + /** concept check by type */ + GTSAM_CONCEPT_TESTABLE_TYPE(X) + public: typedef boost::shared_ptr > shared_ptr; diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index b74efeeb0..37db3b148 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -17,9 +17,10 @@ #include +#include +#include #include #include -#include namespace gtsam { @@ -38,6 +39,9 @@ namespace gtsam { T measured_; /** The measurement */ + /** concept check by type */ + GTSAM_CONCEPT_TESTABLE_TYPE(T) + public: // shorthand for a smart pointer to a factor diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index c116bcc8f..17129f962 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -42,6 +42,8 @@ struct BoundingConstraint1: public NonlinearConstraint1 { Base(key, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) { } + virtual ~BoundingConstraint1() {} + inline double threshold() const { return threshold_; } inline bool isGreaterThan() const { return isGreaterThan_; } @@ -107,6 +109,8 @@ struct BoundingConstraint2: public NonlinearConstraint2 { bool isGreaterThan, double mu = 1000.0) : Base(key1, key2, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {} + virtual ~BoundingConstraint2() {} + inline double threshold() const { return threshold_; } inline bool isGreaterThan() const { return isGreaterThan_; } diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 032bbae39..8f9359955 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -15,6 +15,7 @@ **/ #pragma once +#include #include namespace gtsam { @@ -39,6 +40,9 @@ namespace gtsam { T prior_; /** The measurement */ + /** concept check by type */ + GTSAM_CONCEPT_TESTABLE_TYPE(T) + public: // shorthand for a smart pointer to a factor