From 811be62ed3f42cdfa32fde131db10bf027bf270e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 6 Feb 2012 00:44:25 +0000 Subject: [PATCH] Working on templating factors on Value type instead of key type --- gtsam/nonlinear/NonlinearFactor.h | 306 ++++++++++++---------------- gtsam/slam/BearingFactor.h | 30 +-- gtsam/slam/BearingRangeFactor.h | 40 ++-- gtsam/slam/BetweenFactor.h | 30 ++- gtsam/slam/BoundingConstraint.h | 14 +- gtsam/slam/GeneralSFMFactor.h | 30 +-- gtsam/slam/PartialPriorFactor.h | 16 +- gtsam/slam/PriorFactor.h | 29 ++- gtsam/slam/ProjectionFactor.h | 40 ++-- gtsam/slam/RangeFactor.h | 33 ++- gtsam/slam/planarSLAM.cpp | 28 ++- gtsam/slam/planarSLAM.h | 103 +++++----- gtsam/slam/pose2SLAM.cpp | 12 +- gtsam/slam/pose2SLAM.h | 17 +- gtsam/slam/pose3SLAM.cpp | 14 +- gtsam/slam/pose3SLAM.h | 18 +- gtsam/slam/simulated2D.h | 83 ++++---- gtsam/slam/simulated2DConstraints.h | 60 +++--- gtsam/slam/simulated2DOriented.h | 39 ++-- tests/testNonlinearFactor.cpp | 53 +++-- 20 files changed, 467 insertions(+), 528 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 07334eac9..ba2198b1b 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -68,7 +68,7 @@ protected: public: - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ @@ -77,18 +77,6 @@ public: NonlinearFactor() { } - /** - * Constructor - * @param keys A boost::tuple containing the variables involved in this factor, - * example: NonlinearFactor(make_tuple(symbol1, symbol2, symbol3)) - */ - template - NonlinearFactor(const boost::tuples::cons& keys) { - this->keys_.resize(boost::tuples::length >::value); - // Use helper function to fill key vector, using 'cons' representation of tuple - __fill_from_tuple(this->keys(), 0, keys); - } - /** * Constructor * @param keys The variables involved in this factor @@ -186,16 +174,6 @@ public: /** Destructor */ virtual ~NoiseModelFactor() {} - /** - * Constructor - * @param keys A boost::tuple containing the variables involved in this factor, - * example: NoiseModelFactor(noiseModel, make_tuple(symbol1, symbol2, symbol3) - */ - template - NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons& keys) - : Base(keys), noiseModel_(noiseModel) { - } - /** * Constructor * @param keys The variables involved in this factor @@ -320,21 +298,18 @@ private: /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 1 * variable. To derive from this class, implement evaluateError(). */ -template +template class NonlinearFactor1: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY::Value X; + typedef VALUE X; protected: - // The value of the key. Not const to allow serialization - KEY key_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor1 This; + typedef NonlinearFactor1 This; public: @@ -343,22 +318,24 @@ public: virtual ~NonlinearFactor1() {} - inline const KEY& key() const { return key_; } + inline const Symbol& key() const { return keys_[0]; } /** * Constructor * @param z measurement * @param key by which to look up X value in Values */ - NonlinearFactor1(const SharedNoiseModel& noiseModel, const KEY& key1) : - Base(noiseModel, make_tuple(key1)), key_(key1) { + NonlinearFactor1(const SharedNoiseModel& noiseModel, const Symbol& key1) : + Base(noiseModel) { + keys_.resize(1); + keys_[0] = key1; } /** Calls the 1-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { - const X& x1 = x[key_]; + const X& x1 = x.at(keys_[0]); if(H) { return evaluateError(x1, (*H)[0]); } else { @@ -371,7 +348,7 @@ public: /** Print */ virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor1(" << (std::string) this->key_ << ")\n"; + std::cout << s << ": NonlinearFactor1(" << (std::string) keys_[0] << ")\n"; this->noiseModel_->print(" noise model: "); } @@ -391,7 +368,6 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key_); } };// \class NonlinearFactor1 @@ -399,23 +375,19 @@ private: /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 2 * variables. To derive from this class, implement evaluateError(). */ -template +template class NonlinearFactor2: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; + typedef VALUE1 X1; + typedef VALUE2 X2; protected: - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor2 This; + typedef NonlinearFactor2 This; public: @@ -429,21 +401,25 @@ public: * @param j1 key of the first variable * @param j2 key of the second variable */ - NonlinearFactor2(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2) : - Base(noiseModel, make_tuple(j1,j2)), key1_(j1), key2_(j2) {} + NonlinearFactor2(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) : + Base(noiseModel) { + keys_.resize(2); + keys_[0] = j1; + keys_[1] = j2; + } virtual ~NonlinearFactor2() {} /** methods to retrieve both keys */ - inline const KEY1& key1() const { return key1_; } - inline const KEY2& key2() const { return key2_; } + inline const Symbol& key1() const { return keys_[0]; } + inline const Symbol& key2() const { return keys_[1]; } /** Calls the 2-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { - const X1& x1 = x[key1_]; - const X2& x2 = x[key2_]; + const X1& x1 = x.at(keys_[0]); + const X2& x2 = x.at(keys_[1]); if(H) { return evaluateError(x1, x2, (*H)[0], (*H)[1]); } else { @@ -457,8 +433,8 @@ public: /** Print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearFactor2(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << ")\n"; + << (std::string) keys_[0] << "," + << (std::string) keys_[1] << ")\n"; this->noiseModel_->print(" noise model: "); } @@ -479,33 +455,26 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); } }; // \class NonlinearFactor2 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 3 * variables. To derive from this class, implement evaluateError(). */ -template +template class NonlinearFactor3: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - typedef typename KEY3::Value X3; + typedef VALUE1 X1; + typedef VALUE2 X2; + typedef VALUE3 X3; protected: - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - KEY3 key3_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor3 This; + typedef NonlinearFactor3 This; public: @@ -520,24 +489,29 @@ public: * @param j2 key of the second variable * @param j3 key of the third variable */ - NonlinearFactor3(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3) : - Base(noiseModel, make_tuple(j1,j2,j3)), key1_(j1), key2_(j2), key3_(j3) {} + NonlinearFactor3(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3) : + Base(noiseModel) { + keys_.resize(3); + keys_[0] = j1; + keys_[1] = j2; + keys_[2] = j3; + } virtual ~NonlinearFactor3() {} /** methods to retrieve keys */ - inline const KEY1& key1() const { return key1_; } - inline const KEY2& key2() const { return key2_; } - inline const KEY3& key3() const { return key3_; } + inline const Symbol& key1() const { return keys_[0]; } + inline const Symbol& key2() const { return keys_[1]; } + inline const Symbol& key3() const { return keys_[2]; } /** Calls the 3-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) - return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), (*H)[0], (*H)[1], (*H)[2]); else - return evaluateError(x[key1_], x[key2_], x[key3_]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2])); } else { return zero(this->dim()); } @@ -546,9 +520,9 @@ public: /** Print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearFactor3(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << "," - << (std::string) this->key3_ << ")\n"; + << (std::string) this->keys_[0] << "," + << (std::string) this->keys_[1] << "," + << (std::string) this->keys_[2] << ")\n"; this->noiseModel_->print(" noise model: "); } @@ -572,36 +546,27 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - ar & BOOST_SERIALIZATION_NVP(key3_); } }; // \class NonlinearFactor3 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 4 * variables. To derive from this class, implement evaluateError(). */ -template +template class NonlinearFactor4: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - typedef typename KEY3::Value X3; - typedef typename KEY4::Value X4; + typedef VALUE1 X1; + typedef VALUE2 X2; + typedef VALUE3 X3; + typedef VALUE4 X4; protected: - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - KEY3 key3_; - KEY4 key4_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor4 This; + typedef NonlinearFactor4 This; public: @@ -617,25 +582,31 @@ public: * @param j3 key of the third variable * @param j4 key of the fourth variable */ - NonlinearFactor4(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4) : - Base(noiseModel, make_tuple(j1,j2,j3,j4)), key1_(j1), key2_(j2), key3_(j3), key4_(j4) {} + NonlinearFactor4(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4) : + Base(noiseModel) { + keys_.resize(4); + keys_[0] = j1; + keys_[1] = j2; + keys_[2] = j3; + keys_[3] = j4; + } virtual ~NonlinearFactor4() {} /** methods to retrieve keys */ - inline const KEY1& key1() const { return key1_; } - inline const KEY2& key2() const { return key2_; } - inline const KEY3& key3() const { return key3_; } - inline const KEY4& key4() const { return key4_; } + inline const Symbol& key1() const { return keys_[0]; } + inline const Symbol& key2() const { return keys_[1]; } + inline const Symbol& key3() const { return keys_[2]; } + inline const Symbol& key4() const { return keys_[3]; } /** Calls the 4-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]); else - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3])); } else { return zero(this->dim()); } @@ -644,10 +615,10 @@ public: /** Print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearFactor4(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << "," - << (std::string) this->key3_ << "," - << (std::string) this->key4_ << ")\n"; + << (std::string) this->keys_[0] << "," + << (std::string) this->keys_[1] << "," + << (std::string) this->keys_[2] << "," + << (std::string) this->keys_[3] << ")\n"; this->noiseModel_->print(" noise model: "); } @@ -671,39 +642,28 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - ar & BOOST_SERIALIZATION_NVP(key3_); - ar & BOOST_SERIALIZATION_NVP(key4_); } }; // \class NonlinearFactor4 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 5 * variables. To derive from this class, implement evaluateError(). */ -template +template class NonlinearFactor5: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - typedef typename KEY3::Value X3; - typedef typename KEY4::Value X4; - typedef typename KEY5::Value X5; + typedef VALUE1 X1; + typedef VALUE2 X2; + typedef VALUE3 X3; + typedef VALUE4 X4; + typedef VALUE5 X5; protected: - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - KEY3 key3_; - KEY4 key4_; - KEY5 key5_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor5 This; + typedef NonlinearFactor5 This; public: @@ -720,26 +680,33 @@ public: * @param j4 key of the fourth variable * @param j5 key of the fifth variable */ - NonlinearFactor5(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5) : - Base(noiseModel, make_tuple(j1,j2,j3,j4,j5)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5) {} + NonlinearFactor5(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5) : + Base(noiseModel) { + keys_.resize(5); + keys_[0] = j1; + keys_[1] = j2; + keys_[2] = j3; + keys_[3] = j4; + keys_[4] = j5; + } virtual ~NonlinearFactor5() {} /** methods to retrieve keys */ - inline const KEY1& key1() const { return key1_; } - inline const KEY2& key2() const { return key2_; } - inline const KEY3& key3() const { return key3_; } - inline const KEY4& key4() const { return key4_; } - inline const KEY5& key5() const { return key5_; } + inline const Symbol& key1() const { return keys_[0]; } + inline const Symbol& key2() const { return keys_[1]; } + inline const Symbol& key3() const { return keys_[2]; } + inline const Symbol& key4() const { return keys_[3]; } + inline const Symbol& key5() const { return keys_[4]; } /** Calls the 5-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); else - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4])); } else { return zero(this->dim()); } @@ -748,11 +715,11 @@ public: /** Print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearFactor5(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << "," - << (std::string) this->key3_ << "," - << (std::string) this->key4_ << "," - << (std::string) this->key5_ << ")\n"; + << (std::string) this->keys_[0] << "," + << (std::string) this->keys_[1] << "," + << (std::string) this->keys_[2] << "," + << (std::string) this->keys_[3] << "," + << (std::string) this->keys_[4] << ")\n"; this->noiseModel_->print(" noise model: "); } @@ -777,42 +744,29 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - ar & BOOST_SERIALIZATION_NVP(key3_); - ar & BOOST_SERIALIZATION_NVP(key4_); - ar & BOOST_SERIALIZATION_NVP(key5_); } }; // \class NonlinearFactor5 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 6 * variables. To derive from this class, implement evaluateError(). */ -template +template class NonlinearFactor6: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - typedef typename KEY3::Value X3; - typedef typename KEY4::Value X4; - typedef typename KEY5::Value X5; - typedef typename KEY6::Value X6; + typedef VALUE1 X1; + typedef VALUE2 X2; + typedef VALUE3 X3; + typedef VALUE4 X4; + typedef VALUE5 X5; + typedef VALUE6 X6; protected: - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - KEY3 key3_; - KEY4 key4_; - KEY5 key5_; - KEY6 key6_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor6 This; + typedef NonlinearFactor6 This; public: @@ -830,27 +784,35 @@ public: * @param j5 key of the fifth variable * @param j6 key of the fifth variable */ - NonlinearFactor6(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5, const KEY6& j6) : - Base(noiseModel, make_tuple(j1,j2,j3,j4,j5,j6)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5), key6_(j6) {} + NonlinearFactor6(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5, const Symbol& j6) : + Base(noiseModel) { + keys_.resize(6); + keys_[0] = j1; + keys_[1] = j2; + keys_[2] = j3; + keys_[3] = j4; + keys_[4] = j5; + keys_[5] = j6; + } virtual ~NonlinearFactor6() {} /** methods to retrieve keys */ - inline const KEY1& key1() const { return key1_; } - inline const KEY2& key2() const { return key2_; } - inline const KEY3& key3() const { return key3_; } - inline const KEY4& key4() const { return key4_; } - inline const KEY5& key5() const { return key5_; } - inline const KEY6& key6() const { return key6_; } + inline const Symbol& key1() const { return keys_[0]; } + inline const Symbol& key2() const { return keys_[1]; } + inline const Symbol& key3() const { return keys_[2]; } + inline const Symbol& key4() const { return keys_[3]; } + inline const Symbol& key5() const { return keys_[4]; } + inline const Symbol& key6() const { return keys_[5]; } /** Calls the 6-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); else - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5])); } else { return zero(this->dim()); } @@ -859,12 +821,12 @@ public: /** Print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearFactor6(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << "," - << (std::string) this->key3_ << "," - << (std::string) this->key4_ << "," - << (std::string) this->key5_ << "," - << (std::string) this->key6_ << ")\n"; + << (std::string) this->keys_[0] << "," + << (std::string) this->keys_[1] << "," + << (std::string) this->keys_[2] << "," + << (std::string) this->keys_[3] << "," + << (std::string) this->keys_[4] << "," + << (std::string) this->keys_[5] << ")\n"; this->noiseModel_->print(" noise model: "); } @@ -890,12 +852,6 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - ar & BOOST_SERIALIZATION_NVP(key3_); - ar & BOOST_SERIALIZATION_NVP(key4_); - ar & BOOST_SERIALIZATION_NVP(key5_); - ar & BOOST_SERIALIZATION_NVP(key6_); } }; // \class NonlinearFactor6 diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index eaaf3d4e9..460f32b0c 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -25,18 +25,18 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template - class BearingFactor: public NonlinearFactor2 { + template + class BearingFactor: public NonlinearFactor2 { private: - typedef typename POSEKEY::Value Pose; - typedef typename POSEKEY::Value::Rotation Rot; - typedef typename POINTKEY::Value Point; + typedef typename POSE Pose; + typedef typename Pose::Rotation Rot; + typedef typename POINT Point; - typedef BearingFactor This; - typedef NonlinearFactor2 Base; + typedef BearingFactor This; + typedef NonlinearFactor2 Base; - Rot z_; /** measurement */ + Rot measured_; /** measurement */ /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(Rot) @@ -48,9 +48,9 @@ namespace gtsam { BearingFactor() {} /** primary constructor */ - BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot& z, + BearingFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measured, const SharedNoiseModel& model) : - Base(model, i, j), z_(z) { + Base(model, poseKey, pointKey), measured_(measured) { } virtual ~BearingFactor() {} @@ -59,18 +59,18 @@ namespace gtsam { Vector evaluateError(const Pose& pose, const Point& point, boost::optional H1, boost::optional H2) const { Rot hx = pose.bearing(point, H1, H2); - return Rot::Logmap(z_.between(hx)); + return Rot::Logmap(measured_.between(hx)); } /** return the measured */ - inline const Rot measured() const { - return z_; + const Rot& measured() const { + return measured_; } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol); + return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); } private: @@ -81,7 +81,7 @@ namespace gtsam { void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; // BearingFactor diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index ef71f8f13..a81896b94 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -27,20 +27,20 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template - class BearingRangeFactor: public NonlinearFactor2 { + template + class BearingRangeFactor: public NonlinearFactor2 { private: - typedef typename POSEKEY::Value Pose; - typedef typename POSEKEY::Value::Rotation Rot; - typedef typename POINTKEY::Value Point; + typedef typename POSE Pose; + typedef typename POSE::Rotation Rot; + typedef typename POINT Point; - typedef BearingRangeFactor This; - typedef NonlinearFactor2 Base; + typedef BearingRangeFactor This; + typedef NonlinearFactor2 Base; // the measurement - Rot bearing_; - double range_; + Rot measuredBearing_; + double measuredRange_; /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(Rot) @@ -50,9 +50,9 @@ namespace gtsam { public: BearingRangeFactor() {} /* Default constructor */ - BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot& bearing, const double range, + BearingRangeFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measuredBearing, const double measuredRange, const SharedNoiseModel& model) : - Base(model, i, j), bearing_(bearing), range_(range) { + Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) { } virtual ~BearingRangeFactor() {} @@ -63,7 +63,7 @@ namespace gtsam { << (std::string) this->key1_ << "," << (std::string) this->key2_ << ")\n"; bearing_.print(" measured bearing"); - std::cout << " measured range: " << range_ << std::endl; + std::cout << " measured range: " << measuredRange_ << std::endl; this->noiseModel_->print(" noise model"); } @@ -71,8 +71,8 @@ namespace gtsam { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && - fabs(this->range_ - e->range_) < tol && - this->bearing_.equals(e->bearing_, tol); + fabs(this->measuredRange_ - e->measuredRange_) < tol && + this->measuredBearing_.equals(e->measuredBearing_, tol); } /** h(x)-z -> between(z,h(x)) for Rot manifold */ @@ -85,10 +85,10 @@ namespace gtsam { boost::optional H22_ = H2 ? boost::optional(H22) : boost::optional(); Rot y1 = pose.bearing(point, H11_, H12_); - Vector e1 = Rot::Logmap(bearing_.between(y1)); + Vector e1 = Rot::Logmap(measuredBearing_.between(y1)); double y2 = pose.range(point, H21_, H22_); - Vector e2 = Vector_(1, y2 - range_); + Vector e2 = Vector_(1, y2 - measuredRange_); if (H1) *H1 = gtsam::stack(2, &H11, &H21); if (H2) *H2 = gtsam::stack(2, &H12, &H22); @@ -96,8 +96,8 @@ namespace gtsam { } /** return the measured */ - inline const std::pair measured() const { - return std::make_pair(bearing_, range_); + const std::pair measured() const { + return std::make_pair(measuredBearing_, measuredRange_); } private: @@ -108,8 +108,8 @@ namespace gtsam { void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(bearing_); - ar & BOOST_SERIALIZATION_NVP(range_); + ar & BOOST_SERIALIZATION_NVP(measuredBearing_); + ar & BOOST_SERIALIZATION_NVP(measuredRange_); } }; // BearingRangeFactor diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 707dc3350..0fd8f6846 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -29,15 +29,15 @@ namespace gtsam { * KEY1::Value is the Lie Group type * T is the measurement type, by default the same */ - template - class BetweenFactor: public NonlinearFactor2 { + template + class BetweenFactor: public NonlinearFactor2 { private: - typedef BetweenFactor This; - typedef NonlinearFactor2 Base; + typedef BetweenFactor This; + typedef NonlinearFactor2 Base; - T measured_; /** The measurement */ + VALUE measured_; /** The measurement */ /** concept check by type */ GTSAM_CONCEPT_LIE_TYPE(T) @@ -52,7 +52,7 @@ namespace gtsam { BetweenFactor() {} /** Constructor */ - BetweenFactor(const KEY1& key1, const KEY1& key2, const T& measured, + BetweenFactor(const Symbol& key1, const Symbol& key2, const VALUE& measured, const SharedNoiseModel& model) : Base(model, key1, key2), measured_(measured) { } @@ -88,12 +88,12 @@ namespace gtsam { } /** return the measured */ - inline const T measured() const { + const VALUE& measured() const { return measured_; } /** number of variables attached to this factor */ - inline std::size_t size() const { + std::size_t size() const { return 2; } @@ -114,16 +114,14 @@ namespace gtsam { * This constraint requires the underlying type to a Lie type * */ - template - class BetweenConstraint : public BetweenFactor { + template + class BetweenConstraint : public BetweenFactor { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; /** Syntactic sugar for constrained version */ - BetweenConstraint(const typename KEY::Value& measured, - const KEY& key1, const KEY& key2, double mu = 1000.0) - : BetweenFactor(key1, key2, measured, - noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {} + BetweenConstraint(const VALUE& measured, const Symbol& key1, const Symbol& key2, double mu = 1000.0) : + BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {} private: @@ -132,7 +130,7 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("BetweenFactor", - boost::serialization::base_object >(*this)); + boost::serialization::base_object >(*this)); } }; // \class BetweenConstraint diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index f3df76774..3c28afa07 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -28,16 +28,16 @@ namespace gtsam { * will need to have its value function implemented to return * a scalar for comparison. */ -template -struct BoundingConstraint1: public NonlinearFactor1 { - typedef typename KEY::Value X; - typedef NonlinearFactor1 Base; - typedef boost::shared_ptr > shared_ptr; +template +struct BoundingConstraint1: public NonlinearFactor1 { + typedef VALUE X; + typedef NonlinearFactor1 Base; + typedef boost::shared_ptr > shared_ptr; double threshold_; bool isGreaterThan_; /// flag for greater/less than - BoundingConstraint1(const KEY& key, double threshold, + BoundingConstraint1(const Symbol& key, double threshold, bool isGreaterThan, double mu = 1000.0) : Base(noiseModel::Constrained::All(1, mu), key), threshold_(threshold), isGreaterThan_(isGreaterThan) { @@ -59,7 +59,7 @@ struct BoundingConstraint1: public NonlinearFactor1 { /** active when constraint *NOT* met */ bool active(const Values& c) const { // note: still active at equality to avoid zigzagging - double x = value(c[this->key_]); + double x = value(c[this->key()]); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index c0cd9e6c3..c6d2e2ede 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -29,21 +29,21 @@ namespace gtsam { /** * Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor */ - template + template class GeneralSFMFactor: - public NonlinearFactor2 { + public NonlinearFactor2 { protected: - Point2 z_; ///< the 2D measurement + Point2 measured_; ///< the 2D measurement public: - typedef typename CamK::Value Cam; ///< typedef for camera type - typedef GeneralSFMFactor Self ; ///< typedef for this object - typedef NonlinearFactor2 Base; ///< typedef for the base class + typedef typename CAMERA Cam; ///< typedef for camera type + typedef GeneralSFMFactor This ; ///< typedef for this object + typedef NonlinearFactor2 Base; ///< typedef for the base class typedef Point2 Measurement; ///< typedef for the measurement // shorthand for a smart pointer to a factor - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr shared_ptr; /** * Constructor @@ -52,11 +52,11 @@ namespace gtsam { * @param i is basically the frame number * @param j is the index of the landmark */ - GeneralSFMFactor(const Point2& z, const SharedNoiseModel& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {} + GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Landmark& landmarkKey) : Base(model, i, j), measured_(measured) {} - GeneralSFMFactor():z_(0.0,0.0) {} ///< default constructor - GeneralSFMFactor(const Point2 & p):z_(p) {} ///< constructor that takes a Point2 - GeneralSFMFactor(double x, double y):z_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 + GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor + GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 + GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 virtual ~GeneralSFMFactor() {} ///< destructor @@ -73,7 +73,7 @@ namespace gtsam { * equals */ bool equals(const GeneralSFMFactor &p, double tol = 1e-9) const { - return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ; + return Base::equals(p, tol) && this->measured_.equals(p.z_, tol) ; } /** h(x)-z */ @@ -83,14 +83,14 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const { - Vector error = z_.localCoordinates(camera.project2(point,H1,H2)); + Vector error = measured_.localCoordinates(camera.project2(point,H1,H2)); // gtsam::print(error, "error"); return error; } /** return the measured */ inline const Point2 measured() const { - return z_; + return measured_; } private: @@ -98,7 +98,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index 651e55500..e29893e77 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -38,16 +38,16 @@ namespace gtsam { * For practical use, it would be good to subclass this factor and have the class type * construct the mask. */ - template - class PartialPriorFactor: public NonlinearFactor1 { + template + class PartialPriorFactor: public NonlinearFactor1 { public: - typedef typename KEY::Value T; + typedef VALUE T; protected: - typedef NonlinearFactor1 Base; - typedef PartialPriorFactor This; + typedef NonlinearFactor1 Base; + typedef PartialPriorFactor This; Vector prior_; ///< measurement on logmap parameters, in compressed form std::vector mask_; ///< indices of values to constrain in compressed prior vector @@ -60,7 +60,7 @@ namespace gtsam { * constructor with just minimum requirements for a factor - allows more * computation in the constructor. This should only be used by subclasses */ - PartialPriorFactor(const KEY& key, const SharedNoiseModel& model) + PartialPriorFactor(const Symbol& key, const SharedNoiseModel& model) : Base(model, key) {} public: @@ -71,14 +71,14 @@ namespace gtsam { virtual ~PartialPriorFactor() {} /** Single Element Constructor: acts on a single parameter specified by idx */ - PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedNoiseModel& model) : + PartialPriorFactor(const Symbol& key, size_t idx, double prior, const SharedNoiseModel& model) : Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { assert(model->dim() == 1); this->fillH(); } /** Indices Constructor: specify the mask with a set of indices */ - PartialPriorFactor(const KEY& key, const std::vector& mask, const Vector& prior, + PartialPriorFactor(const Symbol& key, const std::vector& mask, const Vector& prior, const SharedNoiseModel& model) : Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) { assert((size_t)prior_.size() == mask.size()); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index fa33a9bc7..a5ff16c56 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -21,32 +21,30 @@ namespace gtsam { /** - * A class for a soft prior on any Lie type - * It takes two template parameters: - * Key (typically TypedSymbol) is used to look up T's in a Values - * Values where the T's are stored, typically Values or a TupleValues<...> - * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so - * a simple type like int will not work + * A class for a soft prior on any Value type */ - template - class PriorFactor: public NonlinearFactor1 { + template + class PriorFactor: public NonlinearFactor1 { public: - typedef typename KEY::Value T; + typedef VALUE T; private: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; - T prior_; /** The measurement */ + VALUE prior_; /** The measurement */ /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(T) public: - // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + /// shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr > shared_ptr; + + /// Typedef to this class + typedef PriorFactor This; /** default constructor - only use for serialization */ PriorFactor() {} @@ -54,8 +52,7 @@ namespace gtsam { virtual ~PriorFactor() {} /** Constructor */ - PriorFactor(const KEY& key, const T& prior, - const SharedNoiseModel& model) : + PriorFactor(const Symbol& key, const VALUE& prior, const SharedNoiseModel& model) : Base(model, key), prior_(prior) { } @@ -70,7 +67,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const PriorFactor *e = dynamic_cast*> (&expected); + const This* e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index dcf425aaa..646f2a003 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -29,21 +29,24 @@ namespace gtsam { * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. * i.e. the main building block for visual SLAM. */ - template - class GenericProjectionFactor: public NonlinearFactor2 { + template + class GenericProjectionFactor: public NonlinearFactor2 { protected: // Keep a copy of measurement and calibration for I/O - Point2 z_; ///< 2D measurement + Point2 measured_; ///< 2D measurement boost::shared_ptr K_; ///< shared pointer to calibration object public: /// shorthand for base class type - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; + + /// shorthand for this class + typedef GenericProjectionFactor This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr shared_ptr; /// Default constructor GenericProjectionFactor() : @@ -52,15 +55,16 @@ namespace gtsam { /** * Constructor + * TODO: Mark argument order standard (keys, measurement, parameters) * @param z is the 2 dimensional location of point in image (the measurement) * @param model is the standard deviation * @param j_pose is basically the frame number * @param j_landmark is the index of the landmark * @param K shared pointer to the constant calibration */ - GenericProjectionFactor(const Point2& z, const SharedNoiseModel& model, - POSK j_pose, LMK j_landmark, const shared_ptrK& K) : - Base(model, j_pose, j_landmark), z_(z), K_(K) { + GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model, + const Symbol poseKey, const Symbol& pointKey, const shared_ptrK& K) : + Base(model, poseKey, pointKey), measured_(measured), K_(K) { } /** @@ -69,14 +73,13 @@ namespace gtsam { */ void print(const std::string& s = "ProjectionFactor") const { Base::print(s); - z_.print(s + ".z"); + measured_.print(s + ".z"); } /// equals - bool equals(const GenericProjectionFactor& p - , double tol = 1e-9) const { - return Base::equals(p, tol) && this->z_.equals(p.z_, tol) - && this->K_->equals(*p.K_, tol); + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast (&expected); + return e && Base::equals(p, tol) && this->measured_.equals(e->z_, tol) && this->K_->equals(*e->K_, tol); } /// Evaluate error h(x)-z and optionally derivatives @@ -84,10 +87,9 @@ namespace gtsam { boost::optional H1, boost::optional H2) const { try { SimpleCamera camera(*K_, pose); - Point2 reprojectionError(camera.project(point, H1, H2) - z_); + Point2 reprojectionError(camera.project(point, H1, H2) - measured_); return reprojectionError.vector(); - } - catch( CheiralityException& e) { + } catch( CheiralityException& e) { if (H1) *H1 = zeros(2,6); if (H2) *H2 = zeros(2,3); cout << e.what() << ": Landmark "<< this->key2_.index() << @@ -97,8 +99,8 @@ namespace gtsam { } /** return the measurement */ - inline const Point2 measured() const { - return z_; + const Point2& measured() const { + return measured_; } private: @@ -108,7 +110,7 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); } }; diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index b762c5690..77c11d00d 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -25,17 +25,17 @@ namespace gtsam { /** * Binary factor for a range measurement */ - template - class RangeFactor: public NonlinearFactor2 { + template + class RangeFactor: public NonlinearFactor2 { private: - double z_; /** measurement */ + double measured_; /** measurement */ - typedef RangeFactor This; - typedef NonlinearFactor2 Base; + typedef RangeFactor This; + typedef NonlinearFactor2 Base; - typedef typename POSEKEY::Value Pose; - typedef typename POINTKEY::Value Point; + typedef typename POSE Pose; + typedef typename POINT Point; // Concept requirements for this factor GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point) @@ -44,34 +44,33 @@ namespace gtsam { RangeFactor() {} /* Default constructor */ - RangeFactor(const POSEKEY& i, const POINTKEY& j, double z, + RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured, const SharedNoiseModel& model) : - Base(model, i, j), z_(z) { + Base(model, poseKey, PointKey), measured_(measured) { } virtual ~RangeFactor() {} /** h(x)-z */ - Vector evaluateError(const typename POSEKEY::Value& pose, const typename POINTKEY::Value& point, - boost::optional H1, boost::optional H2) const { + Vector evaluateError(const Pose& pose, const Point& point, boost::optional H1, boost::optional H2) const { double hx = pose.range(point, H1, H2); - return Vector_(1, hx - z_); + return Vector_(1, hx - measured_); } /** return the measured */ - inline double measured() const { - return z_; + double measured() const { + return measured_; } /** equals specialized to this factor */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && fabs(this->z_ - e->z_) < tol; + return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->z_) < tol; } /** print contents */ void print(const std::string& s="") const { - Base::print(s + std::string(" range: ") + boost::lexical_cast(z_)); + Base::print(s + std::string(" range: ") + boost::lexical_cast(measured_)); } private: @@ -82,7 +81,7 @@ namespace gtsam { void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; // RangeFactor diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index e8d674f6c..f7e562640 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -26,38 +26,34 @@ namespace planarSLAM { Graph::Graph(const NonlinearFactorGraph& graph) : NonlinearFactorGraph(graph) {} - void Graph::addPrior(const PoseKey& i, const Pose2& p, - const SharedNoiseModel& model) { - sharedFactor factor(new Prior(i, p, model)); + void Graph::addPrior(Index i, const Pose2& p, const SharedNoiseModel& model) { + sharedFactor factor(new Prior(PoseKey(i), p, model)); push_back(factor); } - void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) { - sharedFactor factor(new Constraint(i, p)); + void Graph::addPoseConstraint(Index i, const Pose2& p) { + sharedFactor factor(new Constraint(PoseKey(i), p)); push_back(factor); } - void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, - const SharedNoiseModel& model) { - sharedFactor factor(new Odometry(i, j, z, model)); + void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) { + sharedFactor factor(new Odometry(PoseKey(i), PointKey(j), odometry, model)); push_back(factor); } - void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, - const SharedNoiseModel& model) { - sharedFactor factor(new Bearing(i, j, z, model)); + void Graph::addBearing(Index i, Index j, const Rot2& z, const SharedNoiseModel& model) { + sharedFactor factor(new Bearing(PoseKey(i), PointKey(j), z, model)); push_back(factor); } - void Graph::addRange(const PoseKey& i, const PointKey& j, double z, - const SharedNoiseModel& model) { - sharedFactor factor(new Range(i, j, z, model)); + void Graph::addRange(Index i, Index j, double z, const SharedNoiseModel& model) { + sharedFactor factor(new Range(PoseKey(i), PointKey(j), z, model)); push_back(factor); } - void Graph::addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1, + void Graph::addBearingRange(Index i, Index j, const Rot2& z1, double z2, const SharedNoiseModel& model) { - sharedFactor factor(new BearingRange(i, j, z1, z2, model)); + sharedFactor factor(new BearingRange(PoseKey(i), PointKey(j), z1, z2, model)); push_back(factor); } diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 50fae36a5..8fd568a3b 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -31,28 +31,32 @@ // Use planarSLAM namespace for specific SLAM instance namespace planarSLAM { - using namespace gtsam; + using namespace gtsam; - /// Typedef for a PoseKey with Pose2 data and 'x' symbol - typedef TypedSymbol PoseKey; + /// Typedef for a PoseKey with Pose2 data and 'x' symbol + typedef TypedSymbol PoseKey; - /// Typedef for a PointKey with Point2 data and 'l' symbol - typedef TypedSymbol PointKey; - /** - * List of typedefs for factors - */ - /// A hard constraint for PoseKeys to enforce particular values - typedef NonlinearEquality Constraint; - /// A prior factor to bias the value of a PoseKey - typedef PriorFactor Prior; - /// A factor between two PoseKeys set with a Pose2 - typedef BetweenFactor Odometry; - /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) - typedef BearingFactor Bearing; - /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) - typedef RangeFactor Range; - /// A factor between a PoseKey and a PointKey to express difference in rotation and location - typedef BearingRangeFactor BearingRange; + /// Typedef for a PointKey with Point2 data and 'l' symbol + typedef TypedSymbol PointKey; + + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } + + /* + * List of typedefs for factors + */ + /// A hard constraint for PoseKeys to enforce particular values + typedef NonlinearEquality Constraint; + /// A prior factor to bias the value of a PoseKey + typedef PriorFactor Prior; + /// A factor between two PoseKeys set with a Pose2 + typedef BetweenFactor Odometry; + /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) + typedef BearingFactor Bearing; + /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) + typedef RangeFactor Range; + /// A factor between a PoseKey and a PointKey to express difference in rotation and location + typedef BearingRangeFactor BearingRange; /** Values class, using specific PoseKeys and PointKeys * Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods @@ -80,48 +84,43 @@ namespace planarSLAM { void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); } }; + /// Creates a NonlinearFactorGraph with the Values type + struct Graph: public NonlinearFactorGraph { - /// Creates a NonlinearFactorGraph with the Values type - struct Graph: public NonlinearFactorGraph { + /// Default constructor for a NonlinearFactorGraph + Graph(){} - /// Default constructor for a NonlinearFactorGraph - Graph(){} + /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph + Graph(const NonlinearFactorGraph& graph); - /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph); + /// Biases the value of PoseKey key with Pose2 p given a noise model + void addPrior(Index poseKey, const Pose2& pose, const SharedNoiseModel& noiseModel); - /// Biases the value of PoseKey key with Pose2 p given a noise model - void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel); + /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value + void addPoseConstraint(Index poseKey, const Pose2& pose); - /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value - void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose); + /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement) + void addOdometry(Index poseKey, Index pointKey, const Pose2& odometry, const SharedNoiseModel& model); - /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement) - void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry, - const SharedNoiseModel& model); + /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation + void addBearing(Index poseKey, Index pointKey, const Rot2& bearing, const SharedNoiseModel& model); - /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation - void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing, - const SharedNoiseModel& model); + /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location + void addRange(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model); - /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location - void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range, - const SharedNoiseModel& model); + /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location + void addBearingRange(Index poseKey, Index pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model); - /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location - void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey, - const Rot2& bearing, double range, const SharedNoiseModel& model); + /// Optimize + Values optimize(const Values& initialEstimate) { + typedef NonlinearOptimizer Optimizer; + return *Optimizer::optimizeLM(*this, initialEstimate, + NonlinearOptimizationParameters::LAMBDA); + } + }; - /// Optimize - Values optimize(const Values& initialEstimate) { - typedef NonlinearOptimizer Optimizer; - return *Optimizer::optimizeLM(*this, initialEstimate, - NonlinearOptimizationParameters::LAMBDA); - } - }; - -/// Optimizer -typedef NonlinearOptimizer Optimizer; + /// Optimizer + typedef NonlinearOptimizer Optimizer; } // planarSLAM diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index a425f8fb9..b006c61f2 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -35,20 +35,20 @@ namespace pose2SLAM { } /* ************************************************************************* */ - void Graph::addPrior(const PoseKey& i, const Pose2& p, + void Graph::addPrior(Index i, const Pose2& p, const SharedNoiseModel& model) { - sharedFactor factor(new Prior(i, p, model)); + sharedFactor factor(new Prior(PoseKey(i), p, model)); push_back(factor); } - void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) { - sharedFactor factor(new HardConstraint(i, p)); + void Graph::addPoseConstraint(Index i, const Pose2& p) { + sharedFactor factor(new HardConstraint(PoseKey(i), p)); push_back(factor); } - void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, + void Graph::addOdometry(Index i, Index j, const Pose2& z, const SharedNoiseModel& model) { - sharedFactor factor(new Odometry(i, j, z, model)); + sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), z, model)); push_back(factor); } diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 23e087be6..ab0c60050 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -33,9 +33,8 @@ namespace pose2SLAM { using namespace gtsam; - /// Keys with Pose2 and symbol 'x' - typedef TypedSymbol PoseKey; - + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } /// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper struct Values: public gtsam::Values { @@ -70,11 +69,11 @@ namespace pose2SLAM { */ /// A hard constraint to enforce a specific value for a pose - typedef NonlinearEquality HardConstraint; + typedef NonlinearEquality HardConstraint; /// A prior factor on a pose with Pose2 data type. - typedef PriorFactor Prior; + typedef PriorFactor Prior; /// A factor to add an odometry measurement between two poses. - typedef BetweenFactor Odometry; + typedef BetweenFactor Odometry; /// Graph struct Graph: public NonlinearFactorGraph { @@ -86,13 +85,13 @@ namespace pose2SLAM { Graph(const NonlinearFactorGraph& graph); /// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph - void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model); + void addPrior(Index i, const Pose2& p, const SharedNoiseModel& model); /// Creates a hard constraint for key i with the given Pose2 p. - void addPoseConstraint(const PoseKey& i, const Pose2& p); + void addPoseConstraint(Index i, const Pose2& p); /// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph - void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, + void addOdometry(Index i, Index j, const Pose2& z, const SharedNoiseModel& model); /// Optimize diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 4d063c12b..3ff8fa99c 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -45,20 +45,18 @@ namespace gtsam { } /* ************************************************************************* */ - void Graph::addPrior(const Key& i, const Pose3& p, - const SharedNoiseModel& model) { - sharedFactor factor(new Prior(i, p, model)); + void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) { + sharedFactor factor(new Prior(PoseKey(i), p, model)); push_back(factor); } - void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z, - const SharedNoiseModel& model) { - sharedFactor factor(new Constraint(i, j, z, model)); + void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) { + sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model)); push_back(factor); } - void Graph::addHardConstraint(const Key& i, const Pose3& p) { - sharedFactor factor(new HardConstraint(i, p)); + void Graph::addHardConstraint(Index i, const Pose3& p) { + sharedFactor factor(new HardConstraint(PoseKey(i), p)); push_back(factor); } diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index 686f6bd0f..555e5fc91 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -30,8 +30,8 @@ namespace gtsam { /// Use pose3SLAM namespace for specific SLAM instance namespace pose3SLAM { - /// Creates a Key with data Pose3 and symbol 'x' - typedef TypedSymbol Key; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } /** * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) @@ -42,25 +42,23 @@ namespace gtsam { Values circle(size_t n, double R); /// A prior factor on Key with Pose3 data type. - typedef PriorFactor Prior; + typedef PriorFactor Prior; /// A factor to put constraints between two factors. - typedef BetweenFactor Constraint; + typedef BetweenFactor Constraint; /// A hard constraint would enforce that the given key would have the input value in the results. - typedef NonlinearEquality HardConstraint; + typedef NonlinearEquality HardConstraint; /// Graph struct Graph: public NonlinearFactorGraph { /// Adds a factor between keys of the same type - void addPrior(const Key& i, const Pose3& p, - const SharedNoiseModel& model); + void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model); /// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph - void addConstraint(const Key& i, const Key& j, const Pose3& z, - const SharedNoiseModel& model); + void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model); /// Creates a hard constraint for key i with the given Pose3 p. - void addHardConstraint(const Key& i, const Pose3& p); + void addHardConstraint(Index i, const Pose3& p); }; /// Optimizer diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index a9ab58d3c..ca78bb334 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -29,8 +29,12 @@ namespace simulated2D { using namespace gtsam; // Simulated2D robots have no orientation, just a position - typedef TypedSymbol PoseKey; - typedef TypedSymbol PointKey; + + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } + + /// Convenience function for constructing a landmark key + inline Symbol PointKey(Index j) { return Symbol('l', j); } /** * Custom Values class that holds poses and points, mainly used as a convenience for MATLAB wrapper @@ -115,24 +119,23 @@ namespace simulated2D { /** * Unary factor encoding a soft prior on a vector */ - template - class GenericPrior: public NonlinearFactor1 { + template + class GenericPrior: public NonlinearFactor1 { public: - typedef NonlinearFactor1 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; - typedef typename KEY::Value Pose; ///< shortcut to Pose type + typedef NonlinearFactor1 Base; ///< base class + typedef boost::shared_ptr > shared_ptr; + typedef VALUE Pose; ///< shortcut to Pose type - Pose z_; ///< prior mean + Pose measured_; ///< prior mean /// Create generic prior - GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) : - NonlinearFactor1(model, key), z_(z) { + GenericPrior(const Pose& z, const SharedNoiseModel& model, const Symbol& key) : + Base(model, key), measured_(z) { } /// Return error and optional derivative - Vector evaluateError(const Pose& x, boost::optional H = - boost::none) const { - return (prior(x, H) - z_).vector(); + Vector evaluateError(const Pose& x, boost::optional H = boost::none) const { + return (prior(x, H) - measured_).vector(); } private: @@ -146,33 +149,32 @@ namespace simulated2D { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; /** * Binary factor simulating "odometry" between two Vectors */ - template - class GenericOdometry: public NonlinearFactor2 { + template + class GenericOdometry: public NonlinearFactor2 { public: - typedef NonlinearFactor2 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; - typedef typename KEY::Value Pose; ///< shortcut to Pose type + typedef NonlinearFactor2 Base; ///< base class + typedef boost::shared_ptr > shared_ptr; + typedef VALUE Pose; ///< shortcut to Pose type - Pose z_; ///< odometry measurement + Pose measured_; ///< odometry measurement /// Create odometry - GenericOdometry(const Pose& z, const SharedNoiseModel& model, - const KEY& i1, const KEY& i2) : - NonlinearFactor2(model, i1, i2), z_(z) { + GenericOdometry(const Pose& measured, const SharedNoiseModel& model, const Symbol& key1, const Symbol& key2) : + Base(model, key1, key2), measured_(measured) { } /// Evaluate error and optionally return derivatives Vector evaluateError(const Pose& x1, const Pose& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (odo(x1, x2, H1, H2) - z_).vector(); + return (odo(x1, x2, H1, H2) - measured_).vector(); } private: @@ -186,34 +188,33 @@ namespace simulated2D { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; /** * Binary factor simulating "measurement" between two Vectors */ - template - class GenericMeasurement: public NonlinearFactor2 { + template + class GenericMeasurement: public NonlinearFactor2 { public: - typedef NonlinearFactor2 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; - typedef typename XKEY::Value Pose; ///< shortcut to Pose type - typedef typename LKEY::Value Point; ///< shortcut to Point type + typedef NonlinearFactor2 Base; ///< base class + typedef boost::shared_ptr > shared_ptr; + typedef POSE Pose; ///< shortcut to Pose type + typedef LANDMARK Landmark; ///< shortcut to Landmark type - Point z_; ///< Measurement + Landmark z_; ///< Measurement /// Create measurement factor - GenericMeasurement(const Point& z, const SharedNoiseModel& model, - const XKEY& i, const LKEY& j) : - NonlinearFactor2(model, i, j), z_(z) { + GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey) : + Base(model, poseKey, landmarkKey), measured_(measured) { } /// Evaluate error and optionally return derivatives - Vector evaluateError(const Pose& x1, const Point& x2, + Vector evaluateError(const Pose& x1, const Landmark& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (mea(x1, x2, H1, H2) - z_).vector(); + return (mea(x1, x2, H1, H2) - measured_).vector(); } private: @@ -227,14 +228,14 @@ namespace simulated2D { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; /** Typedefs for regular use */ - typedef GenericPrior Prior; - typedef GenericOdometry Odometry; - typedef GenericMeasurement Measurement; + typedef GenericPrior Prior; + typedef GenericOdometry Odometry; + typedef GenericMeasurement Measurement; // Specialization of a graph for this example domain // TODO: add functions to add factor types diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h index ffd4e54fb..99475ce91 100644 --- a/gtsam/slam/simulated2DConstraints.h +++ b/gtsam/slam/simulated2DConstraints.h @@ -33,13 +33,13 @@ namespace simulated2D { namespace equality_constraints { /** Typedefs for regular use */ - typedef NonlinearEquality1 UnaryEqualityConstraint; - typedef NonlinearEquality1 UnaryEqualityPointConstraint; - typedef BetweenConstraint OdoEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityPointConstraint; + typedef BetweenConstraint OdoEqualityConstraint; /** Equality between variables */ - typedef NonlinearEquality2 PoseEqualityConstraint; - typedef NonlinearEquality2 PointEqualityConstraint; + typedef NonlinearEquality2 PoseEqualityConstraint; + typedef NonlinearEquality2 PointEqualityConstraint; } // \namespace equality_constraints @@ -47,15 +47,14 @@ namespace simulated2D { /** * Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c) - * @tparam VALUES is the values structure for the graph - * @tparam KEY is the key type for the variable constrained + * @tparam VALUE is the value type for the variable constrained, e.g. Pose2, Point3, etc. * @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim() */ - template - struct ScalarCoordConstraint1: public BoundingConstraint1 { - typedef BoundingConstraint1 Base; ///< Base class convenience typedef - typedef boost::shared_ptr > shared_ptr; ///< boost::shared_ptr convenience typedef - typedef typename KEY::Value Point; ///< Constrained variable type + template + struct ScalarCoordConstraint1: public BoundingConstraint1 { + typedef BoundingConstraint1 Base; ///< Base class convenience typedef + typedef boost::shared_ptr > shared_ptr; ///< boost::shared_ptr convenience typedef + typedef VALUE Point; ///< Constrained variable type virtual ~ScalarCoordConstraint1() {} @@ -66,7 +65,7 @@ namespace simulated2D { * @param isGreaterThan is a flag to set inequality as greater than or less than * @param mu is the penalty function gain */ - ScalarCoordConstraint1(const KEY& key, double c, + ScalarCoordConstraint1(const Symbol& key, double c, bool isGreaterThan, double mu = 1000.0) : Base(key, c, isGreaterThan, mu) { } @@ -94,8 +93,8 @@ namespace simulated2D { }; /** typedefs for use with simulated2D systems */ - typedef ScalarCoordConstraint1 PoseXInequality; ///< Simulated2D domain example factor constraining X - typedef ScalarCoordConstraint1 PoseYInequality; ///< Simulated2D domain example factor constraining Y + typedef ScalarCoordConstraint1 PoseXInequality; ///< Simulated2D domain example factor constraining X + typedef ScalarCoordConstraint1 PoseYInequality; ///< Simulated2D domain example factor constraining Y /** * Trait for distance constraints to provide distance @@ -114,10 +113,10 @@ namespace simulated2D { * @tparam VALUES is the variable set for the graph * @tparam KEY is the type of the keys for the variables constrained */ - template - struct MaxDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; ///< Base class for factor - typedef typename KEY::Value Point; ///< Type of variable constrained + template + struct MaxDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; ///< Base class for factor + typedef VALUE Point; ///< Type of variable constrained virtual ~MaxDistanceConstraint() {} @@ -128,8 +127,8 @@ namespace simulated2D { * @param range_bound is the maximum range allowed between the variables * @param mu is the gain for the penalty function */ - MaxDistanceConstraint(const KEY& key1, const KEY& key2, double range_bound, double mu = 1000.0) - : Base(key1, key2, range_bound, false, mu) {} + MaxDistanceConstraint(const Symbol& key1, const Symbol& key2, double range_bound, double mu = 1000.0) : + Base(key1, key2, range_bound, false, mu) {} /** * computes the range with derivatives @@ -153,15 +152,14 @@ namespace simulated2D { /** * Binary inequality constraint forcing a minimum range * NOTE: this is not a convex function! Be careful with initialization. - * @tparam VALUES is the variable set for the graph - * @tparam XKEY is the type of the pose key constrained - * @tparam PKEY is the type of the point key constrained + * @tparam POSE is the type of the pose value constrained + * @tparam POINT is the type of the point value constrained */ - template - struct MinDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; ///< Base class for factor - typedef typename XKEY::Value Pose; ///< Type of pose variable constrained - typedef typename PKEY::Value Point; ///< Type of point variable constrained + template + struct MinDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; ///< Base class for factor + typedef POSE Pose; ///< Type of pose variable constrained + typedef POINT Point; ///< Type of point variable constrained virtual ~MinDistanceConstraint() {} @@ -172,7 +170,7 @@ namespace simulated2D { * @param range_bound is the minimum range allowed between the variables * @param mu is the gain for the penalty function */ - MinDistanceConstraint(const XKEY& key1, const PKEY& key2, + MinDistanceConstraint(const Symbol& key1, const Symbol& key2, double range_bound, double mu = 1000.0) : Base(key1, key2, range_bound, true, mu) {} @@ -193,7 +191,7 @@ namespace simulated2D { } }; - typedef MinDistanceConstraint LandmarkAvoid; ///< Simulated2D domain example factor + typedef MinDistanceConstraint LandmarkAvoid; ///< Simulated2D domain example factor } // \namespace inequality_constraints diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index 0c5533291..d0880b8f3 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -27,9 +27,11 @@ namespace simulated2DOriented { using namespace gtsam; - // The types that take an oriented pose2 rather than point2 - typedef TypedSymbol PointKey; - typedef TypedSymbol PoseKey; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } + + /// Convenience function for constructing a landmark key + inline Symbol PointKey(Index j) { return Symbol('l', j); } /// Specialized Values structure with syntactic sugar for /// compatibility with matlab @@ -75,21 +77,20 @@ namespace simulated2DOriented { boost::none, boost::optional H2 = boost::none); /// Unary factor encoding a soft prior on a vector - template - struct GenericPosePrior: public NonlinearFactor1 { + template + struct GenericPosePrior: public NonlinearFactor1 { - Pose2 z_; ///< measurement + Pose2 measured_; ///< measurement /// Create generic pose prior - GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, - const Key& key) : - NonlinearFactor1(model, key), z_(z) { + GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, const Symbol& key) : + NonlinearFactor1(model, key), measured_(measured) { } /// Evaluate error and optionally derivative Vector evaluateError(const Pose2& x, boost::optional H = boost::none) const { - return z_.localCoordinates(prior(x, H)); + return measured_.localCoordinates(prior(x, H)); } }; @@ -97,31 +98,31 @@ namespace simulated2DOriented { /** * Binary factor simulating "odometry" between two Vectors */ - template - struct GenericOdometry: public NonlinearFactor2 { - Pose2 z_; ///< Between measurement for odometry factor + template + struct GenericOdometry: public NonlinearFactor2 { + Pose2 measured_; ///< Between measurement for odometry factor /** * Creates an odometry factor between two poses */ - GenericOdometry(const Pose2& z, const SharedNoiseModel& model, - const KEY& i1, const KEY& i2) : - NonlinearFactor2(model, i1, i2), z_(z) { + GenericOdometry(const Pose2& measured, const SharedNoiseModel& model, + const Symbol& i1, const Symbol& i2) : + NonlinearFactor2(model, i1, i2), measured_(measured) { } /// Evaluate error and optionally derivative Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return z_.localCoordinates(odo(x1, x2, H1, H2)); + return measured_.localCoordinates(odo(x1, x2, H1, H2)); } }; - typedef GenericOdometry Odometry; + typedef GenericOdometry Odometry; /// Graph specialization for syntactic sugar use with matlab - class Graph : public NonlinearFactorGraph { + class Graph : public NonlinearFactorGraph { public: Graph() {} // TODO: add functions to add factors diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 490692c90..53a2af385 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -235,13 +235,10 @@ TEST( NonlinearFactor, linearize_constraint2 ) } /* ************************************************************************* */ -typedef TypedSymbol TestKey; - -/* ************************************************************************* */ -class TestFactor4 : public NonlinearFactor4 { +class TestFactor4 : public NonlinearFactor4 { public: - typedef NonlinearFactor4 Base; - TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {} + typedef NonlinearFactor4 Base; + TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4") {} virtual Vector evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, @@ -263,13 +260,13 @@ public: TEST(NonlinearFactor, NonlinearFactor4) { TestFactor4 tf; Values tv; - tv.insert(TestKey(1), LieVector(1, 1.0)); - tv.insert(TestKey(2), LieVector(1, 2.0)); - tv.insert(TestKey(3), LieVector(1, 3.0)); - tv.insert(TestKey(4), LieVector(1, 4.0)); + tv.insert("x1", LieVector(1, 1.0)); + tv.insert("x2", LieVector(1, 2.0)); + tv.insert("x3", LieVector(1, 3.0)); + tv.insert("x4", LieVector(1, 4.0)); EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4); + Ordering ordering; ordering += "x1", "x2", "x3", "x4"; JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); @@ -283,9 +280,9 @@ TEST(NonlinearFactor, NonlinearFactor4) { } /* ************************************************************************* */ -class TestFactor5 : public NonlinearFactor5 { +class TestFactor5 : public NonlinearFactor5 { public: - typedef NonlinearFactor5 Base; + typedef NonlinearFactor5 Base; TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {} virtual Vector @@ -310,14 +307,14 @@ public: TEST(NonlinearFactor, NonlinearFactor5) { TestFactor5 tf; Values tv; - tv.insert(TestKey(1), LieVector(1, 1.0)); - tv.insert(TestKey(2), LieVector(1, 2.0)); - tv.insert(TestKey(3), LieVector(1, 3.0)); - tv.insert(TestKey(4), LieVector(1, 4.0)); - tv.insert(TestKey(5), LieVector(1, 5.0)); + tv.insert("x1", LieVector(1, 1.0)); + tv.insert("x2", LieVector(1, 2.0)); + tv.insert("x3", LieVector(1, 3.0)); + tv.insert("x4", LieVector(1, 4.0)); + tv.insert("x5", LieVector(1, 5.0)); EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5); + Ordering ordering; ordering += "x1", "x2", "x3", "x4", "x5"; JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); @@ -333,9 +330,9 @@ TEST(NonlinearFactor, NonlinearFactor5) { } /* ************************************************************************* */ -class TestFactor6 : public NonlinearFactor6 { +class TestFactor6 : public NonlinearFactor6 { public: - typedef NonlinearFactor6 Base; + typedef NonlinearFactor6 Base; TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {} virtual Vector @@ -362,15 +359,15 @@ public: TEST(NonlinearFactor, NonlinearFactor6) { TestFactor6 tf; Values tv; - tv.insert(TestKey(1), LieVector(1, 1.0)); - tv.insert(TestKey(2), LieVector(1, 2.0)); - tv.insert(TestKey(3), LieVector(1, 3.0)); - tv.insert(TestKey(4), LieVector(1, 4.0)); - tv.insert(TestKey(5), LieVector(1, 5.0)); - tv.insert(TestKey(6), LieVector(1, 6.0)); + tv.insert("x1", LieVector(1, 1.0)); + tv.insert("x2", LieVector(1, 2.0)); + tv.insert("x3", LieVector(1, 3.0)); + tv.insert("x4", LieVector(1, 4.0)); + tv.insert("x5", LieVector(1, 5.0)); + tv.insert("x6", LieVector(1, 6.0)); EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6); + Ordering ordering; ordering += "x1", "x2", "x3", "x4", "x5", "x6"; JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1);