diff --git a/.cproject b/.cproject index ccef8a099..027bd68eb 100644 --- a/.cproject +++ b/.cproject @@ -302,7 +302,6 @@ make - all true true @@ -310,7 +309,6 @@ make - clean true true @@ -318,7 +316,6 @@ make - check true true @@ -326,7 +323,6 @@ make - testGaussianConditional.run true true @@ -334,7 +330,6 @@ make - testGaussianFactor.run true true @@ -342,7 +337,6 @@ make - timeGaussianFactor.run true true @@ -350,7 +344,6 @@ make - timeVectorConfig.run true true @@ -358,7 +351,6 @@ make - testVectorBTree.run true true @@ -366,7 +358,6 @@ make - testVectorMap.run true true @@ -374,7 +365,6 @@ make - testNoiseModel.run true true @@ -382,7 +372,6 @@ make - testBayesNetPreconditioner.run true true @@ -390,7 +379,6 @@ make - testErrors.run true false @@ -398,14 +386,22 @@ make + check true true true + + make + + all + true + true + true + make - check true true @@ -413,14 +409,22 @@ make - tests/testGaussianJunctionTree.run true true true + + make + + all + true + true + true + make + check true true @@ -428,6 +432,7 @@ make + clean true true @@ -435,6 +440,7 @@ make + testBTree.run true true @@ -442,6 +448,7 @@ make + testDSF.run true true @@ -449,6 +456,7 @@ make + testDSFVector.run true true @@ -456,6 +464,7 @@ make + testMatrix.run true true @@ -463,6 +472,7 @@ make + testSPQRUtil.run true true @@ -470,6 +480,7 @@ make + testVector.run true true @@ -477,6 +488,7 @@ make + timeMatrix.run true true @@ -484,6 +496,7 @@ make + all true true @@ -491,7 +504,6 @@ make - all true true @@ -499,7 +511,6 @@ make - clean true true @@ -515,7 +526,6 @@ make - testBayesTree.run true false @@ -523,7 +533,6 @@ make - testBinaryBayesNet.run true false @@ -531,7 +540,6 @@ make - testFactorGraph.run true true @@ -539,7 +547,6 @@ make - testISAM.run true true @@ -547,7 +554,6 @@ make - testJunctionTree.run true true @@ -555,7 +561,6 @@ make - testKey.run true true @@ -563,7 +568,6 @@ make - testOrdering.run true true @@ -571,7 +575,6 @@ make - testSymbolicBayesNet.run true false @@ -579,7 +582,6 @@ make - testSymbolicFactor.run true false @@ -587,7 +589,6 @@ make - testSymbolicFactorGraph.run true false @@ -595,7 +596,6 @@ make - timeSymbolMaps.run true true @@ -603,7 +603,6 @@ make - check true true @@ -611,7 +610,6 @@ make - testClusterTree.run true true @@ -619,7 +617,6 @@ make - testJunctionTree.run true true @@ -627,14 +624,22 @@ make - testEliminationTree.run true true true + + make + + all + true + true + true + make + check true true @@ -642,6 +647,7 @@ make + testGaussianFactorGraph.run true true @@ -649,6 +655,7 @@ make + testGaussianISAM.run true true @@ -656,6 +663,7 @@ make + testGaussianISAM2.run true true @@ -663,6 +671,7 @@ make + testGraph.run true false @@ -670,6 +679,7 @@ make + testIterative.run true true @@ -677,6 +687,7 @@ make + testNonlinearEquality.run true true @@ -684,6 +695,7 @@ make + testNonlinearFactor.run true true @@ -691,6 +703,7 @@ make + testNonlinearFactorGraph.run true true @@ -698,6 +711,7 @@ make + testNonlinearOptimizer.run true true @@ -705,6 +719,7 @@ make + testSQP.run true true @@ -712,6 +727,7 @@ make + testSubgraphPreconditioner.run true true @@ -719,6 +735,7 @@ make + testTupleConfig.run true true @@ -726,6 +743,7 @@ make + timeGaussianFactorGraph.run true true @@ -733,6 +751,7 @@ make + testBayesNetPreconditioner.run true true @@ -740,6 +759,7 @@ make + testConstraintOptimizer.run true true @@ -747,6 +767,7 @@ make + testInference.run true false @@ -754,6 +775,7 @@ make + testGaussianBayesNet.run true false @@ -761,6 +783,7 @@ make + testGaussianFactor.run true false @@ -768,6 +791,7 @@ make + testJunctionTree.run true false @@ -775,6 +799,7 @@ make + testSymbolicBayesNet.run true false @@ -782,6 +807,7 @@ make + testSymbolicFactorGraph.run true false @@ -789,7 +815,6 @@ make - clean true true @@ -797,7 +822,6 @@ make - all true true @@ -805,7 +829,6 @@ make - testNonlinearConstraint.run true true @@ -813,7 +836,6 @@ make - testLieConfig.run true true @@ -821,7 +843,6 @@ make - testConstraintOptimizer.run true true @@ -829,7 +850,6 @@ make - install true true @@ -837,7 +857,6 @@ make - clean true true @@ -845,6 +864,7 @@ make + all true true @@ -852,6 +872,7 @@ make + clean true true @@ -859,6 +880,7 @@ make + all true true @@ -866,6 +888,7 @@ make + clean true true @@ -873,6 +896,7 @@ make + all true true @@ -880,6 +904,7 @@ make + clean true true @@ -887,7 +912,6 @@ make - all true true @@ -895,7 +919,6 @@ make - clean true true @@ -903,6 +926,7 @@ make + clean all true true @@ -910,6 +934,7 @@ make + all true true @@ -917,6 +942,7 @@ make + check true true @@ -924,6 +950,7 @@ make + clean true true @@ -931,6 +958,7 @@ make + testPlanarSLAM.run true true @@ -938,6 +966,7 @@ make + testPose2Config.run true true @@ -945,6 +974,7 @@ make + testPose2Factor.run true true @@ -952,6 +982,7 @@ make + testPose2Prior.run true true @@ -959,6 +990,7 @@ make + testPose2SLAM.run true true @@ -966,6 +998,7 @@ make + testPose3Config.run true true @@ -973,6 +1006,7 @@ make + testPose3SLAM.run true true @@ -980,6 +1014,7 @@ make + testSimulated2DOriented.run true false @@ -987,6 +1022,7 @@ make + testVSLAMConfig.run true true @@ -994,6 +1030,7 @@ make + testVSLAMFactor.run true true @@ -1001,6 +1038,7 @@ make + testVSLAMGraph.run true true @@ -1008,6 +1046,7 @@ make + testPose3Factor.run true true @@ -1015,6 +1054,7 @@ make + testSimulated2D.run true false @@ -1022,6 +1062,7 @@ make + testSimulated3D.run true false @@ -1029,18 +1070,60 @@ make + check true true true + + make + + all + true + true + true + make + check true true true + + make + + all + true + true + true + + + make + + clean + true + true + true + + + make + + check + true + true + true + + + make + + all + true + true + true + make -j2 @@ -1059,103 +1142,11 @@ make - clean true true true - - make - check - true - true - true - - - make - testRot3.run - true - true - true - - - make - testRot2.run - true - true - true - - - make - testPose3.run - true - true - true - - - make - timeRot3.run - true - true - true - - - make - testPose2.run - true - true - true - - - make - testCal3_S2.run - true - true - true - - - make - testSimpleCamera.run - true - true - true - - - make - testHomography2.run - true - true - true - - - make - testCalibratedCamera.run - true - true - true - - - make - check - true - true - true - - - make - clean - true - true - true - - - make - testPoint2.run - true - true - true - make -j2 @@ -1182,6 +1173,7 @@ make + all true true @@ -1189,12 +1181,85 @@ make + dist true true true - + + make + + testRot3.run + true + true + true + + + make + + testRot2.run + true + true + true + + + make + + testPose3.run + true + true + true + + + make + + timeRot3.run + true + true + true + + + make + + testPose2.run + true + true + true + + + make + + testCal3_S2.run + true + true + true + + + make + + testSimpleCamera.run + true + true + true + + + make + + testHomography2.run + true + true + true + + + make + + testCalibratedCamera.run + true + true + true + + make check @@ -1202,31 +1267,7 @@ true true - - make - - testGaussianJunctionTree.run - true - true - true - - - make - - testGaussianFactorGraph.run - true - true - true - - - make - - check - true - true - true - - + make clean @@ -1234,9 +1275,67 @@ true true - + make + testPoint2.run + true + true + true + + + make + check + true + true + true + + + make + testGaussianJunctionTree.run + true + true + true + + + make + testGaussianFactorGraph.run + true + true + true + + + make + + all + true + true + true + + + make + + clean + true + true + true + + + make + check + true + true + true + + + make + clean + true + true + true + + + make install true true @@ -1244,7 +1343,6 @@ make - all true true diff --git a/nonlinear/LieConfig.h b/nonlinear/LieConfig.h index 69a3bbde3..cd59a2cfa 100644 --- a/nonlinear/LieConfig.h +++ b/nonlinear/LieConfig.h @@ -100,10 +100,10 @@ namespace gtsam { // imperative methods: - /** Add a variable with the given j */ + /** Add a variable with the given j - does not replace existing values */ void insert(const J& j, const T& val); - /** Add a set of variables */ + /** Add a set of variables - does note replace existing values */ void insert(const LieConfig& cfg); /** update the current available values without adding new ones */ diff --git a/nonlinear/TupleConfig-inl.h b/nonlinear/TupleConfig-inl.h index b4f75ef6f..e34946a2e 100644 --- a/nonlinear/TupleConfig-inl.h +++ b/nonlinear/TupleConfig-inl.h @@ -1,23 +1,15 @@ -/* - * TupleConfig-inl.h - * - * Created on: Jan 14, 2010 - * Author: richard +/** + * @file TupleConfig-inl.h + * @author Richard Roberts + * @author Manohar Paluri + * @author Alex Cunningham */ #pragma once -#include #include "LieConfig-inl.h" #include "TupleConfig.h" -#define INSTANTIATE_PAIR_CONFIG(J1,X1,J2,X2) \ - /*INSTANTIATE_LIE_CONFIG(J1,X1);*/ \ - /*INSTANTIATE_LIE_CONFIG(J2,X2);*/ \ - template class PairConfig; \ - /*template void PairConfig::print(const std::string&) const;*/ \ - template PairConfig expmap(const PairConfig&, const VectorConfig&); - // TupleConfig instantiations for N = 1-6 #define INSTANTIATE_TUPLE_CONFIG2(Config1, Config2) \ template class TupleConfig2; @@ -37,43 +29,6 @@ namespace gtsam { -/* ************************************************************************* */ -/** PairConfig implementations */ -/* ************************************************************************* */ -template -void PairConfig::print(const std::string& s) const { - std::cout << "TupleConfig " << s << ", size " << size() << "\n"; - first().print(s + "Config1: "); - second().print(s + "Config2: "); -} - -/* ************************************************************************* */ -template -void PairConfig::insert(const PairConfig& config) { - for (typename Config1::const_iterator it = config.first().begin(); it!=config.first().end(); it++) { - insert(it->first, it->second); - } - for (typename Config2::const_iterator it = config.second().begin(); it!=config.second().end(); it++) { - insert(it->first, it->second); - } -} - -/* ************************************************************************* */ -/** TupleConfig Implementations */ -/* ************************************************************************* */ - -template -void TupleConfig::print(const std::string& s) const { - std::cout << s << " : " << std::endl; - first_.print(); - second_.print(); -} - -template -void TupleConfigEnd::print(const std::string& s ) const { - first_.print(); -} - /* ************************************************************************* */ /** TupleConfigN Implementations */ /* ************************************************************************* */ diff --git a/nonlinear/TupleConfig.h b/nonlinear/TupleConfig.h index 090e4ee40..e683739f1 100644 --- a/nonlinear/TupleConfig.h +++ b/nonlinear/TupleConfig.h @@ -1,8 +1,8 @@ -/* - * TupleConfig.h - * - * Created on: Jan 13, 2010 - * Author: Richard Roberts and Manohar Paluri +/** + * @file TupleConfig.h + * @author Richard Roberts + * @author Manohar Paluri + * @author Alex Cunningham */ #include "LieConfig.h" @@ -12,24 +12,37 @@ namespace gtsam { -/** - * Tuple configs to handle subconfigs of LieConfigs + /** + * TupleConfigs are a structure to manage heterogenous LieConfigs, so as to + * enable different types of variables/keys to be used simultaneously. The + * interface is designed to mimic that of a single LieConfig. * * This uses a recursive structure of config pairs to form a lisp-like * list, with a special case (TupleConfigEnd) that contains only one config - * at the end. In a final use case, this should be aliased to something clearer - * but still with the same recursive type machinery. + * at the end. Because this recursion is done at compile time, there is no + * runtime performance hit to using this structure. + * + * For an easier to use approach, there are TupleConfigN classes, which wrap + * the recursive TupleConfigs together as a single class, so you can have + * mixed-type classes from 2-6 types. Note that a TupleConfig2 is equivalent + * to the previously used PairConfig. + * + * Design and extension note: + * To implement a recursively templated data structure, note that most operations + * have two versions: one with templates and one without. The templated one allows + * for the arguments to be passed to the next config, while the specialized one + * operates on the "first" config. TupleConfigEnd contains only the specialized version. */ template class TupleConfig : public Testable > { protected: // Data for internal configs - Config1 first_; - Config2 second_; + Config1 first_; /// Arbitrary config + Config2 second_; /// A TupleConfig or TupleConfigEnd, which wraps an arbitrary config public: - // typedefs + // typedefs for config subtypes typedef class Config1::Key Key1; typedef class Config1::Value Value1; @@ -47,19 +60,34 @@ namespace gtsam { virtual ~TupleConfig() {} /** Print */ - void print(const std::string& s = "") const; + void print(const std::string& s = "") const { + first_.print(s); + second_.print(); + } - /** Test for equality in keys and values */ + /** Equality with tolerance for keys and values */ bool equals(const TupleConfig& c, double tol=1e-9) const { return first_.equals(c.first_, tol) && second_.equals(c.second_, tol); } - // insert function that uses the second (recursive) config + /** + * Insert a key/value pair to the config. + * Note: if the key is already in the config, the config will not be changed. + * Use update() to allow for changing existing values. + * @param key is the key - can be an int (second version) if the can can be initialized from an int + * @param value is the value to insert + */ template void insert(const Key& key, const Value& value) {second_.insert(key, value);} + void insert(const int& key, const Value1& value) {first_.insert(Key1(key), value);} void insert(const Key1& key, const Value1& value) {first_.insert(key, value);} - // insert function for whole configs + /** + * Insert a complete config at a time. + * Note: if the key is already in the config, the config will not be changed. + * Use update() to allow for changing existing values. + * @param config is a full config to add + */ template void insert(const TupleConfig& config) { second_.insert(config); } void insert(const TupleConfig& config) { @@ -67,7 +95,10 @@ namespace gtsam { second_.insert(config.second_); } - // update function for whole configs + /** + * Update function for whole configs - this will change existing values + * @param config is a config to add + */ template void update(const TupleConfig& config) { second_.update(config); } void update(const TupleConfig& config) { @@ -75,52 +106,66 @@ namespace gtsam { second_.update(config.second_); } - // update function for single elements + /** + * Update function for single key/value pairs - will change existing values + * @param key is the variable identifier + * @param value is the variable value to update + */ template void update(const Key& key, const Value& value) { second_.update(key, value); } void update(const Key1& key, const Value1& value) { first_.update(key, value); } - // insert a subconfig + /** + * Insert a subconfig + * @param config is the config to insert + */ template void insertSub(const Cfg& config) { second_.insertSub(config); } void insertSub(const Config1& config) { first_.insert(config); } - // erase an element by key + /** erase an element by key */ template void erase(const Key& j) { second_.erase(j); } void erase(const Key1& j) { first_.erase(j); } - // determine whether an element exists + /** determine whether an element exists */ template bool exists(const Key& j) const { return second_.exists(j); } bool exists(const Key1& j) const { return first_.exists(j); } - // a variant of exists + /** a variant of exists */ template boost::optional exists_(const Key& j) const { return second_.exists_(j); } boost::optional exists_(const Key1& j) const { return first_.exists_(j); } - // access operator + /** access operator */ template const typename Key::Value_t & operator[](const Key& j) const { return second_[j]; } const Value1& operator[](const Key1& j) const { return first_[j]; } - // at access function + /** at access function */ template const typename Key::Value_t & at(const Key& j) const { return second_.at(j); } const Value1& at(const Key1& j) const { return first_.at(j); } - // direct config access + /** direct config access */ const Config1& config() const { return first_; } const Config2& rest() const { return second_; } - // size function - adds recursively + /** zero: create VectorConfig of appropriate structure */ + VectorConfig zero() const { + VectorConfig z1 = first_.zero(), z2 = second_.zero(); + z2.insert(z1); + return z2; + } + + /** @return number of key/value pairs stored */ size_t size() const { return first_.size() + second_.size(); } - // dim function + /** @return The dimensionality of the tangent space */ size_t dim() const { return first_.dim() + second_.dim(); } - // Expmap + /** Expmap */ TupleConfig expmap(const VectorConfig& delta) const { return TupleConfig(gtsam::expmap(first_, delta), second_.expmap(delta)); } @@ -132,12 +177,22 @@ namespace gtsam { return ret; } + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(first_); + ar & BOOST_SERIALIZATION_NVP(second_); + } + }; /** * End of a recursive TupleConfig - contains only one config * - * This should not be used directly + * Do not use this class directly - it should only be used as a part + * of a recursive structure */ template class TupleConfigEnd : public Testable > { @@ -161,25 +216,23 @@ namespace gtsam { virtual ~TupleConfigEnd() {} - /** Print */ - void print(const std::string& s = "") const; + void print(const std::string& s = "") const { + first_.print(); + } - /** Test for equality in keys and values */ bool equals(const TupleConfigEnd& c, double tol=1e-9) const { return first_.equals(c.first_, tol); } void insert(const Key1& key, const Value1& value) {first_.insert(key, value); } + void insert(const int& key, const Value1& value) {first_.insert(Key1(key), value);} - // insert function for whole configs void insert(const TupleConfigEnd& config) {first_.insert(config.first_); } - // update function for whole configs void update(const TupleConfigEnd& config) {first_.update(config.first_); } void update(const Key1& key, const Value1& value) { first_.update(key, value); } - // insert function for sub configs void insertSub(const Config& config) {first_.insert(config); } const Value1& operator[](const Key1& j) const { return first_[j]; } @@ -194,6 +247,11 @@ namespace gtsam { const Value1& at(const Key1& j) const { return first_.at(j); } + VectorConfig zero() const { + VectorConfig z = first_.zero(); + return z; + } + size_t size() const { return first_.size(); } size_t dim() const { return first_.dim(); } @@ -206,6 +264,13 @@ namespace gtsam { VectorConfig ret(gtsam::logmap(first_, cp.first_)); return ret; } + + private: + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(first_); + } }; /** Exmap static functions */ @@ -227,15 +292,15 @@ namespace gtsam { * * The interface is designed to mimic PairConfig, but for 2-6 config types. */ - template - class TupleConfig2 : public TupleConfig > { + template + class TupleConfig2 : public TupleConfig > { public: // typedefs - typedef Config1 Config1_t; - typedef Config2 Config2_t; + typedef C1 Config1; + typedef C2 Config2; - typedef TupleConfig > Base; - typedef TupleConfig2 This; + typedef TupleConfig > Base; + typedef TupleConfig2 This; TupleConfig2() {} TupleConfig2(const This& config); @@ -243,50 +308,55 @@ namespace gtsam { TupleConfig2(const Config1& cfg1, const Config2& cfg2); // access functions - inline const Config1_t& first() const { return this->config(); } - inline const Config2_t& second() const { return this->rest().config(); } + inline const Config1& first() const { return this->config(); } + inline const Config2& second() const { return this->rest().config(); } }; - template - TupleConfig2 expmap(const TupleConfig2& c, const VectorConfig& delta) { + template + TupleConfig2 expmap(const TupleConfig2& c, const VectorConfig& delta) { return c.expmap(delta); } - template - class TupleConfig3 : public TupleConfig > > { + template + VectorConfig logmap(const TupleConfig2& c1, const TupleConfig2& c2) { + return c1.logmap(c2); + } + + template + class TupleConfig3 : public TupleConfig > > { public: // typedefs - typedef Config1 Config1_t; - typedef Config2 Config2_t; - typedef Config3 Config3_t; + typedef C1 Config1; + typedef C2 Config2; + typedef C3 Config3; TupleConfig3() {} - TupleConfig3(const TupleConfig > >& config); - TupleConfig3(const TupleConfig3& config); + TupleConfig3(const TupleConfig > >& config); + TupleConfig3(const TupleConfig3& config); TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3); // access functions - inline const Config1_t& first() const { return this->config(); } - inline const Config2_t& second() const { return this->rest().config(); } - inline const Config3_t& third() const { return this->rest().rest().config(); } + inline const Config1& first() const { return this->config(); } + inline const Config2& second() const { return this->rest().config(); } + inline const Config3& third() const { return this->rest().rest().config(); } }; - template - TupleConfig3 expmap(const TupleConfig3& c, const VectorConfig& delta) { + template + TupleConfig3 expmap(const TupleConfig3& c, const VectorConfig& delta) { return c.expmap(delta); } - template - class TupleConfig4 : public TupleConfig > > > { + template + class TupleConfig4 : public TupleConfig > > > { public: // typedefs - typedef Config1 Config1_t; - typedef Config2 Config2_t; - typedef Config3 Config3_t; - typedef Config4 Config4_t; + typedef C1 Config1; + typedef C2 Config2; + typedef C3 Config3; + typedef C4 Config4; - typedef TupleConfig > > > Base; - typedef TupleConfig4 This; + typedef TupleConfig > > > Base; + typedef TupleConfig4 This; TupleConfig4() {} TupleConfig4(const This& config); @@ -294,249 +364,74 @@ namespace gtsam { TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4); // access functions - inline const Config1_t& first() const { return this->config(); } - inline const Config2_t& second() const { return this->rest().config(); } - inline const Config3_t& third() const { return this->rest().rest().config(); } - inline const Config4_t& fourth() const { return this->rest().rest().rest().config(); } + inline const Config1& first() const { return this->config(); } + inline const Config2& second() const { return this->rest().config(); } + inline const Config3& third() const { return this->rest().rest().config(); } + inline const Config4& fourth() const { return this->rest().rest().rest().config(); } }; - template - TupleConfig4 expmap(const TupleConfig4& c, const VectorConfig& delta) { + template + TupleConfig4 expmap(const TupleConfig4& c, const VectorConfig& delta) { return c.expmap(delta); } - template - class TupleConfig5 : public TupleConfig > > > > { + template + class TupleConfig5 : public TupleConfig > > > > { public: // typedefs - typedef Config1 Config1_t; - typedef Config2 Config2_t; - typedef Config3 Config3_t; - typedef Config4 Config4_t; - typedef Config5 Config5_t; + typedef C1 Config1; + typedef C2 Config2; + typedef C3 Config3; + typedef C4 Config4; + typedef C5 Config5; TupleConfig5() {} - TupleConfig5(const TupleConfig5& config); - TupleConfig5(const TupleConfig > > > >& config); + TupleConfig5(const TupleConfig5& config); + TupleConfig5(const TupleConfig > > > >& config); TupleConfig5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, const Config4& cfg4, const Config5& cfg5); // access functions - inline const Config1_t& first() const { return this->config(); } - inline const Config2_t& second() const { return this->rest().config(); } - inline const Config3_t& third() const { return this->rest().rest().config(); } - inline const Config4_t& fourth() const { return this->rest().rest().rest().config(); } - inline const Config5_t& fifth() const { return this->rest().rest().rest().rest().config(); } + inline const Config1& first() const { return this->config(); } + inline const Config2& second() const { return this->rest().config(); } + inline const Config3& third() const { return this->rest().rest().config(); } + inline const Config4& fourth() const { return this->rest().rest().rest().config(); } + inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); } }; - template - TupleConfig5 expmap(const TupleConfig5& c, const VectorConfig& delta) { + template + TupleConfig5 expmap(const TupleConfig5& c, const VectorConfig& delta) { return c.expmap(delta); } - template - class TupleConfig6 : public TupleConfig > > > > > { + template + class TupleConfig6 : public TupleConfig > > > > > { public: // typedefs - typedef Config1 Config1_t; - typedef Config2 Config2_t; - typedef Config3 Config3_t; - typedef Config4 Config4_t; - typedef Config5 Config5_t; - typedef Config6 Config6_t; + typedef C1 Config1; + typedef C2 Config2; + typedef C3 Config3; + typedef C4 Config4; + typedef C5 Config5; + typedef C6 Config6; TupleConfig6() {} - TupleConfig6(const TupleConfig6& config); - TupleConfig6(const TupleConfig > > > > >& config); + TupleConfig6(const TupleConfig6& config); + TupleConfig6(const TupleConfig > > > > >& config); TupleConfig6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3, const Config4& cfg4, const Config5& cfg5, const Config6& cfg6); // access functions - inline const Config1_t& first() const { return this->config(); } - inline const Config2_t& second() const { return this->rest().config(); } - inline const Config3_t& third() const { return this->rest().rest().config(); } - inline const Config4_t& fourth() const { return this->rest().rest().rest().config(); } - inline const Config5_t& fifth() const { return this->rest().rest().rest().rest().config(); } - inline const Config6_t& sixth() const { return this->rest().rest().rest().rest().rest().config(); } + inline const Config1& first() const { return this->config(); } + inline const Config2& second() const { return this->rest().config(); } + inline const Config3& third() const { return this->rest().rest().config(); } + inline const Config4& fourth() const { return this->rest().rest().rest().config(); } + inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); } + inline const Config6& sixth() const { return this->rest().rest().rest().rest().rest().config(); } }; - template - TupleConfig6 expmap(const TupleConfig6& c, const VectorConfig& delta) { + template + TupleConfig6 expmap(const TupleConfig6& c, const VectorConfig& delta) { return c.expmap(delta); } - /** - * PairConfig: an alias for a pair of configs using TupleConfig2 - * STILL IN TESTING - will soon replace PairConfig - */ -// template -// class PairConfig : public TupleConfig2, LieConfig > { -// public: -// PairConfig() {} -// PairConfig(const PairConfig& config) : -// TupleConfig2, LieConfig >(config) {} -// PairConfig(const LieConfig& cfg1,const LieConfig& cfg2) : -// TupleConfig2, LieConfig >(cfg1, cfg2) {} -// }; - - /** - * PairConfig: a config that holds two data types. - * Note: this should eventually be replaced with a wrapper on TupleConfig2 - */ - template - class PairConfig : public Testable > { - - public: - - // publicly available types - typedef LieConfig Config1; - typedef LieConfig Config2; - - protected: - - // Two configs in the pair as in std:pair - LieConfig first_; - LieConfig second_; - - public: - - PairConfig(const LieConfig& config1, const LieConfig& config2) : - first_(config1), second_(config2){} - - - /** - * Default constructor creates an empty config. - */ - PairConfig(){} - - /** - * Copy constructor - */ - PairConfig(const PairConfig& c): - first_(c.first_), second_(c.second_){} - - /** - * Print - */ - void print(const std::string& s = "") const; - - /** - * Test for equality in keys and values - */ - bool equals(const PairConfig& c, double tol=1e-9) const { - return first_.equals(c.first_, tol) && second_.equals(c.second_, tol); } - - /** Returns the real config */ - inline const Config1& first() const { return first_; } - inline const Config2& second() const { return second_; } - - /** - * operator[] syntax to get a value by j, throws invalid_argument if - * value with specified j is not present. Will generate compile-time - * errors if j type does not match that on which the Config is templated. - */ - const X1& operator[](const J1& j) const { return first_[j]; } - const X2& operator[](const J2& j) const { return second_[j]; } - - /** member function version of access function */ - const X1& at(const J1& j) const { return first_[j]; } - const X2& at(const J2& j) const { return second_[j]; } - - /** - * size is the total number of variables in this config. - */ - size_t size() const { return first_.size() + second_.size(); } - - /** - * dim is the dimensionality of the tangent space - */ - size_t dim() const { return first_.dim() + second_.dim(); } - - private: - template - void insert_helper(Config& config, const Key& j, const Value& value) { - config.insert(j, value); - } - - template - void erase_helper(Config& config, const Key& j) { - size_t dim; - config.erase(j, dim); - } - - public: - - /** zero: create VectorConfig of appropriate structure */ - VectorConfig zero() const { - VectorConfig z1 = first_.zero(), z2 = second_.zero(); - z1.insert(z2); - return z1; - } - - /** - * Exponential map: expmap each element - */ - PairConfig expmap(const VectorConfig& delta) const { - return PairConfig(gtsam::expmap(first_, delta), gtsam::expmap(second_, delta)); } - - /** - * Logarithm: logmap each element - */ - VectorConfig logmap(const PairConfig& cp) const { - VectorConfig ret(gtsam::logmap(first_, cp.first_)); - ret.insert(gtsam::logmap(second_, cp.second_)); - return ret; - } - - /** - * Insert a variable with the given j - */ - void insert(const J1& j, const X1& value) { insert_helper(first_, j, value); } - void insert(const J2& j, const X2& value) { insert_helper(second_, j, value); } - - void insert(const PairConfig& config); - - /** Insert a subconfig */ - void insertSub(const Config1& config) { first_.insert(config); } - void insertSub(const Config2& config) { second_.insert(config); } - - /** - * Remove the variable with the given j. Throws invalid_argument if the - * j is not present in the config. - */ - void erase(const J1& j) { erase_helper(first_, j); } - void erase(const J2& j) { erase_helper(second_, j); } - - /** - * Check if a variable exists - */ - bool exists(const J1& j) const { return first_.exists(j); } - bool exists(const J2& j) const { return second_.exists(j); } - boost::optional exists_(const J1& j) const { return first_.exists_(j); } - boost::optional exists_(const J2& j) const { return second_.exists_(j); } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(first_); - ar & BOOST_SERIALIZATION_NVP(second_); - } - - }; - - /** exponential map */ - template - inline PairConfig expmap(const PairConfig& c, - const VectorConfig& delta) { - return c.expmap(delta); - } - - /** log, inverse of exponential map */ - template - inline VectorConfig logmap(const PairConfig& c0, - const PairConfig& cp) { - return c0.logmap(cp); - } - } diff --git a/slam/planarSLAM.cpp b/slam/planarSLAM.cpp index 928b213e9..45e315c09 100644 --- a/slam/planarSLAM.cpp +++ b/slam/planarSLAM.cpp @@ -14,7 +14,7 @@ namespace gtsam { using namespace planarSLAM; INSTANTIATE_LIE_CONFIG(PointKey, Point2) - INSTANTIATE_PAIR_CONFIG(PoseKey, Pose2, PointKey, Point2) + INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) diff --git a/slam/planarSLAM.h b/slam/planarSLAM.h index 1715f0be8..d9b4e9d6d 100644 --- a/slam/planarSLAM.h +++ b/slam/planarSLAM.h @@ -25,7 +25,7 @@ namespace gtsam { typedef TypedSymbol PointKey; typedef LieConfig PoseConfig; typedef LieConfig PointConfig; - typedef PairConfig Config; + typedef TupleConfig2 Config; // Factors typedef NonlinearEquality Constraint; diff --git a/slam/simulated2D.cpp b/slam/simulated2D.cpp index 2a84a82ac..744e1d19c 100644 --- a/slam/simulated2D.cpp +++ b/slam/simulated2D.cpp @@ -5,13 +5,15 @@ */ #include "simulated2D.h" +#include "LieConfig-inl.h" #include "TupleConfig-inl.h" namespace gtsam { using namespace simulated2D; -// INSTANTIATE_LIE_CONFIG(PointKey, Point2) - INSTANTIATE_PAIR_CONFIG(PoseKey, Point2, PointKey, Point2) + INSTANTIATE_LIE_CONFIG(PointKey, Point2) + INSTANTIATE_LIE_CONFIG(PoseKey, Point2) + INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig) // INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) // INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) diff --git a/slam/simulated2D.h b/slam/simulated2D.h index 0431f2e36..eae6b2ecf 100644 --- a/slam/simulated2D.h +++ b/slam/simulated2D.h @@ -21,7 +21,9 @@ namespace gtsam { // Simulated2D robots have no orientation, just a position typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - typedef PairConfig Config; + typedef LieConfig PoseConfig; + typedef LieConfig PointConfig; + typedef TupleConfig2 Config; /** * Prior on a single pose, and optional derivative version diff --git a/slam/simulated2DOriented.cpp b/slam/simulated2DOriented.cpp index 662866db2..519ff93d3 100644 --- a/slam/simulated2DOriented.cpp +++ b/slam/simulated2DOriented.cpp @@ -11,7 +11,6 @@ namespace gtsam { using namespace simulated2DOriented; // INSTANTIATE_LIE_CONFIG(PointKey, Point2) -// INSTANTIATE_PAIR_CONFIG(PoseKey, Pose2, PointKey, Point2) // INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) // INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) diff --git a/slam/simulated2DOriented.h b/slam/simulated2DOriented.h index 75b6d2609..9bb034d53 100644 --- a/slam/simulated2DOriented.h +++ b/slam/simulated2DOriented.h @@ -21,7 +21,9 @@ namespace gtsam { // The types that take an oriented pose2 rather than point2 typedef TypedSymbol PointKey; typedef TypedSymbol PoseKey; - typedef PairConfig Config; + typedef LieConfig PoseConfig; + typedef LieConfig PointConfig; + typedef TupleConfig2 Config; //TODO:: point prior is not implemented right now @@ -71,7 +73,7 @@ namespace gtsam { GenericOdometry(const Pose2& z, const SharedGaussian& model, const Key& i1, const Key& i2) : - z_(z), NonlinearFactor2 (model, i1, i2) { + NonlinearFactor2 (model, i1, i2), z_(z) { } Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional< diff --git a/slam/smallExample.cpp b/slam/smallExample.cpp index 8d777ffad..5cbb5b3ae 100644 --- a/slam/smallExample.cpp +++ b/slam/smallExample.cpp @@ -21,6 +21,7 @@ using namespace std; // template definitions #include "FactorGraph-inl.h" +#include "TupleConfig-inl.h" #include "NonlinearFactorGraph-inl.h" namespace gtsam { diff --git a/slam/visualSLAM.cpp b/slam/visualSLAM.cpp index 8e4992279..26fa92c04 100644 --- a/slam/visualSLAM.cpp +++ b/slam/visualSLAM.cpp @@ -11,7 +11,7 @@ #include "NonlinearFactorGraph-inl.h" namespace gtsam { - INSTANTIATE_PAIR_CONFIG(visualSLAM::PoseKey, Pose3, visualSLAM::PointKey, Point3) + INSTANTIATE_TUPLE_CONFIG2(visualSLAM::PoseConfig, visualSLAM::PointConfig) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(visualSLAM::Config) INSTANTIATE_NONLINEAR_OPTIMIZER(visualSLAM::Graph, visualSLAM::Config) diff --git a/slam/visualSLAM.h b/slam/visualSLAM.h index 42d61bac2..4a3172dd3 100644 --- a/slam/visualSLAM.h +++ b/slam/visualSLAM.h @@ -24,7 +24,10 @@ namespace gtsam { namespace visualSLAM { */ typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - typedef PairConfig Config; + typedef LieConfig PoseConfig; + typedef LieConfig PointConfig; + typedef TupleConfig2 Config; + typedef NonlinearEquality PoseConstraint; typedef NonlinearEquality PointConstraint; diff --git a/tests/testTupleConfig.cpp b/tests/testTupleConfig.cpp index ff3807e3e..f20fe8bf3 100644 --- a/tests/testTupleConfig.cpp +++ b/tests/testTupleConfig.cpp @@ -25,10 +25,12 @@ using namespace std; typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; -typedef PairConfig Config; +typedef LieConfig PoseConfig; +typedef LieConfig PointConfig; +typedef TupleConfig2 Config; /* ************************************************************************* */ -TEST( PairConfig, constructors ) +TEST( TupleConfig, constructors ) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -50,7 +52,7 @@ TEST( PairConfig, constructors ) } /* ************************************************************************* */ -TEST( PairConfig, insert_equals1 ) +TEST( TupleConfig, insert_equals1 ) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -70,7 +72,7 @@ TEST( PairConfig, insert_equals1 ) CHECK(assert_equal(expected,actual)); } -TEST( PairConfig, insert_equals2 ) +TEST( TupleConfig, insert_equals2 ) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -94,7 +96,7 @@ TEST( PairConfig, insert_equals2 ) } ///* ************************************************************************* */ -TEST( PairConfig, insert_duplicate ) +TEST( TupleConfig, insert_duplicate ) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -112,7 +114,7 @@ TEST( PairConfig, insert_duplicate ) } /* ************************************************************************* */ -TEST( PairConfig, size_dim ) +TEST( TupleConfig, size_dim ) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -128,7 +130,7 @@ TEST( PairConfig, size_dim ) } /* ************************************************************************* */ -TEST(PairConfig, at) +TEST(TupleConfig, at) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10); @@ -162,7 +164,7 @@ TEST(PairConfig, at) } /* ************************************************************************* */ -TEST(PairConfig, zero_expmap_logmap) +TEST(TupleConfig, zero_expmap_logmap) { Pose2 x1(1,2,3), x2(6,7,8); Point2 l1(4,5), l2(9,10);