From 5052eb2c6471ca7b29bc5ecf43111716f47168d5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 11 Jul 2015 18:29:39 -0700 Subject: [PATCH] cleaning up --- gtsam.h | 10 +++++----- gtsam/sam/BearingFactor.h | 25 +++++++++++-------------- gtsam_unstable/slam/serialization.cpp | 6 ------ tests/testSerializationSLAM.cpp | 16 ---------------- 4 files changed, 16 insertions(+), 41 deletions(-) diff --git a/gtsam.h b/gtsam.h index 0273205c3..2c3106d97 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2283,9 +2283,9 @@ typedef gtsam::RangeFactor RangeFactorPose3; #include -template +template virtual class BearingFactor : gtsam::NoiseModelFactor { - BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); + BearingFactor(size_t key1, size_t key2, const MEASURED& measured, const gtsam::noiseModel::Base* noiseModel); // enabling serialization functionality void serialize() const; @@ -2295,11 +2295,11 @@ typedef gtsam::BearingFactor BearingFa #include -template +template virtual class BearingRangeFactor : gtsam::NoiseModelFactor { - BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); + BearingRangeFactor(size_t poseKey, size_t pointKey, const MEASURED& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); - pair measured() const; + pair measured() const; // enabling serialization functionality void serialize() const; diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index f7a060744..15fce6239 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -11,7 +11,7 @@ /** * @file BearingFactor.h - * @brief Serializable factor induced by a bearing measurement on a point from a pose + * @brief Serializable factor induced by a bearing measurement of a point from a given pose * @date July 2015 * @author Frank Dellaert **/ @@ -20,7 +20,6 @@ #include #include -#include namespace gtsam { @@ -28,37 +27,35 @@ namespace gtsam { * Binary factor for a bearing measurement * @addtogroup SAM */ -template -struct BearingFactor : public SerializableExpressionFactor { - GTSAM_CONCEPT_POSE_TYPE(Pose) +template +struct BearingFactor : public SerializableExpressionFactor { /// default constructor BearingFactor() {} /// primary constructor - BearingFactor(Key poseKey, Key pointKey, const Measured& measured, const SharedNoiseModel& model) - : SerializableExpressionFactor(model, measured) { + BearingFactor(Key poseKey, Key pointKey, const T& measured, const SharedNoiseModel& model) + : SerializableExpressionFactor(model, measured) { this->keys_.push_back(poseKey); this->keys_.push_back(pointKey); this->initialize(expression()); } // Return measurement expression - virtual Expression expression() const { - return Expression(Expression(this->keys_[0]), &Pose::bearing, - Expression(this->keys_[1])); + virtual Expression expression() const { + return Expression(Expression(this->keys_[0]), &POSE::bearing, + Expression(this->keys_[1])); } /// print void print(const std::string& s = "", const KeyFormatter& kf = DefaultKeyFormatter) const { std::cout << s << "BearingFactor" << std::endl; - SerializableExpressionFactor::print(s, kf); + SerializableExpressionFactor::print(s, kf); } }; // BearingFactor /// traits -template -struct traits > - : public Testable > {}; +template +struct traits > : public Testable > {}; } // namespace gtsam diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 3cbbcc3a3..2e8bf8b4b 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -11,7 +11,6 @@ #include //#include -#include #include #include //#include @@ -80,9 +79,6 @@ typedef RangeFactor RangeFactorSimpleCameraP typedef RangeFactor RangeFactorCalibratedCamera; typedef RangeFactor RangeFactorSimpleCamera; -typedef BearingFactor BearingFactor2D; -typedef BearingFactor BearingFactor3D; - typedef BearingRangeFactor BearingRangeFactor2D; typedef BearingRangeFactor BearingRangeFactor3D; @@ -185,8 +181,6 @@ BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleC BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); -BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D"); - BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 2514c80d9..d13b53dd5 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -22,7 +22,6 @@ #include //#include -#include #include #include //#include @@ -106,9 +105,6 @@ typedef RangeFactor RangeFactorSimpleCameraP typedef RangeFactor RangeFactorCalibratedCamera; typedef RangeFactor RangeFactorSimpleCamera; -typedef BearingFactor BearingFactor2D; -typedef BearingFactor BearingFactor3D; - typedef BearingRangeFactor BearingRangeFactor2D; typedef BearingRangeFactor BearingRangeFactor3D; @@ -217,8 +213,6 @@ BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleC BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); -BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D"); - BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); @@ -393,8 +387,6 @@ TEST (testSerializationSLAM, factors) { RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1); RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1); - BearingFactor2D bearingFactor2D(a08, a03, rot2, model1); - BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2); GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, boost::make_shared(cal3_s2)); @@ -456,8 +448,6 @@ TEST (testSerializationSLAM, factors) { graph += rangeFactorCalibratedCamera; graph += rangeFactorSimpleCamera; - graph += bearingFactor2D; - graph += bearingRangeFactor2D; graph += genericProjectionFactorCal3_S2; @@ -524,8 +514,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(rangeFactorCalibratedCamera)); EXPECT(equalsObj(rangeFactorSimpleCamera)); - EXPECT(equalsObj(bearingFactor2D)); - EXPECT(equalsObj(bearingRangeFactor2D)); EXPECT(equalsObj(genericProjectionFactorCal3_S2)); @@ -592,8 +580,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(rangeFactorCalibratedCamera)); EXPECT(equalsXML(rangeFactorSimpleCamera)); - EXPECT(equalsXML(bearingFactor2D)); - EXPECT(equalsXML(bearingRangeFactor2D)); EXPECT(equalsXML(genericProjectionFactorCal3_S2)); @@ -660,8 +646,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(rangeFactorCalibratedCamera)); EXPECT(equalsBinary(rangeFactorSimpleCamera)); - EXPECT(equalsBinary(bearingFactor2D)); - EXPECT(equalsBinary(bearingRangeFactor2D)); EXPECT(equalsBinary(genericProjectionFactorCal3_S2));