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