From 811be62ed3f42cdfa32fde131db10bf027bf270e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 6 Feb 2012 00:44:25 +0000 Subject: [PATCH 2/7] 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); From 7c7c3e3836d8cfd72075c6f445d3f07418859fd1 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 6 Feb 2012 00:57:05 +0000 Subject: [PATCH 3/7] Fixed some merging errors --- gtsam/slam/planarSLAM.h | 17 +++++++---------- gtsam/slam/pose2SLAM.h | 5 +++-- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 8fd568a3b..5c9c62d46 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -33,15 +33,12 @@ namespace planarSLAM { using namespace gtsam; - /// 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; - /// Convenience function for constructing a pose key inline Symbol PoseKey(Index j) { return Symbol('x', j); } + /// Convenience function for constructing a pose key + inline Symbol PointKey(Index j) { return Symbol('l', j); } + /* * List of typedefs for factors */ @@ -72,16 +69,16 @@ namespace planarSLAM { } /// get a pose - Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } + Pose2 pose(Index key) const { return at(PoseKey(key)); } /// get a point - Point2 point(int key) const { return (*this)[PointKey(key)]; } + Point2 point(Index key) const { return at(PointKey(key)); } /// insert a pose - void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); } + void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); } /// insert a point - void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); } + void insertPoint(Index key, const Point2& point) { insert(PointKey(key), point); } }; /// Creates a NonlinearFactorGraph with the Values type diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index ab0c60050..7917a3c79 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -35,6 +35,7 @@ namespace pose2SLAM { /// 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 { @@ -49,10 +50,10 @@ namespace pose2SLAM { // Convenience for MATLAB wrapper, which does not allow for identically named methods /// get a pose - Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } + Pose2 pose(Index key) const { return at(PoseKey(key)); } /// insert a pose - void insertPose(int key, const Pose2& pose) { insert(PoseKey(key), pose); } + void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); } }; /** From eaa9e4d7399561d7be9bc6d1390c206177a62cb1 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 6 Feb 2012 23:32:59 +0000 Subject: [PATCH 4/7] Continuing changes from templating NonlinearFactors on value instead of key --- gtsam/base/DerivedValue.h | 10 +++ gtsam/nonlinear/NonlinearEquality.h | 63 +++++++++-------- gtsam/nonlinear/NonlinearFactor.h | 10 ++- gtsam/nonlinear/Values-inl.h | 44 ++++++++++++ gtsam/nonlinear/Values.h | 13 ++++ gtsam/slam/BearingFactor.h | 4 +- gtsam/slam/BearingRangeFactor.h | 10 +-- gtsam/slam/BetweenFactor.h | 15 ++-- gtsam/slam/GeneralSFMFactor.h | 18 ++--- gtsam/slam/PriorFactor.h | 2 +- gtsam/slam/ProjectionFactor.h | 8 +-- gtsam/slam/RangeFactor.h | 8 +-- gtsam/slam/StereoFactor.h | 53 ++++++--------- gtsam/slam/dataset.cpp | 4 +- gtsam/slam/pose3SLAM.cpp | 2 +- gtsam/slam/simulated2D.h | 20 +++--- gtsam/slam/simulated2DOriented.h | 14 ++-- gtsam/slam/simulated3D.h | 46 ++++++------- gtsam/slam/smallExample.cpp | 49 ++++++------- gtsam/slam/tests/testAntiFactor.cpp | 18 ++--- gtsam/slam/tests/testGeneralSFMFactor.cpp | 25 +++---- .../testGeneralSFMFactor_Cal3Bundler.cpp | 29 ++++---- gtsam/slam/tests/testPlanarSLAM.cpp | 20 +++--- gtsam/slam/tests/testPose2SLAM.cpp | 63 ++++++++--------- gtsam/slam/tests/testPose3SLAM.cpp | 22 +++--- gtsam/slam/tests/testProjectionFactor.cpp | 6 +- gtsam/slam/tests/testStereoFactor.cpp | 4 +- gtsam/slam/visualSLAM.h | 68 ++++++++++--------- 28 files changed, 358 insertions(+), 290 deletions(-) diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index 0a66eb01f..3cac7c7dc 100644 --- a/gtsam/base/DerivedValue.h +++ b/gtsam/base/DerivedValue.h @@ -83,6 +83,16 @@ public: return (static_cast(this))->operator=(derivedRhs); } + /// Conversion to the derived class + operator const DERIVED& () const { + return static_cast(*this); + } + + /// Conversion to the derived class + operator DERIVED& () { + return static_cast(*this); + } + protected: /// Assignment operator, protected because only the Value or DERIVED /// assignment operators should be used. diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index e01623601..a95044dee 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -47,11 +47,11 @@ namespace gtsam { * * \nosubgrouping */ - template - class NonlinearEquality: public NonlinearFactor1 { + template + class NonlinearEquality: public NonlinearFactor1 { public: - typedef typename KEY::Value T; + typedef VALUE T; private: @@ -64,6 +64,12 @@ namespace gtsam { // error gain in allow error case double error_gain_; + // typedef to this class + typedef NonlinearEquality This; + + // typedef to base class + typedef NonlinearFactor1 Base; + public: /** @@ -71,7 +77,6 @@ namespace gtsam { */ bool (*compare_)(const T& a, const T& b); - typedef NonlinearFactor1 Base; /** default constructor - only for serialization */ NonlinearEquality() {} @@ -84,7 +89,7 @@ namespace gtsam { /** * Constructor - forces exact evaluation */ - NonlinearEquality(const KEY& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : + NonlinearEquality(const Symbol& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), allow_error_(false), error_gain_(0.0), compare_(_compare) { @@ -93,7 +98,7 @@ namespace gtsam { /** * Constructor - allows inexact evaluation */ - NonlinearEquality(const KEY& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) : + NonlinearEquality(const Symbol& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) : Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), compare_(_compare) { @@ -103,17 +108,17 @@ namespace gtsam { /// @name Testable /// @{ - void print(const std::string& s = "") const { - std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n"; + virtual void print(const std::string& s = "") const { + std::cout << "Constraint: " << s << " on [" << (std::string)(this->key()) << "]\n"; gtsam::print(feasible_,"Feasible Point"); std::cout << "Variable Dimension: " << feasible_.dim() << std::endl; } /** Check if two factors are equal */ - bool equals(const NonlinearEquality& f, double tol = 1e-9) const { - if (!Base::equals(f)) return false; - return feasible_.equals(f.feasible_, tol) && - fabs(error_gain_ - f.error_gain_) < tol; + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + const This* e = dynamic_cast(&f); + return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) && + fabs(error_gain_ - e->error_gain_) < tol; } /// @} @@ -122,7 +127,7 @@ namespace gtsam { /** actual error function calculation */ virtual double error(const Values& c) const { - const T& xj = c[this->key_]; + const T& xj = c.at(this->key()); Vector e = this->unwhitenedError(c); if (allow_error_ || !compare_(xj, feasible_)) { return error_gain_ * dot(e,e); @@ -132,7 +137,7 @@ namespace gtsam { } /** error function */ - inline Vector evaluateError(const T& xj, boost::optional H = boost::none) const { + Vector evaluateError(const T& xj, boost::optional H = boost::none) const { size_t nj = feasible_.dim(); if (allow_error_) { if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare @@ -142,18 +147,18 @@ namespace gtsam { return zero(nj); // set error to zero if equal } else { if (H) throw std::invalid_argument( - "Linearization point not feasible for " + (std::string)(this->key_) + "!"); + "Linearization point not feasible for " + (std::string)(this->key()) + "!"); return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal } } // Linearize is over-written, because base linearization tries to whiten virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const { - const T& xj = x[this->key_]; + const T& xj = x.at(this->key()); Matrix A; Vector b = evaluateError(xj, A); SharedDiagonal model = noiseModel::Constrained::All(b.size()); - return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model)); + return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model)); } /// @} @@ -177,14 +182,14 @@ namespace gtsam { /** * Simple unary equality constraint - fixes a value for a variable */ - template - class NonlinearEquality1 : public NonlinearFactor1 { + template + class NonlinearEquality1 : public NonlinearFactor1 { public: - typedef typename KEY::Value X; + typedef VALUE X; protected: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; /** default constructor to allow for serialization */ NonlinearEquality1() {} @@ -196,10 +201,10 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; ///TODO: comment - NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0) + NonlinearEquality1(const X& value, const Symbol& key1, double mu = 1000.0) : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {} virtual ~NonlinearEquality1() {} @@ -236,13 +241,13 @@ namespace gtsam { * Simple binary equality constraint - this constraint forces two factors to * be the same. */ - template - class NonlinearEquality2 : public NonlinearFactor2 { + template + class NonlinearEquality2 : public NonlinearFactor2 { public: - typedef typename KEY::Value X; + typedef VALUE X; protected: - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; GTSAM_CONCEPT_MANIFOLD_TYPE(X); @@ -251,10 +256,10 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; ///TODO: comment - NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0) + NonlinearEquality2(const Symbol& key1, const Symbol& key2, double mu = 1000.0) : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} virtual ~NonlinearEquality2() {} diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ba2198b1b..166ad6980 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -95,6 +95,11 @@ public: std::cout << s << ": NonlinearFactor\n"; } + /** Check if two factors are equal */ + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + return Base::equals(f); + } + /// @} /// @name Standard Interface /// @{ @@ -202,8 +207,9 @@ public: } /** Check if two factors are equal */ - virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const { - return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol); + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + const NoiseModelFactor* e = dynamic_cast(&f); + return e && Base::equals(f, tol) && noiseModel_->equals(*e->noiseModel_, tol); } /** get the dimension of the factor (number of rows on linearization) */ diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 76a1ce035..31d6c318a 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -23,7 +23,10 @@ */ #include +#include +#include +#include #include // Only so Eclipse finds class definition namespace gtsam { @@ -37,6 +40,30 @@ namespace gtsam { ValueCloneAllocator() {} }; + /* ************************************************************************* */ + class ValueAutomaticCasting { + const Symbol& key_; + const Value& value_; + public: + ValueAutomaticCasting(const Symbol& key, const Value& value) : key_(key), value_(value) {} + + template + operator const ValueType& () const { + // Check the type and throw exception if incorrect + if(typeid(ValueType) != typeid(value_)) + throw ValuesIncorrectType(key_, typeid(ValueType), typeid(value_)); + + // We have already checked the type, so do a "blind" static_cast, not dynamic_cast + return static_cast(value_); + } + }; + +// /* ************************************************************************* */ +// template +// ValueType& operator=(ValueType& lhs, const ValueAutomaticCasting& rhs) { +// lhs = rhs; +// } + /* ************************************************************************* */ template const ValueType& Values::at(const Symbol& j) const { @@ -55,6 +82,18 @@ namespace gtsam { return static_cast(*item->second); } + /* ************************************************************************* */ + inline ValueAutomaticCasting Values::at(const Symbol& j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); + + // Throw exception if it does not exist + if(item == values_.end()) + throw ValuesKeyDoesNotExist("retrieve", j); + + return ValueAutomaticCasting(item->first, *item->second); + } + /* ************************************************************************* */ template const typename TypedKey::Value& Values::at(const TypedKey& j) const { @@ -65,6 +104,11 @@ namespace gtsam { return at(symbol); } + /* ************************************************************************* */ + inline ValueAutomaticCasting Values::operator[](const Symbol& j) const { + return at(j); + } + /* ************************************************************************* */ template boost::optional Values::exists(const Symbol& j) const { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 0aa67c63d..f226007f1 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -44,6 +44,7 @@ namespace gtsam { // Forward declarations class ValueCloneAllocator; + class ValueAutomaticCasting; /** * A non-templated config holding any types of Manifold-group elements. A @@ -138,6 +139,15 @@ namespace gtsam { template const ValueType& at(const Symbol& j) const; + /** Retrieve a variable by key \c j. This non-templated version returns a + * special ValueAutomaticCasting object that may be assigned to the proper + * value. + * @param j Retrieve the value associated with this key + * @return A ValueAutomaticCasting object that may be assignmed to a Value + * of the proper type. + */ + ValueAutomaticCasting at(const Symbol& j) const; + /** Retrieve a variable using a special key (typically TypedSymbol), which * contains the type of the value associated with the key, and which must * be conversion constructible to a Symbol, e.g. @@ -153,6 +163,9 @@ namespace gtsam { const typename TypedKey::Value& operator[](const TypedKey& j) const { return at(j); } + /** operator[] syntax for at(const Symbol& j) */ + ValueAutomaticCasting operator[](const Symbol& j) const; + /** Check if a value exists with key \c j. See exists<>(const Symbol& j) * and exists(const TypedKey& j) for versions that return the value if it * exists. */ diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 460f32b0c..70d19061e 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -29,9 +29,9 @@ namespace gtsam { class BearingFactor: public NonlinearFactor2 { private: - typedef typename POSE Pose; + typedef POSE Pose; typedef typename Pose::Rotation Rot; - typedef typename POINT Point; + typedef POINT Point; typedef BearingFactor This; typedef NonlinearFactor2 Base; diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index a81896b94..4e442ad86 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -31,9 +31,9 @@ namespace gtsam { class BearingRangeFactor: public NonlinearFactor2 { private: - typedef typename POSE Pose; + typedef POSE Pose; typedef typename POSE::Rotation Rot; - typedef typename POINT Point; + typedef POINT Point; typedef BearingRangeFactor This; typedef NonlinearFactor2 Base; @@ -60,9 +60,9 @@ namespace gtsam { /** Print */ virtual void print(const std::string& s = "") const { std::cout << s << ": BearingRangeFactor(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << ")\n"; - bearing_.print(" measured bearing"); + << (std::string) this->key1() << "," + << (std::string) this->key2() << ")\n"; + measuredBearing_.print(" measured bearing"); std::cout << " measured range: " << measuredRange_ << std::endl; this->noiseModel_->print(" noise model"); } diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 0fd8f6846..313241eee 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -26,12 +26,15 @@ namespace gtsam { /** * A class for a measurement predicted by "between(config[key1],config[key2])" - * KEY1::Value is the Lie Group type - * T is the measurement type, by default the same + * @tparam VALUE the Value type */ template class BetweenFactor: public NonlinearFactor2 { + public: + + typedef VALUE T; + private: typedef BetweenFactor This; @@ -64,8 +67,8 @@ namespace gtsam { /** print */ virtual void print(const std::string& s) const { std::cout << s << "BetweenFactor(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << ")\n"; + << (std::string) this->key1() << "," + << (std::string) this->key2() << ")\n"; measured_.print(" measured"); this->noiseModel_->print(" noise model"); } @@ -79,7 +82,7 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const typename KEY1::Value& p1, const typename KEY1::Value& p2, + Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { T hx = p1.between(p2, H1, H2); // h(x) @@ -121,7 +124,7 @@ namespace gtsam { /** Syntactic sugar for constrained version */ 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))) {} + BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {} private: diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index c6d2e2ede..60092c15d 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -37,10 +37,10 @@ namespace gtsam { public: - 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 + typedef 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; @@ -52,7 +52,8 @@ namespace gtsam { * @param i is basically the frame number * @param j is the index of the landmark */ - GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Landmark& landmarkKey) : Base(model, i, j), measured_(measured) {} + GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Symbol& landmarkKey) : + Base(model, cameraKey, cameraKey), measured_(measured) {} GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 @@ -66,14 +67,15 @@ namespace gtsam { */ void print(const std::string& s = "SFMFactor") const { Base::print(s); - z_.print(s + ".z"); + measured_.print(s + ".z"); } /** * equals */ - bool equals(const GeneralSFMFactor &p, double tol = 1e-9) const { - return Base::equals(p, tol) && this->measured_.equals(p.z_, tol) ; + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; } /** h(x)-z */ diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index a5ff16c56..43a33a9fe 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -60,7 +60,7 @@ namespace gtsam { /** print */ virtual void print(const std::string& s) const { - std::cout << s << "PriorFactor(" << (std::string) this->key_ << ")\n"; + std::cout << s << "PriorFactor(" << (std::string) this->key() << ")\n"; prior_.print(" prior"); this->noiseModel_->print(" noise model"); } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 646f2a003..f654e5d33 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -78,8 +78,8 @@ namespace gtsam { /// equals 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); + const This *e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol); } /// Evaluate error h(x)-z and optionally derivatives @@ -92,8 +92,8 @@ namespace gtsam { } catch( CheiralityException& e) { if (H1) *H1 = zeros(2,6); if (H2) *H2 = zeros(2,3); - cout << e.what() << ": Landmark "<< this->key2_.index() << - " moved behind camera " << this->key1_.index() << endl; + cout << e.what() << ": Landmark "<< this->key2().index() << + " moved behind camera " << this->key1().index() << endl; return ones(2) * 2.0 * K_->fx(); } } diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 77c11d00d..43cc3da57 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -34,8 +34,8 @@ namespace gtsam { typedef RangeFactor This; typedef NonlinearFactor2 Base; - typedef typename POSE Pose; - typedef typename POINT Point; + typedef POSE Pose; + typedef POINT Point; // Concept requirements for this factor GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point) @@ -46,7 +46,7 @@ namespace gtsam { RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured, const SharedNoiseModel& model) : - Base(model, poseKey, PointKey), measured_(measured) { + Base(model, poseKey, pointKey), measured_(measured) { } virtual ~RangeFactor() {} @@ -65,7 +65,7 @@ namespace gtsam { /** 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->measured_ - e->z_) < tol; + return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol; } /** print contents */ diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 7f1e8077d..76359d8c9 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -22,20 +22,20 @@ namespace gtsam { -template -class GenericStereoFactor: public NonlinearFactor2 { +template +class GenericStereoFactor: public NonlinearFactor2 { private: // Keep a copy of measurement and calibration for I/O - StereoPoint2 z_; ///< the measurement + StereoPoint2 measured_; ///< the measurement boost::shared_ptr K_; ///< shared pointer to calibration public: // shorthand for base class type - typedef NonlinearFactor2 Base; ///< typedef for base class + typedef NonlinearFactor2 Base; ///< typedef for base class typedef boost::shared_ptr shared_ptr; ///< typedef for shared pointer to this object - typedef typename KEY1::Value CamPose; ///< typedef for Pose Lie Value type + typedef POSE CamPose; ///< typedef for Pose Lie Value type /** * Default constructor @@ -44,18 +44,17 @@ public: /** * Constructor - * @param z is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair + * @param measured is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair * @param model is the noise model in on the measurement - * @param j_pose the pose index - * @param j_landmark the landmark index + * @param poseKey the pose variable key + * @param landmarkKey the landmark variable key * @param K the constant calibration */ - GenericStereoFactor(const StereoPoint2& z, const SharedNoiseModel& model, KEY1 j_pose, - KEY2 j_landmark, const shared_ptrKStereo& K) : - Base(model, j_pose, j_landmark), z_(z), K_(K) { + GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey, const shared_ptrKStereo& K) : + Base(model, poseKey, landmarkKey), measured_(measured), K_(K) { } - ~GenericStereoFactor() {} ///< desctructor + ~GenericStereoFactor() {} ///< destructor /** * print @@ -63,41 +62,27 @@ public: */ void print(const std::string& s) const { Base::print(s); - z_.print(s + ".z"); + measured_.print(s + ".z"); } /** * equals */ - bool equals(const GenericStereoFactor& f, double tol) const { - const GenericStereoFactor* p = dynamic_cast (&f); - if (p == NULL) - goto fail; - //if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail; - if (!z_.equals(p->z_, tol)) - goto fail; - return true; - - fail: print("actual"); - p->print("expected"); - return false; + virtual bool equals(const NonlinearFactor& f, double tol) const { + const GenericStereoFactor* p = dynamic_cast (&f); + return p && Base::equals(f) && measured_.equals(p->measured_, tol); } /** h(x)-z */ Vector evaluateError(const Pose3& pose, const Point3& point, boost::optional H1, boost::optional H2) const { StereoCamera stereoCam(pose, K_); - return (stereoCam.project(point, H1, H2) - z_).vector(); - } - - /// get the measurement z - StereoPoint2 z() { - return z_; + return (stereoCam.project(point, H1, H2) - measured_).vector(); } /** return the measured */ - inline const StereoPoint2 measured() const { - return z_; + const StereoPoint2& measured() const { + return measured_; } private: @@ -105,7 +90,7 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); } }; diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 73fee971e..3f0551863 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -133,9 +133,9 @@ pair load2D(const string& filename, // Insert vertices if pure odometry file if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2()); - if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at(pose2SLAM::PoseKey(id1)) * l1Xl2); + if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at(pose2SLAM::PoseKey(id1)) * l1Xl2); - pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model)); + pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(pose2SLAM::PoseKey(id1), pose2SLAM::PoseKey(id2), l1Xl2, *model)); graph->push_back(factor); } is.ignore(LINESIZE, '\n'); diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 3ff8fa99c..2b9a3db1d 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -39,7 +39,7 @@ namespace gtsam { Point3 gti(radius*cos(theta), radius*sin(theta), 0); Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down ! Pose3 gTi(gR0 * _0Ri, gti); - x.insert(Key(i), gTi); + x.insert(PoseKey(i), gTi); } return x; } diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index ca78bb334..bb9f143c5 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -57,14 +57,14 @@ namespace simulated2D { } /// Insert a pose - void insertPose(const simulated2D::PoseKey& i, const Point2& p) { - insert(i, p); + void insertPose(Index j, const Point2& p) { + insert(PoseKey(j), p); nrPoses_++; } /// Insert a point - void insertPoint(const simulated2D::PointKey& j, const Point2& p) { - insert(j, p); + void insertPoint(Index j, const Point2& p) { + insert(PointKey(j), p); nrPoints_++; } @@ -79,13 +79,13 @@ namespace simulated2D { } /// Return pose i - Point2 pose(const simulated2D::PoseKey& i) const { - return (*this)[i]; + Point2 pose(Index j) const { + return at(PoseKey(j)); } /// Return point j - Point2 point(const simulated2D::PointKey& j) const { - return (*this)[j]; + Point2 point(Index j) const { + return at(PointKey(j)); } }; @@ -156,7 +156,7 @@ namespace simulated2D { /** * Binary factor simulating "odometry" between two Vectors */ - template + template class GenericOdometry: public NonlinearFactor2 { public: typedef NonlinearFactor2 Base; ///< base class @@ -203,7 +203,7 @@ namespace simulated2D { typedef POSE Pose; ///< shortcut to Pose type typedef LANDMARK Landmark; ///< shortcut to Landmark type - Landmark z_; ///< Measurement + Landmark measured_; ///< Measurement /// Create measurement factor GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey) : diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index d0880b8f3..f38e9c34d 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -40,21 +40,21 @@ namespace simulated2DOriented { public: Values() : nrPoses_(0), nrPoints_(0) {} - void insertPose(const PoseKey& i, const Pose2& p) { - insert(i, p); + void insertPose(Index j, const Pose2& p) { + insert(PoseKey(j), p); nrPoses_++; } - void insertPoint(const PointKey& j, const Point2& p) { - insert(j, p); + void insertPoint(Index j, const Point2& p) { + insert(PointKey(j), p); nrPoints_++; } int nrPoses() const { return nrPoses_; } int nrPoints() const { return nrPoints_; } - Pose2 pose(const PoseKey& i) const { return (*this)[i]; } - Point2 point(const PointKey& j) const { return (*this)[j]; } + Pose2 pose(Index j) const { return at(PoseKey(j)); } + Point2 point(Index j) const { return at(PointKey(j)); } }; //TODO:: point prior is not implemented right now @@ -111,7 +111,7 @@ namespace simulated2DOriented { } /// Evaluate error and optionally derivative - Vector evaluateError(const Pose2& x1, const Pose2& x2, + Vector evaluateError(const VALUE& x1, const VALUE& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { return measured_.localCoordinates(odo(x1, x2, H1, H2)); diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h index 4a24bcaa6..40adcdbe2 100644 --- a/gtsam/slam/simulated3D.h +++ b/gtsam/slam/simulated3D.h @@ -36,8 +36,11 @@ namespace simulated3D { * the simulated2D domain. */ -typedef gtsam::TypedSymbol PoseKey; -typedef gtsam::TypedSymbol PointKey; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } + + /// Convenience function for constructing a pose key + inline Symbol PointKey(Index j) { return Symbol('l', j); } /** * Prior on a single pose @@ -61,19 +64,18 @@ Point3 mea(const Point3& x, const Point3& l, /** * A prior factor on a single linear robot pose */ -struct PointPrior3D: public NonlinearFactor1 { +struct PointPrior3D: public NonlinearFactor1 { - Point3 z_; ///< The prior pose value for the variable attached to this factor + Point3 measured_; ///< The prior pose value for the variable attached to this factor /** * Constructor for a prior factor - * @param z is the measured/prior position for the pose + * @param measured is the measured/prior position for the pose * @param model is the measurement model for the factor (Dimension: 3) - * @param j is the key for the pose + * @param key is the key for the pose */ - PointPrior3D(const Point3& z, - const SharedNoiseModel& model, const PoseKey& j) : - NonlinearFactor1 (model, j), z_(z) { + PointPrior3D(const Point3& measured, const SharedNoiseModel& model, const Symbol& key) : + NonlinearFactor1 (model, key), measured_(measured) { } /** @@ -85,29 +87,27 @@ struct PointPrior3D: public NonlinearFactor1 { */ Vector evaluateError(const Point3& x, boost::optional H = boost::none) { - return (prior(x, H) - z_).vector(); + return (prior(x, H) - measured_).vector(); } }; /** * Models a linear 3D measurement between 3D points */ -struct Simulated3DMeasurement: public NonlinearFactor2 { +struct Simulated3DMeasurement: public NonlinearFactor2 { - Point3 z_; ///< Linear displacement between a pose and landmark + Point3 measured_; ///< Linear displacement between a pose and landmark /** * Creates a measurement factor with a given measurement - * @param z is the measurement, a linear displacement between poses and landmarks + * @param measured is the measurement, a linear displacement between poses and landmarks * @param model is a measurement model for the factor (Dimension: 3) - * @param j1 is the pose key of the robot - * @param j2 is the point key for the landmark + * @param poseKey is the pose key of the robot + * @param pointKey is the point key for the landmark */ - Simulated3DMeasurement(const Point3& z, - const SharedNoiseModel& model, PoseKey& j1, PointKey j2) : - NonlinearFactor2 ( - model, j1, j2), z_(z) { - } + Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, + const Symbol& poseKey, const Symbol& pointKey) : + NonlinearFactor2(model, poseKey, pointKey), measured_(measured) {} /** * Error function with optional derivatives @@ -117,9 +117,9 @@ struct Simulated3DMeasurement: public NonlinearFactor2 { * @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3) * @return vector error between measurement and prediction (Dimension: 3) */ - Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional< - Matrix&> H1 = boost::none, boost::optional H2 = boost::none) { - return (mea(x1, x2, H1, H2) - z_).vector(); + Vector evaluateError(const Point3& x1, const Point3& x2, + boost::optional H1 = boost::none, boost::optional H2 = boost::none) { + return (mea(x1, x2, H1, H2) - measured_).vector(); } }; diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 948616ea6..8ee8cb09a 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -39,7 +39,10 @@ using namespace std; namespace gtsam { namespace example { - typedef boost::shared_ptr shared; + using simulated2D::PoseKey; + using simulated2D::PointKey; + + typedef boost::shared_ptr shared; static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); @@ -57,22 +60,22 @@ namespace example { // prior on x1 Point2 mu; - shared f1(new simulated2D::Prior(mu, sigma0_1, 1)); + shared f1(new simulated2D::Prior(mu, sigma0_1, PoseKey(1))); nlfg->push_back(f1); // odometry between x1 and x2 Point2 z2(1.5, 0); - shared f2(new simulated2D::Odometry(z2, sigma0_1, 1, 2)); + shared f2(new simulated2D::Odometry(z2, sigma0_1, PoseKey(1), PoseKey(2))); nlfg->push_back(f2); // measurement between x1 and l1 Point2 z3(0, -1); - shared f3(new simulated2D::Measurement(z3, sigma0_2, 1, 1)); + shared f3(new simulated2D::Measurement(z3, sigma0_2, PoseKey(1), PointKey(1))); nlfg->push_back(f3); // measurement between x2 and l1 Point2 z4(-1.5, -1.); - shared f4(new simulated2D::Measurement(z4, sigma0_2, 2, 1)); + shared f4(new simulated2D::Measurement(z4, sigma0_2, PoseKey(2), PointKey(1))); nlfg->push_back(f4); return nlfg; @@ -86,9 +89,9 @@ namespace example { /* ************************************************************************* */ Values createValues() { Values c; - c.insert(simulated2D::PoseKey(1), Point2(0.0, 0.0)); - c.insert(simulated2D::PoseKey(2), Point2(1.5, 0.0)); - c.insert(simulated2D::PointKey(1), Point2(0.0, -1.0)); + c.insert(PoseKey(1), Point2(0.0, 0.0)); + c.insert(PoseKey(2), Point2(1.5, 0.0)); + c.insert(PointKey(1), Point2(0.0, -1.0)); return c; } @@ -104,9 +107,9 @@ namespace example { /* ************************************************************************* */ boost::shared_ptr sharedNoisyValues() { boost::shared_ptr c(new Values); - c->insert(simulated2D::PoseKey(1), Point2(0.1, 0.1)); - c->insert(simulated2D::PoseKey(2), Point2(1.4, 0.2)); - c->insert(simulated2D::PointKey(1), Point2(0.1, -1.1)); + c->insert(PoseKey(1), Point2(0.1, 0.1)); + c->insert(PoseKey(2), Point2(1.4, 0.2)); + c->insert(PointKey(1), Point2(0.1, -1.1)); return c; } @@ -195,17 +198,15 @@ namespace example { 0.0, cos(v.y())); } - struct UnaryFactor: public gtsam::NonlinearFactor1 { + struct UnaryFactor: public gtsam::NonlinearFactor1 { Point2 z_; - UnaryFactor(const Point2& z, const SharedNoiseModel& model, - const simulated2D::PoseKey& key) : - gtsam::NonlinearFactor1(model, key), z_(z) { + UnaryFactor(const Point2& z, const SharedNoiseModel& model, const Symbol& key) : + gtsam::NonlinearFactor1(model, key), z_(z) { } - Vector evaluateError(const Point2& x, boost::optional A = - boost::none) const { + Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { if (A) *A = H(x); return (h(x) - z_).vector(); } @@ -220,7 +221,7 @@ namespace example { Vector z = Vector_(2, 1.0, 0.0); double sigma = 0.1; boost::shared_ptr factor( - new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), 1)); + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), PoseKey(1))); fg->push_back(factor); return fg; } @@ -238,23 +239,23 @@ namespace example { // prior on x1 Point2 x1(1.0, 0.0); - shared prior(new simulated2D::Prior(x1, sigma1_0, 1)); + shared prior(new simulated2D::Prior(x1, sigma1_0, PoseKey(1))); nlfg.push_back(prior); poses.insert(simulated2D::PoseKey(1), x1); for (int t = 2; t <= T; t++) { // odometry between x_t and x_{t-1} Point2 odo(1.0, 0.0); - shared odometry(new simulated2D::Odometry(odo, sigma1_0, t - 1, t)); + shared odometry(new simulated2D::Odometry(odo, sigma1_0, PoseKey(t - 1), PoseKey(t))); nlfg.push_back(odometry); // measurement on x_t is like perfect GPS Point2 xt(t, 0); - shared measurement(new simulated2D::Prior(xt, sigma1_0, t)); + shared measurement(new simulated2D::Prior(xt, sigma1_0, PoseKey(t))); nlfg.push_back(measurement); // initial estimate - poses.insert(simulated2D::PoseKey(t), xt); + poses.insert(PoseKey(t), xt); } return make_pair(nlfg, poses); @@ -414,8 +415,8 @@ namespace example { /* ************************************************************************* */ // Create key for simulated planar graph - simulated2D::PoseKey key(int x, int y) { - return simulated2D::PoseKey(1000*x+y); + Symbol key(int x, int y) { + return PoseKey(1000*x+y); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 99be5d09a..183b0c600 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -27,6 +27,8 @@ using namespace std; using namespace gtsam; +using pose3SLAM::PoseKey; + /* ************************************************************************* */ TEST( AntiFactor, NegativeHessian) { @@ -40,17 +42,17 @@ TEST( AntiFactor, NegativeHessian) // Create a configuration corresponding to the ground truth boost::shared_ptr values(new Values()); - values->insert(pose3SLAM::Key(1), pose1); - values->insert(pose3SLAM::Key(2), pose2); + values->insert(PoseKey(1), pose1); + values->insert(PoseKey(2), pose2); // Define an elimination ordering Ordering::shared_ptr ordering(new Ordering()); - ordering->insert(pose3SLAM::Key(1), 0); - ordering->insert(pose3SLAM::Key(2), 1); + ordering->insert(PoseKey(1), 0); + ordering->insert(PoseKey(2), 1); // Create a "standard" factor - BetweenFactor::shared_ptr originalFactor(new BetweenFactor(1, 2, z, sigma)); + BetweenFactor::shared_ptr originalFactor(new BetweenFactor(PoseKey(1), PoseKey(2), z, sigma)); // Linearize it into a Jacobian Factor GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); @@ -101,8 +103,8 @@ TEST( AntiFactor, EquivalentBayesNet) // Create a configuration corresponding to the ground truth boost::shared_ptr values(new Values()); - values->insert(pose3SLAM::Key(1), pose1); - values->insert(pose3SLAM::Key(2), pose2); + values->insert(PoseKey(1), pose1); + values->insert(PoseKey(2), pose2); // Define an elimination ordering Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); @@ -115,7 +117,7 @@ TEST( AntiFactor, EquivalentBayesNet) VectorValues expectedDeltas = optimize(*expectedBayesNet); // Add an additional factor between Pose1 and Pose2 - pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(1, 2, z, sigma)); + pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(PoseKey(1), PoseKey(2), z, sigma)); graph->push_back(f1); // Add the corresponding AntiFactor between Pose1 and Pose2 diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index e887cac06..022a76960 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -28,25 +28,23 @@ using namespace std; using namespace gtsam; typedef PinholeCamera GeneralCamera; -typedef TypedSymbol CameraKey; -typedef TypedSymbol PointKey; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared(z, model, i, j)); + void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { + push_back(boost::make_shared(z, model, Symbol('x',i), Symbol('l',j))); } void addCameraConstraint(int j, const GeneralCamera& p) { - boost::shared_ptr factor(new CameraConstraint(j, p)); + boost::shared_ptr factor(new CameraConstraint(Symbol('x',j), p)); push_back(factor); } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(j, p)); + boost::shared_ptr factor(new Point3Constraint(Symbol('x',j), p)); push_back(factor); } @@ -76,7 +74,7 @@ TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); - const int cameraFrameNumber=1, landmarkNumber=1; + const Symbol cameraFrameNumber="x1", landmarkNumber="l1"; const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); @@ -90,17 +88,16 @@ TEST( GeneralSFMFactor, equals ) /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); - const int cameraFrameNumber=1, landmarkNumber=1; const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr - factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + factor(new Projection(z, sigma, "x1", "l1")); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); - values.insert(CameraKey(1), GeneralCamera(x1)); - Point3 l1; values.insert(PointKey(1), l1); + values.insert("x1", GeneralCamera(x1)); + Point3 l1; values.insert("l1", l1); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 5a626d095..7c1d395ba 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -28,26 +28,24 @@ using namespace std; using namespace gtsam; typedef PinholeCamera GeneralCamera; -typedef TypedSymbol CameraKey; -typedef TypedSymbol PointKey; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; /* ************************************************************************* */ class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared(z, model, i, j)); + void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { + push_back(boost::make_shared(z, model, Symbol('x',i), Symbol('l',j))); } void addCameraConstraint(int j, const GeneralCamera& p) { - boost::shared_ptr factor(new CameraConstraint(j, p)); + boost::shared_ptr factor(new CameraConstraint(Symbol('x', j), p)); push_back(factor); } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(j, p)); + boost::shared_ptr factor(new Point3Constraint(Symbol('l', j), p)); push_back(factor); } @@ -77,7 +75,7 @@ TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); - const int cameraFrameNumber=1, landmarkNumber=1; + const Symbol cameraFrameNumber="x1", landmarkNumber="l1"; const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); @@ -91,17 +89,16 @@ TEST( GeneralSFMFactor, equals ) /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); - const int cameraFrameNumber=1, landmarkNumber=1; const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr - factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + factor(new Projection(z, sigma, "x1", "l1")); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); - values.insert(CameraKey(1), GeneralCamera(x1)); - Point3 l1; values.insert(PointKey(1), l1); + values.insert("x1", GeneralCamera(x1)); + Point3 l1; values.insert("l1", l1); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } @@ -171,13 +168,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index 56ac71923..3daf5f4ab 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -37,7 +37,7 @@ SharedNoiseModel /* ************************************************************************* */ TEST( planarSLAM, PriorFactor_equals ) { - planarSLAM::Prior factor1(2, x1, I3), factor2(2, x2, I3); + planarSLAM::Prior factor1(PoseKey(2), x1, I3), factor2(PoseKey(2), x2, I3); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -48,7 +48,7 @@ TEST( planarSLAM, BearingFactor ) { // Create factor Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - planarSLAM::Bearing factor(2, 3, z, sigma); + planarSLAM::Bearing factor(PoseKey(2), PointKey(3), z, sigma); // create config planarSLAM::Values c; @@ -64,8 +64,8 @@ TEST( planarSLAM, BearingFactor ) TEST( planarSLAM, BearingFactor_equals ) { planarSLAM::Bearing - factor1(2, 3, Rot2::fromAngle(0.1), sigma), - factor2(2, 3, Rot2::fromAngle(2.3), sigma); + factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), sigma), + factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(2.3), sigma); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -76,7 +76,7 @@ TEST( planarSLAM, RangeFactor ) { // Create factor double z(sqrt(2) - 0.22); // h(x) - z = 0.22 - planarSLAM::Range factor(2, 3, z, sigma); + planarSLAM::Range factor(PoseKey(2), PointKey(3), z, sigma); // create config planarSLAM::Values c; @@ -91,7 +91,7 @@ TEST( planarSLAM, RangeFactor ) /* ************************************************************************* */ TEST( planarSLAM, RangeFactor_equals ) { - planarSLAM::Range factor1(2, 3, 1.2, sigma), factor2(2, 3, 7.2, sigma); + planarSLAM::Range factor1(PoseKey(2), PointKey(3), 1.2, sigma), factor2(PoseKey(2), PointKey(3), 7.2, sigma); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -103,7 +103,7 @@ TEST( planarSLAM, BearingRangeFactor ) // Create factor Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 double b(sqrt(2) - 0.22); // h(x) - z = 0.22 - planarSLAM::BearingRange factor(2, 3, r, b, sigma2); + planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2); // create config planarSLAM::Values c; @@ -119,8 +119,8 @@ TEST( planarSLAM, BearingRangeFactor ) TEST( planarSLAM, BearingRangeFactor_equals ) { planarSLAM::BearingRange - factor1(2, 3, Rot2::fromAngle(0.1), 7.3, sigma2), - factor2(2, 3, Rot2::fromAngle(3), 2.0, sigma2); + factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), 7.3, sigma2), + factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(3), 2.0, sigma2); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -129,7 +129,7 @@ TEST( planarSLAM, BearingRangeFactor_equals ) /* ************************************************************************* */ TEST( planarSLAM, PoseConstraint_equals ) { - planarSLAM::Constraint factor1(2, x2), factor2(2, x3); + planarSLAM::Constraint factor1(PoseKey(2), x2), factor2(PoseKey(2), x3); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 73fec2a29..29c28dbc1 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -34,6 +34,7 @@ using namespace boost::assign; using namespace std; typedef pose2SLAM::Odometry Pose2Factor; +using pose2SLAM::PoseKey; // common measurement covariance static double sx=0.5, sy=0.5,st=0.1; @@ -49,7 +50,7 @@ static noiseModel::Gaussian::shared_ptr covariance( // Test constraint, small displacement Vector f1(const Pose2& pose1, const Pose2& pose2) { Pose2 z(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, z, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance); return constraint.evaluateError(pose1, pose2); } @@ -57,7 +58,7 @@ TEST( Pose2SLAM, constraint1 ) { // create a factor between unknown poses p1 and p2 Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, pose2, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance); Matrix H1, H2; Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); @@ -72,7 +73,7 @@ TEST( Pose2SLAM, constraint1 ) // Test constraint, large displacement Vector f2(const Pose2& pose1, const Pose2& pose2) { Pose2 z(2,2,M_PI_2); - Pose2Factor constraint(1, 2, z, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance); return constraint.evaluateError(pose1, pose2); } @@ -80,7 +81,7 @@ TEST( Pose2SLAM, constraint2 ) { // create a factor between unknown poses p1 and p2 Pose2 pose1, pose2(2,2,M_PI_2); - Pose2Factor constraint(1, 2, pose2, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance); Matrix H1, H2; Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); @@ -110,7 +111,7 @@ TEST( Pose2SLAM, linearization ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); - Pose2Factor constraint(1,2,measured, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance); pose2SLAM::Graph graph; graph.addOdometry(1,2,measured, covariance); @@ -178,8 +179,8 @@ TEST(Pose2Graph, optimize) { TEST(Pose2Graph, optimizeThreePoses) { // Create a hexagon of poses - Values hexagon = pose2SLAM::circle(3,1.0); - Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)]; + pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0); + Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new pose2SLAM::Graph); @@ -190,10 +191,10 @@ TEST(Pose2Graph, optimizeThreePoses) { fg->addOdometry(2, 0, delta, covariance); // Create initial config - boost::shared_ptr initial(new Values()); - initial->insert(pose2SLAM::PoseKey(0), p0); - initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1))); + boost::shared_ptr initial(new pose2SLAM::Values()); + initial->insertPose(0, p0); + initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); // Choose an ordering shared_ptr ordering(new Ordering); @@ -207,7 +208,7 @@ TEST(Pose2Graph, optimizeThreePoses) { Values actual = *optimizer.values(); // Check with ground truth - CHECK(assert_equal(hexagon, actual)); + CHECK(assert_equal((const Values&)hexagon, actual)); } /* ************************************************************************* */ @@ -215,8 +216,8 @@ TEST(Pose2Graph, optimizeThreePoses) { TEST_UNSAFE(Pose2SLAM, optimizeCircle) { // Create a hexagon of poses - Values hexagon = pose2SLAM::circle(6,1.0); - Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)]; + pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0); + Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new pose2SLAM::Graph); @@ -230,13 +231,13 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { fg->addOdometry(5, 0, delta, covariance); // Create initial config - boost::shared_ptr initial(new Values()); - initial->insert(pose2SLAM::PoseKey(0), p0); - initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(pose2SLAM::PoseKey(3), hexagon[pose2SLAM::PoseKey(3)].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(pose2SLAM::PoseKey(4), hexagon[pose2SLAM::PoseKey(4)].retract(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(pose2SLAM::PoseKey(5), hexagon[pose2SLAM::PoseKey(5)].retract(Vector_(3,-0.1, 0.1,-0.1))); + boost::shared_ptr initial(new pose2SLAM::Values()); + initial->insertPose(0, p0); + initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); + initial->insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1))); + initial->insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1))); // Choose an ordering shared_ptr ordering(new Ordering); @@ -250,10 +251,10 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { Values actual = *optimizer.values(); // Check with ground truth - CHECK(assert_equal(hexagon, actual)); + CHECK(assert_equal((const Values&)hexagon, actual)); // Check loop closure - CHECK(assert_equal(delta,actual[pose2SLAM::PoseKey(5)].between(actual[pose2SLAM::PoseKey(0)]))); + CHECK(assert_equal(delta, actual.at(PoseKey(5)).between(actual.at(PoseKey(0))))); // Pose2SLAMOptimizer myOptimizer("3"); @@ -387,10 +388,10 @@ TEST( Pose2Prior, error ) // Choose a linearization point Pose2 p1(1, 0, 0); // robot at (1,0) pose2SLAM::Values x0; - x0.insert(pose2SLAM::PoseKey(1), p1); + x0.insert(PoseKey(1), p1); // Create factor - pose2SLAM::Prior factor(1, p1, sigmas); + pose2SLAM::Prior factor(PoseKey(1), p1, sigmas); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -417,7 +418,7 @@ TEST( Pose2Prior, error ) /* ************************************************************************* */ // common Pose2Prior for tests below static gtsam::Pose2 priorVal(2,2,M_PI_2); -static pose2SLAM::Prior priorFactor(1,priorVal, sigmas); +static pose2SLAM::Prior priorFactor(PoseKey(1), priorVal, sigmas); /* ************************************************************************* */ // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector @@ -431,7 +432,7 @@ TEST( Pose2Prior, linearize ) { // Choose a linearization point at ground truth pose2SLAM::Values x0; - x0.insert(pose2SLAM::PoseKey(1),priorVal); + x0.insertPose(1, priorVal); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -451,12 +452,12 @@ TEST( Pose2Factor, error ) Pose2 p1; // robot at origin Pose2 p2(1, 0, 0); // robot at (1,0) pose2SLAM::Values x0; - x0.insert(pose2SLAM::PoseKey(1), p1); - x0.insert(pose2SLAM::PoseKey(2), p2); + x0.insertPose(1, p1); + x0.insertPose(2, p2); // Create factor Pose2 z = p1.between(p2); - Pose2Factor factor(1, 2, z, covariance); + Pose2Factor factor(PoseKey(1), PoseKey(2), z, covariance); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -483,7 +484,7 @@ TEST( Pose2Factor, error ) /* ************************************************************************* */ // common Pose2Factor for tests below static Pose2 measured(2,2,M_PI_2); -static Pose2Factor factor(1,2,measured, covariance); +static Pose2Factor factor(PoseKey(1),PoseKey(2),measured, covariance); /* ************************************************************************* */ TEST( Pose2Factor, rhs ) diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 79cc9fe1e..a9b743145 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -50,7 +50,7 @@ TEST(Pose3Graph, optimizeCircle) { // Create a hexagon of poses double radius = 10; Values hexagon = pose3SLAM::circle(6,radius); - Pose3 gT0 = hexagon[Key(0)], gT1 = hexagon[Key(1)]; + Pose3 gT0 = hexagon[PoseKey(0)], gT1 = hexagon[PoseKey(1)]; // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose3Graph); @@ -67,12 +67,12 @@ TEST(Pose3Graph, optimizeCircle) { // Create initial config boost::shared_ptr initial(new Values()); - initial->insert(Key(0), gT0); - initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert(Key(3), hexagon[Key(3)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert(Key(4), hexagon[Key(4)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert(Key(5), hexagon[Key(5)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(PoseKey(0), gT0); + initial->insert(PoseKey(1), hexagon.at(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(PoseKey(2), hexagon.at(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(PoseKey(3), hexagon.at(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(PoseKey(4), hexagon.at(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(PoseKey(5), hexagon.at(PoseKey(5)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); // Choose an ordering and optimize shared_ptr ordering(new Ordering); @@ -87,17 +87,17 @@ TEST(Pose3Graph, optimizeCircle) { CHECK(assert_equal(hexagon, actual,1e-4)); // Check loop closure - CHECK(assert_equal(_0T1,actual[Key(5)].between(actual[Key(0)]),1e-5)); + CHECK(assert_equal(_0T1, actual.at(PoseKey(5)).between(actual.at(PoseKey(0))),1e-5)); } /* ************************************************************************* */ TEST(Pose3Graph, partial_prior_height) { - typedef PartialPriorFactor Partial; + typedef PartialPriorFactor Partial; // reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3) // height prior - single element interface - pose3SLAM::Key key(1); + Symbol key = PoseKey(1); double exp_height = 5.0; SharedDiagonal model = noiseModel::Unit::Create(1); Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height)); @@ -117,7 +117,7 @@ TEST(Pose3Graph, partial_prior_height) { EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); - EXPECT(assert_equal(expected, actual[key], tol)); + EXPECT(assert_equal(expected, actual.at(key), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 1d8616c30..f7f804fb9 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -49,7 +49,7 @@ TEST( ProjectionFactor, error ) Point2 z(323.,240.); int cameraFrameNumber=1, landmarkNumber=1; boost::shared_ptr - factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + factor(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); // For the following values structure, the factor predicts 320,240 Values config; @@ -98,10 +98,10 @@ TEST( ProjectionFactor, equals ) Vector z = Vector_(2,323.,240.); int cameraFrameNumber=1, landmarkNumber=1; boost::shared_ptr - factor1(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + factor1(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); boost::shared_ptr - factor2(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + factor2(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); CHECK(assert_equal(*factor1, *factor2)); } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 94a4d4060..fc026fb35 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -52,11 +52,11 @@ TEST( StereoFactor, singlePoint) boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); boost::shared_ptr graph(new visualSLAM::Graph()); - graph->add(visualSLAM::PoseConstraint(1,camera1)); + graph->add(visualSLAM::PoseConstraint(PoseKey(1),camera1)); StereoPoint2 z14(320,320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K)); + graph->add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K)); // Create a configuration corresponding to the ground truth boost::shared_ptr values(new Values()); diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index c1a9123d9..4ecbf2e43 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -34,22 +34,24 @@ namespace visualSLAM { using namespace gtsam; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } + + /// Convenience function for constructing a pose key + inline Symbol PointKey(Index j) { return Symbol('l', j); } + /** * Typedefs that make up the visualSLAM namespace. */ - typedef TypedSymbol PoseKey; ///< The key type used for poses - typedef TypedSymbol PointKey; ///< The key type used for points - typedef boost::shared_ptr shared_values; ///< shared pointer to values data structure - - typedef NonlinearEquality PoseConstraint; ///< put a hard constraint on a pose - typedef NonlinearEquality PointConstraint; ///< put a hard constraint on a point - typedef PriorFactor PosePrior; ///< put a soft prior on a Pose - typedef PriorFactor PointPrior; ///< put a soft prior on a point - typedef RangeFactor RangeFactor; ///< put a factor on the range from a pose to a point + typedef NonlinearEquality PoseConstraint; ///< put a hard constraint on a pose + typedef NonlinearEquality PointConstraint; ///< put a hard constraint on a point + typedef PriorFactor PosePrior; ///< put a soft prior on a Pose + typedef PriorFactor PointPrior; ///< put a soft prior on a point + typedef RangeFactor RangeFactor; ///< put a factor on the range from a pose to a point /// monocular and stereo camera typedefs for general use - typedef GenericProjectionFactor ProjectionFactor; - typedef GenericStereoFactor StereoFactor; + typedef GenericProjectionFactor ProjectionFactor; + typedef GenericStereoFactor StereoFactor; /** * Non-linear factor graph for vanilla visual SLAM @@ -76,69 +78,69 @@ namespace visualSLAM { /** * Add a projection factor measurement (monocular) - * @param z the measurement + * @param measured the measurement * @param model the noise model for the measurement - * @param i index of camera - * @param j index of point + * @param poseKey variable key for the camera pose + * @param pointKey variable key for the landmark * @param K shared pointer to calibration object */ - void addMeasurement(const Point2& z, const SharedNoiseModel& model, - PoseKey i, PointKey j, const shared_ptrK& K) { - boost::shared_ptr factor(new ProjectionFactor(z, model, i, j, K)); + void addMeasurement(const Point2& measured, const SharedNoiseModel& model, + const Symbol& poseKey, const Symbol& pointKey, const shared_ptrK& K) { + boost::shared_ptr factor(new ProjectionFactor(measured, model, poseKey, pointKey, K)); push_back(factor); } /** * Add a constraint on a pose (for now, *must* be satisfied in any Values) - * @param j index of camera + * @param key variable key of the camera pose * @param p to which pose to constrain it to */ - void addPoseConstraint(int j, const Pose3& p = Pose3()) { - boost::shared_ptr factor(new PoseConstraint(j, p)); + void addPoseConstraint(const Symbol& key, const Pose3& p = Pose3()) { + boost::shared_ptr factor(new PoseConstraint(key, p)); push_back(factor); } /** * Add a constraint on a point (for now, *must* be satisfied in any Values) - * @param j index of landmark + * @param key variable key of the landmark * @param p point around which soft prior is defined */ - void addPointConstraint(int j, const Point3& p = Point3()) { - boost::shared_ptr factor(new PointConstraint(j, p)); + void addPointConstraint(const Symbol& key, const Point3& p = Point3()) { + boost::shared_ptr factor(new PointConstraint(key, p)); push_back(factor); } /** * Add a prior on a pose - * @param j index of camera + * @param key variable key of the camera pose * @param p around which soft prior is defined * @param model uncertainty model of this prior */ - void addPosePrior(int j, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { - boost::shared_ptr factor(new PosePrior(j, p, model)); + void addPosePrior(const Symbol& key, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { + boost::shared_ptr factor(new PosePrior(key, p, model)); push_back(factor); } /** * Add a prior on a landmark - * @param j index of landmark + * @param key variable key of the landmark * @param p to which point to constrain it to * @param model uncertainty model of this prior */ - void addPointPrior(int j, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { - boost::shared_ptr factor(new PointPrior(j, p, model)); + void addPointPrior(const Symbol& key, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { + boost::shared_ptr factor(new PointPrior(key, p, model)); push_back(factor); } /** * Add a range prior to a landmark - * @param i index of pose - * @param j index of landmark + * @param poseKey variable key of the camera pose + * @param pointKey variable key of the landmark * @param range approximate range to landmark * @param model uncertainty model of this prior */ - void addRangeFactor(PoseKey i, PointKey j, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { - push_back(boost::shared_ptr(new RangeFactor(i, j, range, model))); + void addRangeFactor(const Symbol& poseKey, const Symbol& pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { + push_back(boost::shared_ptr(new RangeFactor(poseKey, pointKey, range, model))); } }; // Graph From 2f7f535f347bd6cf41f27df55731968ea7c6f3b4 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 7 Feb 2012 04:02:20 +0000 Subject: [PATCH 5/7] All unit tests pass with nonlinear factors templated on value instead of key --- examples/CameraResectioning.cpp | 13 ++-- examples/PlanarSLAMExample_easy.cpp | 29 ++++---- examples/PlanarSLAMSelfContained_advanced.cpp | 25 +++---- examples/Pose2SLAMExample_advanced.cpp | 19 ++--- examples/Pose2SLAMExample_easy.cpp | 14 ++-- examples/SimpleRotation.cpp | 35 ++------- examples/easyPoint2KalmanFilter.cpp | 21 +++--- examples/elaboratePoint2KalmanFilter.cpp | 36 +++++---- gtsam/inference/graph-inl.h | 2 +- gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 26 +++---- gtsam/nonlinear/ExtendedKalmanFilter.h | 12 +-- gtsam/nonlinear/NonlinearEquality.h | 2 +- gtsam/slam/BoundingConstraint.h | 16 ++-- gtsam/slam/GeneralSFMFactor.h | 2 +- gtsam/slam/planarSLAM.cpp | 2 +- gtsam/slam/planarSLAM.h | 2 +- gtsam/slam/simulated2DConstraints.h | 2 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 29 ++++---- .../testGeneralSFMFactor_Cal3Bundler.cpp | 20 ++--- gtsam/slam/tests/testPose3SLAM.cpp | 28 +++---- gtsam/slam/visualSLAM.h | 24 +++--- tests/testBoundingConstraint.cpp | 15 ++-- tests/testExtendedKalmanFilter.cpp | 73 +++++++++---------- tests/testGaussianISAM2.cpp | 20 ++--- tests/testGaussianJunctionTree.cpp | 6 +- tests/testGraph.cpp | 12 +-- tests/testInference.cpp | 12 +-- tests/testNonlinearEquality.cpp | 49 ++++++------- tests/testNonlinearFactor.cpp | 15 ++-- tests/testNonlinearISAM.cpp | 17 ++--- tests/testRot3Optimization.cpp | 13 ++-- tests/testSerialization.cpp | 14 ++-- 32 files changed, 283 insertions(+), 322 deletions(-) diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 511e7eb3f..957931af4 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -26,28 +26,25 @@ using namespace gtsam; -typedef TypedSymbol PoseKey; - /** * Unary factor for the pose. */ -class ResectioningFactor: public NonlinearFactor1 { - typedef NonlinearFactor1 Base; +class ResectioningFactor: public NonlinearFactor1 { + typedef NonlinearFactor1 Base; shared_ptrK K_; // camera's intrinsic parameters Point3 P_; // 3D point on the calibration rig Point2 p_; // 2D measurement of the 3D point public: - ResectioningFactor(const SharedNoiseModel& model, const PoseKey& key, + ResectioningFactor(const SharedNoiseModel& model, const Symbol& key, const shared_ptrK& calib, const Point2& p, const Point3& P) : Base(model, key), K_(calib), P_(P), p_(p) { } virtual ~ResectioningFactor() {} - virtual Vector evaluateError(const Pose3& X, boost::optional H = - boost::none) const { + virtual Vector evaluateError(const Pose3& X, boost::optional H = boost::none) const { SimpleCamera camera(*K_, X); Point2 reprojectionError(camera.project(P_, H) - p_); return reprojectionError.vector(); @@ -69,7 +66,7 @@ int main(int argc, char* argv[]) { /* create keys for variables */ // we have only 1 variable to solve: the camera pose - PoseKey X(1); + Symbol X('x',1); /* 1. create graph */ NonlinearFactorGraph graph; diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index d3aecb4b6..1ff3a8677 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -36,25 +36,22 @@ using namespace planarSLAM; * - Landmarks are 2 meters away from the robot trajectory */ int main(int argc, char** argv) { - // create keys for variables - PoseKey x1(1), x2(2), x3(3); - PointKey l1(1), l2(2); - // create graph container and add factors to it + // create graph container and add factors to it Graph graph; /* add prior */ // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph + graph.addPrior(1, prior_measurement, prior_model); // add directly to graph /* add odometry */ // general noisemodel for odometry SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph.addOdometry(x1, x2, odom_measurement, odom_model); - graph.addOdometry(x2, x3, odom_measurement, odom_model); + graph.addOdometry(1, 2, odom_measurement, odom_model); + graph.addOdometry(2, 3, odom_measurement, odom_model); /* add measurements */ // general noisemodel for measurements @@ -69,24 +66,24 @@ int main(int argc, char** argv) { range32 = 2.0; // create bearing/range factors and add them - graph.addBearingRange(x1, l1, bearing11, range11, meas_model); - graph.addBearingRange(x2, l1, bearing21, range21, meas_model); - graph.addBearingRange(x3, l2, bearing32, range32, meas_model); + graph.addBearingRange(1, 1, bearing11, range11, meas_model); + graph.addBearingRange(2, 1, bearing21, range21, meas_model); + graph.addBearingRange(3, 2, bearing32, range32, meas_model); graph.print("full graph"); // initialize to noisy points planarSLAM::Values initialEstimate; - initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); - initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.insert(l1, Point2(1.8, 2.1)); - initialEstimate.insert(l2, Point2(4.1, 1.8)); + initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); + initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); + initialEstimate.insertPoint(1, Point2(1.8, 2.1)); + initialEstimate.insertPoint(2, Point2(4.1, 1.8)); initialEstimate.print("initial estimate"); // optimize using Levenberg-Marquardt optimization with an ordering from colamd - planarSLAM::Values result = optimize(graph, initialEstimate); + planarSLAM::Values result = optimize(graph, initialEstimate); result.print("final result"); return 0; diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index fd6d2cbfe..9dc908bc0 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -40,11 +40,8 @@ #include // Main typedefs -typedef gtsam::TypedSymbol PoseKey; // Key for poses, with type included -typedef gtsam::TypedSymbol PointKey; // Key for points, with type included -typedef gtsam::NonlinearFactorGraph Graph; // graph structure -typedef gtsam::NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain -typedef gtsam::NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain +typedef gtsam::NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain +typedef gtsam::NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain using namespace std; using namespace gtsam; @@ -60,17 +57,17 @@ using namespace gtsam; */ int main(int argc, char** argv) { // create keys for variables - PoseKey x1(1), x2(2), x3(3); - PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2), x3('x',3); + Symbol l1('l',1), l2('l',2); // create graph container and add factors to it - Graph::shared_ptr graph(new Graph); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); /* add prior */ // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - PriorFactor posePrior(x1, prior_measurement, prior_model); // create the factor + PriorFactor posePrior(x1, prior_measurement, prior_model); // create the factor graph->add(posePrior); // add the factor to the graph /* add odometry */ @@ -78,8 +75,8 @@ int main(int argc, char** argv) { SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) // create between factors to represent odometry - BetweenFactor odom12(x1, x2, odom_measurement, odom_model); - BetweenFactor odom23(x2, x3, odom_measurement, odom_model); + BetweenFactor odom12(x1, x2, odom_measurement, odom_model); + BetweenFactor odom23(x2, x3, odom_measurement, odom_model); graph->add(odom12); // add both to graph graph->add(odom23); @@ -96,9 +93,9 @@ int main(int argc, char** argv) { range32 = 2.0; // create bearing/range factors - BearingRangeFactor meas11(x1, l1, bearing11, range11, meas_model); - BearingRangeFactor meas21(x2, l1, bearing21, range21, meas_model); - BearingRangeFactor meas32(x3, l2, bearing32, range32, meas_model); + BearingRangeFactor meas11(x1, l1, bearing11, range11, meas_model); + BearingRangeFactor meas21(x2, l1, bearing21, range21, meas_model); + BearingRangeFactor meas32(x3, l2, bearing32, range32, meas_model); // add the factors graph->add(meas11); diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index cab263047..1c196488f 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -33,9 +33,6 @@ using namespace boost; using namespace pose2SLAM; int main(int argc, char** argv) { - // create keys for robot positions - PoseKey x1(1), x2(2), x3(3); - /* 1. create graph container and add factors to it */ shared_ptr graph(new Graph); @@ -43,7 +40,7 @@ int main(int argc, char** argv) { // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph->addPrior(x1, prior_measurement, prior_model); // add directly to graph + graph->addPrior(1, prior_measurement, prior_model); // add directly to graph /* 2.b add odometry */ // general noisemodel for odometry @@ -51,16 +48,16 @@ int main(int argc, char** argv) { /* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph->addOdometry(x1, x2, odom_measurement, odom_model); - graph->addOdometry(x2, x3, odom_measurement, odom_model); + graph->addOdometry(1, 2, odom_measurement, odom_model); + graph->addOdometry(2, 3, odom_measurement, odom_model); graph->print("full graph"); /* 3. Create the data structure to hold the initial estimate to the solution * initialize to noisy points */ shared_ptr initial(new pose2SLAM::Values); - initial->insert(x1, Pose2(0.5, 0.0, 0.2)); - initial->insert(x2, Pose2(2.3, 0.1,-0.2)); - initial->insert(x3, Pose2(4.1, 0.1, 0.1)); + initial->insertPose(1, Pose2(0.5, 0.0, 0.2)); + initial->insertPose(2, Pose2(2.3, 0.1,-0.2)); + initial->insertPose(3, Pose2(4.1, 0.1, 0.1)); initial->print("initial estimate"); /* 4.2.1 Alternatively, you can go through the process step by step @@ -76,8 +73,8 @@ int main(int argc, char** argv) { result.print("final result"); /* Get covariances */ - Matrix covariance1 = optimizer_result.marginalCovariance(x1); - Matrix covariance2 = optimizer_result.marginalCovariance(x2); + Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1)); + Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(1)); print(covariance1, "Covariance1"); print(covariance2, "Covariance2"); diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index 43a648398..2593c043c 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -31,8 +31,6 @@ using namespace gtsam; using namespace pose2SLAM; int main(int argc, char** argv) { - // create keys for robot positions - PoseKey x1(1), x2(2), x3(3); /* 1. create graph container and add factors to it */ Graph graph ; @@ -41,7 +39,7 @@ int main(int argc, char** argv) { // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph + graph.addPrior(1, prior_measurement, prior_model); // add directly to graph /* 2.b add odometry */ // general noisemodel for odometry @@ -49,16 +47,16 @@ int main(int argc, char** argv) { /* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph.addOdometry(x1, x2, odom_measurement, odom_model); - graph.addOdometry(x2, x3, odom_measurement, odom_model); + graph.addOdometry(1, 2, odom_measurement, odom_model); + graph.addOdometry(2, 3, odom_measurement, odom_model); graph.print("full graph"); /* 3. Create the data structure to hold the initial estinmate to the solution * initialize to noisy points */ pose2SLAM::Values initial; - initial.insert(x1, Pose2(0.5, 0.0, 0.2)); - initial.insert(x2, Pose2(2.3, 0.1,-0.2)); - initial.insert(x3, Pose2(4.1, 0.1, 0.1)); + initial.insertPose(1, Pose2(0.5, 0.0, 0.2)); + initial.insertPose(2, Pose2(2.3, 0.1,-0.2)); + initial.insertPose(3, Pose2(4.1, 0.1, 0.1)); initial.print("initial estimate"); /* 4 Single Step Optimization diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 821b177dc..29bab981a 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -35,25 +35,6 @@ using namespace std; using namespace gtsam; -/** - * Step 1: Setup basic types for optimization of a single variable type - * This can be considered to be specifying the domain of the problem we wish - * to solve. In this case, we will create a very simple domain that operates - * on variables of a specific type, in this case, Rot2. - * - * To create a domain: - * - variable types need to have a key defined to act as a label in graphs - * - a "RotValues" structure needs to be defined to store the system state - * - a graph container acting on a given RotValues - * - * In a typical scenario, these typedefs could be placed in a header - * file and reused between projects. Also, RotValues can be combined to - * form a "TupleValues" to enable multiple variable types, such as 2D points - * and 2D poses. - */ -typedef TypedSymbol Key; /// Variable labels for a specific type -typedef NonlinearFactorGraph Graph; /// Graph container for constraints - needs to know type of variables - const double degree = M_PI / 180; int main() { @@ -64,7 +45,7 @@ int main() { */ /** - * Step 2: create a factor on to express a unary constraint + * Step 1: create a factor on to express a unary constraint * The "prior" in this case is the measurement from a sensor, * with a model of the noise on the measurement. * @@ -80,11 +61,11 @@ int main() { Rot2 prior = Rot2::fromAngle(30 * degree); prior.print("goal angle"); SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree); - Key key(1); - PriorFactor factor(key, prior, model); + Symbol key('x',1); + PriorFactor factor(key, prior, model); /** - * Step 3: create a graph container and add the factor to it + * Step 2: create a graph container and add the factor to it * Before optimizing, all factors need to be added to a Graph container, * which provides the necessary top-level functionality for defining a * system of constraints. @@ -92,12 +73,12 @@ int main() { * In this case, there is only one factor, but in a practical scenario, * many more factors would be added. */ - Graph graph; + NonlinearFactorGraph graph; graph.add(factor); graph.print("full graph"); /** - * Step 4: create an initial estimate + * Step 3: create an initial estimate * An initial estimate of the solution for the system is necessary to * start optimization. This system state is the "RotValues" structure, * which is similar in structure to a STL map, in that it maps @@ -117,14 +98,14 @@ int main() { initial.print("initial estimate"); /** - * Step 5: optimize + * Step 4: optimize * After formulating the problem with a graph of constraints * and an initial estimate, executing optimization is as simple * as calling a general optimization function with the graph and * initial estimate. This will yield a new RotValues structure * with the final state of the optimization. */ - Values result = optimize(graph, initial); + Values result = optimize(graph, initial); result.print("final result"); return 0; diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index d190c38dc..b13058a07 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -30,7 +30,6 @@ using namespace std; using namespace gtsam; // Define Types for Linear System Test -typedef TypedSymbol LinearKey; typedef Point2 LinearMeasurement; int main() { @@ -40,7 +39,7 @@ int main() { SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(x_initial, P_initial); @@ -61,11 +60,11 @@ int main() { // This simple motion can be modeled with a BetweenFactor // Create Keys - LinearKey x0(0), x1(1); + Symbol x0('x',0), x1('x',1); // Predict delta based on controls Point2 difference(1,0); // Create Factor - BetweenFactor factor1(x0, x1, difference, Q); + BetweenFactor factor1(x0, x1, difference, Q); // Predict the new value with the EKF class Point2 x1_predict = ekf.predict(factor1); @@ -86,7 +85,7 @@ int main() { // This simple measurement can be modeled with a PriorFactor Point2 z1(1.0, 0.0); - PriorFactor factor2(x1, z1, R); + PriorFactor factor2(x1, z1, R); // Update the Kalman Filter with the measurement Point2 x1_update = ekf.update(factor2); @@ -96,15 +95,15 @@ int main() { // Do the same thing two more times... // Predict - LinearKey x2(2); + Symbol x2('x',2); difference = Point2(1,0); - BetweenFactor factor3(x1, x2, difference, Q); + BetweenFactor factor3(x1, x2, difference, Q); Point2 x2_predict = ekf.predict(factor1); x2_predict.print("X2 Predict"); // Update Point2 z2(2.0, 0.0); - PriorFactor factor4(x2, z2, R); + PriorFactor factor4(x2, z2, R); Point2 x2_update = ekf.update(factor4); x2_update.print("X2 Update"); @@ -112,15 +111,15 @@ int main() { // Do the same thing one more time... // Predict - LinearKey x3(3); + Symbol x3('x',3); difference = Point2(1,0); - BetweenFactor factor5(x2, x3, difference, Q); + BetweenFactor factor5(x2, x3, difference, Q); Point2 x3_predict = ekf.predict(factor5); x3_predict.print("X3 Predict"); // Update Point2 z3(3.0, 0.0); - PriorFactor factor6(x3, z3, R); + PriorFactor factor6(x3, z3, R); Point2 x3_update = ekf.update(factor6); x3_update.print("X3 Update"); diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 30d77aedc..1c3415e61 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -33,8 +33,6 @@ using namespace std; using namespace gtsam; -typedef TypedSymbol Key; /// Variable labels for a specific type - int main() { // [code below basically does SRIF with LDL] @@ -55,14 +53,14 @@ int main() { // i.e., we should get 0,0 0,1 0,2 if there is no noise // Create new state variable - Key x0(0); + Symbol x0('x',0); ordering->insert(x0, 0); // Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0) // This is equivalent to x_0 and P_0 Point2 x_initial(0,0); SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - PriorFactor factor1(x0, x_initial, P_initial); + PriorFactor factor1(x0, x_initial, P_initial); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x0, x_initial); linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering)); @@ -88,12 +86,12 @@ int main() { // so, difference = x_{t+1} - x_{t} = F*x_{t} + B*u_{t} - I*x_{t} // = (F - I)*x_{t} + B*u_{t} // = B*u_{t} (for our example) - Key x1(1); + Symbol x1('x',1); ordering->insert(x1, 1); Point2 difference(1,0); SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor2(x0, x1, difference, Q); + BetweenFactor factor2(x0, x1, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x1, x_initial); linearFactorGraph->push_back(factor2.linearize(linearizationPoints, *ordering)); @@ -116,7 +114,7 @@ int main() { // Extract the current estimate of x1,P1 from the Bayes Network VectorValues result = optimize(*linearBayesNet); - Point2 x1_predict = linearizationPoints[x1].retract(result[ordering->at(x1)]); + Point2 x1_predict = linearizationPoints.at(x1).retract(result[ordering->at(x1)]); x1_predict.print("X1 Predict"); // Update the new linearization point to the new estimate @@ -171,7 +169,7 @@ int main() { // This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R. Point2 z1(1.0, 0.0); SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor4(x1, z1, R1); + PriorFactor factor4(x1, z1, R1); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering)); @@ -188,7 +186,7 @@ int main() { // Extract the current estimate of x1 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x1_update = linearizationPoints[x1].retract(result[ordering->at(x1)]); + Point2 x1_update = linearizationPoints.at(x1).retract(result[ordering->at(x1)]); x1_update.print("X1 Update"); // Update the linearization point to the new estimate @@ -213,7 +211,7 @@ int main() { linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model()); // Create a key for the new state - Key x2(2); + Symbol x2('x',2); // Create the desired ordering ordering = Ordering::shared_ptr(new Ordering); @@ -223,7 +221,7 @@ int main() { // Create a nonlinear factor describing the motion model difference = Point2(1,0); Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor6(x1, x2, difference, Q); + BetweenFactor factor6(x1, x2, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x2, x1_update); @@ -235,7 +233,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x2_predict = linearizationPoints[x2].retract(result[ordering->at(x2)]); + Point2 x2_predict = linearizationPoints.at(x2).retract(result[ordering->at(x2)]); x2_predict.print("X2 Predict"); // Update the linearization point to the new estimate @@ -261,7 +259,7 @@ int main() { // And update using z2 ... Point2 z2(2.0, 0.0); SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor8(x2, z2, R2); + PriorFactor factor8(x2, z2, R2); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering)); @@ -279,7 +277,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x2_update = linearizationPoints[x2].retract(result[ordering->at(x2)]); + Point2 x2_update = linearizationPoints.at(x2).retract(result[ordering->at(x2)]); x2_update.print("X2 Update"); // Update the linearization point to the new estimate @@ -302,7 +300,7 @@ int main() { linearFactorGraph->add(0, tmpPrior3.getA(tmpPrior3.begin()), tmpPrior3.getb() - tmpPrior3.getA(tmpPrior3.begin()) * result[ordering->at(x2)], tmpPrior3.get_model()); // Create a key for the new state - Key x3(3); + Symbol x3('x',3); // Create the desired ordering ordering = Ordering::shared_ptr(new Ordering); @@ -312,7 +310,7 @@ int main() { // Create a nonlinear factor describing the motion model difference = Point2(1,0); Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor10(x2, x3, difference, Q); + BetweenFactor factor10(x2, x3, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x3, x2_update); @@ -324,7 +322,7 @@ int main() { // Extract the current estimate of x3 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x3_predict = linearizationPoints[x3].retract(result[ordering->at(x3)]); + Point2 x3_predict = linearizationPoints.at(x3).retract(result[ordering->at(x3)]); x3_predict.print("X3 Predict"); // Update the linearization point to the new estimate @@ -350,7 +348,7 @@ int main() { // And update using z3 ... Point2 z3(3.0, 0.0); SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor12(x3, z3, R3); + PriorFactor factor12(x3, z3, R3); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering)); @@ -368,7 +366,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x3_update = linearizationPoints[x3].retract(result[ordering->at(x3)]); + Point2 x3_update = linearizationPoints.at(x3).retract(result[ordering->at(x3)]); x3_update.print("X3 Update"); // Update the linearization point to the new estimate diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index be4ddf840..d8395942a 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -153,7 +153,7 @@ public: KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); POSE relativePose = boost::get(boost::edge_weight, g, edge); - config_->insert(key_to, (*config_)[key_from].compose(relativePose)); + config_->insert(key_to, config_->at(key_from).compose(relativePose)); } }; diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 2c4e382a1..7cd67f6d2 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -27,10 +27,10 @@ namespace gtsam { /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, - const Values& linearizationPoints, const KEY& lastKey, + const Values& linearizationPoints, const Symbol& lastKey, JacobianFactor::shared_ptr& newPrior) const { // Extract the Index of the provided last key @@ -43,7 +43,7 @@ namespace gtsam { // Extract the current estimate of x1,P1 from the Bayes Network VectorValues result = optimize(*linearBayesNet); - T x = linearizationPoints[lastKey].retract(result[lastIndex]); + T x = linearizationPoints.at(lastKey).retract(result[lastIndex]); // Create a Jacobian Factor from the root node of the produced Bayes Net. // This will act as a prior for the next iteration. @@ -66,8 +66,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - ExtendedKalmanFilter::ExtendedKalmanFilter(T x_initial, + template + ExtendedKalmanFilter::ExtendedKalmanFilter(T x_initial, noiseModel::Gaussian::shared_ptr P_initial) { // Set the initial linearization point to the provided mean @@ -82,8 +82,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::predict( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::predict( const MotionFactor& motionFactor) { // TODO: This implementation largely ignores the actual factor symbols. @@ -91,8 +91,8 @@ namespace gtsam { // different keys will still compute as if a common key-set was used // Create Keys - KEY x0 = motionFactor.key1(); - KEY x1 = motionFactor.key2(); + Symbol x0 = motionFactor.key1(); + Symbol x1 = motionFactor.key2(); // Create an elimination ordering Ordering ordering; @@ -122,8 +122,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::update( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::update( const MeasurementFactor& measurementFactor) { // TODO: This implementation largely ignores the actual factor symbols. @@ -131,7 +131,7 @@ namespace gtsam { // different keys will still compute as if a common key-set was used // Create Keys - KEY x0 = measurementFactor.key(); + Symbol x0 = measurementFactor.key(); // Create an elimination ordering Ordering ordering; diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 6e85d6e27..bdfaa7651 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -40,14 +40,14 @@ namespace gtsam { * \nosubgrouping */ - template + template class ExtendedKalmanFilter { public: - typedef boost::shared_ptr > shared_ptr; - typedef typename KEY::Value T; - typedef NonlinearFactor2 MotionFactor; - typedef NonlinearFactor1 MeasurementFactor; + typedef boost::shared_ptr > shared_ptr; + typedef VALUE T; + typedef NonlinearFactor2 MotionFactor; + typedef NonlinearFactor1 MeasurementFactor; protected: T x_; // linearization point @@ -55,7 +55,7 @@ namespace gtsam { T solve_(const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, const Values& linearizationPoints, - const KEY& x, JacobianFactor::shared_ptr& newPrior) const; + const Symbol& x, JacobianFactor::shared_ptr& newPrior) const; public: diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index a95044dee..1a1fce839 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -219,7 +219,7 @@ namespace gtsam { /** Print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearEquality1(" - << (std::string) this->key_ << "),"<< "\n"; + << (std::string) this->key() << "),"<< "\n"; this->noiseModel_->print(); value_.print("Value"); } diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 3c28afa07..42f42e429 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -95,18 +95,18 @@ private: * Binary scalar inequality constraint, with a similar value() function * to implement for specific systems */ -template -struct BoundingConstraint2: public NonlinearFactor2 { - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; +template +struct BoundingConstraint2: public NonlinearFactor2 { + typedef VALUE1 X1; + typedef VALUE2 X2; - typedef NonlinearFactor2 Base; - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor2 Base; + typedef boost::shared_ptr > shared_ptr; double threshold_; bool isGreaterThan_; /// flag for greater/less than - BoundingConstraint2(const KEY1& key1, const KEY2& key2, double threshold, + BoundingConstraint2(const Symbol& key1, const Symbol& key2, double threshold, bool isGreaterThan, double mu = 1000.0) : Base(noiseModel::Constrained::All(1, mu), key1, key2), threshold_(threshold), isGreaterThan_(isGreaterThan) {} @@ -127,7 +127,7 @@ struct BoundingConstraint2: public NonlinearFactor2 { /** active when constraint *NOT* met */ bool active(const Values& c) const { // note: still active at equality to avoid zigzagging - double x = value(c[this->key1_], c[this->key2_]); + double x = value(c[this->key1()], c[this->key2()]); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 60092c15d..868b064d7 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -53,7 +53,7 @@ namespace gtsam { * @param j is the index of the landmark */ GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Symbol& landmarkKey) : - Base(model, cameraKey, cameraKey), measured_(measured) {} + Base(model, cameraKey, landmarkKey), measured_(measured) {} GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index f7e562640..db3d6b146 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -37,7 +37,7 @@ namespace planarSLAM { } void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) { - sharedFactor factor(new Odometry(PoseKey(i), PointKey(j), odometry, model)); + sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), odometry, model)); push_back(factor); } diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 5c9c62d46..33dc97a8c 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -97,7 +97,7 @@ namespace planarSLAM { void addPoseConstraint(Index 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); + void addOdometry(Index poseKey1, Index poseKey2, 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); diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h index 99475ce91..5a6b39861 100644 --- a/gtsam/slam/simulated2DConstraints.h +++ b/gtsam/slam/simulated2DConstraints.h @@ -147,7 +147,7 @@ namespace simulated2D { } }; - typedef MaxDistanceConstraint PoseMaxDistConstraint; ///< Simulated2D domain example factor + typedef MaxDistanceConstraint PoseMaxDistConstraint; ///< Simulated2D domain example factor /** * Binary inequality constraint forcing a minimum range diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 022a76960..8c59d63ac 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -44,7 +44,7 @@ public: } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(Symbol('x',j), p)); + boost::shared_ptr factor(new Point3Constraint(Symbol('l',j), p)); push_back(factor); } @@ -89,8 +89,7 @@ TEST( GeneralSFMFactor, equals ) TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor(new Projection(z, sigma, "x1", "l1")); + boost::shared_ptr factor(new Projection(z, sigma, "x1", "l1")); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; @@ -167,13 +166,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); @@ -206,7 +205,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { @@ -214,10 +213,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } else { - values->insert(PointKey(i), L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } } @@ -252,14 +251,14 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); //Point3 pt(L[i].x(), L[i].y(), L[i].z()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } for ( size_t i = 0 ; i < X.size() ; ++i ) @@ -301,7 +300,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { focal_noise = 1, skew_noise = 1e-5; if ( i == 0 ) { - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; } else { @@ -312,12 +311,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { skew_noise, // s trans_noise, trans_noise // ux, uy ) ; - values->insert(CameraKey((int)i), X[i].retract(delta)) ; + values->insert(Symbol('x',i), X[i].retract(delta)) ; } } for ( size_t i = 0 ; i < L.size() ; ++i ) { - values->insert(PointKey(i), L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } // fix X0 and all landmarks, allow only the X[1] to move @@ -357,14 +356,14 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 7c1d395ba..6b1ace287 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -207,7 +207,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { @@ -215,10 +215,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } else { - values->insert(PointKey(i), L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } } @@ -253,14 +253,14 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); //Point3 pt(L[i].x(), L[i].y(), L[i].z()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } for ( size_t i = 0 ; i < X.size() ; ++i ) @@ -300,7 +300,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, distort_noise = 1e-3; if ( i == 0 ) { - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; } else { @@ -309,12 +309,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { trans_noise, trans_noise, trans_noise, // translation focal_noise, distort_noise, distort_noise // f, k1, k2 ) ; - values->insert(CameraKey((int)i), X[i].retract(delta)) ; + values->insert(Symbol('x',i), X[i].retract(delta)) ; } } for ( size_t i = 0 ; i < L.size() ; ++i ) { - values->insert(PointKey(i), L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } // fix X0 and all landmarks, allow only the X[1] to move @@ -353,14 +353,14 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { const double noise = baseline*0.1; boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert(CameraKey((int)i), X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(PointKey(i), pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index a9b743145..21c0fd458 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -131,12 +131,12 @@ TEST( Pose3Factor, error ) // Create factor SharedNoiseModel I6(noiseModel::Unit::Create(6)); - Pose3Factor factor(1,2, z, I6); + Pose3Factor factor(PoseKey(1), PoseKey(2), z, I6); // Create config Values x; - x.insert(Key(1),t1); - x.insert(Key(2),t2); + x.insert(PoseKey(1),t1); + x.insert(PoseKey(2),t2); // Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2)) Vector actual = factor.unwhitenedError(x); @@ -146,10 +146,10 @@ TEST( Pose3Factor, error ) /* ************************************************************************* */ TEST(Pose3Graph, partial_prior_xy) { - typedef PartialPriorFactor Partial; + typedef PartialPriorFactor Partial; // XY prior - full mask interface - pose3SLAM::Key key(1); + Symbol key = PoseKey(1); Vector exp_xy = Vector_(2, 3.0, 4.0); SharedDiagonal model = noiseModel::Unit::Create(2); Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0)); @@ -169,7 +169,7 @@ TEST(Pose3Graph, partial_prior_xy) { values.insert(key, init); Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); - EXPECT(assert_equal(expected, actual[key], tol)); + EXPECT(assert_equal(expected, actual.at(key), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } @@ -185,10 +185,10 @@ TEST( Values, pose3Circle ) { // expected is 4 poses tangent to circle with radius 1m Values expected; - expected.insert(Key(0), Pose3(R1, Point3( 1, 0, 0))); - expected.insert(Key(1), Pose3(R2, Point3( 0, 1, 0))); - expected.insert(Key(2), Pose3(R3, Point3(-1, 0, 0))); - expected.insert(Key(3), Pose3(R4, Point3( 0,-1, 0))); + expected.insert(PoseKey(0), Pose3(R1, Point3( 1, 0, 0))); + expected.insert(PoseKey(1), Pose3(R2, Point3( 0, 1, 0))); + expected.insert(PoseKey(2), Pose3(R3, Point3(-1, 0, 0))); + expected.insert(PoseKey(3), Pose3(R4, Point3( 0,-1, 0))); Values actual = pose3SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); @@ -198,10 +198,10 @@ TEST( Values, pose3Circle ) TEST( Values, expmap ) { Values expected; - expected.insert(Key(0), Pose3(R1, Point3( 1.0, 0.1, 0))); - expected.insert(Key(1), Pose3(R2, Point3(-0.1, 1.0, 0))); - expected.insert(Key(2), Pose3(R3, Point3(-1.0,-0.1, 0))); - expected.insert(Key(3), Pose3(R4, Point3( 0.1,-1.0, 0))); + expected.insert(PoseKey(0), Pose3(R1, Point3( 1.0, 0.1, 0))); + expected.insert(PoseKey(1), Pose3(R2, Point3(-0.1, 1.0, 0))); + expected.insert(PoseKey(2), Pose3(R3, Point3(-1.0,-0.1, 0))); + expected.insert(PoseKey(3), Pose3(R4, Point3( 0.1,-1.0, 0))); Ordering ordering(*expected.orderingArbitrary()); VectorValues delta(expected.dims(ordering)); diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 4ecbf2e43..df137ad47 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -85,8 +85,8 @@ namespace visualSLAM { * @param K shared pointer to calibration object */ void addMeasurement(const Point2& measured, const SharedNoiseModel& model, - const Symbol& poseKey, const Symbol& pointKey, const shared_ptrK& K) { - boost::shared_ptr factor(new ProjectionFactor(measured, model, poseKey, pointKey, K)); + Index poseKey, Index pointKey, const shared_ptrK& K) { + boost::shared_ptr factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K)); push_back(factor); } @@ -95,8 +95,8 @@ namespace visualSLAM { * @param key variable key of the camera pose * @param p to which pose to constrain it to */ - void addPoseConstraint(const Symbol& key, const Pose3& p = Pose3()) { - boost::shared_ptr factor(new PoseConstraint(key, p)); + void addPoseConstraint(Index poseKey, const Pose3& p = Pose3()) { + boost::shared_ptr factor(new PoseConstraint(PoseKey(poseKey), p)); push_back(factor); } @@ -105,8 +105,8 @@ namespace visualSLAM { * @param key variable key of the landmark * @param p point around which soft prior is defined */ - void addPointConstraint(const Symbol& key, const Point3& p = Point3()) { - boost::shared_ptr factor(new PointConstraint(key, p)); + void addPointConstraint(Index pointKey, const Point3& p = Point3()) { + boost::shared_ptr factor(new PointConstraint(PointKey(pointKey), p)); push_back(factor); } @@ -116,8 +116,8 @@ namespace visualSLAM { * @param p around which soft prior is defined * @param model uncertainty model of this prior */ - void addPosePrior(const Symbol& key, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { - boost::shared_ptr factor(new PosePrior(key, p, model)); + void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { + boost::shared_ptr factor(new PosePrior(PoseKey(poseKey), p, model)); push_back(factor); } @@ -127,8 +127,8 @@ namespace visualSLAM { * @param p to which point to constrain it to * @param model uncertainty model of this prior */ - void addPointPrior(const Symbol& key, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { - boost::shared_ptr factor(new PointPrior(key, p, model)); + void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { + boost::shared_ptr factor(new PointPrior(PointKey(pointKey), p, model)); push_back(factor); } @@ -139,8 +139,8 @@ namespace visualSLAM { * @param range approximate range to landmark * @param model uncertainty model of this prior */ - void addRangeFactor(const Symbol& poseKey, const Symbol& pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { - push_back(boost::shared_ptr(new RangeFactor(poseKey, pointKey, range, model))); + void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { + push_back(boost::shared_ptr(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model))); } }; // Graph diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index c15070b4c..4fddf1ba0 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -17,6 +17,8 @@ #include +#define GTSAM_MAGIC_KEY + #include #include #include @@ -37,7 +39,7 @@ typedef boost::shared_ptr shared_values; typedef NonlinearOptimizer Optimizer; // some simple inequality constraints -simulated2D::PoseKey key(1); +Symbol key(simulated2D::PoseKey(1)); double mu = 10.0; // greater than iq2D::PoseXInequality constraint1(key, 1.0, true, mu); @@ -151,7 +153,7 @@ TEST( testBoundingConstraint, unary_simple_optimization1) { Point2 start_pt(0.0, 1.0); shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1); + Symbol x1("x1"); graph->add(iq2D::PoseXInequality(x1, 1.0, true)); graph->add(iq2D::PoseYInequality(x1, 2.0, true)); graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); @@ -173,7 +175,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { Point2 start_pt(2.0, 3.0); shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1); + Symbol x1("x1"); graph->add(iq2D::PoseXInequality(x1, 1.0, false)); graph->add(iq2D::PoseYInequality(x1, 2.0, false)); graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); @@ -189,7 +191,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { /* ************************************************************************* */ TEST( testBoundingConstraint, MaxDistance_basics) { - simulated2D::PoseKey key1(1), key2(2); + Symbol key1("x1"), key2("x2"); Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0); iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu); EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol); @@ -231,7 +233,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { TEST( testBoundingConstraint, MaxDistance_simple_optimization) { Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0); - simulated2D::PoseKey x1(1), x2(2); + Symbol x1("x1"), x2("x2"); Graph graph; graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1)); @@ -254,8 +256,7 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) { /* ************************************************************************* */ TEST( testBoundingConstraint, avoid_demo) { - simulated2D::PoseKey x1(1), x2(2), x3(3); - simulated2D::PointKey l1(1); + Symbol x1("x1"), x2("x2"), x3("x3"), l1("l1"); double radius = 1.0; Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0); Point2 odo(2.0, 0.0); diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 5a87950d0..532305f24 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -16,6 +16,8 @@ #include +#define GTSAM_MAGIC_KEY + #include #include #include @@ -24,9 +26,6 @@ using namespace gtsam; -// Define Types for System Test -typedef TypedSymbol TestKey; - /* ************************************************************************* */ TEST( ExtendedKalmanFilter, linear ) { @@ -35,10 +34,10 @@ TEST( ExtendedKalmanFilter, linear ) { SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(x_initial, P_initial); // Create the TestKeys for our example - TestKey x0(0), x1(1), x2(2), x3(3); + Symbol x0("x0"), x1("x1"), x2("x2"), x3("x3"); // Create the controls and measurement properties for our example double dt = 1.0; @@ -57,30 +56,30 @@ TEST( ExtendedKalmanFilter, linear ) { // Run iteration 1 // Create motion factor - BetweenFactor factor1(x0, x1, difference, Q); + BetweenFactor factor1(x0, x1, difference, Q); Point2 predict1 = ekf.predict(factor1); EXPECT(assert_equal(expected1,predict1)); // Create the measurement factor - PriorFactor factor2(x1, z1, R); + PriorFactor factor2(x1, z1, R); Point2 update1 = ekf.update(factor2); EXPECT(assert_equal(expected1,update1)); // Run iteration 2 - BetweenFactor factor3(x1, x2, difference, Q); + BetweenFactor factor3(x1, x2, difference, Q); Point2 predict2 = ekf.predict(factor3); EXPECT(assert_equal(expected2,predict2)); - PriorFactor factor4(x2, z2, R); + PriorFactor factor4(x2, z2, R); Point2 update2 = ekf.update(factor4); EXPECT(assert_equal(expected2,update2)); // Run iteration 3 - BetweenFactor factor5(x2, x3, difference, Q); + BetweenFactor factor5(x2, x3, difference, Q); Point2 predict3 = ekf.predict(factor5); EXPECT(assert_equal(expected3,predict3)); - PriorFactor factor6(x3, z3, R); + PriorFactor factor6(x3, z3, R); Point2 update3 = ekf.update(factor6); EXPECT(assert_equal(expected3,update3)); @@ -89,12 +88,12 @@ TEST( ExtendedKalmanFilter, linear ) { // Create Motion Model Factor -class NonlinearMotionModel : public NonlinearFactor2 { +class NonlinearMotionModel : public NonlinearFactor2 { public: - typedef TestKey::Value T; + typedef Point2 T; private: - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; typedef NonlinearMotionModel This; protected: @@ -104,7 +103,7 @@ protected: public: NonlinearMotionModel(){} - NonlinearMotionModel(const TestKey& TestKey1, const TestKey& TestKey2) : + NonlinearMotionModel(const Symbol& TestKey1, const Symbol& TestKey2) : Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) { // Initialize motion model parameters: @@ -156,14 +155,14 @@ public: /* print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearMotionModel\n"; - std::cout << " TestKey1: " << (std::string) key1_ << std::endl; - std::cout << " TestKey2: " << (std::string) key2_ << std::endl; + std::cout << " TestKey1: " << (std::string) key1() << std::endl; + std::cout << " TestKey2: " << (std::string) key2() << std::endl; } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - virtual bool equals(const NonlinearFactor2& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && (key1_ == e->key1_) && (key2_ == e->key2_); + return (e != NULL) && (key1() == e->key1()) && (key2() == e->key2()); } /** @@ -182,7 +181,7 @@ public: /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return QInvSqrt(c[key1_])*unwhitenedError(c); + return QInvSqrt(c[key1()])*unwhitenedError(c); } /** @@ -191,11 +190,11 @@ public: * Hence b = z - h(x1,x2) = - error_vector(x) */ boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { - const X1& x1 = c[key1_]; - const X2& x2 = c[key2_]; + const X1& x1 = c[key1()]; + const X2& x2 = c[key2()]; Matrix A1, A2; Vector b = -evaluateError(x1, x2, A1, A2); - const Index var1 = ordering[key1_], var2 = ordering[key2_]; + const Index var1 = ordering[key1()], var2 = ordering[key2()]; SharedDiagonal constrained = boost::shared_dynamic_cast(this->noiseModel_); if (constrained.get() != NULL) { @@ -236,13 +235,13 @@ public: }; // Create Measurement Model Factor -class NonlinearMeasurementModel : public NonlinearFactor1 { +class NonlinearMeasurementModel : public NonlinearFactor1 { public: - typedef TestKey::Value T; + typedef Point2 T; private: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; typedef NonlinearMeasurementModel This; protected: @@ -253,7 +252,7 @@ protected: public: NonlinearMeasurementModel(){} - NonlinearMeasurementModel(const TestKey& TestKey, Vector z) : + NonlinearMeasurementModel(const Symbol& TestKey, Vector z) : Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey), z_(z), R_(1,1) { // Initialize nonlinear measurement model parameters: @@ -294,13 +293,13 @@ public: /* print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearMeasurementModel\n"; - std::cout << " TestKey: " << (std::string) key_ << std::endl; + std::cout << " TestKey: " << (std::string) key() << std::endl; } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - virtual bool equals(const NonlinearFactor1& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && (key_ == e->key_); + return (e != NULL) && Base::equals(f); } /** @@ -319,7 +318,7 @@ public: /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return RInvSqrt(c[key_])*unwhitenedError(c); + return RInvSqrt(c[key()])*unwhitenedError(c); } /** @@ -328,10 +327,10 @@ public: * Hence b = z - h(x1) = - error_vector(x) */ boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { - const X& x1 = c[key_]; + const X& x1 = c[key()]; Matrix A1; Vector b = -evaluateError(x1, A1); - const Index var1 = ordering[key_]; + const Index var1 = ordering[key()]; SharedDiagonal constrained = boost::shared_dynamic_cast(this->noiseModel_); if (constrained.get() != NULL) { @@ -345,7 +344,7 @@ public: } /** vector of errors */ - Vector evaluateError(const TestKey::Value& p, boost::optional H1 = boost::none) const { + Vector evaluateError(const T& p, boost::optional H1 = boost::none) const { // error = z - h(p) // H = d error / d p = -d h/ d p = -H Vector z_hat = h(p); @@ -406,17 +405,17 @@ TEST( ExtendedKalmanFilter, nonlinear ) { SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(x_initial, P_initial); // Enter Predict-Update Loop Point2 x_predict, x_update; for(unsigned int i = 0; i < 10; ++i){ // Create motion factor - NonlinearMotionModel motionFactor(TestKey(i-1), TestKey(i)); + NonlinearMotionModel motionFactor(Symbol('x',i-1), Symbol('x',i)); x_predict = ekf.predict(motionFactor); // Create a measurement factor - NonlinearMeasurementModel measurementFactor(TestKey(i), Vector_(1, z[i])); + NonlinearMeasurementModel measurementFactor(Symbol('x',i), Vector_(1, z[i])); x_update = ekf.update(measurementFactor); EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6)); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index c4f8c69fc..b0a955484 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -202,8 +202,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton) // SETDEBUG("ISAM2 recalculate", true); // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); @@ -335,8 +335,8 @@ TEST(ISAM2, slamlike_solution_dogleg) // SETDEBUG("ISAM2 recalculate", true); // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); @@ -463,8 +463,8 @@ TEST(ISAM2, slamlike_solution_dogleg) TEST(ISAM2, clone) { // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); @@ -636,8 +636,8 @@ TEST(ISAM2, removeFactors) // then removes the 2nd-to-last landmark measurement // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); @@ -775,8 +775,8 @@ TEST(ISAM2, constrained_ordering) // SETDEBUG("ISAM2 recalculate", true); // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index 271f625b6..ff1732ec5 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -125,8 +125,8 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2) /* ************************************************************************* */ TEST(GaussianJunctionTree, slamlike) { - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; Values init; planarSLAM::Graph newfactors; @@ -195,7 +195,7 @@ TEST(GaussianJunctionTree, simpleMarginal) { // Create a simple graph pose2SLAM::Graph fg; - fg.addPrior(pose2SLAM::PoseKey(0), Pose2(), sharedSigma(3, 10.0)); + fg.addPrior(0, Pose2(), sharedSigma(3, 10.0)); fg.addOdometry(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0))); Values init; diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index b41da9286..34439b73a 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -86,16 +86,16 @@ TEST( Graph, composePoses ) graph.addOdometry(2,3, p23, cov); graph.addOdometry(4,3, p43, cov); - PredecessorMap tree; - tree.insert(pose2SLAM::PoseKey(1),2); - tree.insert(pose2SLAM::PoseKey(2),2); - tree.insert(pose2SLAM::PoseKey(3),2); - tree.insert(pose2SLAM::PoseKey(4),3); + PredecessorMap tree; + tree.insert(pose2SLAM::PoseKey(1),pose2SLAM::PoseKey(2)); + tree.insert(pose2SLAM::PoseKey(2),pose2SLAM::PoseKey(2)); + tree.insert(pose2SLAM::PoseKey(3),pose2SLAM::PoseKey(2)); + tree.insert(pose2SLAM::PoseKey(4),pose2SLAM::PoseKey(3)); Pose2 rootPose = p2; boost::shared_ptr actual = composePoses (graph, tree, rootPose); + Pose2, Symbol> (graph, tree, rootPose); Values expected; expected.insert(pose2SLAM::PoseKey(1), p1); diff --git a/tests/testInference.cpp b/tests/testInference.cpp index e530393f8..5c60f08e7 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -54,12 +54,12 @@ TEST( Inference, marginals2) SharedDiagonal poseModel(sharedSigma(3, 0.1)); SharedDiagonal pointModel(sharedSigma(3, 0.1)); - fg.addPrior(planarSLAM::PoseKey(0), Pose2(), poseModel); - fg.addOdometry(planarSLAM::PoseKey(0), planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0), poseModel); - fg.addOdometry(planarSLAM::PoseKey(1), planarSLAM::PoseKey(2), Pose2(1.0,0.0,0.0), poseModel); - fg.addBearingRange(planarSLAM::PoseKey(0), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(planarSLAM::PoseKey(1), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(planarSLAM::PoseKey(2), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel); + fg.addPrior(0, Pose2(), poseModel); + fg.addOdometry(0, 1, Pose2(1.0,0.0,0.0), poseModel); + fg.addOdometry(1, 2, Pose2(1.0,0.0,0.0), poseModel); + fg.addBearingRange(0, 0, Rot2(), 1.0, pointModel); + fg.addBearingRange(1, 0, Rot2(), 1.0, pointModel); + fg.addBearingRange(2, 0, Rot2(), 1.0, pointModel); Values init; init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index bdbb3fdd2..2ceb066e2 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -31,15 +31,14 @@ namespace eq2D = simulated2D::equality_constraints; static const double tol = 1e-5; -typedef TypedSymbol PoseKey; -typedef PriorFactor PosePrior; -typedef NonlinearEquality PoseNLE; +typedef PriorFactor PosePrior; +typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; typedef NonlinearFactorGraph PoseGraph; typedef NonlinearOptimizer PoseOptimizer; -PoseKey key(1); +Symbol key('x',1); /* ************************************************************************* */ TEST ( NonlinearEquality, linearization ) { @@ -60,7 +59,7 @@ TEST ( NonlinearEquality, linearization ) { /* ********************************************************************** */ TEST ( NonlinearEquality, linearization_pose ) { - PoseKey key(1); + Symbol key('x',1); Pose2 value; Values config; config.insert(key, value); @@ -89,7 +88,7 @@ TEST ( NonlinearEquality, linearization_fail ) { /* ********************************************************************** */ TEST ( NonlinearEquality, linearization_fail_pose ) { - PoseKey key(1); + Symbol key('x',1); Pose2 value(2.0, 1.0, 2.0), wrong(2.0, 3.0, 4.0); Values bad_linearize; @@ -105,7 +104,7 @@ TEST ( NonlinearEquality, linearization_fail_pose ) { /* ********************************************************************** */ TEST ( NonlinearEquality, linearization_fail_pose_origin ) { - PoseKey key(1); + Symbol key('x',1); Pose2 value, wrong(2.0, 3.0, 4.0); Values bad_linearize; @@ -155,7 +154,7 @@ TEST ( NonlinearEquality, equals ) { /* ************************************************************************* */ TEST ( NonlinearEquality, allow_error_pose ) { - PoseKey key1(1); + Symbol key1('x',1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; PoseNLE nle(key1, feasible1, error_gain); @@ -183,7 +182,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { /* ************************************************************************* */ TEST ( NonlinearEquality, allow_error_optimize ) { - PoseKey key1(1); + Symbol key1('x',1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; PoseNLE nle(key1, feasible1, error_gain); @@ -200,7 +199,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { // optimize boost::shared_ptr ord(new Ordering()); ord->push_back(key1); - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5); PoseOptimizer optimizer(graph, init, ord, params); PoseOptimizer result = optimizer.levenbergMarquardt(); @@ -214,7 +213,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // create a hard constraint - PoseKey key1(1); + Symbol key1('x',1); Pose2 feasible1(1.0, 2.0, 3.0); // initialize away from the ideal @@ -236,7 +235,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // optimize boost::shared_ptr ord(new Ordering()); ord->push_back(key1); - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5); PoseOptimizer optimizer(graph, init, ord, params); PoseOptimizer result = optimizer.levenbergMarquardt(); @@ -258,7 +257,7 @@ typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, unary_basics ) { Point2 pt(1.0, 2.0); - simulated2D::PoseKey key(1); + Symbol key1('x',1); double mu = 1000.0; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); @@ -281,7 +280,7 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, unary_linearization ) { Point2 pt(1.0, 2.0); - simulated2D::PoseKey key(1); + Symbol key1('x',1); double mu = 1000.0; Ordering ordering; ordering += key; @@ -306,7 +305,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { // create a single-node graph with a soft and hard constraint to // ensure that the hard constraint overrides the soft constraint Point2 truth_pt(1.0, 2.0); - simulated2D::PoseKey key(1); + Symbol key('x',1); double mu = 10.0; eq2D::UnaryEqualityConstraint::shared_ptr constraint( new eq2D::UnaryEqualityConstraint(truth_pt, key, mu)); @@ -337,7 +336,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, odo_basics ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - simulated2D::PoseKey key1(1), key2(2); + Symbol key1('x',1), key2('x',2); double mu = 1000.0; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); @@ -363,7 +362,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, odo_linearization ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - simulated2D::PoseKey key1(1), key2(2); + Symbol key1('x',1), key2('x',2); double mu = 1000.0; Ordering ordering; ordering += key1, key2; @@ -396,7 +395,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { // a hard prior on one variable, and a conflicting soft prior // on the other variable - the constraints should override the soft constraint Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0); - simulated2D::PoseKey key1(1), key2(2); + Symbol key1('x',1), key2('x',2); // hard prior on x1 eq2D::UnaryEqualityConstraint::shared_ptr constraint1( @@ -438,8 +437,8 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0); graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1)); @@ -476,8 +475,8 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { shared_graph graph(new Graph()); // keys - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); // constant constraint on x1 Point2 pose1(1.0, 1.0); @@ -526,7 +525,7 @@ typedef visualSLAM::Graph VGraph; typedef NonlinearOptimizer VOptimizer; // factors for visual slam -typedef NonlinearEquality2 Point3Equality; +typedef NonlinearEquality2 Point3Equality; /* ********************************************************************* */ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { @@ -543,8 +542,8 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away // keys - visualSLAM::PoseKey x1(1), x2(2); - visualSLAM::PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); // create graph VGraph::shared_graph graph(new VGraph()); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 53a2af385..488d406dc 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -40,6 +40,9 @@ using namespace std; using namespace gtsam; using namespace example; +using simulated2D::PoseKey; +using simulated2D::PointKey; + typedef boost::shared_ptr shared_nlf; /* ************************************************************************* */ @@ -49,11 +52,11 @@ TEST( NonlinearFactor, equals ) // create two nonlinear2 factors Point2 z3(0.,-1.); - simulated2D::Measurement f0(z3, sigma, 1,1); + simulated2D::Measurement f0(z3, sigma, PoseKey(1),PointKey(1)); // measurement between x2 and l1 Point2 z4(-1.5, -1.); - simulated2D::Measurement f1(z4, sigma, 2,1); + simulated2D::Measurement f1(z4, sigma, PoseKey(2),PointKey(1)); CHECK(assert_equal(f0,f0)); CHECK(f0.equals(f0)); @@ -199,7 +202,7 @@ TEST( NonlinearFactor, linearize_constraint1 ) SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 mu(1., -1.); - Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1)); + Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, PoseKey(1))); Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); @@ -219,7 +222,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 z3(1.,-1.); - simulated2D::Measurement f0(z3, constraint, 1,1); + simulated2D::Measurement f0(z3, constraint, PoseKey(1),PointKey(1)); Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); @@ -283,7 +286,7 @@ TEST(NonlinearFactor, NonlinearFactor4) { class TestFactor5 : public NonlinearFactor5 { public: typedef NonlinearFactor5 Base; - TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {} + TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5") {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, @@ -333,7 +336,7 @@ TEST(NonlinearFactor, NonlinearFactor5) { class TestFactor6 : public NonlinearFactor6 { public: typedef NonlinearFactor6 Base; - TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {} + TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5", "x6") {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 2316c5652..09a3d4db0 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -25,15 +25,14 @@ TEST(testNonlinearISAM, markov_chain ) { Sampler sampler(model, 42u); // create initial graph - PoseKey key(0); Pose2 cur_pose; // start at origin Graph start_factors; - start_factors.addPoseConstraint(key, cur_pose); + start_factors.addPoseConstraint(0, cur_pose); planarSLAM::Values init; planarSLAM::Values expected; - init.insert(key, cur_pose); - expected.insert(key, cur_pose); + init.insertPose(0, cur_pose); + expected.insertPose(0, cur_pose); isam.update(start_factors, init); // loop for a period of time to verify memory usage @@ -41,8 +40,7 @@ TEST(testNonlinearISAM, markov_chain ) { Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { Graph new_factors; - PoseKey key1(i-1), key2(i); - new_factors.addOdometry(key1, key2, z, model); + new_factors.addOdometry(i-1, i, z, model); planarSLAM::Values new_init; // perform a check on changing orderings @@ -61,16 +59,15 @@ TEST(testNonlinearISAM, markov_chain ) { } cur_pose = cur_pose.compose(z); - new_init.insert(key2, cur_pose.retract(sampler.sample())); - expected.insert(key2, cur_pose); + new_init.insertPose(i, cur_pose.retract(sampler.sample())); + expected.insertPose(i, cur_pose); isam.update(new_factors, new_init); } // verify values - all but the last one should be very close planarSLAM::Values actual = isam.estimate(); for (size_t i=0; i Key; -typedef PriorFactor Prior; -typedef BetweenFactor Between; +typedef PriorFactor Prior; +typedef BetweenFactor Between; typedef NonlinearFactorGraph Graph; /* ************************************************************************* */ @@ -41,11 +40,11 @@ TEST(Rot3, optimize) { Values truth; Values initial; Graph fg; - fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01))); + fg.add(Prior(Symbol('r',0), Rot3(), sharedSigma(3, 0.01))); for(int j=0; j<6; ++j) { - truth.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j))); - initial.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); - fg.add(Between(Key(j), Key((j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01))); + truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j))); + initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); + fg.add(Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01))); } NonlinearOptimizationParameters params; diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index 23551bbae..6ab1ad929 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -525,8 +525,8 @@ TEST (Serialization, planar_system) { graph.add(constraint); // text - EXPECT(equalsObj(PoseKey(2))); - EXPECT(equalsObj(PointKey(3))); + EXPECT(equalsObj(PoseKey(2))); + EXPECT(equalsObj(PointKey(3))); EXPECT(equalsObj(values)); EXPECT(equalsObj(prior)); EXPECT(equalsObj(bearing)); @@ -537,8 +537,8 @@ TEST (Serialization, planar_system) { EXPECT(equalsObj(graph)); // xml - EXPECT(equalsXML(PoseKey(2))); - EXPECT(equalsXML(PointKey(3))); + EXPECT(equalsXML(PoseKey(2))); + EXPECT(equalsXML(PointKey(3))); EXPECT(equalsXML(values)); EXPECT(equalsXML(prior)); EXPECT(equalsXML(bearing)); @@ -562,8 +562,8 @@ BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoF TEST (Serialization, visual_system) { using namespace visualSLAM; Values values; - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); Pose3 pose1 = pose3, pose2 = pose3.inverse(); Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); values.insert(x1, pose1); @@ -574,7 +574,7 @@ TEST (Serialization, visual_system) { boost::shared_ptr K(new Cal3_S2(cal1)); Graph graph; - graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K); + graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K); graph.addPointConstraint(1, pt1); graph.addPointPrior(1, pt2, model3); graph.addPoseConstraint(1, pose1); From 3d40f5e6fc602ab8525a0d0c441c335323da7e0a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 7 Feb 2012 04:58:11 +0000 Subject: [PATCH 6/7] All unit tests pass with TypedSymbol removed --- examples/CameraResectioning.cpp | 2 +- examples/PlanarSLAMSelfContained_advanced.cpp | 2 +- examples/SimpleRotation.cpp | 2 +- examples/elaboratePoint2KalmanFilter.cpp | 2 +- examples/vSLAMexample/vISAMexample.cpp | 2 +- examples/vSLAMexample/vSFMexample.cpp | 2 +- gtsam/inference/tests/testFactorGraph.cpp | 2 +- gtsam/inference/tests/testISAM.cpp | 2 +- gtsam/inference/tests/testJunctionTree.cpp | 2 +- gtsam/inference/tests/timeSymbolMaps.cpp | 2 +- gtsam/linear/tests/timeGaussianFactor.cpp | 3 - gtsam/nonlinear/Key.h | 371 ------------------ gtsam/nonlinear/Makefile.am | 2 +- gtsam/nonlinear/Ordering.h | 2 +- gtsam/nonlinear/Symbol.h | 136 +++++++ gtsam/nonlinear/Values.h | 2 +- gtsam/nonlinear/tests/testKey.cpp | 97 +---- gtsam/nonlinear/tests/testValues.cpp | 28 +- gtsam/slam/pose2SLAM.h | 2 +- gtsam/slam/pose3SLAM.h | 2 +- gtsam/slam/simulated3D.h | 2 +- gtsam/slam/smallExample.cpp | 2 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 2 +- .../testGeneralSFMFactor_Cal3Bundler.cpp | 2 +- gtsam/slam/tests/testPose2SLAM.cpp | 2 +- gtsam/slam/tests/testPose3SLAM.cpp | 2 +- gtsam/slam/tests/testProjectionFactor.cpp | 2 +- gtsam/slam/tests/testVSLAM.cpp | 2 +- gtsam/slam/visualSLAM.h | 2 +- tests/testGaussianBayesNet.cpp | 2 +- tests/testGaussianFactor.cpp | 2 +- tests/testGaussianFactorGraph.cpp | 2 +- tests/testGaussianISAM.cpp | 2 +- tests/testGaussianJunctionTree.cpp | 2 +- tests/testGraph.cpp | 25 +- tests/testInference.cpp | 2 +- tests/testNonlinearFactor.cpp | 2 +- tests/testNonlinearFactorGraph.cpp | 2 +- tests/testNonlinearOptimizer.cpp | 2 +- tests/testSerialization.cpp | 14 +- tests/testSymbolicBayesNet.cpp | 2 +- tests/testSymbolicFactorGraph.cpp | 2 +- tests/testTupleValues.cpp | 4 +- tests/timeGaussianFactorGraph.cpp | 2 +- 44 files changed, 206 insertions(+), 544 deletions(-) delete mode 100644 gtsam/nonlinear/Key.h create mode 100644 gtsam/nonlinear/Symbol.h diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 957931af4..1bfd6c785 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -16,7 +16,7 @@ * @date Aug 23, 2011 */ -#include +#include #include #include #include diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index 9dc908bc0..65b201ab7 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -19,7 +19,7 @@ #include // for all nonlinear keys -#include +#include // for points and poses #include diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 29bab981a..71727f750 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 1c3415e61..ee0d82501 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 89ee250f9..ae26a9c47 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -20,7 +20,7 @@ #include using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index 0ecd2e3e9..468620f29 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -19,7 +19,7 @@ #include using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index 832d03cea..641b1f4f9 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -25,7 +25,7 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/inference/tests/testISAM.cpp b/gtsam/inference/tests/testISAM.cpp index 55922589c..1c6a2897c 100644 --- a/gtsam/inference/tests/testISAM.cpp +++ b/gtsam/inference/tests/testISAM.cpp @@ -21,7 +21,7 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp index 13f047e88..7374bbad8 100644 --- a/gtsam/inference/tests/testJunctionTree.cpp +++ b/gtsam/inference/tests/testJunctionTree.cpp @@ -24,7 +24,7 @@ using namespace boost::assign; #include #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/inference/tests/timeSymbolMaps.cpp b/gtsam/inference/tests/timeSymbolMaps.cpp index 1bf83ca5d..61ce1b30c 100644 --- a/gtsam/inference/tests/timeSymbolMaps.cpp +++ b/gtsam/inference/tests/timeSymbolMaps.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include using namespace std; using namespace boost; diff --git a/gtsam/linear/tests/timeGaussianFactor.cpp b/gtsam/linear/tests/timeGaussianFactor.cpp index d39be1061..3895ab520 100644 --- a/gtsam/linear/tests/timeGaussianFactor.cpp +++ b/gtsam/linear/tests/timeGaussianFactor.cpp @@ -15,9 +15,6 @@ * @author Alireza Fathi */ -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include /*STL/C++*/ diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h deleted file mode 100644 index 9bff0511d..000000000 --- a/gtsam/nonlinear/Key.h +++ /dev/null @@ -1,371 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Key.h - * @date Jan 12, 2010 - * @author: Frank Dellaert - * @author: Richard Roberts - */ - -#pragma once - -#include -#include -#include -#include -#include -#ifdef GTSAM_MAGIC_KEY -#include -#endif - -#define ALPHA '\224' - -namespace gtsam { - - // Forward declarations - class Symbol; - -/** - * TypedSymbol key class is templated on - * 1) the type T it is supposed to retrieve, for extra type checking - * 2) the character constant used for its string representation - */ -template -class TypedSymbol { - -protected: - size_t j_; - -public: - - // typedefs - typedef T Value; - typedef boost::mpl::char_ Chr; // to reconstruct the type: use Chr::value - - // Constructors: - - TypedSymbol() : - j_(0) { - } - TypedSymbol(size_t j) : - j_(j) { - } - - virtual ~TypedSymbol() {} - - // Get stuff: - ///TODO: comment - size_t index() const { - return j_; - } - static char chr() { - return C; - } - const char* c_str() const { - return ((std::string) (*this)).c_str(); - } - operator std::string() const { - return (boost::format("%c%d") % C % j_).str(); - } - std::string latex() const { - return (boost::format("%c_{%d}") % C % j_).str(); - } - Symbol symbol() const; - - // logic: - - bool operator<(const TypedSymbol& compare) const { - return j_ < compare.j_; - } - bool operator==(const TypedSymbol& compare) const { - return j_ == compare.j_; - } - bool operator!=(const TypedSymbol& compare) const { - return j_ != compare.j_; - } - int compare(const TypedSymbol& compare) const { - return j_ - compare.j_; - } - - // Testable Requirements - virtual void print(const std::string& s = "") const { - std::cout << s << ": " << (std::string) (*this) << std::endl; - } - bool equals(const TypedSymbol& expected, double tol = 0.0) const { - return (*this) == expected; - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(j_); - } -}; - -/** forward declaration to avoid circular dependencies */ -template -class TypedLabeledSymbol; - -/** - * Character and index key used in VectorValues, GaussianFactorGraph, - * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol - * keys when linearizing a nonlinear factor graph. This key is not type - * safe, so cannot be used with any Nonlinear* classes. - */ -class Symbol { -protected: - unsigned char c_; - size_t j_; - -public: - /** Default constructor */ - Symbol() : - c_(0), j_(0) { - } - - /** Copy constructor */ - Symbol(const Symbol& key) : - c_(key.c_), j_(key.j_) { - } - - /** Constructor */ - Symbol(unsigned char c, size_t j) : - c_(c), j_(j) { - } - - /** Casting constructor from TypedSymbol */ - template - Symbol(const TypedSymbol& symbol) : - c_(C), j_(symbol.index()) { - } - - /** Casting constructor from TypedLabeledSymbol */ - template - Symbol(const TypedLabeledSymbol& symbol) : - c_(C), j_(symbol.encode()) { - } - - /** "Magic" key casting constructor from string */ -#ifdef GTSAM_MAGIC_KEY - Symbol(const std::string& str) { - if(str.length() < 1) - throw std::invalid_argument("Cannot parse string key '" + str + "'"); - else { - const char *c_str = str.c_str(); - c_ = c_str[0]; - if(str.length() > 1) - j_ = boost::lexical_cast(c_str+1); - else - j_ = 0; - } - } - - Symbol(const char *c_str) { - std::string str(c_str); - if(str.length() < 1) - throw std::invalid_argument("Cannot parse string key '" + str + "'"); - else { - c_ = c_str[0]; - if(str.length() > 1) - j_ = boost::lexical_cast(c_str+1); - else - j_ = 0; - } - } -#endif - - // Testable Requirements - void print(const std::string& s = "") const { - std::cout << s << ": " << (std::string) (*this) << std::endl; - } - bool equals(const Symbol& expected, double tol = 0.0) const { - return (*this) == expected; - } - - /** Retrieve key character */ - unsigned char chr() const { - return c_; - } - - /** Retrieve key index */ - size_t index() const { - return j_; - } - - /** Create a string from the key */ - operator std::string() const { - return str(boost::format("%c%d") % c_ % j_); - } - - /** Comparison for use in maps */ - bool operator<(const Symbol& comp) const { - return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_); - } - bool operator==(const Symbol& comp) const { - return comp.c_ == c_ && comp.j_ == j_; - } - bool operator!=(const Symbol& comp) const { - return comp.c_ != c_ || comp.j_ != j_; - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(c_); - ar & BOOST_SERIALIZATION_NVP(j_); - } -}; - -// Conversion utilities - -template Symbol key2symbol(KEY key) { - return Symbol(key); -} - -template std::list keys2symbols(std::list keys) { - std::list symbols; - std::transform(keys.begin(), keys.end(), std::back_inserter(symbols), - key2symbol ); - return symbols; -} - -/** - * TypedLabeledSymbol is a variation of the TypedSymbol that allows - * for a runtime label to be placed on the label, so as to express - * "Pose 5 for robot 3" - * Labels should be kept to base datatypes (int, char, etc) to - * minimize cost of comparisons - * - * The labels will be compared first when comparing Keys, followed by the - * index - */ -template -class TypedLabeledSymbol: public TypedSymbol { - -protected: - // Label - L label_; - -public: - - typedef TypedSymbol Base; - - // Constructors: - - TypedLabeledSymbol() { - } - TypedLabeledSymbol(size_t j, L label) : - Base(j), label_(label) { - } - - /** Constructor that decodes encoded labels */ - TypedLabeledSymbol(const Symbol& sym) : - TypedSymbol (0) { - size_t shift = (sizeof(size_t) - sizeof(short)) * 8; - this->j_ = (sym.index() << shift) >> shift; // truncate upper bits - label_ = (L) (sym.index() >> shift); // remove lower bits - } - - /** Constructor to upgrade an existing typed label with a label */ - TypedLabeledSymbol(const Base& key, L label) : - Base(key.index()), label_(label) { - } - - // Get stuff: - - L label() const { - return label_; - } - const char* c_str() const { - return ((std::string)(*this)).c_str(); - } - operator std::string() const { - std::string label_s = (boost::format("%1%") % label_).str(); - return (boost::format("%c%s_%d") % C % label_s % this->j_).str(); - } - std::string latex() const { - std::string label_s = (boost::format("%1%") % label_).str(); - return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str(); - } - Symbol symbol() const { - return Symbol(*this); - } - - // Needed for conversion to LabeledSymbol - size_t convertLabel() const { - return label_; - } - - /** - * Encoding two numbers into a single size_t for conversion to Symbol - * Stores the label in the upper bytes of the index - */ - size_t encode() const { - short label = (short) label_; //bound size of label to 2 bytes - size_t shift = (sizeof(size_t) - sizeof(short)) * 8; - size_t modifier = ((size_t) label) << shift; - return this->j_ + modifier; - } - - // logic: - - bool operator<(const TypedLabeledSymbol& compare) const { - if (label_ == compare.label_) // sort by label first - return this->j_ < compare.j_; - else - return label_ < compare.label_; - } - bool operator==(const TypedLabeledSymbol& compare) const { - return this->j_ == compare.j_ && label_ == compare.label_; - } - int compare(const TypedLabeledSymbol& compare) const { - if (label_ == compare.label_) // sort by label first - return this->j_ - compare.j_; - else - return label_ - compare.label_; - } - - // Testable Requirements - void print(const std::string& s = "") const { - std::cout << s << ": " << (std::string) (*this) << std::endl; - } - bool equals(const TypedLabeledSymbol& expected, double tol = 0.0) const { - return (*this) == expected; - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - typedef TypedSymbol Base; - ar & boost::serialization::make_nvp("TypedLabeledSymbol", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(label_); - } -}; - -/* ************************************************************************* */ -template -Symbol TypedSymbol::symbol() const { - return Symbol(*this); -} - -} // namespace gtsam - diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 76a41247c..0512c36e1 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -21,7 +21,7 @@ sources += Values.cpp check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering # Nonlinear nonlinear -headers += Key.h +headers += Symbol.h headers += NonlinearFactorGraph.h headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h headers += NonlinearFactor.h diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index 4d10b15b5..47d048f2b 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h new file mode 100644 index 000000000..f2963dc32 --- /dev/null +++ b/gtsam/nonlinear/Symbol.h @@ -0,0 +1,136 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Symbol.h + * @date Jan 12, 2010 + * @author: Frank Dellaert + * @author: Richard Roberts + */ + +#pragma once + +#include +#include +#include +#include +#include +#ifdef GTSAM_MAGIC_KEY +#include +#endif + +#define ALPHA '\224' + +namespace gtsam { + +/** + * Character and index key used in VectorValues, GaussianFactorGraph, + * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol + * keys when linearizing a nonlinear factor graph. This key is not type + * safe, so cannot be used with any Nonlinear* classes. + */ +class Symbol { +protected: + unsigned char c_; + size_t j_; + +public: + /** Default constructor */ + Symbol() : + c_(0), j_(0) { + } + + /** Copy constructor */ + Symbol(const Symbol& key) : + c_(key.c_), j_(key.j_) { + } + + /** Constructor */ + Symbol(unsigned char c, size_t j) : + c_(c), j_(j) { + } + + /** "Magic" key casting constructor from string */ +#ifdef GTSAM_MAGIC_KEY + Symbol(const std::string& str) { + if(str.length() < 1) + throw std::invalid_argument("Cannot parse string key '" + str + "'"); + else { + const char *c_str = str.c_str(); + c_ = c_str[0]; + if(str.length() > 1) + j_ = boost::lexical_cast(c_str+1); + else + j_ = 0; + } + } + + Symbol(const char *c_str) { + std::string str(c_str); + if(str.length() < 1) + throw std::invalid_argument("Cannot parse string key '" + str + "'"); + else { + c_ = c_str[0]; + if(str.length() > 1) + j_ = boost::lexical_cast(c_str+1); + else + j_ = 0; + } + } +#endif + + // Testable Requirements + void print(const std::string& s = "") const { + std::cout << s << ": " << (std::string) (*this) << std::endl; + } + bool equals(const Symbol& expected, double tol = 0.0) const { + return (*this) == expected; + } + + /** Retrieve key character */ + unsigned char chr() const { + return c_; + } + + /** Retrieve key index */ + size_t index() const { + return j_; + } + + /** Create a string from the key */ + operator std::string() const { + return str(boost::format("%c%d") % c_ % j_); + } + + /** Comparison for use in maps */ + bool operator<(const Symbol& comp) const { + return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_); + } + bool operator==(const Symbol& comp) const { + return comp.c_ == c_ && comp.j_ == j_; + } + bool operator!=(const Symbol& comp) const { + return comp.c_ != c_ || comp.j_ != j_; + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(c_); + ar & BOOST_SERIALIZATION_NVP(j_); + } +}; + +} // namespace gtsam + diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index f226007f1..bad281a9a 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include namespace gtsam { diff --git a/gtsam/nonlinear/tests/testKey.cpp b/gtsam/nonlinear/tests/testKey.cpp index e05e5d8b5..19c3b6325 100644 --- a/gtsam/nonlinear/tests/testKey.cpp +++ b/gtsam/nonlinear/tests/testKey.cpp @@ -19,106 +19,11 @@ using namespace boost::assign; #include #include -#include +#include using namespace std; using namespace gtsam; -class Pose3; - -/* ************************************************************************* */ -TEST ( TypedSymbol, basic_operations ) { - typedef TypedSymbol Key; - - Key key1(0), - key2(0), - key3(1), - key4(2); - - CHECK(key1.index()==0); - CHECK(key1 == key2); - CHECK(assert_equal(key1, key2)); - CHECK(!(key1 == key3)); - CHECK(key1 < key3); - CHECK(key3 < key4); -} - -/* ************************************************************************* */ -TEST ( TypedLabledSymbol, basic_operations ) { - typedef TypedSymbol SimpleKey; - typedef TypedLabeledSymbol RobotKey; - - SimpleKey key7(1); - RobotKey key1(0, 1), - key2(0, 1), - key3(1, 1), - key4(2, 1), - key5(0, 2), - key6(1, 2), - key8(1, 3), - key9(key7, 3); - - - CHECK(key1.label()==1); - CHECK(key1.index()==0); - CHECK(key1 == key2); - CHECK(assert_equal(key1, key2)); - CHECK(!(key1 == key3)); - CHECK(key1 < key3); - CHECK(key3 < key4); - CHECK(!(key1 == key5)); - CHECK(key1 < key5); - CHECK(key5 < key6); - CHECK(assert_equal(key9, key8)); -} - -/* ************************************************************************* */ -TEST ( TypedLabledSymbol, encoding ) { - typedef TypedLabeledSymbol RobotKey; - - RobotKey key1(37, 'A'); - - // Note: calculations done in test due to possible differences between machines - // take the upper two bytes for the label - short label = key1.label(); - - // find the shift necessary - size_t shift = (sizeof(size_t)-sizeof(short)) * 8; - size_t modifier = label; - modifier = modifier << shift; - size_t index = key1.index() + modifier; - - // check index encoding - Symbol act1(key1), exp('x', index); - CHECK(assert_equal(exp, act1)); - - // check casting - Symbol act2 = (Symbol) key1; - CHECK(assert_equal(exp, act2)); - - // decode - CHECK(assert_equal(key1, RobotKey(act1))); -} - -/* ************************************************************************* */ -TEST ( TypedLabledSymbol, template_reconstruction ) { - typedef TypedSymbol Key; - typedef TypedLabeledSymbol NewKey; - NewKey k(1, 'A'); -} - -/* ************************************************************************* */ -TEST ( Key, keys2symbols ) -{ - typedef TypedSymbol Key; - list expected; - expected += Key(1), Key(2), Key(3); - - list > typeds; - typeds += 1, 2, 3; - CHECK(expected == keys2symbols(typeds)); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index c66e59ead..bb68ef9e6 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -20,6 +20,8 @@ #include // for operator += using namespace boost::assign; +#define GTSAM_MAGIC_KEY + #include #include #include @@ -29,9 +31,7 @@ using namespace gtsam; using namespace std; static double inf = std::numeric_limits::infinity(); -typedef TypedSymbol VecKey; - -VecKey key1(1), key2(2), key3(3), key4(4); +Symbol key1("v1"), key2("v2"), key3("v3"), key4("v4"); /* ************************************************************************* */ TEST( Values, equals1 ) @@ -114,11 +114,11 @@ TEST( Values, update_element ) cfg.insert(key1, v1); CHECK(cfg.size() == 1); - CHECK(assert_equal(v1, cfg.at(key1))); + CHECK(assert_equal(v1, cfg.at(key1))); cfg.update(key1, v2); CHECK(cfg.size() == 1); - CHECK(assert_equal(v2, cfg.at(key1))); + CHECK(assert_equal(v2, cfg.at(key1))); } ///* ************************************************************************* */ @@ -200,10 +200,9 @@ TEST(Values, expmap_d) CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); - typedef TypedSymbol PoseKey; Values poseconfig; - poseconfig.insert(PoseKey(1), Pose2(1,2,3)); - poseconfig.insert(PoseKey(2), Pose2(0.3, 0.4, 0.5)); + poseconfig.insert("p1", Pose2(1,2,3)); + poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); @@ -212,16 +211,15 @@ TEST(Values, expmap_d) /* ************************************************************************* */ TEST(Values, extract_keys) { - typedef TypedSymbol PoseKey; Values config; - config.insert(PoseKey(1), Pose2()); - config.insert(PoseKey(2), Pose2()); - config.insert(PoseKey(4), Pose2()); - config.insert(PoseKey(5), Pose2()); + config.insert("x1", Pose2()); + config.insert("x2", Pose2()); + config.insert("x4", Pose2()); + config.insert("x5", Pose2()); FastList expected, actual; - expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5); + expected += "x1", "x2", "x4", "x5"; actual = config.keys(); CHECK(actual.size() == expected.size()); @@ -238,7 +236,7 @@ TEST(Values, exists_) config0.insert(key1, LieVector(Vector_(1, 1.))); config0.insert(key2, LieVector(Vector_(1, 2.))); - boost::optional v = config0.exists(key1); + boost::optional v = config0.exists(key1); CHECK(assert_equal(Vector_(1, 1.),*v)); } diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 7917a3c79..121e0c062 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index 555e5fc91..eb4eb9573 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h index 40adcdbe2..9cd1ed74d 100644 --- a/gtsam/slam/simulated3D.h +++ b/gtsam/slam/simulated3D.h @@ -23,7 +23,7 @@ #include #include #include -#include +#include // \namespace diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 8ee8cb09a..4c13ab2b3 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -24,7 +24,7 @@ using namespace std; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 8c59d63ac..1f1e616f1 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -12,7 +12,7 @@ #include using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 6b1ace287..89dbf15ea 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -12,7 +12,7 @@ #include using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 29c28dbc1..4646bb5d7 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -14,7 +14,7 @@ * @author Frank Dellaert, Viorela Ila **/ -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 21c0fd458..77c206918 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -28,7 +28,7 @@ using namespace boost::assign; // TODO: DANGEROUS, create shared pointers #define GTSAM_MAGIC_GAUSSIAN 6 -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index f7f804fb9..00c41aaad 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -18,7 +18,7 @@ #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp index ccae629eb..fb53d4b65 100644 --- a/gtsam/slam/tests/testVSLAM.cpp +++ b/gtsam/slam/tests/testVSLAM.cpp @@ -22,7 +22,7 @@ #include using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index df137ad47..40e5eafcc 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testGaussianBayesNet.cpp b/tests/testGaussianBayesNet.cpp index 7f746397c..dd8f49988 100644 --- a/tests/testGaussianBayesNet.cpp +++ b/tests/testGaussianBayesNet.cpp @@ -25,7 +25,7 @@ #include // for operator += using namespace boost::assign; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index 1260c51a2..f6cf7d54b 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -26,7 +26,7 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index d95a05830..73b1f2d63 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -28,7 +28,7 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 3b432393d..bd592fc68 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -21,7 +21,7 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index ff1732ec5..19b0c676d 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -25,7 +25,7 @@ #include using namespace boost::assign; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 34439b73a..2ac2ce691 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -23,6 +23,8 @@ using namespace boost::assign; #include +#define GTSAM_MAGIC_KEY + // TODO: DANGEROUS, create shared pointers #define GTSAM_MAGIC_GAUSSIAN 3 @@ -37,22 +39,21 @@ using namespace gtsam; // -> x3 -> x4 // -> x5 TEST ( Ordering, predecessorMap2Keys ) { - typedef TypedSymbol PoseKey; - PredecessorMap p_map; - p_map.insert(1,1); - p_map.insert(2,1); - p_map.insert(3,1); - p_map.insert(4,3); - p_map.insert(5,1); + PredecessorMap p_map; + p_map.insert("x1","x1"); + p_map.insert("x2","x1"); + p_map.insert("x3","x1"); + p_map.insert("x4","x3"); + p_map.insert("x5","x1"); - list expected; - expected += 4,5,3,2,1;//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1); + list expected; + expected += "x4","x5","x3","x2","x1";//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1); - list actual = predecessorMap2Keys(p_map); + list actual = predecessorMap2Keys(p_map); LONGS_EQUAL(expected.size(), actual.size()); - list::const_iterator it1 = expected.begin(); - list::const_iterator it2 = actual.begin(); + list::const_iterator it1 = expected.begin(); + list::const_iterator it2 = actual.begin(); for(; it1!=expected.end(); it1++, it2++) CHECK(*it1 == *it2) } diff --git a/tests/testInference.cpp b/tests/testInference.cpp index 5c60f08e7..d1ef745e6 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -17,7 +17,7 @@ #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 488d406dc..2b9037ac2 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -25,7 +25,7 @@ // TODO: DANGEROUS, create shared pointers #define GTSAM_MAGIC_GAUSSIAN 2 -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 1cca4c988..938067508 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -27,7 +27,7 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 88d142bf6..f7a09f95f 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -26,7 +26,7 @@ using namespace boost::assign; #include using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index 6ab1ad929..fa9944d1b 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -145,7 +145,7 @@ bool equalsDereferencedXML(const T& input = T()) { // Actual Tests /* ************************************************************************* */ -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include @@ -255,16 +255,12 @@ typedef PinholeCamera PinholeCal3S2; typedef PinholeCamera PinholeCal3DS2; typedef PinholeCamera PinholeCal3Bundler; -typedef TypedSymbol PinholeCal3S2Key; -typedef TypedSymbol PinholeCal3DS2Key; -typedef TypedSymbol PinholeCal3BundlerKey; - TEST (Serialization, TemplatedValues) { Values values; - values.insert(PinholeCal3S2Key(0), PinholeCal3S2(pose3, cal1)); - values.insert(PinholeCal3DS2Key(5), PinholeCal3DS2(pose3, cal2)); - values.insert(PinholeCal3BundlerKey(47), PinholeCal3Bundler(pose3, cal3)); - values.insert(PinholeCal3S2Key(5), PinholeCal3S2(pose3, cal1)); + values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1)); + values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2)); + values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3)); + values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1)); EXPECT(equalsObj(values)); EXPECT(equalsXML(values)); } diff --git a/tests/testSymbolicBayesNet.cpp b/tests/testSymbolicBayesNet.cpp index de51cb8dc..efa794a25 100644 --- a/tests/testSymbolicBayesNet.cpp +++ b/tests/testSymbolicBayesNet.cpp @@ -21,7 +21,7 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraph.cpp index 01515f079..4295a5263 100644 --- a/tests/testSymbolicFactorGraph.cpp +++ b/tests/testSymbolicFactorGraph.cpp @@ -20,7 +20,7 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp index 3df49061c..06232c64e 100644 --- a/tests/testTupleValues.cpp +++ b/tests/testTupleValues.cpp @@ -20,7 +20,7 @@ #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include @@ -29,7 +29,7 @@ #include #include -#include +#include #include using namespace gtsam; diff --git a/tests/timeGaussianFactorGraph.cpp b/tests/timeGaussianFactorGraph.cpp index e6b028e6d..3af046971 100644 --- a/tests/timeGaussianFactorGraph.cpp +++ b/tests/timeGaussianFactorGraph.cpp @@ -15,7 +15,7 @@ * @author Frank Dellaert */ -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h +// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h #define GTSAM_MAGIC_KEY #include From 669c7c8dc8626e88c9bf1f0a96c4c25aac282ff2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 8 Feb 2012 21:53:02 +0000 Subject: [PATCH 7/7] Split up serialization unit tests into subdirectories to speed up --- gtsam/base/Makefile.am | 10 + gtsam/base/serializationTestHelpers.h | 148 +++++ gtsam/base/tests/testSerializationBase.cpp | 41 ++ gtsam/geometry/Makefile.am | 10 + .../tests/testSerializationGeometry.cpp | 115 ++++ gtsam/inference/Makefile.am | 10 + .../tests/testSerializationInference.cpp | 91 +++ gtsam/linear/Makefile.am | 10 + .../linear/tests/testSerializationLinear.cpp | 161 +++++ gtsam/nonlinear/Makefile.am | 10 + .../tests/testSerializationNonlinear.cpp | 69 ++ gtsam/slam/Makefile.am | 10 + gtsam/slam/tests/testSerializationSLAM.cpp | 209 +++++++ tests/Makefile.am | 10 - tests/testSerialization.cpp | 588 ------------------ 15 files changed, 894 insertions(+), 598 deletions(-) create mode 100644 gtsam/base/serializationTestHelpers.h create mode 100644 gtsam/base/tests/testSerializationBase.cpp create mode 100644 gtsam/geometry/tests/testSerializationGeometry.cpp create mode 100644 gtsam/inference/tests/testSerializationInference.cpp create mode 100644 gtsam/linear/tests/testSerializationLinear.cpp create mode 100644 gtsam/nonlinear/tests/testSerializationNonlinear.cpp create mode 100644 gtsam/slam/tests/testSerializationSLAM.cpp delete mode 100644 tests/testSerialization.cpp diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am index aa02ffec9..25abacbaf 100644 --- a/gtsam/base/Makefile.am +++ b/gtsam/base/Makefile.am @@ -36,6 +36,11 @@ headers += BTree.h DSF.h FastMap.h FastSet.h FastList.h FastVector.h sources += DSFVector.cpp check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationBase +endif + # Timing tests noinst_PROGRAMS = tests/timeMatrix tests/timeVirtual tests/timeVirtual2 tests/timeTest noinst_PROGRAMS += tests/timeMatrixOps @@ -56,6 +61,11 @@ AM_CPPFLAGS = AM_CPPFLAGS += $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationBase_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h new file mode 100644 index 000000000..ef2e9393d --- /dev/null +++ b/gtsam/base/serializationTestHelpers.h @@ -0,0 +1,148 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file serializationTestHelpers.h + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#pragma once + +#include +#include + +// includes for standard serialization types +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// whether to print the serialized text to stdout +const bool verbose = false; + +namespace gtsam { namespace serializationTestHelpers { + + /* ************************************************************************* */ + // Serialization testing code. + /* ************************************************************************* */ + +template +std::string serialize(const T& input) { + std::ostringstream out_archive_stream; + boost::archive::text_oarchive out_archive(out_archive_stream); + out_archive << input; + return out_archive_stream.str(); +} + +template +void deserialize(const std::string& serialized, T& output) { + std::istringstream in_archive_stream(serialized); + boost::archive::text_iarchive in_archive(in_archive_stream); + in_archive >> output; +} + +// Templated round-trip serialization +template +void roundtrip(const T& input, T& output) { + // Serialize + std::string serialized = serialize(input); + if (verbose) std::cout << serialized << std::endl << std::endl; + + deserialize(serialized, output); +} + +// This version requires equality operator +template +bool equality(const T& input = T()) { + T output; + roundtrip(input,output); + return input==output; +} + +// This version requires equals +template +bool equalsObj(const T& input = T()) { + T output; + roundtrip(input,output); + return input.equals(output); +} + +// De-referenced version for pointers +template +bool equalsDereferenced(const T& input) { + T output; + roundtrip(input,output); + return input->equals(*output); +} + +/* ************************************************************************* */ +template +std::string serializeXML(const T& input) { + std::ostringstream out_archive_stream; + boost::archive::xml_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp("data", input); + return out_archive_stream.str(); +} + +template +void deserializeXML(const std::string& serialized, T& output) { + std::istringstream in_archive_stream(serialized); + boost::archive::xml_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp("data", output); +} + +// Templated round-trip serialization using XML +template +void roundtripXML(const T& input, T& output) { + // Serialize + std::string serialized = serializeXML(input); + if (verbose) std::cout << serialized << std::endl << std::endl; + + // De-serialize + deserializeXML(serialized, output); +} + +// This version requires equality operator +template +bool equalityXML(const T& input = T()) { + T output; + roundtripXML(input,output); + return input==output; +} + +// This version requires equals +template +bool equalsXML(const T& input = T()) { + T output; + roundtripXML(input,output); + return input.equals(output); +} + +// This version is for pointers +template +bool equalsDereferencedXML(const T& input = T()) { + T output; + roundtripXML(input,output); + return input->equals(*output); +} + +} } diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp new file mode 100644 index 000000000..211a1c319 --- /dev/null +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -0,0 +1,41 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationBase.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + + +/* ************************************************************************* */ +TEST (Serialization, matrix_vector) { + EXPECT(equality(Vector_(4, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equality(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); + + EXPECT(equalityXML(Vector_(4, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityXML(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am index 5f9b34610..b87ba8d07 100644 --- a/gtsam/geometry/Makefile.am +++ b/gtsam/geometry/Makefile.am @@ -32,6 +32,11 @@ headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Ex sources += projectiveGeometry.cpp tensorInterface.cpp check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationGeometry +endif + # Timing tests noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera tests/timeStereoCamera @@ -53,6 +58,11 @@ libgeometry_la_SOURCES = $(allsources) AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationGeometry_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp new file mode 100644 index 000000000..c1fba6b2c --- /dev/null +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationGeometry.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Export all classes derived from Value +BOOST_CLASS_EXPORT(gtsam::Cal3_S2) +BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo) +BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) +BOOST_CLASS_EXPORT(gtsam::CalibratedCamera) +BOOST_CLASS_EXPORT(gtsam::Point2) +BOOST_CLASS_EXPORT(gtsam::Point3) +BOOST_CLASS_EXPORT(gtsam::Pose2) +BOOST_CLASS_EXPORT(gtsam::Pose3) +BOOST_CLASS_EXPORT(gtsam::Rot2) +BOOST_CLASS_EXPORT(gtsam::Rot3) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::StereoPoint2) + +/* ************************************************************************* */ +Point3 pt3(1.0, 2.0, 3.0); +Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +Pose3 pose3(rt3, pt3); + +Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); +Cal3Bundler cal3(1.0, 2.0, 3.0); +Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); +Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); +CalibratedCamera cal5(Pose3(rt3, pt3)); + +PinholeCamera cam1(pose3, cal1); +StereoCamera cam2(pose3, cal4ptr); +StereoPoint2 spt(1.0, 2.0, 3.0); + +/* ************************************************************************* */ +TEST (Serialization, text_geometry) { + EXPECT(equalsObj(Point2(1.0, 2.0))); + EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); + EXPECT(equalsObj(Rot2::fromDegrees(30.0))); + + EXPECT(equalsObj(pt3)); + EXPECT(equalsObj(rt3)); + EXPECT(equalsObj(Pose3(rt3, pt3))); + + EXPECT(equalsObj(cal1)); + EXPECT(equalsObj(cal2)); + EXPECT(equalsObj(cal3)); + EXPECT(equalsObj(cal4)); + EXPECT(equalsObj(cal5)); + + EXPECT(equalsObj(cam1)); + EXPECT(equalsObj(cam2)); + EXPECT(equalsObj(spt)); +} + +/* ************************************************************************* */ +TEST (Serialization, xml_geometry) { + EXPECT(equalsXML(Point2(1.0, 2.0))); + EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); + EXPECT(equalsXML(Rot2::fromDegrees(30.0))); + + EXPECT(equalsXML(pt3)); + EXPECT(equalsXML(rt3)); + EXPECT(equalsXML(Pose3(rt3, pt3))); + + EXPECT(equalsXML(cal1)); + EXPECT(equalsXML(cal2)); + EXPECT(equalsXML(cal3)); + EXPECT(equalsXML(cal4)); + EXPECT(equalsXML(cal5)); + + EXPECT(equalsXML(cam1)); + EXPECT(equalsXML(cam2)); + EXPECT(equalsXML(spt)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/inference/Makefile.am b/gtsam/inference/Makefile.am index da84df61b..cab5c5cbb 100644 --- a/gtsam/inference/Makefile.am +++ b/gtsam/inference/Makefile.am @@ -45,6 +45,11 @@ check_PROGRAMS += tests/testClusterTree check_PROGRAMS += tests/testJunctionTree check_PROGRAMS += tests/testPermutation +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationInference +endif + #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am @@ -64,6 +69,11 @@ AM_CPPFLAGS = $(ccolamd_inc) $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) AM_CXXFLAGS = +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationInference_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/inference/tests/testSerializationInference.cpp b/gtsam/inference/tests/testSerializationInference.cpp new file mode 100644 index 000000000..2db7014f1 --- /dev/null +++ b/gtsam/inference/tests/testSerializationInference.cpp @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationInference.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#define GTSAM_MAGIC_KEY + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +TEST (Serialization, symbolic_graph) { + Ordering o; o += "x1","l1","x2"; + // construct expected symbolic graph + SymbolicFactorGraph sfg; + sfg.push_factor(o["x1"]); + sfg.push_factor(o["x1"],o["x2"]); + sfg.push_factor(o["x1"],o["l1"]); + sfg.push_factor(o["l1"],o["x2"]); + + EXPECT(equalsObj(sfg)); + EXPECT(equalsXML(sfg)); +} + +/* ************************************************************************* */ +TEST (Serialization, symbolic_bn) { + Ordering o; o += "x2","l1","x1"; + + IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"])); + IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"])); + IndexConditional::shared_ptr x1(new IndexConditional(o["x1"])); + + SymbolicBayesNet sbn; + sbn.push_back(x2); + sbn.push_back(l1); + sbn.push_back(x1); + + EXPECT(equalsObj(sbn)); + EXPECT(equalsXML(sbn)); +} + +/* ************************************************************************* */ +TEST (Serialization, symbolic_bayes_tree ) { + typedef BayesTree SymbolicBayesTree; + static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; + IndexConditional::shared_ptr + B(new IndexConditional(_B_)), + L(new IndexConditional(_L_, _B_)), + E(new IndexConditional(_E_, _L_, _B_)), + S(new IndexConditional(_S_, _L_, _B_)), + T(new IndexConditional(_T_, _E_, _L_)), + X(new IndexConditional(_X_, _E_)); + + // Bayes Tree for Asia example + SymbolicBayesTree bayesTree; + SymbolicBayesTree::insert(bayesTree, B); + SymbolicBayesTree::insert(bayesTree, L); + SymbolicBayesTree::insert(bayesTree, E); + SymbolicBayesTree::insert(bayesTree, S); + SymbolicBayesTree::insert(bayesTree, T); + SymbolicBayesTree::insert(bayesTree, X); + + EXPECT(equalsObj(bayesTree)); + EXPECT(equalsXML(bayesTree)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am index 811ae7458..bd78bae52 100644 --- a/gtsam/linear/Makefile.am +++ b/gtsam/linear/Makefile.am @@ -41,6 +41,11 @@ sources += iterative.cpp SubgraphPreconditioner.cpp SubgraphSolver.cpp headers += IterativeSolver.h IterativeOptimizationParameters.h headers += SubgraphSolver-inl.h +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationLinear +endif + # Timing tests noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike @@ -58,6 +63,11 @@ AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) AM_CXXFLAGS = +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationLinear_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp new file mode 100644 index 000000000..8082d3626 --- /dev/null +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationLinear.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Noise model components +/* ************************************************************************* */ + +/* ************************************************************************* */ +// Export Noisemodels +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); + +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +/* ************************************************************************* */ +// example noise models +noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); +noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); +noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); +noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); + +/* ************************************************************************* */ +TEST (Serialization, noiseModels) { + // tests using pointers to the derived class + EXPECT( equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT( equalsDereferenced(gaussian3)); + EXPECT(equalsDereferencedXML(gaussian3)); + + EXPECT( equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT( equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); + + EXPECT( equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); +} + +/* ************************************************************************* */ +TEST (Serialization, SharedNoiseModel_noiseModels) { + SharedNoiseModel diag3_sg = diag3; + EXPECT(equalsDereferenced(diag3_sg)); + EXPECT(equalsDereferencedXML(diag3_sg)); + + EXPECT(equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT(equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT(equalsDereferenced(gaussian3)); + EXPECT(equalsDereferencedXML(gaussian3)); + + EXPECT(equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); + + EXPECT(equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); +} + +/* ************************************************************************* */ +TEST (Serialization, SharedDiagonal_noiseModels) { + EXPECT(equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT(equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT(equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); + + EXPECT(equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); +} + +/* ************************************************************************* */ +// Linear components +/* ************************************************************************* */ + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); + +/* ************************************************************************* */ +TEST (Serialization, linear_factors) { + VectorValues values; + values.insert(0, Vector_(1, 1.0)); + values.insert(1, Vector_(2, 2.0,3.0)); + values.insert(2, Vector_(2, 4.0,5.0)); + EXPECT(equalsObj(values)); + EXPECT(equalsXML(values)); + + Index i1 = 4, i2 = 7; + Matrix A1 = eye(3), A2 = -1.0 * eye(3); + Vector b = ones(3); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); + JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); + EXPECT(equalsObj(jacobianfactor)); + EXPECT(equalsXML(jacobianfactor)); + + HessianFactor hessianfactor(jacobianfactor); + EXPECT(equalsObj(hessianfactor)); + EXPECT(equalsXML(hessianfactor)); +} + +/* ************************************************************************* */ +TEST (Serialization, gaussian_conditional) { + Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.); + Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4); + Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34); + Vector d(2); d << 0.2, 0.5; + GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2)); + + EXPECT(equalsObj(cg)); + EXPECT(equalsXML(cg)); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 0512c36e1..15651a22a 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -44,6 +44,11 @@ headers += WhiteNoiseFactor.h # Kalman Filter headers += ExtendedKalmanFilter.h ExtendedKalmanFilter-inl.h +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationNonlinear +endif + #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am @@ -58,6 +63,11 @@ AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) AM_CXXFLAGS = +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationNonlinear_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp new file mode 100644 index 000000000..120a9fa94 --- /dev/null +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationNonlinear.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Export all classes derived from Value +BOOST_CLASS_EXPORT(gtsam::Cal3_S2) +BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) +BOOST_CLASS_EXPORT(gtsam::Point3) +BOOST_CLASS_EXPORT(gtsam::Pose3) +BOOST_CLASS_EXPORT(gtsam::Rot3) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) + +/* ************************************************************************* */ +typedef PinholeCamera PinholeCal3S2; +typedef PinholeCamera PinholeCal3DS2; +typedef PinholeCamera PinholeCal3Bundler; + +/* ************************************************************************* */ +Point3 pt3(1.0, 2.0, 3.0); +Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +Pose3 pose3(rt3, pt3); + +Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); +Cal3Bundler cal3(1.0, 2.0, 3.0); + +TEST (Serialization, TemplatedValues) { + Values values; + values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1)); + values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2)); + values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3)); + values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1)); + EXPECT(equalsObj(values)); + EXPECT(equalsXML(values)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 71eb94d64..cfa37c29d 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -54,6 +54,11 @@ check_PROGRAMS += tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bund headers += StereoFactor.h check_PROGRAMS += tests/testStereoFactor +## flag disabled to force this test to get updated properly +if ENABLE_SERIALIZATION +check_PROGRAMS += tests/testSerializationSLAM +endif + #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am @@ -67,6 +72,11 @@ libslam_la_SOURCES = $(sources) AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) +# link to serialization library for test +if ENABLE_SERIALIZATION +tests_testSerializationSLAM_LDFLAGS = -lboost_serialization +endif + #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- diff --git a/gtsam/slam/tests/testSerializationSLAM.cpp b/gtsam/slam/tests/testSerializationSLAM.cpp new file mode 100644 index 000000000..c75e0a148 --- /dev/null +++ b/gtsam/slam/tests/testSerializationSLAM.cpp @@ -0,0 +1,209 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationSLAM.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#define GTSAM_MAGIC_KEY + +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); + +/* ************************************************************************* */ +// Export Noisemodels +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); + +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +/* ************************************************************************* */ +TEST (Serialization, smallExample_linear) { + using namespace example; + + Ordering ordering; ordering += "x1","x2","l1"; + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + EXPECT(equalsObj(ordering)); + EXPECT(equalsXML(ordering)); + + EXPECT(equalsObj(fg)); + EXPECT(equalsXML(fg)); + + GaussianBayesNet cbn = createSmallGaussianBayesNet(); + EXPECT(equalsObj(cbn)); + EXPECT(equalsXML(cbn)); +} + +/* ************************************************************************* */ +TEST (Serialization, gaussianISAM) { + using namespace example; + Ordering ordering; + GaussianFactorGraph smoother; + boost::tie(smoother, ordering) = createSmoother(7); + BayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianISAM isam(bayesTree); + + EXPECT(equalsObj(isam)); + EXPECT(equalsXML(isam)); +} + +/* ************************************************************************* */ +/* Create GUIDs for factors in simulated2D */ +BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" ) +BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" ) +BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement") +BOOST_CLASS_EXPORT(gtsam::Point2) + +/* ************************************************************************* */ +TEST (Serialization, smallExample) { + using namespace example; + Graph nfg = createNonlinearFactorGraph(); + Values c1 = createValues(); + EXPECT(equalsObj(nfg)); + EXPECT(equalsXML(nfg)); + + EXPECT(equalsObj(c1)); + EXPECT(equalsXML(c1)); +} + +/* ************************************************************************* */ +/* Create GUIDs for factors */ +BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior, "gtsam::planarSLAM::Prior"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing, "gtsam::planarSLAM::Bearing"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Range, "gtsam::planarSLAM::Range"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry, "gtsam::planarSLAM::Odometry"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint"); + +BOOST_CLASS_EXPORT(gtsam::Pose2) + +/* ************************************************************************* */ +TEST (Serialization, planar_system) { + using namespace planarSLAM; + planarSLAM::Values values; + values.insert(PointKey(3), Point2(1.0, 2.0)); + values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); + + SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); + SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); + SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); + + Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1); + Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1); + Range range(PoseKey(2), PointKey(9), 7.0, model1); + BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2); + Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3); + Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2)); + + Graph graph; + graph.add(prior); + graph.add(bearing); + graph.add(range); + graph.add(bearingRange); + graph.add(odometry); + graph.add(constraint); + + // text + EXPECT(equalsObj(PoseKey(2))); + EXPECT(equalsObj(PointKey(3))); + EXPECT(equalsObj(values)); + EXPECT(equalsObj(prior)); + EXPECT(equalsObj(bearing)); + EXPECT(equalsObj(bearingRange)); + EXPECT(equalsObj(range)); + EXPECT(equalsObj(odometry)); + EXPECT(equalsObj(constraint)); + EXPECT(equalsObj(graph)); + + // xml + EXPECT(equalsXML(PoseKey(2))); + EXPECT(equalsXML(PointKey(3))); + EXPECT(equalsXML(values)); + EXPECT(equalsXML(prior)); + EXPECT(equalsXML(bearing)); + EXPECT(equalsXML(bearingRange)); + EXPECT(equalsXML(range)); + EXPECT(equalsXML(odometry)); + EXPECT(equalsXML(constraint)); + EXPECT(equalsXML(graph)); +} + +/* ************************************************************************* */ +/* Create GUIDs for factors */ +BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor"); + +BOOST_CLASS_EXPORT(gtsam::Pose3) +BOOST_CLASS_EXPORT(gtsam::Point3) + +Point3 pt3(1.0, 2.0, 3.0); +Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +Pose3 pose3(rt3, pt3); +Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); + +/* ************************************************************************* */ +TEST (Serialization, visual_system) { + using namespace visualSLAM; + Values values; + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); + Pose3 pose1 = pose3, pose2 = pose3.inverse(); + Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); + values.insert(x1, pose1); + values.insert(l1, pt1); + SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); + SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); + SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); + boost::shared_ptr K(new Cal3_S2(cal1)); + + Graph graph; + graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K); + graph.addPointConstraint(1, pt1); + graph.addPointPrior(1, pt2, model3); + graph.addPoseConstraint(1, pose1); + graph.addPosePrior(1, pose2, model6); + + EXPECT(equalsObj(values)); + EXPECT(equalsObj(graph)); + + EXPECT(equalsXML(values)); + EXPECT(equalsXML(graph)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/tests/Makefile.am b/tests/Makefile.am index fb1172222..66044ccfc 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -24,11 +24,6 @@ check_PROGRAMS += testGaussianISAM2 check_PROGRAMS += testExtendedKalmanFilter check_PROGRAMS += testRot3Optimization -## flag disabled to force this test to get updated properly -#if ENABLE_SERIALIZATION -check_PROGRAMS += testSerialization -#endif - # Timing tests noinst_PROGRAMS = timeGaussianFactorGraph timeSequentialOnDataset timeMultifrontalOnDataset @@ -39,11 +34,6 @@ TESTS = $(check_PROGRAMS) AM_LDFLAGS = $(BOOST_LDFLAGS) AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -# link to serialization library for test -#if ENABLE_SERIALIZATION -AM_LDFLAGS += -lboost_serialization -#endif - LDADD = ../gtsam/libgtsam.la ../CppUnitLite/libCppUnitLite.a AM_DEFAULT_SOURCE_EXT = .cpp diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp deleted file mode 100644 index fa9944d1b..000000000 --- a/tests/testSerialization.cpp +++ /dev/null @@ -1,588 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @brief Unit tests for serialization of library classes - * @author Frank Dellaert - * @author Alex Cunningham - */ - -/* ************************************************************************* */ -// Serialization testing code. -/* ************************************************************************* */ - -#include -#include - -// includes for standard serialization types -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -// whether to print the serialized text to stdout -const bool verbose = false; - -template -std::string serialize(const T& input) { - std::ostringstream out_archive_stream; - boost::archive::text_oarchive out_archive(out_archive_stream); - out_archive << input; - return out_archive_stream.str(); -} - -template -void deserialize(const std::string& serialized, T& output) { - std::istringstream in_archive_stream(serialized); - boost::archive::text_iarchive in_archive(in_archive_stream); - in_archive >> output; -} - -// Templated round-trip serialization -template -void roundtrip(const T& input, T& output) { - // Serialize - std::string serialized = serialize(input); - if (verbose) std::cout << serialized << std::endl << std::endl; - - deserialize(serialized, output); -} - -// This version requires equality operator -template -bool equality(const T& input = T()) { - T output; - roundtrip(input,output); - return input==output; -} - -// This version requires equals -template -bool equalsObj(const T& input = T()) { - T output; - roundtrip(input,output); - return input.equals(output); -} - -// De-referenced version for pointers -template -bool equalsDereferenced(const T& input) { - T output; - roundtrip(input,output); - return input->equals(*output); -} - -/* ************************************************************************* */ -template -std::string serializeXML(const T& input) { - std::ostringstream out_archive_stream; - boost::archive::xml_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp("data", input); - return out_archive_stream.str(); -} - -template -void deserializeXML(const std::string& serialized, T& output) { - std::istringstream in_archive_stream(serialized); - boost::archive::xml_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp("data", output); -} - -// Templated round-trip serialization using XML -template -void roundtripXML(const T& input, T& output) { - // Serialize - std::string serialized = serializeXML(input); - if (verbose) std::cout << serialized << std::endl << std::endl; - - // De-serialize - deserializeXML(serialized, output); -} - -// This version requires equality operator -template -bool equalityXML(const T& input = T()) { - T output; - roundtripXML(input,output); - return input==output; -} - -// This version requires equals -template -bool equalsXML(const T& input = T()) { - T output; - roundtripXML(input,output); - return input.equals(output); -} - -// This version is for pointers -template -bool equalsDereferencedXML(const T& input = T()) { - T output; - roundtripXML(input,output); - return input->equals(*output); -} - -/* ************************************************************************* */ -// Actual Tests -/* ************************************************************************* */ - -// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using namespace std; -using namespace gtsam; - - -/* ************************************************************************* */ -TEST (Serialization, matrix_vector) { - EXPECT(equality(Vector_(4, 1.0, 2.0, 3.0, 4.0))); - EXPECT(equality(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); - - EXPECT(equalityXML(Vector_(4, 1.0, 2.0, 3.0, 4.0))); - EXPECT(equalityXML(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); -} - -/* ************************************************************************* */ -// Export all classes derived from Value -BOOST_CLASS_EXPORT(gtsam::Cal3_S2) -BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo) -BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) -BOOST_CLASS_EXPORT(gtsam::CalibratedCamera) -BOOST_CLASS_EXPORT(gtsam::Point2) -BOOST_CLASS_EXPORT(gtsam::Point3) -BOOST_CLASS_EXPORT(gtsam::Pose2) -BOOST_CLASS_EXPORT(gtsam::Pose3) -BOOST_CLASS_EXPORT(gtsam::Rot2) -BOOST_CLASS_EXPORT(gtsam::Rot3) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::StereoPoint2) - -/* ************************************************************************* */ -Point3 pt3(1.0, 2.0, 3.0); -Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -Pose3 pose3(rt3, pt3); - -Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); -Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); -Cal3Bundler cal3(1.0, 2.0, 3.0); -Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); -Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); -CalibratedCamera cal5(Pose3(rt3, pt3)); - -PinholeCamera cam1(pose3, cal1); -StereoCamera cam2(pose3, cal4ptr); -StereoPoint2 spt(1.0, 2.0, 3.0); - -/* ************************************************************************* */ -TEST (Serialization, text_geometry) { - EXPECT(equalsObj(Point2(1.0, 2.0))); - EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); - EXPECT(equalsObj(Rot2::fromDegrees(30.0))); - - EXPECT(equalsObj(pt3)); - EXPECT(equalsObj(rt3)); - EXPECT(equalsObj(Pose3(rt3, pt3))); - - EXPECT(equalsObj(cal1)); - EXPECT(equalsObj(cal2)); - EXPECT(equalsObj(cal3)); - EXPECT(equalsObj(cal4)); - EXPECT(equalsObj(cal5)); - - EXPECT(equalsObj(cam1)); - EXPECT(equalsObj(cam2)); - EXPECT(equalsObj(spt)); -} - -/* ************************************************************************* */ -TEST (Serialization, xml_geometry) { - EXPECT(equalsXML(Point2(1.0, 2.0))); - EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); - EXPECT(equalsXML(Rot2::fromDegrees(30.0))); - - EXPECT(equalsXML(pt3)); - EXPECT(equalsXML(rt3)); - EXPECT(equalsXML(Pose3(rt3, pt3))); - - EXPECT(equalsXML(cal1)); - EXPECT(equalsXML(cal2)); - EXPECT(equalsXML(cal3)); - EXPECT(equalsXML(cal4)); - EXPECT(equalsXML(cal5)); - - EXPECT(equalsXML(cam1)); - EXPECT(equalsXML(cam2)); - EXPECT(equalsXML(spt)); -} - -/* ************************************************************************* */ -typedef PinholeCamera PinholeCal3S2; -typedef PinholeCamera PinholeCal3DS2; -typedef PinholeCamera PinholeCal3Bundler; - -TEST (Serialization, TemplatedValues) { - Values values; - values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1)); - values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2)); - values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3)); - values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1)); - EXPECT(equalsObj(values)); - EXPECT(equalsXML(values)); -} - -/* ************************************************************************* */ -// Export Noisemodels -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); - -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); - -/* ************************************************************************* */ -// example noise models -noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); -noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); -noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); -noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); -noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); - -/* ************************************************************************* */ -TEST (Serialization, noiseModels) { - // tests using pointers to the derived class - EXPECT( equalsDereferenced(diag3)); - EXPECT(equalsDereferencedXML(diag3)); - - EXPECT( equalsDereferenced(gaussian3)); - EXPECT(equalsDereferencedXML(gaussian3)); - - EXPECT( equalsDereferenced(iso3)); - EXPECT(equalsDereferencedXML(iso3)); - - EXPECT( equalsDereferenced(constrained3)); - EXPECT(equalsDereferencedXML(constrained3)); - - EXPECT( equalsDereferenced(unit3)); - EXPECT(equalsDereferencedXML(unit3)); -} - -/* ************************************************************************* */ -TEST (Serialization, SharedNoiseModel_noiseModels) { - SharedNoiseModel diag3_sg = diag3; - EXPECT(equalsDereferenced(diag3_sg)); - EXPECT(equalsDereferencedXML(diag3_sg)); - - EXPECT(equalsDereferenced(diag3)); - EXPECT(equalsDereferencedXML(diag3)); - - EXPECT(equalsDereferenced(iso3)); - EXPECT(equalsDereferencedXML(iso3)); - - EXPECT(equalsDereferenced(gaussian3)); - EXPECT(equalsDereferencedXML(gaussian3)); - - EXPECT(equalsDereferenced(unit3)); - EXPECT(equalsDereferencedXML(unit3)); - - EXPECT(equalsDereferenced(constrained3)); - EXPECT(equalsDereferencedXML(constrained3)); -} - -/* ************************************************************************* */ -TEST (Serialization, SharedDiagonal_noiseModels) { - EXPECT(equalsDereferenced(diag3)); - EXPECT(equalsDereferencedXML(diag3)); - - EXPECT(equalsDereferenced(iso3)); - EXPECT(equalsDereferencedXML(iso3)); - - EXPECT(equalsDereferenced(unit3)); - EXPECT(equalsDereferencedXML(unit3)); - - EXPECT(equalsDereferenced(constrained3)); - EXPECT(equalsDereferencedXML(constrained3)); -} - -/* ************************************************************************* */ -// Linear components -/* ************************************************************************* */ - -/* ************************************************************************* */ -TEST (Serialization, linear_factors) { - VectorValues values; - values.insert(0, Vector_(1, 1.0)); - values.insert(1, Vector_(2, 2.0,3.0)); - values.insert(2, Vector_(2, 4.0,5.0)); - EXPECT(equalsObj(values)); - EXPECT(equalsXML(values)); - - Index i1 = 4, i2 = 7; - Matrix A1 = eye(3), A2 = -1.0 * eye(3); - Vector b = ones(3); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); - JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); - EXPECT(equalsObj(jacobianfactor)); - EXPECT(equalsXML(jacobianfactor)); - - HessianFactor hessianfactor(jacobianfactor); - EXPECT(equalsObj(hessianfactor)); - EXPECT(equalsXML(hessianfactor)); -} - -/* ************************************************************************* */ -TEST (Serialization, gaussian_conditional) { - Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.); - Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4); - Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34); - Vector d(2); d << 0.2, 0.5; - GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2)); - - EXPECT(equalsObj(cg)); - EXPECT(equalsXML(cg)); -} - -/* Create GUIDs for factors */ -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -/* ************************************************************************* */ -TEST (Serialization, smallExample_linear) { - using namespace example; - - Ordering ordering; ordering += "x1","x2","l1"; - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - EXPECT(equalsObj(ordering)); - EXPECT(equalsXML(ordering)); - - EXPECT(equalsObj(fg)); - EXPECT(equalsXML(fg)); - - GaussianBayesNet cbn = createSmallGaussianBayesNet(); - EXPECT(equalsObj(cbn)); - EXPECT(equalsXML(cbn)); -} - -/* ************************************************************************* */ -TEST (Serialization, symbolic_graph) { - Ordering o; o += "x1","l1","x2"; - // construct expected symbolic graph - SymbolicFactorGraph sfg; - sfg.push_factor(o["x1"]); - sfg.push_factor(o["x1"],o["x2"]); - sfg.push_factor(o["x1"],o["l1"]); - sfg.push_factor(o["l1"],o["x2"]); - - EXPECT(equalsObj(sfg)); - EXPECT(equalsXML(sfg)); -} - -/* ************************************************************************* */ -TEST (Serialization, symbolic_bn) { - Ordering o; o += "x2","l1","x1"; - - IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"])); - IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"])); - IndexConditional::shared_ptr x1(new IndexConditional(o["x1"])); - - SymbolicBayesNet sbn; - sbn.push_back(x2); - sbn.push_back(l1); - sbn.push_back(x1); - - EXPECT(equalsObj(sbn)); - EXPECT(equalsXML(sbn)); -} - -/* ************************************************************************* */ -TEST (Serialization, symbolic_bayes_tree ) { - typedef BayesTree SymbolicBayesTree; - static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; - IndexConditional::shared_ptr - B(new IndexConditional(_B_)), - L(new IndexConditional(_L_, _B_)), - E(new IndexConditional(_E_, _L_, _B_)), - S(new IndexConditional(_S_, _L_, _B_)), - T(new IndexConditional(_T_, _E_, _L_)), - X(new IndexConditional(_X_, _E_)); - - // Bayes Tree for Asia example - SymbolicBayesTree bayesTree; - SymbolicBayesTree::insert(bayesTree, B); - SymbolicBayesTree::insert(bayesTree, L); - SymbolicBayesTree::insert(bayesTree, E); - SymbolicBayesTree::insert(bayesTree, S); - SymbolicBayesTree::insert(bayesTree, T); - SymbolicBayesTree::insert(bayesTree, X); - - EXPECT(equalsObj(bayesTree)); - EXPECT(equalsXML(bayesTree)); -} - -/* ************************************************************************* */ -TEST (Serialization, gaussianISAM) { - using namespace example; - Ordering ordering; - GaussianFactorGraph smoother; - boost::tie(smoother, ordering) = createSmoother(7); - BayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); - GaussianISAM isam(bayesTree); - - EXPECT(equalsObj(isam)); - EXPECT(equalsXML(isam)); -} - -/* ************************************************************************* */ -/* Create GUIDs for factors in simulated2D */ -BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" ) -BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" ) -BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement") - -/* ************************************************************************* */ -TEST (Serialization, smallExample) { - using namespace example; - Graph nfg = createNonlinearFactorGraph(); - Values c1 = createValues(); - EXPECT(equalsObj(nfg)); - EXPECT(equalsXML(nfg)); - - EXPECT(equalsObj(c1)); - EXPECT(equalsXML(c1)); -} - -/* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior, "gtsam::planarSLAM::Prior"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing, "gtsam::planarSLAM::Bearing"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Range, "gtsam::planarSLAM::Range"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry, "gtsam::planarSLAM::Odometry"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint"); -/* ************************************************************************* */ -TEST (Serialization, planar_system) { - using namespace planarSLAM; - planarSLAM::Values values; - values.insert(PointKey(3), Point2(1.0, 2.0)); - values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); - - SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); - SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); - SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); - - Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1); - Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1); - Range range(PoseKey(2), PointKey(9), 7.0, model1); - BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2); - Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3); - Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2)); - - Graph graph; - graph.add(prior); - graph.add(bearing); - graph.add(range); - graph.add(bearingRange); - graph.add(odometry); - graph.add(constraint); - - // text - EXPECT(equalsObj(PoseKey(2))); - EXPECT(equalsObj(PointKey(3))); - EXPECT(equalsObj(values)); - EXPECT(equalsObj(prior)); - EXPECT(equalsObj(bearing)); - EXPECT(equalsObj(bearingRange)); - EXPECT(equalsObj(range)); - EXPECT(equalsObj(odometry)); - EXPECT(equalsObj(constraint)); - EXPECT(equalsObj(graph)); - - // xml - EXPECT(equalsXML(PoseKey(2))); - EXPECT(equalsXML(PointKey(3))); - EXPECT(equalsXML(values)); - EXPECT(equalsXML(prior)); - EXPECT(equalsXML(bearing)); - EXPECT(equalsXML(bearingRange)); - EXPECT(equalsXML(range)); - EXPECT(equalsXML(odometry)); - EXPECT(equalsXML(constraint)); - EXPECT(equalsXML(graph)); -} - -/* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor"); - -/* ************************************************************************* */ -TEST (Serialization, visual_system) { - using namespace visualSLAM; - Values values; - Symbol x1('x',1), x2('x',2); - Symbol l1('l',1), l2('l',2); - Pose3 pose1 = pose3, pose2 = pose3.inverse(); - Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); - values.insert(x1, pose1); - values.insert(l1, pt1); - SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); - SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); - SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); - boost::shared_ptr K(new Cal3_S2(cal1)); - - Graph graph; - graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K); - graph.addPointConstraint(1, pt1); - graph.addPointPrior(1, pt2, model3); - graph.addPoseConstraint(1, pose1); - graph.addPosePrior(1, pose2, model6); - - EXPECT(equalsObj(values)); - EXPECT(equalsObj(graph)); - - EXPECT(equalsXML(values)); - EXPECT(equalsXML(graph)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */