From 93465945e935141c9d8b377c429d60e1625f4011 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 13 Jan 2010 22:25:03 +0000 Subject: [PATCH] Large gtsam refactoring To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors. The following are the biggest changes: 1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function. 2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional arguments are provided, and tin EvaluateErrors you just says something like if (H) *H = Matrix_(3,6,....); 3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol will not match a Symbol 4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar ! --- cpp/BetweenFactor.h | 68 ++---- cpp/FactorGraph-inl.h | 2 +- cpp/FactorGraph.h | 6 +- cpp/Key.h | 58 +++++ cpp/LieConfig-inl.h | 84 +++---- cpp/LieConfig.h | 96 +++++--- cpp/Makefile.am | 4 +- cpp/NonlinearConstraint-inl.h | 6 +- cpp/NonlinearConstraint.h | 4 + cpp/NonlinearEquality.h | 93 +++----- cpp/NonlinearFactor.cpp | 108 +-------- cpp/NonlinearFactor.h | 405 ++++++++++++++++++++------------ cpp/Ordering.cpp | 5 +- cpp/Ordering.h | 2 +- cpp/Point2.h | 2 +- cpp/Point2Prior.h | 20 +- cpp/Point3.h | 2 +- cpp/Pose2Config.cpp | 20 +- cpp/Pose2Config.h | 4 +- cpp/Pose2Factor.h | 2 +- cpp/Pose2Graph.cpp | 5 +- cpp/Pose2Graph.h | 10 +- cpp/Pose3Config.cpp | 12 +- cpp/Pose3Config.h | 4 +- cpp/Pose3Factor.h | 2 +- cpp/Pose3Graph.h | 10 +- cpp/Simulated2DMeasurement.h | 27 ++- cpp/Simulated2DOdometry.h | 24 +- cpp/Simulated3D.cpp | 22 +- cpp/Simulated3D.h | 104 +++++--- cpp/VSLAMConfig.cpp | 109 ++------- cpp/VSLAMConfig.h | 110 +++------ cpp/VSLAMFactor.cpp | 99 ++------ cpp/VSLAMFactor.h | 130 +++++----- cpp/VSLAMGraph.cpp | 12 +- cpp/VSLAMGraph.h | 2 +- cpp/graph-inl.h | 110 ++++----- cpp/graph.h | 59 +++++ cpp/simulated2D.cpp | 4 +- cpp/simulated2D.h | 52 ++-- cpp/smallExample.cpp | 48 ++-- cpp/testGaussianFactorGraph.cpp | 4 +- cpp/testGraph.cpp | 24 +- cpp/testLieConfig.cpp | 24 +- cpp/testNonlinearEquality.cpp | 39 +-- cpp/testNonlinearFactor.cpp | 18 +- cpp/testOrdering.cpp | 4 +- cpp/testPose2Config.cpp | 20 +- cpp/testPose2Factor.cpp | 39 ++- cpp/testPose2Graph.cpp | 60 ++--- cpp/testPose3Config.cpp | 20 +- cpp/testPose3Factor.cpp | 10 +- cpp/testPose3Graph.cpp | 36 +-- cpp/testSQP.cpp | 31 ++- cpp/testSQPOptimizer.cpp | 51 ++-- cpp/testSimulated2D.cpp | 1 + cpp/testSimulated3D.cpp | 15 +- cpp/testVSLAMConfig.cpp | 17 +- cpp/testVSLAMFactor.cpp | 2 +- 59 files changed, 1182 insertions(+), 1179 deletions(-) create mode 100644 cpp/Key.h create mode 100644 cpp/graph.h diff --git a/cpp/BetweenFactor.h b/cpp/BetweenFactor.h index 82ee8f3f4..99e68f414 100644 --- a/cpp/BetweenFactor.h +++ b/cpp/BetweenFactor.h @@ -17,14 +17,14 @@ namespace gtsam { * A class for a measurement predicted by "between(config[key1],config[key2])" * T is the Lie group type, Config where the T's are gotten from */ - template - class BetweenFactor: public NonlinearFactor { + template + class BetweenFactor: public NonlinearFactor2 { private: + typedef NonlinearFactor2 Base; + T measured_; /** The measurement */ - std::string key1_, key2_; /** The keys of the two poses, order matters */ - std::list keys_; /** The keys as a list */ Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ public: @@ -33,72 +33,50 @@ namespace gtsam { typedef typename boost::shared_ptr shared_ptr; /** Constructor */ - BetweenFactor(const std::string& key1, const std::string& key2, - const T& measured, const Matrix& measurement_covariance) : - key1_(key1), key2_(key2), measured_(measured) { + BetweenFactor(const Key& key1, const Key& key2, const T& measured, + const Matrix& measurement_covariance) : + Base(1, key1, key2), measured_(measured) { square_root_inverse_covariance_ = inverse_square_root( measurement_covariance); - keys_.push_back(key1); - keys_.push_back(key2); } /** implement functions needed for Testable */ /** print */ - void print(const std::string& name) const { - std::cout << name << std::endl; - std::cout << "Factor " << std::endl; - std::cout << "key1 " << key1_ << std::endl; - std::cout << "key2 " << key2_ << std::endl; + void print(const std::string& s) const { + Base::print(s); measured_.print("measured "); gtsam::print(square_root_inverse_covariance_, "MeasurementCovariance"); } /** equals */ bool equals(const NonlinearFactor& expected, double tol) const { - return key1_ == expected.key1_ && key2_ == expected.key2_ - && measured_.equals(expected.measured_, tol); + const BetweenFactor *e = + dynamic_cast*> (&expected); + return e != NULL && Base::equals(expected) + && this->measured_.equals(e->measured_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector error_vector(const T& p1, const T& p2) const { - //z-h - T hx = between(p1,p2); - // manifold equivalent of z-h(x) -> log(h(x),z) - return square_root_inverse_covariance_ * logmap(hx,measured_); + Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const { + // h - z + T hx = between(p1, p2); + // TODO should be done by noise model + if (H1) *H1 = square_root_inverse_covariance_ * Dbetween1(p1, p2); + if (H2) *H2 = square_root_inverse_covariance_ * Dbetween2(p1, p2); + // manifold equivalent of h(x)-z -> log(z,h(x)) + // TODO use noise model, error vector is not whitened yet + return square_root_inverse_covariance_ * logmap(measured_, hx); } - /** vector of errors */ - Vector error_vector(const Config& c) const { - return error_vector(c.get(key1_), c.get(key2_)); - } - - /** methods to retrieve both keys */ - inline const std::string& key1() const { return key1_;} - inline const std::string& key2() const { return key2_;} - /** return the measured */ inline const T measured() const {return measured_;} - /** keys as a list */ - inline std::list keys() const { return keys_;} - /** number of variables attached to this factor */ inline std::size_t size() const { return 2;} - - /** linearize */ - boost::shared_ptr linearize(const Config& x0) const { - const T& p1 = x0.get(key1_), p2 = x0.get(key2_); - Matrix A1 = Dbetween1(p1, p2); - Matrix A2 = Dbetween2(p1, p2); - Vector b = error_vector(p1, p2); // already has sigmas in ! - boost::shared_ptr linearized(new GaussianFactor( - key1_, square_root_inverse_covariance_ * A1, - key2_, square_root_inverse_covariance_ * A2, b, 1.0)); - return linearized; - } }; } /// namespace gtsam diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index a5738868a..203dec0e2 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -269,7 +269,7 @@ void FactorGraph::associateFactor(int index, sharedFactor factor) { } } -/* ************************************************************************* */ +/* ************************************************************************* * template map FactorGraph::findMinimumSpanningTree() const { diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index b1874a017..6e126609e 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -120,14 +120,14 @@ namespace gtsam { /** * find the minimum spanning tree. */ - std::map findMinimumSpanningTree() const; +// std::map findMinimumSpanningTree() const; /** * Split the graph into two parts: one corresponds to the given spanning tre, * and the other corresponds to the rest of the factors */ - void split(std::map tree, - FactorGraph& Ab1, FactorGraph& Ab2) const; +// void split(std::map tree, +// FactorGraph& Ab1, FactorGraph& Ab2) const; private: /** Associate factor index with the variables connected to the factor */ diff --git a/cpp/Key.h b/cpp/Key.h new file mode 100644 index 000000000..97c6e44a9 --- /dev/null +++ b/cpp/Key.h @@ -0,0 +1,58 @@ +/* + * Key.h + * + * Created on: Jan 12, 2010 + * @author: Frank Dellaert + * @author: Richard Roberts + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * Symbol 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 + * TODO: make Testable + */ + template + class Symbol { + + private: + size_t j_; + + public: + + // Constructors: + + Symbol():j_(999999) {} + Symbol(size_t j):j_(j) {} + + // Get stuff: + + size_t index() const { return j_;} + operator std::string() const { return (boost::format("%c%d") % C % j_).str(); } + std::string latex() const { return (boost::format("%c_{%d}") % C % j_).str(); } + + // logic: + + bool operator< (const Symbol& compare) const { return j_ + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(j_); + } + + }; + +} // namespace gtsam + diff --git a/cpp/LieConfig-inl.h b/cpp/LieConfig-inl.h index e24e4a604..2f9f36f23 100644 --- a/cpp/LieConfig-inl.h +++ b/cpp/LieConfig-inl.h @@ -17,58 +17,57 @@ using namespace std; -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) - namespace gtsam { - template - void LieConfig::print(const string &s) const { + template + void LieConfig::print(const string &s) const { cout << "LieConfig " << s << ", size " << values_.size() << "\n"; - pair v; - BOOST_FOREACH(v, values_) - gtsam::print(v.second, v.first + ": "); + BOOST_FOREACH(const typename Values::value_type& v, values_) + gtsam::print(v.second, (string)(v.first)); } - template - bool LieConfig::equals(const LieConfig& expected, double tol) const { + template + bool LieConfig::equals(const LieConfig& expected, double tol) const { if (values_.size() != expected.values_.size()) return false; - pair v; - BOOST_FOREACH(v, values_) { - boost::optional expval = expected.gettry(v.first); - if(!expval || !gtsam::equal(v.second, *expval, tol)) + BOOST_FOREACH(const typename Values::value_type& v, values_) { + if (!exists(v.first)) return false; + if(!gtsam::equal(v.second, expected[v.first], tol)) return false; } return true; } - template - const T& LieConfig::get(const std::string& key) const { - iterator it = values_.find(key); - if (it == values_.end()) throw std::invalid_argument("invalid key"); - else return it->second; - } - - template - boost::optional LieConfig::gettry(const std::string& key) const { + template + const T& LieConfig::at(const Key& key) const { const_iterator it = values_.find(key); - if (it == values_.end()) return boost::optional(); + if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key); else return it->second; } - template - void LieConfig::insert(const std::string& name, const T& val) { + template + void LieConfig::insert(const Key& name, const T& val) { values_.insert(make_pair(name, val)); dim_ += dim(val); } + template + void LieConfig::erase(const Key& key) { + iterator it = values_.find(key); + if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key); + values_.erase(it); + } + // todo: insert for every element is inefficient - template - LieConfig expmap(const LieConfig& c, const VectorConfig& delta) { - LieConfig newConfig; - string j; T pj; - FOREACH_PAIR(j, pj, c) { - if (delta.contains(j)) { - const Vector& dj = delta[j]; + template + LieConfig expmap(const LieConfig& c, const VectorConfig& delta) { + LieConfig newConfig; + typedef pair::Key,T> Value; + BOOST_FOREACH(const Value& value, c) { + const typename LieConfig::Key& j = value.first; + const T& pj = value.second; + string key = (string)j; + if (delta.contains(key)) { + const Vector& dj = delta[key]; newConfig.insert(j, expmap(pj,dj)); } else newConfig.insert(j, pj); @@ -77,21 +76,22 @@ namespace gtsam { } // todo: insert for every element is inefficient - template - LieConfig expmap(const LieConfig& c, const Vector& delta) { + template + LieConfig expmap(const LieConfig& c, const Vector& delta) { if(delta.size() != dim(c)) throw invalid_argument("Delta vector length does not match config dimensionality."); - LieConfig newConfig; - pair value; + LieConfig newConfig; int delta_offset = 0; - BOOST_FOREACH(value, c) { - int cur_dim = dim(value.second); - newConfig.insert(value.first, - expmap(value.second, - sub(delta, delta_offset, delta_offset+cur_dim))); + typedef pair::Key,T> Value; + BOOST_FOREACH(const Value& value, c) { + const typename LieConfig::Key& j = value.first; + const T& pj = value.second; + int cur_dim = dim(pj); + newConfig.insert(j,expmap(pj,sub(delta, delta_offset, delta_offset+cur_dim))); delta_offset += cur_dim; } return newConfig; } - } + + diff --git a/cpp/LieConfig.h b/cpp/LieConfig.h index 0813bae17..09bdd3937 100644 --- a/cpp/LieConfig.h +++ b/cpp/LieConfig.h @@ -9,61 +9,92 @@ #pragma once - #include #include "Vector.h" #include "Testable.h" #include "Lie.h" +#include "Key.h" namespace boost { template class optional; } -namespace gtsam { class VectorConfig; } namespace gtsam { - template class LieConfig; + class VectorConfig; + + // TODO: why are these defined *before* the class ? + template class LieConfig; /** Dimensionality of the tangent space */ - template - size_t dim(const LieConfig& c); + template + size_t dim(const LieConfig& c); /** Add a delta config */ - template - LieConfig expmap(const LieConfig& c, const VectorConfig& delta); + template + LieConfig expmap(const LieConfig& c, const VectorConfig& delta); /** Add a delta vector, uses iterator order */ - template - LieConfig expmap(const LieConfig& c, const Vector& delta); + template + LieConfig expmap(const LieConfig& c, const Vector& delta); + /** + * Lie type configuration + */ + template + class LieConfig : public Testable > { + + public: + + /** + * Typedefs + */ + typedef J Key; + typedef std::map Values; + typedef typename Values::iterator iterator; + typedef typename Values::const_iterator const_iterator; - template - class LieConfig : public Testable > { private: - std::map values_; + + Values values_; size_t dim_; public: - typedef typename std::map::const_iterator iterator; - typedef iterator const_iterator; LieConfig() : dim_(0) {} LieConfig(const LieConfig& config) : values_(config.values_), dim_(dim(config)) {} virtual ~LieConfig() {} + /** print */ + void print(const std::string &s) const; + + /** Test whether configs are identical in keys and values */ + bool equals(const LieConfig& expected, double tol=1e-9) const; + /** Retrieve a variable by key, throws std::invalid_argument if not found */ - const T& get(const std::string& key) const; + const T& at(const Key& key) const; /** operator[] syntax for get */ - inline const T& operator[](const std::string& name) const { - return get(name); - } + inline const T& operator[](const Key& key) const { return at(key);} - /** Retrieve a variable by key, returns nothing if not found */ - boost::optional gettry(const std::string& key) const; + /** Check if a variable exists */ + bool exists(const Key& i) const {return values_.find(i)!=values_.end();} + + /** The number of variables in this config */ + int size() const { return values_.size(); } + + const_iterator begin() const { return values_.begin(); } + const_iterator end() const { return values_.end(); } + iterator begin() { return values_.begin(); } + iterator end() { return values_.end(); } + + // imperative methods: /** Add a variable with the given key */ - void insert(const std::string& name, const T& val); + void insert(const Key& key, const T& val); + + /** Remove a variable from the config */ + void erase(const Key& key) ; /** Replace all keys and variables */ LieConfig& operator=(const LieConfig& rhs) { @@ -78,27 +109,14 @@ namespace gtsam { dim_ = 0; } - /** The number of variables in this config */ - int size() { return values_.size(); } - - /** Test whether configs are identical in keys and values */ - bool equals(const LieConfig& expected, double tol=1e-9) const; - - void print(const std::string &s) const; - - const_iterator begin() const { return values_.begin(); } - const_iterator end() const { return values_.end(); } - iterator begin() { return values_.begin(); } - iterator end() { return values_.end(); } - - friend LieConfig expmap(const LieConfig& c, const VectorConfig& delta); - friend LieConfig expmap(const LieConfig& c, const Vector& delta); - friend size_t dim(const LieConfig& c); +// friend LieConfig expmap(const LieConfig& c, const VectorConfig& delta); +// friend LieConfig expmap(const LieConfig& c, const Vector& delta); + friend size_t dim(const LieConfig& c); }; /** Dimensionality of the tangent space */ - template - size_t dim(const LieConfig& c) { return c.dim_; } + template + size_t dim(const LieConfig& c) { return c.dim_; } } diff --git a/cpp/Makefile.am b/cpp/Makefile.am index b2d00eec2..550020c7d 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -66,7 +66,7 @@ testSymbolicBayesNet_LDADD = libgtsam.la # Inference headers += inference.h inference-inl.h -headers += graph-inl.h +headers += graph.h graph-inl.h headers += FactorGraph.h FactorGraph-inl.h headers += BayesNet.h BayesNet-inl.h headers += BayesTree.h BayesTree-inl.h @@ -134,7 +134,7 @@ testSubgraphPreconditioner_LDADD = libgtsam.la #timeGaussianFactorGraph_LDADD = libgtsam.la # Nonlinear inference -headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h +headers += Key.h NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h sources += NonlinearFactor.cpp check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 60fd59540..79fe90676 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -22,7 +22,7 @@ NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key size_t dim_lagrange, Vector (*g)(const Config& config), bool isEquality) -: NonlinearFactor(zero(dim_lagrange), 1.0), +: NonlinearFactor(1.0),z_(zero(dim_lagrange)), lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality), g_(boost::bind(g, _1)) {} @@ -32,8 +32,8 @@ NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key size_t dim_lagrange, boost::function g, bool isEquality) -: NonlinearFactor(zero(dim_lagrange), 1.0), - lagrange_key_(lagrange_key), p_(dim_lagrange), +: NonlinearFactor(1.0), + lagrange_key_(lagrange_key), p_(dim_lagrange),z_(zero(dim_lagrange)), g_(g), isEquality_(isEquality) {} /* ************************************************************************* */ diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index 294fd1a04..69fab216e 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -27,6 +27,10 @@ template class NonlinearConstraint : public NonlinearFactor { protected: + + /** Lagrange multipliers? */ + Vector z_; + /** key for the lagrange multipliers */ std::string lagrange_key_; diff --git a/cpp/NonlinearEquality.h b/cpp/NonlinearEquality.h index 35097e394..1fffc5a6a 100644 --- a/cpp/NonlinearEquality.h +++ b/cpp/NonlinearEquality.h @@ -8,19 +8,17 @@ #include #include + +#include "Key.h" #include "NonlinearFactor.h" namespace gtsam { /** - * Template default compare function that assumes Congig.get yields a testable T + * Template default compare function that assumes a testable T */ - template - bool compare(const std::string& key, const Config& feasible, const Config& input) { - const T& t1 = feasible.get(key); - const T& t2 = input.get(key); - return t1.equals(t2); - } + template + bool compare(const T& a, const T& b) {return a.equals(b); } /** @@ -28,85 +26,54 @@ namespace gtsam { * or a set of variables to be equal to each other. * Throws an error at linearization if the constraints are not met. */ - template - class NonlinearEquality: public NonlinearFactor { + template + class NonlinearEquality: public NonlinearFactor1 { private: - // node to constrain - std::string key_; - - // config containing the necessary feasible point - Config feasible_; - - // dimension of the variable - size_t dim_; + // feasible value + T feasible_; public: /** - * Function that compares a value from a config with - * another to determine whether a linearization point is - * a feasible point. - * @param key is the identifier for the key - * @param feasible is the value which is constrained - * @param input is the config to be tested for feasibility - * @return true if the linearization point is feasible + * Function that compares two values */ - bool (*compare_)(const std::string& key, const Config& feasible, - const Config& input); + bool (*compare_)(const T& a, const T& b); - /** Constructor */ - NonlinearEquality(const std::string& key, const Config& feasible, - size_t dim, bool(*compare)(const std::string& key, - const Config& feasible, const Config& input)) : - key_(key), dim_(dim), feasible_(feasible), compare_(compare) { - - } + typedef NonlinearFactor1 Base; /** - * Constructor with default compare - * Needs class T to have dim() - * and Config to have insert and get + * Constructor */ - template - NonlinearEquality(const std::string& key, const T& feasible) : - key_(key), dim_(dim(feasible)), compare_(compare) { - feasible_.insert(key,feasible); + NonlinearEquality(const Key& j, const T& feasible, bool (*compare)(const T&, const T&) = compare) : + Base(0, j), feasible_(feasible), compare_(compare) { } void print(const std::string& s = "") const { - std::cout << "Constraint: " << s << " on [" << key_ << "]\n"; - feasible_.print("Feasible Point"); - std::cout << "Variable Dimension: " << dim_ << std::endl; + std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n"; + gtsam::print(feasible_,"Feasible Point"); + std::cout << "Variable Dimension: " << dim(feasible_) << std::endl; } /** Check if two factors are equal */ bool equals(const Factor& f, double tol = 1e-9) const { - const NonlinearEquality* p = - dynamic_cast*> (&f); + const NonlinearEquality* p = + dynamic_cast*> (&f); if (p == NULL) return false; - if (key_ != p->key_) return false; - if (!compare_(key_, feasible_, p->feasible_)) return false; // only check the relevant value - return dim_ == p->dim_; + if (!Base::equals(*p)) return false; + return compare_(feasible_, p->feasible_); } /** error function */ - inline Vector error_vector(const Config& c) const { - if (!compare_(key_, feasible_, c)) - return repeat(dim_, std::numeric_limits::infinity()); // set error to infinity if not equal - else - return zero(dim_); // set error to zero if equal - } - - /** linearize a nonlinear constraint into a linear constraint */ - boost::shared_ptr linearize(const Config& c) const { - if (!compare_(key_, feasible_, c)) { - throw std::invalid_argument("Linearization point not feasible for " - + key_ + "!"); + inline Vector evaluateError(const T& xj, boost::optional H) const { + size_t nj = dim(feasible_); + if (compare_(feasible_,xj)) { + if (H) *H = eye(nj); + return zero(nj); // set error to zero if equal } else { - GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_), - zero(dim_), 0.0)); - return ret; + if (H) throw std::invalid_argument( + "Linearization point not feasible for " + (std::string)(this->key_) + "!"); + return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal } } }; // NonlinearEquality diff --git a/cpp/NonlinearFactor.cpp b/cpp/NonlinearFactor.cpp index ef8d92f62..541a06677 100644 --- a/cpp/NonlinearFactor.cpp +++ b/cpp/NonlinearFactor.cpp @@ -2,10 +2,10 @@ * @file NonlinearFactor.cpp * @brief nonlinear factor versions which assume a Gaussian noise on a measurement * @brief predicted by a non-linear function h nonlinearFactor - * @author Kai Ni - * @author Carlos Nieto - * @author Christian Potthast * @author Frank Dellaert + * @author Richard Roberts + * + * Earlier prototype contributors: Kai Ni, Carlos Nieto, Christian Potthast */ #include "NonlinearFactor.h" @@ -14,105 +14,3 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -NonlinearFactor1::NonlinearFactor1(const Vector& z, - const double sigma, - Vector (*h)(const Vector&), - const string& key1, - Matrix (*H)(const Vector&)) - : NonlinearFactor(z, sigma), h_(h), key_(key1), H_(H) -{ - keys_.push_front(key1); -} - -/* ************************************************************************* */ -void NonlinearFactor1::print(const string& s) const { - cout << "NonlinearFactor1 " << s << endl; - cout << "h : " << (void*)h_ << endl; - cout << "key: " << key_ << endl; - cout << "H : " << (void*)H_ << endl; - NonlinearFactor::print("parent"); -} - -/* ************************************************************************* */ -GaussianFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) const { - // get argument 1 from config - Vector x1 = c[key_]; - - // Jacobian A = H(x1)/sigma - Matrix A = H_(x1); - - // Right-hand-side b = error(c) = (z - h(x1))/sigma - Vector b = (z_ - h_(x1)); - - GaussianFactor::shared_ptr p(new GaussianFactor(key_, A, b, sigma_)); - return p; -} - -/* ************************************************************************* */ -/** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html */ -/* ************************************************************************* */ -bool NonlinearFactor1::equals(const Factor& f, double tol) const { - const NonlinearFactor1* p = dynamic_cast (&f); - if (p == NULL) return false; - return NonlinearFactor::equals(*p, tol) - && (h_ == p->h_) - && (key_ == p->key_) - && (H_ == p->H_); -} - -/* ************************************************************************* */ -NonlinearFactor2::NonlinearFactor2(const Vector& z, - const double sigma, - Vector (*h)(const Vector&, const Vector&), - const string& key1, - Matrix (*H1)(const Vector&, const Vector&), - const string& key2, - Matrix (*H2)(const Vector&, const Vector&) -) -: NonlinearFactor(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2) -{ - keys_.push_front(key1); - keys_.push_front(key2); -} - -/* ************************************************************************* */ -void NonlinearFactor2::print(const string& s) const { - cout << "NonlinearFactor2 " << s << endl; - cout << "h : " << (void*)h_ << endl; - cout << "key1: " << key1_ << endl; - cout << "H2 : " << (void*)H2_ << endl; - cout << "key2: " << key2_ << endl; - cout << "H1 : " << (void*)H1_ << endl; - NonlinearFactor::print("parent"); -} - -/* ************************************************************************* */ -GaussianFactor::shared_ptr NonlinearFactor2::linearize(const VectorConfig& c) const { - // get arguments from config - Vector x1 = c[key1_]; - Vector x2 = c[key2_]; - - // Jacobian A = H(x)/sigma - Matrix A1 = H1_(x1, x2); - Matrix A2 = H2_(x1, x2); - - // Right-hand-side b = (z - h(x))/sigma - Vector b = (z_ - h_(x1, x2)); - - GaussianFactor::shared_ptr p(new GaussianFactor(key1_, A1, key2_, A2, b, sigma_)); - return p; -} - -/* ************************************************************************* */ -bool NonlinearFactor2::equals(const Factor& f, double tol) const { - const NonlinearFactor2* p = dynamic_cast (&f); - if (p == NULL) return false; - return NonlinearFactor::equals(*p, tol) - && (h_ == p->h_) - && (key1_ == p->key1_) - && (H1_ == p->H1_) - && (key2_ == p->key2_) - && (H2_ == p->H2_); -} - -/* ************************************************************************* */ diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h index 21e188fdc..538ba9b63 100644 --- a/cpp/NonlinearFactor.h +++ b/cpp/NonlinearFactor.h @@ -1,13 +1,10 @@ /** * @file NonlinearFactor.h * @brief Non-linear factor class - * @author Kai Ni - * @author Carlos Nieto - * @author Christian Potthast * @author Frank Dellaert + * @author Richard Roberts */ - // \callgraph #pragma once @@ -16,184 +13,304 @@ #include #include -#include +#include #include "Factor.h" #include "Vector.h" #include "Matrix.h" #include "GaussianFactor.h" -/** - * Base Class - * Just has the measurement and noise model - */ - namespace gtsam { - // forward declaration of GaussianFactor - //class GaussianFactor; - //typedef boost::shared_ptr shared_ptr; + // TODO class NoiseModel {}; + // TODO class Isotropic : public NoiseModel {}; + // TODO class Diagonal : public NoiseModel {}; + // TODO class Full : public NoiseModel {}; + // TODO class Robust : public NoiseModel {}; - /** - * Nonlinear factor which assumes Gaussian noise on a measurement - * predicted by a non-linear function h. - * - * Templated on a configuration type. The configurations are typically more general - * than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds. - */ - template - class NonlinearFactor : public Factor - { - protected: + /** + * Nonlinear factor which assumes zero-mean Gaussian noise on the + * on a measurement predicted by a non-linear function h. + * + * Templated on a configuration type. The configurations are typically + * more general than just vectors, e.g., Rot3 or Pose3, + * which are objects in non-linear manifolds (Lie groups). + */ + template + class NonlinearFactor: public Factor { - Vector z_; // measurement - double sigma_; // noise standard deviation - std::list keys_; // keys - - public: + protected: - /** Default constructor, with easily identifiable bogus values */ - NonlinearFactor():z_(Vector_(2,888.0,999.0)),sigma_(0.1234567) {} + double sigma_; // noise standard deviation + std::list keys_; // keys - /** - * Constructor - * @param z the measurement - * @param sigma the standard deviation - */ - NonlinearFactor(const Vector& z, const double sigma) { - z_ = z; - sigma_ = sigma; - } + typedef NonlinearFactor This; - /** print */ - void print(const std::string& s = "") const { - std::cout << "NonlinearFactor " << s << std::endl; - gtsam::print(z_, " z = "); - std::cout << " sigma = " << sigma_ << std::endl; - } + public: - /** Check if two NonlinearFactor objects are equal */ - bool equals(const Factor& f, double tol=1e-9) const { - const NonlinearFactor* p = dynamic_cast*> (&f); - if (p == NULL) return false; - return equal_with_abs_tol(z_,p->z_,tol) && fabs(sigma_ - p->sigma_)<=tol; - } + /** Default constructor for I/O only */ + NonlinearFactor() { + } - /** Vector of errors */ - virtual Vector error_vector(const Config& c) const = 0; + /** + * Constructor + * @param sigma the standard deviation + * // TODO: take a NoiseModel shared pointer + */ + NonlinearFactor(const double sigma) : + sigma_(sigma) { + } - /** linearize to a GaussianFactor */ - virtual boost::shared_ptr linearize(const Config& c) const = 0; + /** print */ + void print(const std::string& s = "") const { + std::cout << "NonlinearFactor " << s << std::endl; + std::cout << " sigma = " << sigma_ << std::endl; + } - /** get functions */ - double sigma() const {return sigma_;} - Vector measurement() const {return z_;} - std::list keys() const {return keys_;} + /** Check if two NonlinearFactor objects are equal */ + bool equals(const Factor& f, double tol = 1e-9) const { + const This* p = dynamic_cast*> (&f); + if (p == NULL) return false; + return fabs(sigma_ - p->sigma_) <= tol; + } - /** calculate the error of the factor */ - double error(const Config& c) const { - if (sigma_==0.0) { - Vector e = error_vector(c); - return (inner_prod(e,e)>0) ? std::numeric_limits::infinity() : 0.0; - } - Vector e = error_vector(c) / sigma_; - return 0.5 * inner_prod(e,e); - }; - - /** get the size of the factor */ - std::size_t size() const{return keys_.size();} + /** + * calculate the error of the factor + * TODO: use NoiseModel + */ + double error(const Config& c) const { + // return NoiseModel.mahalanobis(error_vector(c)); // e'*inv(C)*e + if (sigma_ == 0.0) { + Vector e = error_vector(c); + return (inner_prod(e, e) > 0) ? std::numeric_limits::infinity() + : 0.0; + } + Vector e = error_vector(c) / sigma_; + return 0.5 * inner_prod(e, e); + } + ; - private: + /** return keys */ + std::list keys() const { + return keys_; + } + + /** get the size of the factor */ + std::size_t size() const { + return keys_.size(); + } + + /** get functions */ + double sigma() const { + return sigma_; + } // TODO obsolete when using NoiseModel + + /** Vector of errors */ + virtual Vector error_vector(const Config& c) const = 0; + + /** linearize to a GaussianFactor */ + virtual boost::shared_ptr + linearize(const Config& c) const = 0; + + private: /** Serialization function */ friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(z_); - ar & BOOST_SERIALIZATION_NVP(sigma_); - ar & BOOST_SERIALIZATION_NVP(keys_); + ar & BOOST_SERIALIZATION_NVP(sigma_); // TODO NoiseModel } - }; // NonlinearFactor + }; // NonlinearFactor - /** - * a Gaussian nonlinear factor that takes 1 parameter - * Note: cannot be serialized as contains function pointers - * Specialized derived classes could do this - */ - class NonlinearFactor1 : public NonlinearFactor { - private: - - std::string key_; - - public: - - Vector (*h_)(const Vector&); - Matrix (*H_)(const Vector&); - - /** Constructor */ - NonlinearFactor1(const Vector& z, // measurement - const double sigma, // variance - Vector (*h)(const Vector&), // measurement function - const std::string& key1, // key of the variable - Matrix (*H)(const Vector&)); // derivative of the measurement function - - void print(const std::string& s = "") const; - - /** Check if two factors are equal */ - bool equals(const Factor& f, double tol=1e-9) const; - - /** error function */ - inline Vector error_vector(const VectorConfig& c) const { - return z_ - h_(c[key_]); - } - - /** linearize a non-linearFactor1 to get a linearFactor1 */ - boost::shared_ptr linearize(const VectorConfig& c) const; - }; - /** - * a Gaussian nonlinear factor that takes 2 parameters + * A Gaussian nonlinear factor that takes 1 parameter + * implementing the density P(z|x) \propto exp -0.5*|z-h(x)|^2_C + * Templated on the parameter type X and the configuration Config + * There is no return type specified for h(x). Instead, we require + * the derived class implements error_vector(c) = h(x)-z \approx Ax-b + * This allows a graph to have factors with measurements of mixed type. + */ + template + class NonlinearFactor1: public NonlinearFactor { + + protected: + + // The value of the key. Not const to allow serialization + Key key_; + + typedef NonlinearFactor Base; + typedef NonlinearFactor1 This; + + public: + + /** + * Constructor + * @param z measurement + * @param key by which to look up X value in Config + */ + NonlinearFactor1(double sigma, const Key& key1) : + Base(sigma), key_(key1) { + this->keys_.push_back(key_); + } + + /* print */ + void print(const std::string& s = "") const { + std::cout << "NonlinearFactor1 " << s << std::endl; + std::cout << "key: " << (std::string) key_ << std::endl; + Base::print("parent"); + } + + /** Check if two factors are equal. Note type is Factor and needs cast. */ + bool equals(const Factor& f, double tol = 1e-9) const { + const This* p = dynamic_cast (&f); + if (p == NULL) return false; + return Base::equals(*p, tol) && (key_ == p->key_); + } + + /** error function z-h(x) */ + inline Vector error_vector(const Config& x) const { + Key j = key_; + const X& xj = x[j]; + return evaluateError(xj); + } + + /** + * Linearize a non-linearFactor1 to get a GaussianFactor + * Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z + * Hence b = z - h(x0) = - error_vector(x) + */ + boost::shared_ptr linearize(const Config& x) const { + const X& xj = x[key_]; + Matrix A; + Vector b = -evaluateError(xj, A); + return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b, + this->sigma())); + } + + /* + * Override this method to finish implementing a unary factor. + * If the optional Matrix reference argument is specified, it should compute + * both the function evaluation and its derivative in X. + */ + virtual Vector evaluateError(const X& x, boost::optional H = + boost::none) const = 0; + + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(key_); + } + +}; + + /** + * A Gaussian nonlinear factor that takes 2 parameters * Note: cannot be serialized as contains function pointers * Specialized derived classes could do this - */ - class NonlinearFactor2 : public NonlinearFactor { + */ + template + class NonlinearFactor2: public NonlinearFactor { - private: + protected: - std::string key1_; - std::string key2_; + // The values of the keys. Not const to allow serialization + Key1 key1_; + Key2 key2_; - public: + typedef NonlinearFactor Base; + typedef NonlinearFactor2 This; - Vector (*h_)(const Vector&, const Vector&); - Matrix (*H1_)(const Vector&, const Vector&); - Matrix (*H2_)(const Vector&, const Vector&); + public: - /** Constructor */ - NonlinearFactor2(const Vector& z, // the measurement - const double sigma, // the variance - Vector (*h)(const Vector&, const Vector&), // the measurement function - const std::string& key1, // key of the first variable - Matrix (*H1)(const Vector&, const Vector&), // derivative of h in first variable - const std::string& key2, // key of the second variable - Matrix (*H2)(const Vector&, const Vector&));// derivative of h in second variable - - /** Print */ - void print(const std::string& s = "") const; + /** + * Default Constructor for I/O + */ + NonlinearFactor2(); - /** Check if two factors are equal */ - bool equals(const Factor& f, double tol=1e-9) const; + /** + * Constructor + * @param j1 key of the first variable + * @param j2 key of the second variable + */ + NonlinearFactor2(double sigma, Key1 j1, Key2 j2) : + Base(sigma), key1_(j1), key2_(j2) { + this->keys_.push_back(key1_); + this->keys_.push_back(key2_); + } - /** error function */ - inline Vector error_vector(const VectorConfig& c) const { - return z_ - h_(c[key1_], c[key2_]); - } + /** Print */ + void print(const std::string& s = "") const { + std::cout << "NonlinearFactor2 " << s << std::endl; + std::cout << "key1: " << (std::string) key1_ << std::endl; + std::cout << "key2: " << (std::string) key2_ << std::endl; + Base::print("parent"); + } - /** Linearize a non-linearFactor2 to get a linearFactor2 */ - boost::shared_ptr linearize(const VectorConfig& c) const; - }; + /** Check if two factors are equal */ + bool equals(const Factor& f, double tol = 1e-9) const { + const This* p = dynamic_cast (&f); + if (p == NULL) return false; + return Base::equals(*p, tol) && (key1_ == p->key1_) + && (key2_ == p->key2_); + } - /* ************************************************************************* */ + /** error function z-h(x1,x2) */ + inline Vector error_vector(const Config& x) const { + const X1& x1 = x[key1_]; + const X2& x2 = x[key2_]; + return evaluateError(x1, x2); + } + + /** + * Linearize a non-linearFactor1 to get a GaussianFactor + * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z + * Hence b = z - h(x1,x2) = - error_vector(x) + */ + boost::shared_ptr linearize(const Config& c) const { + const X1& x1 = c[key1_]; + const X2& x2 = c[key2_]; + Matrix A1, A2; + Vector b = -evaluateError(x1, x2, A1, A2); + return GaussianFactor::shared_ptr(new GaussianFactor(key1_, A1, key2_, + A2, b, this->sigma())); + } + + /** methods to retrieve both keys */ + inline const Key1& key1() const { + return key1_; + } + inline const Key2& key2() const { + return key2_; + } + + /* + * Override this method to finish implementing a binary factor. + * If any of the optional Matrix reference arguments are specified, it should compute + * both the function evaluation and its derivative(s) in X1 (and/or X2). + */ + virtual Vector evaluateError(const X1&, const X2&, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const = 0; + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(key1_); + ar & BOOST_SERIALIZATION_NVP(key2_); + } + + }; + +/* ************************************************************************* */ } diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp index 8f14582e0..e3fae0d96 100644 --- a/cpp/Ordering.cpp +++ b/cpp/Ordering.cpp @@ -20,6 +20,7 @@ using namespace boost::assign; #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) +/* class ordering_key_visitor : public boost::default_bfs_visitor { public: ordering_key_visitor(Ordering& ordering_in) : ordering_(ordering_in) {} @@ -36,8 +37,8 @@ public: //boost::add_edge(v1, v2, g); } }; - -/* ************************************************************************* */ +*/ +/* ************************************************************************* * Ordering::Ordering(const map& p_map) { SGraph g; diff --git a/cpp/Ordering.h b/cpp/Ordering.h index 5fbec35ab..68cbd96ce 100644 --- a/cpp/Ordering.h +++ b/cpp/Ordering.h @@ -42,7 +42,7 @@ namespace gtsam { /** * Generate the ordering from a spanning tree represented by its parent map */ - Ordering(const std::map& p_map); + //Ordering(const std::map& p_map); /** * Remove a set of keys from an ordering diff --git a/cpp/Point2.h b/cpp/Point2.h index c9c35dfe2..3dafe50ea 100644 --- a/cpp/Point2.h +++ b/cpp/Point2.h @@ -70,7 +70,7 @@ namespace gtsam { /** Lie group functions */ /** Global print calls member function */ - inline void print(const Point2& p, std::string& s) { p.print(s); } + inline void print(const Point2& p, const std::string& s) { p.print(s); } inline void print(const Point2& p) { p.print(); } /** Dimensionality of the tangent space */ diff --git a/cpp/Point2Prior.h b/cpp/Point2Prior.h index 3a8310ab5..72bc3cbcc 100644 --- a/cpp/Point2Prior.h +++ b/cpp/Point2Prior.h @@ -4,13 +4,21 @@ #include "NonlinearFactor.h" #include "simulated2D.h" -namespace gtsam { +namespace simulated2D { - class Point2Prior: public NonlinearFactor1 { - public: - Point2Prior(const Vector& mu, double sigma, const std::string& key) : - NonlinearFactor1(mu, sigma, prior, key, Dprior) { + struct Point2Prior: public gtsam::NonlinearFactor1 { + + Vector z_; + + Point2Prior(const Vector& z, double sigma, const std::string& key) : + gtsam::NonlinearFactor1(sigma, key), z_(z) { } + + Vector evaluateError(const Vector& x, boost::optional H = boost::none) const { + if (H) *H = Dprior(x); + return prior(x) - z_; + } + }; -} // namespace gtsam +} // namespace simulated2D diff --git a/cpp/Point3.h b/cpp/Point3.h index a1f873f83..e6d3dc7b3 100644 --- a/cpp/Point3.h +++ b/cpp/Point3.h @@ -79,7 +79,7 @@ namespace gtsam { /** Global print calls member function */ - inline void print(const Point3& p, std::string& s) { p.print(s); } + inline void print(const Point3& p, const std::string& s) { p.print(s); } inline void print(const Point3& p) { p.print(); } /** return DOF, dimensionality of tangent space */ diff --git a/cpp/Pose2Config.cpp b/cpp/Pose2Config.cpp index 597f048da..be1ba1133 100644 --- a/cpp/Pose2Config.cpp +++ b/cpp/Pose2Config.cpp @@ -12,25 +12,17 @@ using namespace std; namespace gtsam { /** Explicit instantiation of templated methods and functions */ - template class LieConfig; - template size_t dim(const LieConfig& c); - template LieConfig expmap(const LieConfig& c, const Vector& delta); - template LieConfig expmap(const LieConfig& c, const VectorConfig& delta); + template class LieConfig,Pose2>; + template size_t dim(const Pose2Config& c); + template Pose2Config expmap(const Pose2Config& c, const Vector& delta); + template Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta); /* ************************************************************************* */ - // TODO: local version, should probably defined in LieConfig - static string symbol(char c, int index) { - stringstream ss; - ss << c << index; - return ss.str(); - } - - /* ************************************************************************* */ - Pose2Config pose2Circle(size_t n, double R, char c) { + Pose2Config pose2Circle(size_t n, double R) { Pose2Config x; double theta = 0, dtheta = 2*M_PI/n; for(size_t i=0;i Pose2Config; + typedef LieConfig,Pose2> Pose2Config; /** * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) @@ -23,6 +23,6 @@ namespace gtsam { * @param c character to use for keys * @return circle of n 2D poses */ - Pose2Config pose2Circle(size_t n, double R, char c = 'p'); + Pose2Config pose2Circle(size_t n, double R); } // namespace diff --git a/cpp/Pose2Factor.h b/cpp/Pose2Factor.h index dc5a77398..71aa707d8 100644 --- a/cpp/Pose2Factor.h +++ b/cpp/Pose2Factor.h @@ -12,6 +12,6 @@ namespace gtsam { /** This is just a typedef now */ - typedef BetweenFactor Pose2Factor; + typedef BetweenFactor Pose2Factor; } /// namespace gtsam diff --git a/cpp/Pose2Graph.cpp b/cpp/Pose2Graph.cpp index a11d22e7a..d36bbc1dd 100644 --- a/cpp/Pose2Graph.cpp +++ b/cpp/Pose2Graph.cpp @@ -3,10 +3,11 @@ * @brief A factor graph for the 2D PoseSLAM problem * @authors Frank Dellaert, Viorela Ila */ + +#include "Pose2Graph.h" #include "FactorGraph-inl.h" #include "NonlinearFactorGraph-inl.h" #include "graph-inl.h" -#include "Pose2Graph.h" using namespace std; using namespace gtsam; @@ -14,7 +15,7 @@ using namespace gtsam; namespace gtsam { // explicit instantiation so all the code is there and we can link with it -template class FactorGraph > ; +template class FactorGraph > ; template class NonlinearFactorGraph ; //template class NonlinearOptimizer ; diff --git a/cpp/Pose2Graph.h b/cpp/Pose2Graph.h index 29645e883..3ba246206 100644 --- a/cpp/Pose2Graph.h +++ b/cpp/Pose2Graph.h @@ -7,10 +7,10 @@ #pragma once -#include "NonlinearFactorGraph.h" -#include "NonlinearEquality.h" #include "Pose2Factor.h" #include "Pose2Config.h" +#include "NonlinearFactorGraph.h" +#include "NonlinearEquality.h" namespace gtsam { @@ -33,7 +33,7 @@ namespace gtsam { /** * Add a factor without having to do shared factor dance */ - inline void add(const std::string& key1, const std::string& key2, + inline void add(const Pose2Config::Key& key1, const Pose2Config::Key& key2, const Pose2& measured, const Matrix& covariance) { push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance))); } @@ -43,8 +43,8 @@ namespace gtsam { * @param key of pose * @param pose which pose to constrain it to */ - inline void addConstraint(const std::string& key, const Pose2& pose = Pose2()) { - push_back(sharedFactor(new NonlinearEquality(key, pose))); + inline void addConstraint(const Pose2Config::Key& key, const Pose2& pose = Pose2()) { + push_back(sharedFactor(new NonlinearEquality(key, pose))); } private: diff --git a/cpp/Pose3Config.cpp b/cpp/Pose3Config.cpp index b307959f2..574bf88fd 100644 --- a/cpp/Pose3Config.cpp +++ b/cpp/Pose3Config.cpp @@ -12,10 +12,10 @@ using namespace std; namespace gtsam { /** Explicit instantiation of templated methods and functions */ - template class LieConfig; - template size_t dim(const LieConfig& c); - template LieConfig expmap(const LieConfig& c, const Vector& delta); - template LieConfig expmap(const LieConfig& c, const VectorConfig& delta); + template class LieConfig,Pose3>; + template size_t dim(const Pose3Config& c); + template Pose3Config expmap(const Pose3Config& c, const Vector& delta); + template Pose3Config expmap(const Pose3Config& c, const VectorConfig& delta); /* ************************************************************************* */ // TODO: local version, should probably defined in LieConfig @@ -26,13 +26,13 @@ namespace gtsam { } /* ************************************************************************* */ - Pose3Config pose3Circle(size_t n, double R, char c) { + Pose3Config pose3Circle(size_t n, double R) { Pose3Config x; double theta = 0, dtheta = 2*M_PI/n; // Vehicle at p0 is looking towards y axis Rot3 R0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); for (size_t i = 0; i < n; i++, theta += dtheta) - x.insert(symbol(c,i), Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0))); + x.insert(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0))); return x; } diff --git a/cpp/Pose3Config.h b/cpp/Pose3Config.h index 655bc4cbe..ac8fbacf3 100644 --- a/cpp/Pose3Config.h +++ b/cpp/Pose3Config.h @@ -14,7 +14,7 @@ namespace gtsam { /** * Pose3Config is now simply a typedef */ - typedef LieConfig Pose3Config; + typedef LieConfig,Pose3> Pose3Config; /** * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0,0) @@ -24,6 +24,6 @@ namespace gtsam { * @param c character to use for keys * @return circle of n 2D poses */ - Pose3Config pose3Circle(size_t n, double R, char c = 'p'); + Pose3Config pose3Circle(size_t n, double R); } // namespace diff --git a/cpp/Pose3Factor.h b/cpp/Pose3Factor.h index 03d891f79..600c43e4b 100644 --- a/cpp/Pose3Factor.h +++ b/cpp/Pose3Factor.h @@ -14,6 +14,6 @@ namespace gtsam { * A Factor for 3D pose measurements * This is just a typedef now */ - typedef BetweenFactor Pose3Factor; + typedef BetweenFactor Pose3Factor; } /// namespace gtsam diff --git a/cpp/Pose3Graph.h b/cpp/Pose3Graph.h index 3105b0d6e..c1237830c 100644 --- a/cpp/Pose3Graph.h +++ b/cpp/Pose3Graph.h @@ -7,10 +7,10 @@ #pragma once -#include "NonlinearFactorGraph.h" -#include "NonlinearEquality.h" #include "Pose3Factor.h" #include "Pose3Config.h" +#include "NonlinearFactorGraph.h" +#include "NonlinearEquality.h" namespace gtsam { @@ -33,7 +33,7 @@ namespace gtsam { /** * Add a factor without having to do shared factor dance */ - inline void add(const std::string& key1, const std::string& key2, + inline void add(const Pose3Config::Key& key1, const Pose3Config::Key& key2, const Pose3& measured, const Matrix& covariance) { push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance))); } @@ -43,8 +43,8 @@ namespace gtsam { * @param key of pose * @param pose which pose to constrain it to */ - inline void addConstraint(const std::string& key, const Pose3& pose = Pose3()) { - push_back(sharedFactor(new NonlinearEquality (key, pose))); + inline void addConstraint(const Pose3Config::Key& key, const Pose3& pose =Pose3()) { + push_back(sharedFactor(new NonlinearEquality (key, pose))); } private: diff --git a/cpp/Simulated2DMeasurement.h b/cpp/Simulated2DMeasurement.h index d355ebbc1..920eeb251 100644 --- a/cpp/Simulated2DMeasurement.h +++ b/cpp/Simulated2DMeasurement.h @@ -4,11 +4,26 @@ #include "NonlinearFactor.h" #include "simulated2D.h" -namespace gtsam { +namespace simulated2D { - struct Simulated2DMeasurement : public NonlinearFactor2 { - Simulated2DMeasurement(const Vector& z, double sigma, const std::string& key1, const std::string& key2): - NonlinearFactor2(z, sigma, mea, key1, Dmea1, key2, Dmea2) {} - }; + struct Simulated2DMeasurement: public gtsam::NonlinearFactor2 { -} // namespace gtsam + Vector z_; + + Simulated2DMeasurement(const Vector& z, double sigma, const std::string& j1, + const std::string& j2) : + z_(z), gtsam::NonlinearFactor2(sigma, j1, j2) { + } + + Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< + Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { + if (H1) *H1 = Dmea1(x1, x2); + if (H2) *H2 = Dmea2(x1, x2); + return mea(x1, x2) - z_; + } + + }; + +} // namespace simulated2D diff --git a/cpp/Simulated2DOdometry.h b/cpp/Simulated2DOdometry.h index 3de11dde9..1d966d06c 100644 --- a/cpp/Simulated2DOdometry.h +++ b/cpp/Simulated2DOdometry.h @@ -4,11 +4,25 @@ #include "NonlinearFactor.h" #include "simulated2D.h" -namespace gtsam { +namespace simulated2D { - struct Simulated2DOdometry : public NonlinearFactor2 { - Simulated2DOdometry(const Vector& z, double sigma, const std::string& key1, const std::string& key2): - NonlinearFactor2(z, sigma, odo, key1, Dodo1, key2, Dodo2) {} - }; + struct Simulated2DOdometry: public gtsam::NonlinearFactor2 { + Vector z_; + + Simulated2DOdometry(const Vector& z, double sigma, const std::string& j1, + const std::string& j2) : + z_(z), gtsam::NonlinearFactor2(sigma, j1, j2) { + } + + Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< + Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { + if (H1) *H1 = Dodo1(x1, x2); + if (H2) *H2 = Dodo2(x1, x2); + return odo(x1, x2) - z_; + } + + }; } // namespace gtsam diff --git a/cpp/Simulated3D.cpp b/cpp/Simulated3D.cpp index a3c8b749e..37c83d787 100644 --- a/cpp/Simulated3D.cpp +++ b/cpp/Simulated3D.cpp @@ -6,52 +6,54 @@ #include "Simulated3D.h" -namespace gtsam { +namespace simulated3D { -Vector prior_3D (const Vector& x) +using namespace gtsam; + +Vector prior (const Vector& x) { return x; } -Matrix Dprior_3D(const Vector& x) +Matrix Dprior(const Vector& x) { Matrix H = eye((int) x.size()); return H; } -Vector odo_3D(const Vector& x1, const Vector& x2) +Vector odo(const Vector& x1, const Vector& x2) { return x2 - x1; } -Matrix Dodo1_3D(const Vector& x1, const Vector& x2) +Matrix Dodo1(const Vector& x1, const Vector& x2) { Matrix H = -1 * eye((int) x1.size()); return H; } -Matrix Dodo2_3D(const Vector& x1, const Vector& x2) +Matrix Dodo2(const Vector& x1, const Vector& x2) { Matrix H = eye((int) x1.size()); return H; } -Vector mea_3D(const Vector& x, const Vector& l) +Vector mea(const Vector& x, const Vector& l) { return l - x; } -Matrix Dmea1_3D(const Vector& x, const Vector& l) +Matrix Dmea1(const Vector& x, const Vector& l) { Matrix H = -1 * eye((int) x.size()); return H; } -Matrix Dmea2_3D(const Vector& x, const Vector& l) +Matrix Dmea2(const Vector& x, const Vector& l) { Matrix H = eye((int) x.size()); return H; } -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/cpp/Simulated3D.h b/cpp/Simulated3D.h index 1cde91065..fd9d75035 100644 --- a/cpp/Simulated3D.h +++ b/cpp/Simulated3D.h @@ -1,47 +1,81 @@ /** -* @file Simulated3D.h -* @brief measurement functions and derivatives for simulated 3D robot -* @author Alex Cunningham -**/ + * @file Simulated3D.h + * @brief measurement functions and derivatives for simulated 3D robot + * @author Alex Cunningham + **/ // \callgraph #pragma once +#include "Matrix.h" +#include "VectorConfig.h" #include "NonlinearFactor.h" // \namespace -namespace gtsam { - - /** - * Prior on a single pose - */ - Vector prior_3D (const Vector& x); - Matrix Dprior_3D(const Vector& x); - - /** - * odometry between two poses - */ - Vector odo_3D(const Vector& x1, const Vector& x2); - Matrix Dodo1_3D(const Vector& x1, const Vector& x2); - Matrix Dodo2_3D(const Vector& x1, const Vector& x2); - - /** - * measurement between landmark and pose - */ - Vector mea_3D(const Vector& x, const Vector& l); - Matrix Dmea1_3D(const Vector& x, const Vector& l); - Matrix Dmea2_3D(const Vector& x, const Vector& l); - - struct Point2Prior3D : public NonlinearFactor1 { - Point2Prior3D(const Vector& mu, double sigma, const std::string& key): - NonlinearFactor1(mu, sigma, prior_3D, key, Dprior_3D) {} +namespace simulated3D { + + typedef gtsam::VectorConfig VectorConfig; + + struct PoseKey: public std::string { }; - - struct Simulated3DMeasurement : public NonlinearFactor2 { - Simulated3DMeasurement(const Vector& z, double sigma, const std::string& key1, const std::string& key2): - NonlinearFactor2(z, sigma, mea_3D, key1, Dmea1_3D, key2, Dmea2_3D) {} + struct PointKey: public std::string { }; - -} // namespace gtsam \ No newline at end of file + + /** + * Prior on a single pose + */ + Vector prior(const Vector& x); + Matrix Dprior(const Vector& x); + + /** + * odometry between two poses + */ + Vector odo(const Vector& x1, const Vector& x2); + Matrix Dodo1(const Vector& x1, const Vector& x2); + Matrix Dodo2(const Vector& x1, const Vector& x2); + + /** + * measurement between landmark and pose + */ + Vector mea(const Vector& x, const Vector& l); + Matrix Dmea1(const Vector& x, const Vector& l); + Matrix Dmea2(const Vector& x, const Vector& l); + + struct Point2Prior3D: public gtsam::NonlinearFactor1 { + + Vector z_; + + Point2Prior3D(const Vector& z, double sigma, const PoseKey& j) : + gtsam::NonlinearFactor1(sigma, j), z_(z) { + } + + Vector evaluateError(const Vector& x, boost::optional H = + boost::none) { + if (H) *H = Dprior(x); + return prior(x) - z_; + } + }; + + struct Simulated3DMeasurement: public gtsam::NonlinearFactor2 { + + Vector z_; + + Simulated3DMeasurement(const Vector& z, double sigma, PoseKey& j1, + PointKey j2) : + z_(z), gtsam::NonlinearFactor2(sigma, j1, j2) { + } + + Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< + Matrix&> H1 = boost::none, boost::optional H2 = boost::none) { + if (H1) *H1 = Dmea1(x1, x2); + if (H2) *H2 = Dmea2(x1, x2); + return mea(x1, x2) - z_; + } + }; + +} // namespace simulated3D diff --git a/cpp/VSLAMConfig.cpp b/cpp/VSLAMConfig.cpp index 46a09d5fe..8fb4a32eb 100644 --- a/cpp/VSLAMConfig.cpp +++ b/cpp/VSLAMConfig.cpp @@ -11,109 +11,32 @@ #include #include "VSLAMConfig.h" +#include "LieConfig-inl.h" using namespace std; -// trick from some reading group -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) - -#define SIGMA 1.0 - namespace gtsam { -/* ************************************************************************* */ -// Exponential map -VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta) { - - VSLAMConfig newConfig; - - for (map::const_iterator it = delta.begin(); it - != delta.end(); it++) { - string key = it->first; - if (key[0] == 'x') { - int cameraNumber = atoi(key.substr(1, key.size() - 1).c_str()); - if (c.cameraPoseExists(cameraNumber)) { - Pose3 basePose = c.cameraPose(cameraNumber); - newConfig.addCameraPose(cameraNumber, expmap(basePose, it->second)); - } - } - if (key[0] == 'l') { - int landmarkNumber = atoi(key.substr(1, key.size() - 1).c_str()); - if (c.landmarkPointExists(landmarkNumber)) { - Point3 basePoint = c.landmarkPoint(landmarkNumber); - newConfig.addLandmarkPoint(landmarkNumber, expmap(basePoint, it->second)); - } - } + /* ************************************************************************* */ + // Exponential map + VSLAMConfig expmap(const VSLAMConfig& x0, const VectorConfig & delta) { + VSLAMConfig x; + x.poses_ = expmap(x0.poses_, delta); + x.points_ = expmap(x0.points_, delta); + return x; } - return newConfig; -} - -/* ************************************************************************* */ -void VSLAMConfig::print(const std::string& s) const -{ - printf("%s:\n", s.c_str()); - printf("Camera Poses:\n"); - for(PoseMap::const_iterator it = cameraIteratorBegin(); it != cameraIteratorEnd(); it++) - { - printf("x%d:\n", it->first); - it->second.print(); - } - printf("Landmark Points:\n"); - for(PointMap::const_iterator it = landmarkIteratorBegin(); it != landmarkIteratorEnd(); it++) - { - printf("l%d:\n", (*it).first); - (*it).second.print(); - } -} - -/* ************************************************************************* */ -bool VSLAMConfig::equals(const VSLAMConfig& c, double tol) const { - for (PoseMap::const_iterator it = cameraIteratorBegin(); it - != cameraIteratorEnd(); it++) { - if (!c.cameraPoseExists(it->first)) return false; - if (!it->second.equals(c.cameraPose(it->first), tol)) return false; + /* ************************************************************************* */ + void VSLAMConfig::print(const std::string& s) const { + printf("%s:\n", s.c_str()); + poses_.print("Poses"); + points_.print("Points"); } - for (PointMap::const_iterator it = landmarkIteratorBegin(); it - != landmarkIteratorEnd(); it++) { - if (!c.landmarkPointExists(it->first)) return false; - if (!it->second.equals(c.landmarkPoint(it->first), tol)) return false; + /* ************************************************************************* */ + bool VSLAMConfig::equals(const VSLAMConfig& c, double tol) const { + return poses_.equals(c.poses_, tol) && points_.equals(c.points_, tol); } - return true; -} - -/* ************************************************************************* */ -void VSLAMConfig::addCameraPose(const int i, Pose3 cp) -{ - pair camera; - camera.first = i; - camera.second = cp; - cameraPoses_.insert(camera); -} - -/* ************************************************************************* */ -void VSLAMConfig::addLandmarkPoint(const int i, Point3 lp) -{ - pair landmark; - landmark.first = i; - landmark.second = lp; - landmarkPoints_.insert(landmark); -} - -/* ************************************************************************* */ -void VSLAMConfig::removeCameraPose(const int i) -{ - if(cameraPoseExists(i)) - cameraPoses_.erase(i); -} - -/* ************************************************************************* */ -void VSLAMConfig::removeLandmarkPose(const int i) -{ - if(landmarkPointExists(i)) - landmarkPoints_.erase(i); -} /* ************************************************************************* */ diff --git a/cpp/VSLAMConfig.h b/cpp/VSLAMConfig.h index 7b3213f22..172dcbf4e 100644 --- a/cpp/VSLAMConfig.h +++ b/cpp/VSLAMConfig.h @@ -9,10 +9,8 @@ #include #include #include -#include "VectorConfig.h" -#include "Pose3.h" -#include "Cal3_S2.h" -#include "Testable.h" + +#include "Pose3Config.h" #pragma once @@ -23,91 +21,59 @@ namespace gtsam{ */ class VSLAMConfig : Testable { - private: - typedef std::map PoseMap; - typedef std::map PointMap; - PointMap landmarkPoints_; - PoseMap cameraPoses_; +public: + + typedef Symbol PoseKey; + typedef Symbol PointKey; + +private: + + LieConfig poses_; + LieConfig points_; + +public: - public: - typedef std::map::const_iterator const_iterator; - typedef PoseMap::const_iterator const_Pose_iterator; - typedef PointMap::const_iterator const_Point_iterator; /** * default constructor */ VSLAMConfig() {} - /* - * copy constructor - */ - VSLAMConfig(const VSLAMConfig& original): - cameraPoses_(original.cameraPoses_), landmarkPoints_(original.landmarkPoints_){} - - PoseMap::const_iterator cameraIteratorBegin() const { return cameraPoses_.begin();} - PoseMap::const_iterator cameraIteratorEnd() const { return cameraPoses_.end();} - PointMap::const_iterator landmarkIteratorBegin() const { return landmarkPoints_.begin();} - PointMap::const_iterator landmarkIteratorEnd() const { return landmarkPoints_.end();} - /** * print */ void print(const std::string& s = "") const; - /** - * Retrieve robot pose - */ - bool cameraPoseExists(int i) const - { - PoseMap::const_iterator it = cameraPoses_.find(i); - if (it==cameraPoses_.end()) - return false; - return true; - } - - Pose3 cameraPose(int i) const { - PoseMap::const_iterator it = cameraPoses_.find(i); - if (it==cameraPoses_.end()) - throw(std::invalid_argument("robotPose: invalid key")); - return it->second; - } - - /** - * Check whether a landmark point exists - */ - bool landmarkPointExists(int i) const - { - PointMap::const_iterator it = landmarkPoints_.find(i); - if (it==landmarkPoints_.end()) - return false; - return true; - } - - /** - * Retrieve landmark point - */ - Point3 landmarkPoint(int i) const { - PointMap::const_iterator it = landmarkPoints_.find(i); - if (it==landmarkPoints_.end()) - throw(std::invalid_argument("markerPose: invalid key")); - return it->second; - } - /** * check whether two configs are equal */ bool equals(const VSLAMConfig& c, double tol=1e-6) const; - void addCameraPose(const int i, Pose3 cp); - void addLandmarkPoint(const int i, Point3 lp); - void removeCameraPose(const int i); - void removeLandmarkPose(const int i); + /** + * Get Poses or Points + */ + inline const Pose3& operator[](const PoseKey& key) const {return poses_[key];} + inline const Point3& operator[](const PointKey& key) const {return points_[key];} - void clear() {landmarkPoints_.clear(); cameraPoses_.clear();} + // (Awkwardly named) backwards compatibility: - inline size_t size(){ - return landmarkPoints_.size() + cameraPoses_.size(); - } + inline bool cameraPoseExists (const PoseKey& key) const {return poses_.exists(key);} + inline bool landmarkPointExists(const PointKey& key) const {return points_.exists(key);} + + inline Pose3 cameraPose (const PoseKey& key) const {return poses_[key];} + inline Point3 landmarkPoint(const PointKey& key) const {return points_[key];} + + inline size_t size() const {return points_.size() + poses_.size();} + inline size_t dim() const {return gtsam::dim(points_) + gtsam::dim(poses_);} + + // Imperative functions: + + inline void addCameraPose(const PoseKey& key, Pose3 cp) {poses_.insert(key,cp);} + inline void addLandmarkPoint(const PointKey& key, Point3 lp) {points_.insert(key,lp);} + + inline void removeCameraPose(const PoseKey& key) { poses_.erase(key);} + inline void removeLandmarkPose(const PointKey& key) { points_.erase(key);} + + inline void clear() {points_.clear(); poses_.clear();} friend VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta); }; @@ -119,7 +85,5 @@ class VSLAMConfig : Testable { * Needed for use in nonlinear optimization */ VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta); - - } // namespace gtsam diff --git a/cpp/VSLAMFactor.cpp b/cpp/VSLAMFactor.cpp index 54fdc2de8..837934049 100644 --- a/cpp/VSLAMFactor.cpp +++ b/cpp/VSLAMFactor.cpp @@ -4,88 +4,37 @@ * @author Alireza Fathi */ -#include "VSLAMConfig.h" +#include +#include + #include "VSLAMFactor.h" -#include "Pose3.h" #include "SimpleCamera.h" using namespace std; -namespace gtsam{ +namespace gtsam { -/* ************************************************************************* */ -VSLAMFactor::VSLAMFactor() { - /// Arbitrary values - cameraFrameNumber_ = 111; - landmarkNumber_ = 222; - cameraFrameName_ = symbol('x',cameraFrameNumber_); - landmarkName_ = symbol('l',landmarkNumber_); - keys_.push_back(cameraFrameName_); - keys_.push_back(landmarkName_); - K_ = shared_ptrK(new Cal3_S2(444,555,666,777,888)); -} -/* ************************************************************************* */ -VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln, const shared_ptrK &K) - : NonlinearFactor(z.vector(), sigma) -{ - cameraFrameNumber_ = cn; - landmarkNumber_ = ln; - cameraFrameName_ = symbol('x',cameraFrameNumber_); - landmarkName_ = symbol('l',landmarkNumber_); - keys_.push_back(cameraFrameName_); - keys_.push_back(landmarkName_); - K_ = K; -} + /* ************************************************************************* */ + VSLAMFactor::VSLAMFactor() { + /// Arbitrary values + K_ = shared_ptrK(new Cal3_S2(444, 555, 666, 777, 888)); + } + /* ************************************************************************* */ + VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln, + const shared_ptrK &K) : + VSLAMFactorBase(sigma, cn, ln), z_(z), K_(K) { + } -/* ************************************************************************* */ -void VSLAMFactor::print(const std::string& s) const { - printf("%s %s %s\n", s.c_str(), cameraFrameName_.c_str(), landmarkName_.c_str()); - gtsam::print(this->z_, s+".z"); -} + /* ************************************************************************* */ + void VSLAMFactor::print(const std::string& s) const { + VSLAMFactorBase::print(s); + z_.print(s + ".z"); + } -/* ************************************************************************* */ -bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const { - if (&p == NULL) return false; - if (cameraFrameNumber_ != p.cameraFrameNumber_ || landmarkNumber_ != p.landmarkNumber_) return false; - if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false; - return true; -} - -/* ************************************************************************* */ -Vector VSLAMFactor::predict(const VSLAMConfig& c) const { - Pose3 pose = c.cameraPose(cameraFrameNumber_); - Point3 landmark = c.landmarkPoint(landmarkNumber_); - return project(SimpleCamera(*K_,pose), landmark).vector(); -} - -/* ************************************************************************* */ -Vector VSLAMFactor::error_vector(const VSLAMConfig& c) const { - // Right-hand-side b = (z - h(x))/sigma - Vector h = predict(c); - return (this->z_ - h); -} - -/* ************************************************************************* */ -GaussianFactor::shared_ptr VSLAMFactor::linearize(const VSLAMConfig& c) const -{ - // get arguments from config - Pose3 pose = c.cameraPose(cameraFrameNumber_); - Point3 landmark = c.landmarkPoint(landmarkNumber_); - - SimpleCamera camera(*K_,pose); - - // Jacobians - Matrix Dh1 = Dproject_pose(camera, landmark); - Matrix Dh2 = Dproject_point(camera, landmark); - - // Right-hand-side b = (z - h(x)) - Vector h = project(camera, landmark).vector(); - Vector b = this->z_ - h; - - // Make new linearfactor, divide by sigma - GaussianFactor::shared_ptr - p(new GaussianFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, this->sigma_)); - return p; -} + /* ************************************************************************* */ + bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const { + return VSLAMFactorBase::equals(p, tol) && z_.equals(p.z_, tol) + && K_->equals(*p.K_, tol); + } /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/VSLAMFactor.h b/cpp/VSLAMFactor.h index f6f7ea034..55986e14e 100644 --- a/cpp/VSLAMFactor.h +++ b/cpp/VSLAMFactor.h @@ -6,88 +6,86 @@ #pragma once +#include + #include "NonlinearFactor.h" -#include "GaussianFactor.h" +#include "SimpleCamera.h" +#include "VSLAMConfig.h" #include "Cal3_S2.h" -#include "Testable.h" namespace gtsam { -class VSLAMConfig; - -/** - * Non-linear factor for a constraint derived from a 2D measurement, - * i.e. the main building block for visual SLAM. - */ -class VSLAMFactor : public NonlinearFactor, Testable -{ -private: - - int cameraFrameNumber_, landmarkNumber_; - std::string cameraFrameName_, landmarkName_; - boost::shared_ptr K_; // Calibration stored in each factor. FD: need to think about this. - typedef NonlinearFactor ConvenientFactor; - -public: - - typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor + typedef NonlinearFactor2 VSLAMFactorBase; /** - * Default constructor + * Non-linear factor for a constraint derived from a 2D measurement, + * i.e. the main building block for visual SLAM. */ - VSLAMFactor(); + class VSLAMFactor: public VSLAMFactorBase , Testable { + private: - /** - * Constructor - * @param z is the 2 dimensional location of point in image (the measurement) - * @param sigma is the standard deviation - * @param cameraFrameNumber is basically the frame number - * @param landmarkNumber is the index of the landmark - * @param K the constant calibration - */ - VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, int landmarkNumber, const shared_ptrK & K); + // Keep a copy of measurement and calibration for I/O + Point2 z_; + boost::shared_ptr K_; + public: - /** - * print - * @param s optional string naming the factor - */ - void print(const std::string& s="VSLAMFactor") const; + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; - /** - * equals - */ - bool equals(const VSLAMFactor&, double tol=1e-9) const; + /** + * Default constructor + */ + VSLAMFactor(); - /** - * predict the measurement - */ - Vector predict(const VSLAMConfig&) const; + /** + * Constructor + * @param z is the 2 dimensional location of point in image (the measurement) + * @param sigma is the standard deviation + * @param cameraFrameNumber is basically the frame number + * @param landmarkNumber is the index of the landmark + * @param K the constant calibration + */ + VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, + int landmarkNumber, const shared_ptrK & K); - /** - * calculate the error of the factor - */ - Vector error_vector(const VSLAMConfig&) const; + /** + * print + * @param s optional string naming the factor + */ + void print(const std::string& s = "VSLAMFactor") const; - /** - * linerarization - */ - GaussianFactor::shared_ptr linearize(const VSLAMConfig&) const; + /** + * equals + */ + bool equals(const VSLAMFactor&, double tol = 1e-9) const; - int getCameraFrameNumber() const { return cameraFrameNumber_; } - int getLandmarkNumber() const { return landmarkNumber_; } + /** h(x) */ + Point2 predict(const Pose3& pose, const Point3& point) const { + return SimpleCamera(*K_, pose).project(point); + } -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(cameraFrameNumber_); - ar & BOOST_SERIALIZATION_NVP(landmarkNumber_); - ar & BOOST_SERIALIZATION_NVP(cameraFrameName_); - ar & BOOST_SERIALIZATION_NVP(landmarkName_); - ar & BOOST_SERIALIZATION_NVP(K_); - } -}; + /** h(x)-z */ + Vector evaluateError(const Pose3& pose, const Point3& point, + boost::optional H1, boost::optional H2) const { + SimpleCamera camera(*K_, pose); + if (H1) *H1=Dproject_pose(camera,point); + if (H2) *H2=Dproject_point(camera,point); + Point2 reprojectionError(project(camera, point) - z_); + return reprojectionError.vector(); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(key1_); + ar & BOOST_SERIALIZATION_NVP(key2_); + ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(K_); + } + }; } diff --git a/cpp/VSLAMGraph.cpp b/cpp/VSLAMGraph.cpp index 9c180e717..ed85c9263 100644 --- a/cpp/VSLAMGraph.cpp +++ b/cpp/VSLAMGraph.cpp @@ -33,10 +33,8 @@ bool compareLandmark(const std::string& key, /* ************************************************************************* */ void VSLAMGraph::addLandmarkConstraint(int j, const gtsam::Point3& p) { - typedef NonlinearEquality NLE; - VSLAMConfig feasible; - feasible.addLandmarkPoint(j,p); - boost::shared_ptr factor(new NLE(symbol('l',j), feasible, 3, compareLandmark)); + typedef NonlinearEquality NLE; + boost::shared_ptr factor(new NLE(j, p)); push_back(factor); } @@ -50,10 +48,8 @@ bool compareCamera(const std::string& key, /* ************************************************************************* */ void VSLAMGraph::addCameraConstraint(int j, const gtsam::Pose3& p) { - typedef NonlinearEquality NLE; - VSLAMConfig feasible; - feasible.addCameraPose(j,p); - boost::shared_ptr factor(new NLE(symbol('x',j), feasible, 6, compareCamera)); + typedef NonlinearEquality NLE; + boost::shared_ptr factor(new NLE(j,p)); push_back(factor); } diff --git a/cpp/VSLAMGraph.h b/cpp/VSLAMGraph.h index 0e2b8b1a8..24ff9557d 100644 --- a/cpp/VSLAMGraph.h +++ b/cpp/VSLAMGraph.h @@ -12,9 +12,9 @@ #include #include +#include "VSLAMFactor.h" #include "NonlinearFactorGraph.h" #include "FactorGraph-inl.h" -#include "VSLAMFactor.h" #include "VSLAMConfig.h" #include "Testable.h" diff --git a/cpp/graph-inl.h b/cpp/graph-inl.h index fb2a197c3..28f948c69 100644 --- a/cpp/graph-inl.h +++ b/cpp/graph-inl.h @@ -1,19 +1,15 @@ /* * graph-inl.h - * - * Created on: Jan 11, 2010 - * Author: nikai - * Description: Graph algorithm using boost library + * @brief Graph algorithm using boost library + * @author: Kai Ni */ #pragma once #include -#include -#include #include -#include +#include "graph.h" using namespace std; @@ -21,31 +17,22 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -/** - * type definitions - */ -typedef boost::adjacency_list< - boost::vecS, boost::vecS, boost::undirectedS, - boost::property, - boost::property > SDGraph; -typedef boost::graph_traits::vertex_descriptor BoostVertex; -typedef boost::graph_traits::vertex_iterator BoostVertexIterator; +// some typedefs we need + +//typedef boost::graph_traits::vertex_iterator BoostVertexIterator; + +//typedef boost::adjacency_list > SGraph; +//typedef boost::graph_traits::vertex_descriptor SVertex; -typedef boost::adjacency_list< - boost::vecS, boost::vecS, boost::directedS, - boost::property > SGraph; -typedef boost::graph_traits::vertex_descriptor SVertex; /* ************************************************************************* */ -/** - * Convert the factor graph to a boost undirected graph - */ -template -SDGraph toBoostGraph(const G& graph) { +template +SDGraph toBoostGraph(const G& graph) { // convert the factor graph to boost graph - SDGraph g(0); - map key2vertex; + SDGraph g(0); + typedef typename boost::graph_traits >::vertex_descriptor BoostVertex; + map key2vertex; BoostVertex v1, v2; BOOST_FOREACH(F factor, graph) { if (factor->keys().size() > 2) @@ -54,8 +41,8 @@ SDGraph toBoostGraph(const G& graph) { if (factor->keys().size() == 1) continue; - string key1 = factor->keys().front(); - string key2 = factor->keys().back(); + Key key1 = factor->key1(); + Key key2 = factor->key2(); if (key2vertex.find(key1) == key2vertex.end()) { v1 = add_vertex(key1, g); @@ -77,16 +64,13 @@ SDGraph toBoostGraph(const G& graph) { } /* ************************************************************************* */ -/** - * build the graph corresponding to the predecessor map. Excute action for each edge. - */ -template -boost::tuple > predecessorMap2Graph(const map& p_map) { +template +boost::tuple > predecessorMap2Graph(const PredecessorMap& p_map) { G g(0); - map key2vertex; + map key2vertex; V v1, v2, root; - string child, parent; + Key child, parent; bool foundRoot = false; FOREACH_PAIR(child, parent, p_map) { if (key2vertex.find(child) == key2vertex.end()) { @@ -101,7 +85,7 @@ boost::tuple > predecessorMap2Graph(const map > predecessorMap2Graph(const map >(g, root, key2vertex); + return boost::tuple >(g, root, key2vertex); } /* ************************************************************************* */ -/** - * Visit each edge and compose the poses - */ -template +template class compose_key_visitor : public boost::default_bfs_visitor { -public: - compose_key_visitor(boost::shared_ptr config_in) { config = config_in; } - template void tree_edge(Edge edge, const Graph& g) const { - string key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); - string 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, compose(relativePose, config->get(key_from))); - } private: - boost::shared_ptr config; + boost::shared_ptr config_; + +public: + + compose_key_visitor(boost::shared_ptr config_in) {config_ = config_in;} + + template void tree_edge(Edge edge, const Graph& g) const { + typename Config::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); + typename Config::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, compose(relativePose, (*config_)[key_from])); + } }; /* ************************************************************************* */ -/** - * Compose the poses by following the chain sepcified by the spanning tree - */ template -boost::shared_ptr composePoses(const G& graph, const map& tree, +boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, const Pose& rootPose) { //TODO: change edge_weight_t to edge_pose_t typedef typename boost::adjacency_list< boost::vecS, boost::vecS, boost::directedS, - boost::property, + boost::property, boost::property > PoseGraph; typedef typename boost::graph_traits::vertex_descriptor PoseVertex; typedef typename boost::graph_traits::edge_descriptor PoseEdge; PoseGraph g; PoseVertex root; - map key2vertex; - boost::tie(g, root, key2vertex) = predecessorMap2Graph(tree); + map key2vertex; + boost::tie(g, root, key2vertex) = + predecessorMap2Graph(tree); // attach the relative poses to the edges PoseEdge edge1, edge2; @@ -167,8 +149,11 @@ boost::shared_ptr composePoses(const G& graph, const map boost::shared_ptr factor = boost::dynamic_pointer_cast(nl_factor); if (!factor) continue; - PoseVertex v_from = key2vertex.find(factor->keys().front())->second; - PoseVertex v_to = key2vertex.find(factor->keys().back())->second; + typename Config::Key key1 = factor->key1(); + typename Config::Key key2 = factor->key2(); + + PoseVertex v_from = key2vertex.find(key1)->second; + PoseVertex v_to = key2vertex.find(key2)->second; Pose measured = factor->measured(); tie(edge1, found1) = boost::edge(v_from, v_to, g); @@ -183,7 +168,8 @@ boost::shared_ptr composePoses(const G& graph, const map // compose poses boost::shared_ptr config(new Config); - config->insert(boost::get(boost::vertex_name, g, root), rootPose); + typename Config::Key rootKey = boost::get(boost::vertex_name, g, root); + config->insert(rootKey, rootPose); compose_key_visitor vis(config); boost::breadth_first_search(g, root, boost::visitor(vis)); diff --git a/cpp/graph.h b/cpp/graph.h new file mode 100644 index 000000000..ba42ffc92 --- /dev/null +++ b/cpp/graph.h @@ -0,0 +1,59 @@ +/* + * graph.h + * @brief Graph algorithm using boost library + * @author: Kai Ni + * Created on: Jan 11, 2010 + */ + +#pragma once + +#include + +#include +#include +#include + +namespace gtsam { + + // type definitions : + + /** + * SDGraph is undirected graph with variable keys and double edge weights + */ + template + class SDGraph: public boost::adjacency_list, boost::property< + boost::edge_weight_t, double> > { + }; + + /** + * Map from variable key to parent key + */ + template + class PredecessorMap : public std::map {}; + + /** + * Convert the factor graph to an SDGraph + * G = Graph type + * F = Factor type + * Key = Key type + */ + template SDGraph toBoostGraph(const G& graph); + + /** + * Build takes a predecessor map, and builds a directed graph corresponding to the tree. + * G = Graph type + * V = Vertex type + */ + template + boost::tuple > + predecessorMap2Graph(const PredecessorMap& p_map); + + /** + * Compose the poses by following the chain specified by the spanning tree + */ + template + boost::shared_ptr + composePoses(const G& graph, const PredecessorMap& tree, const Pose& rootPose); + +} // namespace gtsam diff --git a/cpp/simulated2D.cpp b/cpp/simulated2D.cpp index 45d3f533c..6b67c7196 100644 --- a/cpp/simulated2D.cpp +++ b/cpp/simulated2D.cpp @@ -6,7 +6,7 @@ #include "simulated2D.h" -namespace gtsam { +namespace simulated2D { /** prior on a single pose */ @@ -64,4 +64,4 @@ Matrix Dmea2(const Vector& x, const Vector& l) { } /* ************************************************************************* */ -} // namespace gtsam +} // namespace simulated2D diff --git a/cpp/simulated2D.h b/cpp/simulated2D.h index b2bdcdbf2..c9a078a3d 100644 --- a/cpp/simulated2D.h +++ b/cpp/simulated2D.h @@ -8,30 +8,44 @@ #pragma once +#include "VectorConfig.h" #include "NonlinearFactor.h" // \namespace -namespace gtsam { +namespace simulated2D { - /** - * Prior on a single pose - */ - Vector prior (const Vector& x); - Matrix Dprior(const Vector& x); + typedef gtsam::VectorConfig VectorConfig; - /** - * odometry between two poses - */ - Vector odo(const Vector& x1, const Vector& x2); - Matrix Dodo1(const Vector& x1, const Vector& x2); - Matrix Dodo2(const Vector& x1, const Vector& x2); + struct PoseKey: public std::string { + PoseKey(const std::string&s) : + std::string(s) { + } + }; + struct PointKey: public std::string { + PointKey(const std::string&s) : + std::string(s) { + } + }; - /** - * measurement between landmark and pose - */ - Vector mea(const Vector& x, const Vector& l); - Matrix Dmea1(const Vector& x, const Vector& l); - Matrix Dmea2(const Vector& x, const Vector& l); + /** + * Prior on a single pose + */ + Vector prior(const Vector& x); + Matrix Dprior(const Vector& x); -} // namespace gtsam + /** + * odometry between two poses + */ + Vector odo(const Vector& x1, const Vector& x2); + Matrix Dodo1(const Vector& x1, const Vector& x2); + Matrix Dodo2(const Vector& x1, const Vector& x2); + + /** + * measurement between landmark and pose + */ + Vector mea(const Vector& x, const Vector& l); + Matrix Dmea1(const Vector& x, const Vector& l); + Matrix Dmea2(const Vector& x, const Vector& l); + +} // namespace simulated2D diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index 4403ab378..056eb185c 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -8,6 +8,7 @@ #include #include +#include using namespace std; @@ -37,7 +38,7 @@ namespace gtsam { // prior on x1 double sigma1 = 0.1; Vector mu = zero(2); - shared f1(new Point2Prior(mu, sigma1, "x1")); + shared f1(new simulated2D::Point2Prior(mu, sigma1, "x1")); nlfg->push_back(f1); // odometry between x1 and x2 @@ -45,7 +46,7 @@ namespace gtsam { Vector z2(2); z2(0) = 1.5; z2(1) = 0; - shared f2(new Simulated2DOdometry(z2, sigma2, "x1", "x2")); + shared f2(new simulated2D::Simulated2DOdometry(z2, sigma2, "x1", "x2")); nlfg->push_back(f2); // measurement between x1 and l1 @@ -53,7 +54,7 @@ namespace gtsam { Vector z3(2); z3(0) = 0.; z3(1) = -1.; - shared f3(new Simulated2DMeasurement(z3, sigma3, "x1", "l1")); + shared f3(new simulated2D::Simulated2DMeasurement(z3, sigma3, "x1", "l1")); nlfg->push_back(f3); // measurement between x2 and l1 @@ -61,7 +62,7 @@ namespace gtsam { Vector z4(2); z4(0) = -1.5; z4(1) = -1.; - shared f4(new Simulated2DMeasurement(z4, sigma4, "x2", "l1")); + shared f4(new simulated2D::Simulated2DMeasurement(z4, sigma4, "x2", "l1")); nlfg->push_back(f4); return nlfg; @@ -173,16 +174,35 @@ namespace gtsam { // Some nonlinear functions to optimize /* ************************************************************************* */ namespace smallOptimize { + Vector h(const Vector& v) { double x = v(0); return Vector_(2, cos(x), sin(x)); } - ; + Matrix H(const Vector& v) { double x = v(0); return Matrix_(2, 1, -sin(x), cos(x)); } - ; + + struct UnaryFactor: public gtsam::NonlinearFactor1 { + + Vector z_; + + UnaryFactor(const Vector& z, double sigma, const std::string& key) : + gtsam::NonlinearFactor1(sigma, key), + z_(z) { + } + + Vector evaluateError(const Vector& x, boost::optional A = + boost::none) const { + if (A) *A = H(x); + return h(x) - z_; + } + + }; + } /* ************************************************************************* */ @@ -191,8 +211,8 @@ namespace gtsam { new ExampleNonlinearFactorGraph); Vector z = Vector_(2, 1.0, 0.0); double sigma = 0.1; - boost::shared_ptr factor(new NonlinearFactor1(z, sigma, - &smallOptimize::h, "x", &smallOptimize::H)); + boost::shared_ptr factor(new smallOptimize::UnaryFactor( + z, sigma, "x")); fg->push_back(factor); return fg; } @@ -214,7 +234,7 @@ namespace gtsam { // prior on x1 Vector x1 = Vector_(2, 1.0, 0.0); string key1 = symbol('x', 1); - shared prior(new Point2Prior(x1, sigma1, key1)); + shared prior(new simulated2D::Point2Prior(x1, sigma1, key1)); nlfg.push_back(prior); poses.insert(key1, x1); @@ -222,13 +242,13 @@ namespace gtsam { // odometry between x_t and x_{t-1} Vector odo = Vector_(2, 1.0, 0.0); string key = symbol('x', t); - shared odometry(new Simulated2DOdometry(odo, sigma2, symbol('x', t - 1), + shared odometry(new simulated2D::Simulated2DOdometry(odo, sigma2, symbol('x', t - 1), key)); nlfg.push_back(odometry); // measurement on x_t is like perfect GPS Vector xt = Vector_(2, (double) t, 0.0); - shared measurement(new Point2Prior(xt, sigma1, key)); + shared measurement(new simulated2D::Point2Prior(xt, sigma1, key)); nlfg.push_back(measurement); // initial estimate @@ -509,7 +529,7 @@ namespace gtsam { // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal double sigma0 = 1e-3; - shared constraint(new Point2Prior(Vector_(2, 1.0, 1.0), sigma0, "x11")); + shared constraint(new simulated2D::Point2Prior(Vector_(2, 1.0, 1.0), sigma0, "x11")); nlfg.push_back(constraint); double sigma = 0.01; @@ -518,7 +538,7 @@ namespace gtsam { Vector z1 = Vector_(2, 1.0, 0.0); // move right for (size_t x = 1; x < N; x++) for (size_t y = 1; y <= N; y++) { - shared f(new Simulated2DOdometry(z1, sigma, key(x, y), key(x + 1, y))); + shared f(new simulated2D::Simulated2DOdometry(z1, sigma, key(x, y), key(x + 1, y))); nlfg.push_back(f); } @@ -526,7 +546,7 @@ namespace gtsam { Vector z2 = Vector_(2, 0.0, 1.0); // move up for (size_t x = 1; x <= N; x++) for (size_t y = 1; y < N; y++) { - shared f(new Simulated2DOdometry(z2, sigma, key(x, y), key(x, y + 1))); + shared f(new simulated2D::Simulated2DOdometry(z2, sigma, key(x, y), key(x, y + 1))); nlfg.push_back(f); } diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index a013c3f8c..3afed4df8 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -730,7 +730,7 @@ TEST( GaussianFactorGraph, constrained_multi2 ) CHECK(assert_equal(expected, actual)); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( GaussianFactorGraph, findMinimumSpanningTree ) { GaussianFactorGraph g; @@ -750,7 +750,7 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree ) CHECK(tree["x4"].compare("x1")==0); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( GaussianFactorGraph, split ) { GaussianFactorGraph g; diff --git a/cpp/testGraph.cpp b/cpp/testGraph.cpp index af1024284..5a4e06ebb 100644 --- a/cpp/testGraph.cpp +++ b/cpp/testGraph.cpp @@ -13,6 +13,7 @@ #include #include "Pose2Graph.h" +#include "LieConfig-inl.h" #include "graph-inl.h" using namespace std; @@ -24,22 +25,23 @@ TEST( Graph, composePoses ) { Pose2Graph graph; Matrix cov = eye(3); - graph.push_back(boost::shared_ptr(new Pose2Factor("x1", "x2", Pose2(2.0, 0.0, 0.0), cov))); - graph.push_back(boost::shared_ptr(new Pose2Factor("x2", "x3", Pose2(3.0, 0.0, 0.0), cov))); + graph.push_back(boost::shared_ptr(new Pose2Factor(1,2, Pose2(2.0, 0.0, 0.0), cov))); + graph.push_back(boost::shared_ptr(new Pose2Factor(2,3, Pose2(3.0, 0.0, 0.0), cov))); - map tree; - tree.insert(make_pair("x1", "x2")); - tree.insert(make_pair("x2", "x2")); - tree.insert(make_pair("x3", "x2")); + PredecessorMap tree; + tree.insert(make_pair(1,2)); + tree.insert(make_pair(2,2)); + tree.insert(make_pair(3,2)); Pose2 rootPose(3.0, 0.0, 0.0); - boost::shared_ptr actual = composePoses - (graph, tree, rootPose); + boost::shared_ptr actual = composePoses (graph, tree, rootPose); + Pose2Config expected; - expected.insert("x1", Pose2(1.0, 0.0, 0.0)); - expected.insert("x2", Pose2(3.0, 0.0, 0.0)); - expected.insert("x3", Pose2(6.0, 0.0, 0.0)); + expected.insert(1, Pose2(1.0, 0.0, 0.0)); + expected.insert(2, Pose2(3.0, 0.0, 0.0)); + expected.insert(3, Pose2(6.0, 0.0, 0.0)); LONGS_EQUAL(3, actual->size()); CHECK(assert_equal(expected, *actual)); diff --git a/cpp/testLieConfig.cpp b/cpp/testLieConfig.cpp index 6d28d9398..535234872 100644 --- a/cpp/testLieConfig.cpp +++ b/cpp/testLieConfig.cpp @@ -18,10 +18,10 @@ using namespace std; /* ************************************************************************* */ TEST( LieConfig, equals1 ) { - LieConfig expected; + LieConfig expected; Vector v = Vector_(3, 5.0, 6.0, 7.0); expected.insert("a",v); - LieConfig actual; + LieConfig actual; actual.insert("a",v); CHECK(assert_equal(expected,actual)); } @@ -29,7 +29,7 @@ TEST( LieConfig, equals1 ) /* ************************************************************************* */ TEST( LieConfig, equals2 ) { - LieConfig cfg1, cfg2; + LieConfig cfg1, cfg2; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, 5.0, 6.0, 8.0); cfg1.insert("x", v1); @@ -41,7 +41,7 @@ TEST( LieConfig, equals2 ) /* ************************************************************************* */ TEST( LieConfig, equals_nan ) { - LieConfig cfg1, cfg2; + LieConfig cfg1, cfg2; Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v2 = Vector_(3, 0.0/0.0, 0.0/0.0, 0.0/0.0); cfg1.insert("x", v1); @@ -53,7 +53,7 @@ TEST( LieConfig, equals_nan ) /* ************************************************************************* */ TEST(LieConfig, expmap_a) { - LieConfig config0; + LieConfig config0; config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); @@ -61,7 +61,7 @@ TEST(LieConfig, expmap_a) increment.insert("v1", Vector_(3, 1.0, 1.1, 1.2)); increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5)); - LieConfig expected; + LieConfig expected; expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2)); expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); @@ -71,14 +71,14 @@ TEST(LieConfig, expmap_a) /* ************************************************************************* */ TEST(LieConfig, expmap_b) { - LieConfig config0; + LieConfig config0; config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); VectorConfig increment; increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5)); - LieConfig expected; + LieConfig expected; expected.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); @@ -88,7 +88,7 @@ TEST(LieConfig, expmap_b) /* ************************************************************************* */ TEST(LieConfig, expmap_c) { - LieConfig config0; + LieConfig config0; config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); @@ -96,7 +96,7 @@ TEST(LieConfig, expmap_c) 1.0, 1.1, 1.2, 1.3, 1.4, 1.5); - LieConfig expected; + LieConfig expected; expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2)); expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5)); @@ -106,14 +106,14 @@ TEST(LieConfig, expmap_c) /* ************************************************************************* */ TEST(LieConfig, expmap_d) { - LieConfig config0; + LieConfig config0; config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); //config0.print("config0"); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); - LieConfig poseconfig; + LieConfig poseconfig; poseconfig.insert("p1", Pose2(1,2,3)); poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); //poseconfig.print("poseconfig"); diff --git a/cpp/testNonlinearEquality.cpp b/cpp/testNonlinearEquality.cpp index ff2f58bd4..91a990aef 100644 --- a/cpp/testNonlinearEquality.cpp +++ b/cpp/testNonlinearEquality.cpp @@ -10,27 +10,22 @@ using namespace std; using namespace gtsam; -typedef boost::shared_ptr > shared_nle; +typedef NonlinearEquality NLE; +typedef boost::shared_ptr shared_nle; -bool vector_compare(const std::string& key, - const VectorConfig& feasible, - const VectorConfig& input) { - Vector feas, lin; - feas = feasible[key]; - lin = input[key]; - return equal_with_abs_tol(lin, feas, 1e-5); +bool vector_compare(const Vector& a, const Vector& b) { + return equal_with_abs_tol(a, b, 1e-5); } /* ************************************************************************* */ TEST ( NonlinearEquality, linearization ) { string key = "x"; Vector value = Vector_(2, 1.0, 2.0); - VectorConfig feasible, linearize; - feasible.insert(key, value); + VectorConfig linearize; linearize.insert(key, value); // create a nonlinear equality constraint - shared_nle nle(new NonlinearEquality(key, feasible, 2, *vector_compare)); + shared_nle nle(new NLE(key, value,vector_compare)); // check linearize GaussianFactor expLF(key, eye(2), zero(2), 0.0); @@ -38,17 +33,16 @@ TEST ( NonlinearEquality, linearization ) { CHECK(assert_equal(*actualLF, expLF)); } -/* ************************************************************************* */ +/* ********************************************************************** */ TEST ( NonlinearEquality, linearization_fail ) { string key = "x"; Vector value = Vector_(2, 1.0, 2.0); Vector wrong = Vector_(2, 3.0, 4.0); - VectorConfig feasible, bad_linearize; - feasible.insert(key, value); + VectorConfig bad_linearize; bad_linearize.insert(key, wrong); // create a nonlinear equality constraint - shared_nle nle(new NonlinearEquality(key, feasible, 2, *vector_compare)); + shared_nle nle(new NLE(key, value,vector_compare)); // check linearize to ensure that it fails for bad linearization points try { @@ -69,7 +63,7 @@ TEST ( NonlinearEquality, error ) { bad_linearize.insert(key, wrong); // create a nonlinear equality constraint - shared_nle nle(new NonlinearEquality(key, feasible, 2, *vector_compare)); + shared_nle nle(new NLE(key, value,vector_compare)); // check error function outputs Vector actual = nle->error_vector(feasible); @@ -84,23 +78,16 @@ TEST ( NonlinearEquality, equals ) { string key1 = "x"; Vector value1 = Vector_(2, 1.0, 2.0); Vector value2 = Vector_(2, 3.0, 4.0); - VectorConfig feasible1, feasible2, feasible3; - feasible1.insert(key1, value1); - feasible2.insert(key1, value2); - feasible3.insert(key1, value1); - feasible3.insert("y", value2); // create some constraints to compare - shared_nle nle1(new NonlinearEquality(key1, feasible1, 2, *vector_compare)); - shared_nle nle2(new NonlinearEquality(key1, feasible1, 2, *vector_compare)); - shared_nle nle3(new NonlinearEquality(key1, feasible2, 2, *vector_compare)); - shared_nle nle4(new NonlinearEquality(key1, feasible3, 2, *vector_compare)); + shared_nle nle1(new NLE(key1, value1,vector_compare)); + shared_nle nle2(new NLE(key1, value1,vector_compare)); + shared_nle nle3(new NLE(key1, value2,vector_compare)); // verify CHECK(nle1->equals(*nle2)); // basic equality = true CHECK(nle2->equals(*nle1)); // test symmetry of equals() CHECK(!nle1->equals(*nle3)); // test config - CHECK(nle4->equals(*nle1)); // test the full feasible set isn't getting compared } diff --git a/cpp/testNonlinearFactor.cpp b/cpp/testNonlinearFactor.cpp index 3577a6d58..5bd0b097d 100644 --- a/cpp/testNonlinearFactor.cpp +++ b/cpp/testNonlinearFactor.cpp @@ -27,12 +27,12 @@ TEST( NonlinearFactor, equals ) double sigma = 1.0; // create two nonlinear2 factors - Vector z3(2); z3(0) = 0. ; z3(1) = -1.; - Simulated2DMeasurement f0(z3, sigma, "x1", "l1"); + Vector z3 = Vector_(2,0.,-1.); + simulated2D::Simulated2DMeasurement f0(z3, sigma, "x1", "l1"); // measurement between x2 and l1 - Vector z4(2); z4(0)= -1.5 ; z4(1) = -1.; - Simulated2DMeasurement f1(z4, sigma, "x2", "l1"); + Vector z4 = Vector_(2,-1.5, -1.); + simulated2D::Simulated2DMeasurement f1(z4, sigma, "x2", "l1"); CHECK(assert_equal(f0,f0)); CHECK(f0.equals(f0)); @@ -68,7 +68,7 @@ TEST( NonlinearFactor, NonlinearFactor ) // calculate the error_vector from the factor "f1" Vector actual_e = factor->error_vector(cfg); - Vector e(2); e(0) = -0.1; e(1) = -0.1; + Vector e(2); e(0) = 0.1; e(1) = 0.1; CHECK(assert_equal(e,actual_e)); // the expected value for the error from the factor @@ -81,7 +81,7 @@ TEST( NonlinearFactor, NonlinearFactor ) DOUBLES_EQUAL(expected,actual,0.00000001); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( NonlinearFactor, linearize_f1 ) { // Grab a non-linear factor @@ -103,7 +103,7 @@ TEST( NonlinearFactor, linearize_f1 ) CHECK(assert_equal(nlf->error_vector(c),actual->get_b())); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( NonlinearFactor, linearize_f2 ) { // Grab a non-linear factor @@ -121,7 +121,7 @@ TEST( NonlinearFactor, linearize_f2 ) CHECK(expected->equals(*actual)); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( NonlinearFactor, linearize_f3 ) { // Grab a non-linear factor @@ -139,7 +139,7 @@ TEST( NonlinearFactor, linearize_f3 ) CHECK(expected->equals(*actual)); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( NonlinearFactor, linearize_f4 ) { // Grab a non-linear factor diff --git a/cpp/testOrdering.cpp b/cpp/testOrdering.cpp index 4c3b664d5..8872d8cee 100644 --- a/cpp/testOrdering.cpp +++ b/cpp/testOrdering.cpp @@ -11,11 +11,11 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -/* ************************************************************************* */ +/* ************************************************************************* * // x1 -> x2 // -> x3 -> x4 // -> x5 -TEST ( Ordering, constructor ) { +TEST ( Ordering, constructorFromTree ) { map p_map; p_map.insert(make_pair("x1", "x1")); p_map.insert(make_pair("x2", "x1")); diff --git a/cpp/testPose2Config.cpp b/cpp/testPose2Config.cpp index 1b3efe870..085e20bcf 100644 --- a/cpp/testPose2Config.cpp +++ b/cpp/testPose2Config.cpp @@ -16,12 +16,12 @@ TEST( Pose2Config, pose2Circle ) { // expected is 4 poses tangent to circle with radius 1m Pose2Config expected; - expected.insert("p0", Pose2( 1, 0, M_PI_2)); - expected.insert("p1", Pose2( 0, 1, - M_PI )); - expected.insert("p2", Pose2(-1, 0, - M_PI_2)); - expected.insert("p3", Pose2( 0, -1, 0 )); + expected.insert(0, Pose2( 1, 0, M_PI_2)); + expected.insert(1, Pose2( 0, 1, - M_PI )); + expected.insert(2, Pose2(-1, 0, - M_PI_2)); + expected.insert(3, Pose2( 0, -1, 0 )); - Pose2Config actual = pose2Circle(4,1.0,'p'); + Pose2Config actual = pose2Circle(4,1.0); CHECK(assert_equal(expected,actual)); } @@ -30,14 +30,14 @@ TEST( Pose2Config, expmap ) { // expected is circle shifted to right Pose2Config expected; - expected.insert("p0", Pose2( 1.1, 0, M_PI_2)); - expected.insert("p1", Pose2( 0.1, 1, - M_PI )); - expected.insert("p2", Pose2(-0.9, 0, - M_PI_2)); - expected.insert("p3", Pose2( 0.1, -1, 0 )); + expected.insert(0, Pose2( 1.1, 0, M_PI_2)); + expected.insert(1, Pose2( 0.1, 1, - M_PI )); + expected.insert(2, Pose2(-0.9, 0, - M_PI_2)); + expected.insert(3, Pose2( 0.1, -1, 0 )); // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! Vector delta = Vector_(12, 0.0,-0.1,0.0, -0.1,0.0,0.0, 0.0,0.1,0.0, 0.1,0.0,0.0); - Pose2Config actual = expmap(pose2Circle(4,1.0,'p'),delta); + Pose2Config actual = expmap(pose2Circle(4,1.0),delta); CHECK(assert_equal(expected,actual)); } diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index 6ad7b18db..79a2dac79 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -20,43 +20,43 @@ static Matrix covariance = Matrix_(3,3, ); /* ************************************************************************* */ -// Very simple test establishing Ax-b \approx h(x)-z +// Very simple test establishing Ax-b \approx z-h(x) TEST( Pose2Factor, error ) { // Choose a linearization point Pose2 p1; // robot at origin Pose2 p2(1, 0, 0); // robot at (1,0) Pose2Config x0; - x0.insert("p1", p1); - x0.insert("p2", p2); + x0.insert(1, p1); + x0.insert(2, p2); // Create factor Pose2 z = between(p1,p2); - Pose2Factor factor("p1", "p2", z, covariance); + Pose2Factor factor(1, 2, z, covariance); // Actual linearization boost::shared_ptr linear = factor.linearize(x0); - // Check error at x0 = zero ! + // Check error at x0, i.e. delta = zero ! VectorConfig delta; - delta.insert("p1", zero(3)); - delta.insert("p2", zero(3)); + delta.insert("x1", zero(3)); + delta.insert("x2", zero(3)); Vector error_at_zero = Vector_(3,0.0,0.0,0.0); CHECK(assert_equal(error_at_zero,factor.error_vector(x0))); CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); // Check error after increasing p2 - VectorConfig plus = delta + VectorConfig("p2", Vector_(3, 0.1, 0.0, 0.0)); + VectorConfig plus = delta + VectorConfig("x2", Vector_(3, 0.1, 0.0, 0.0)); Pose2Config x1 = expmap(x0, plus); - Vector error_at_plus = Vector_(3,-0.1/sx,0.0,0.0); + Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! CHECK(assert_equal(error_at_plus,factor.error_vector(x1))); - CHECK(assert_equal(-error_at_plus,linear->error_vector(plus))); + CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); } /* ************************************************************************* */ // common Pose2Factor for tests below static Pose2 measured(2,2,M_PI_2); -static Pose2Factor factor("p1","p2",measured, covariance); +static Pose2Factor factor(1,2,measured, covariance); /* ************************************************************************* */ TEST( Pose2Factor, rhs ) @@ -65,8 +65,8 @@ TEST( Pose2Factor, rhs ) Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4) Pose2Config x0; - x0.insert("p1",p1); - x0.insert("p2",p2); + x0.insert(1,p1); + x0.insert(2,p2); // Actual linearization boost::shared_ptr linear = factor.linearize(x0); @@ -75,7 +75,7 @@ TEST( Pose2Factor, rhs ) Pose2 hx0 = between(p1,p2); CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0)); Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0); - CHECK(assert_equal(expected_b,factor.error_vector(x0))); + CHECK(assert_equal(expected_b,-factor.error_vector(x0))); CHECK(assert_equal(expected_b,linear->get_b())); } @@ -83,10 +83,7 @@ TEST( Pose2Factor, rhs ) // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector // Hence i.e., b = approximates z-h(x0) = error_vector(x0) Vector h(const Pose2& p1,const Pose2& p2) { - Pose2Config x; - x.insert("p1",p1); - x.insert("p2",p2); - return -factor.error_vector(x); + return factor.evaluateError(p1,p2); } /* ************************************************************************* */ @@ -96,8 +93,8 @@ TEST( Pose2Factor, linearize ) Pose2 p1(1,2,M_PI_2); Pose2 p2(-1,4,M_PI); Pose2Config x0; - x0.insert("p1",p1); - x0.insert("p2",p2); + x0.insert(1,p1); + x0.insert(2,p2); // expected linearization Matrix square_root_inverse_covariance = Matrix_(3,3, @@ -118,7 +115,7 @@ TEST( Pose2Factor, linearize ) Vector expected_b = Vector_(3, 0.0, 0.0, 0.0); // expected linear factor - GaussianFactor expected("p1", expectedH1, "p2", expectedH2, expected_b, 1.0); + GaussianFactor expected("x1", expectedH1, "x2", expectedH2, expected_b, 1.0); // Actual linearization boost::shared_ptr actual = factor.linearize(x0); diff --git a/cpp/testPose2Graph.cpp b/cpp/testPose2Graph.cpp index 56321d01d..67f16ba0c 100644 --- a/cpp/testPose2Graph.cpp +++ b/cpp/testPose2Graph.cpp @@ -32,9 +32,9 @@ TEST( Pose2Graph, constructor ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); - Pose2Factor constraint("x1","x2",measured, covariance); + Pose2Factor constraint(1,2,measured, covariance); Pose2Graph graph; - graph.add("x1","x2",measured, covariance); + graph.add(1,2,measured, covariance); // get the size of the graph size_t actual = graph.size(); // verify @@ -48,16 +48,16 @@ TEST( Pose2Graph, linerization ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); - Pose2Factor constraint("x1","x2",measured, covariance); + Pose2Factor constraint(1,2,measured, covariance); Pose2Graph graph; - graph.add("x1","x2",measured, covariance); + graph.add(1,2,measured, covariance); // Choose a linearization point Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) Pose2Config config; - config.insert("x1",p1); - config.insert("x2",p2); + config.insert(1,p1); + config.insert(2,p2); // Linearize GaussianFactorGraph lfg_linearized = graph.linearize(config); //lfg_linearized.print("lfg_actual"); @@ -86,17 +86,17 @@ TEST(Pose2Graph, optimize) { // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose2Graph); - fg->addConstraint("p0", Pose2(0,0,0)); - fg->add("p0", "p1", Pose2(1,2,M_PI_2), covariance); + fg->addConstraint(0, Pose2(0,0,0)); + fg->add(0, 1, Pose2(1,2,M_PI_2), covariance); // Create initial config boost::shared_ptr initial(new Pose2Config()); - initial->insert("p0", Pose2(0,0,0)); - initial->insert("p1", Pose2(0,0,0)); + initial->insert(0, Pose2(0,0,0)); + initial->insert(1, Pose2(0,0,0)); // Choose an ordering and optimize shared_ptr ordering(new Ordering); - *ordering += "p0","p1"; + *ordering += "x0","x1"; typedef NonlinearOptimizer Optimizer; Optimizer optimizer0(fg, ordering, initial); Optimizer::verbosityLevel verbosity = Optimizer::SILENT; @@ -105,8 +105,8 @@ TEST(Pose2Graph, optimize) { // Check with expected config Pose2Config expected; - expected.insert("p0", Pose2(0,0,0)); - expected.insert("p1", Pose2(1,2,M_PI_2)); + expected.insert(0, Pose2(0,0,0)); + expected.insert(1, Pose2(1,2,M_PI_2)); CHECK(assert_equal(expected, *optimizer.config())); } @@ -115,32 +115,32 @@ TEST(Pose2Graph, optimize) { TEST(Pose2Graph, optimizeCircle) { // Create a hexagon of poses - Pose2Config hexagon = pose2Circle(6,1.0,'p'); - Pose2 p0 = hexagon["p0"], p1 = hexagon["p1"]; + Pose2Config hexagon = pose2Circle(6,1.0); + Pose2 p0 = hexagon[0], p1 = hexagon[1]; // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose2Graph); - fg->addConstraint("p0", p0); + fg->addConstraint(0, p0); Pose2 delta = between(p0,p1); - fg->add("p0", "p1", delta, covariance); - fg->add("p1", "p2", delta, covariance); - fg->add("p2", "p3", delta, covariance); - fg->add("p3", "p4", delta, covariance); - fg->add("p4", "p5", delta, covariance); - fg->add("p5", "p0", delta, covariance); + fg->add(0, 1, delta, covariance); + fg->add(1,2, delta, covariance); + fg->add(2,3, delta, covariance); + fg->add(3,4, delta, covariance); + fg->add(4,5, delta, covariance); + fg->add(5, 0, delta, covariance); // Create initial config boost::shared_ptr initial(new Pose2Config()); - initial->insert("p0", p0); - initial->insert("p1", expmap(hexagon["p1"],Vector_(3,-0.1, 0.1,-0.1))); - initial->insert("p2", expmap(hexagon["p2"],Vector_(3, 0.1,-0.1, 0.1))); - initial->insert("p3", expmap(hexagon["p3"],Vector_(3,-0.1, 0.1,-0.1))); - initial->insert("p4", expmap(hexagon["p4"],Vector_(3, 0.1,-0.1, 0.1))); - initial->insert("p5", expmap(hexagon["p5"],Vector_(3,-0.1, 0.1,-0.1))); + initial->insert(0, p0); + initial->insert(1, expmap(hexagon[1],Vector_(3,-0.1, 0.1,-0.1))); + initial->insert(2, expmap(hexagon[2],Vector_(3, 0.1,-0.1, 0.1))); + initial->insert(3, expmap(hexagon[3],Vector_(3,-0.1, 0.1,-0.1))); + initial->insert(4, expmap(hexagon[4],Vector_(3, 0.1,-0.1, 0.1))); + initial->insert(5, expmap(hexagon[5],Vector_(3,-0.1, 0.1,-0.1))); // Choose an ordering and optimize shared_ptr ordering(new Ordering); - *ordering += "p0","p1","p2","p3","p4","p5"; + *ordering += "x0","x1","x2","x3","x4","x5"; typedef NonlinearOptimizer Optimizer; Optimizer optimizer0(fg, ordering, initial); Optimizer::verbosityLevel verbosity = Optimizer::SILENT; @@ -153,7 +153,7 @@ TEST(Pose2Graph, optimizeCircle) { CHECK(assert_equal(hexagon, actual)); // Check loop closure - CHECK(assert_equal(delta,between(actual["p5"],actual["p0"]))); + CHECK(assert_equal(delta,between(actual[5],actual[0]))); } /* ************************************************************************* */ diff --git a/cpp/testPose3Config.cpp b/cpp/testPose3Config.cpp index 9500cb5fc..f3d989124 100644 --- a/cpp/testPose3Config.cpp +++ b/cpp/testPose3Config.cpp @@ -23,12 +23,12 @@ TEST( Pose3Config, pose3Circle ) { // expected is 4 poses tangent to circle with radius 1m Pose3Config expected; - expected.insert("p0", Pose3(R1, Point3( 1, 0, 0))); - expected.insert("p1", Pose3(R2, Point3( 0, 1, 0))); - expected.insert("p2", Pose3(R3, Point3(-1, 0, 0))); - expected.insert("p3", Pose3(R4, Point3( 0,-1, 0))); + expected.insert(0, Pose3(R1, Point3( 1, 0, 0))); + expected.insert(1, Pose3(R2, Point3( 0, 1, 0))); + expected.insert(2, Pose3(R3, Point3(-1, 0, 0))); + expected.insert(3, Pose3(R4, Point3( 0,-1, 0))); - Pose3Config actual = pose3Circle(4,1.0,'p'); + Pose3Config actual = pose3Circle(4,1.0); CHECK(assert_equal(expected,actual)); } @@ -37,10 +37,10 @@ TEST( Pose3Config, expmap ) { // expected is circle shifted to East Pose3Config expected; - expected.insert("p0", Pose3(R1, Point3( 1.1, 0, 0))); - expected.insert("p1", Pose3(R2, Point3( 0.1, 1, 0))); - expected.insert("p2", Pose3(R3, Point3(-0.9, 0, 0))); - expected.insert("p3", Pose3(R4, Point3( 0.1,-1, 0))); + expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0))); + expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0))); + expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0))); + expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0))); // Note expmap coordinates are in global coordinates with non-compose expmap // so shifting to East requires little thought, different from with Pose2 !!! @@ -49,7 +49,7 @@ TEST( Pose3Config, expmap ) 0.0,0.0,0.0, 0.1, 0.0, 0.0, 0.0,0.0,0.0, 0.1, 0.0, 0.0, 0.0,0.0,0.0, 0.1, 0.0, 0.0); - Pose3Config actual = expmap(pose3Circle(4,1.0,'p'),delta); + Pose3Config actual = expmap(pose3Circle(4,1.0),delta); CHECK(assert_equal(expected,actual)); } diff --git a/cpp/testPose3Factor.cpp b/cpp/testPose3Factor.cpp index ecf4df66c..3314ceddd 100644 --- a/cpp/testPose3Factor.cpp +++ b/cpp/testPose3Factor.cpp @@ -25,16 +25,16 @@ TEST( Pose3Factor, error ) // Create factor Matrix measurement_covariance = eye(6); - Pose3Factor factor("t1", "t2", z, measurement_covariance); + Pose3Factor factor(1,2, z, measurement_covariance); // Create config Pose3Config x; - x.insert("t1",t1); - x.insert("t2",t2); + x.insert(1,t1); + x.insert(2,t2); - // Get error z-h(x) -> logmap(h(x),z) = logmap(between(t1,t2),z) + // Get error h(x)-z -> logmap(z,h(x)) = logmap(z,between(t1,t2)) Vector actual = factor.error_vector(x); - Vector expected = logmap(between(t1,t2),z); + Vector expected = logmap(z,between(t1,t2)); CHECK(assert_equal(expected,actual)); } diff --git a/cpp/testPose3Graph.cpp b/cpp/testPose3Graph.cpp index 05529011f..cfad6d797 100644 --- a/cpp/testPose3Graph.cpp +++ b/cpp/testPose3Graph.cpp @@ -13,10 +13,10 @@ using namespace boost::assign; #include +#include "Pose3Graph.h" #include "NonlinearOptimizer-inl.h" #include "NonlinearEquality.h" #include "Ordering.h" -#include "Pose3Graph.h" using namespace std; using namespace gtsam; @@ -29,32 +29,32 @@ static Matrix covariance = eye(6); TEST(Pose3Graph, optimizeCircle) { // Create a hexagon of poses - Pose3Config hexagon = pose3Circle(6,1.0,'p'); - Pose3 p0 = hexagon["p0"], p1 = hexagon["p1"]; + Pose3Config hexagon = pose3Circle(6,1.0); + Pose3 p0 = hexagon[0], p1 = hexagon[1]; // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose3Graph); - fg->addConstraint("p0", p0); + fg->addConstraint(0, p0); Pose3 delta = between(p0,p1); - fg->add("p0", "p1", delta, covariance); - fg->add("p1", "p2", delta, covariance); - fg->add("p2", "p3", delta, covariance); - fg->add("p3", "p4", delta, covariance); - fg->add("p4", "p5", delta, covariance); - fg->add("p5", "p0", delta, covariance); + fg->add(0,1, delta, covariance); + fg->add(1,2, delta, covariance); + fg->add(2,3, delta, covariance); + fg->add(3,4, delta, covariance); + fg->add(4,5, delta, covariance); + fg->add(5,0, delta, covariance); // Create initial config boost::shared_ptr initial(new Pose3Config()); - initial->insert("p0", p0); - initial->insert("p1", expmap(hexagon["p1"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert("p2", expmap(hexagon["p2"],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert("p3", expmap(hexagon["p3"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert("p4", expmap(hexagon["p4"],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert("p5", expmap(hexagon["p5"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(0, p0); + initial->insert(1, expmap(hexagon[1],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(2, expmap(hexagon[2],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(3, expmap(hexagon[3],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(4, expmap(hexagon[4],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(5, expmap(hexagon[5],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); // Choose an ordering and optimize shared_ptr ordering(new Ordering); - *ordering += "p0","p1","p2","p3","p4","p5"; + *ordering += "x0","x1","x2","x3","x4","x5"; typedef NonlinearOptimizer Optimizer; Optimizer optimizer0(fg, ordering, initial); Optimizer::verbosityLevel verbosity = Optimizer::SILENT; @@ -67,7 +67,7 @@ TEST(Pose3Graph, optimizeCircle) { CHECK(assert_equal(hexagon, actual,1e-5)); // Check loop closure - CHECK(assert_equal(delta,between(actual["p5"],actual["p0"]),1e-5)); + CHECK(assert_equal(delta,between(actual[5],actual[0]),1e-5)); } /* ************************************************************************* */ diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 532b3fb96..ff45bd85d 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -242,19 +242,16 @@ TEST (SQP, problem1_sqp ) { /* ********************************************************************* */ // components for nonlinear factor graphs -bool vector_compare(const std::string& key, - const VectorConfig& feasible, - const VectorConfig& input) { - Vector feas, lin; - feas = feasible[key]; - lin = input[key]; - return equal_with_abs_tol(lin, feas, 1e-5); +bool vector_compare(const Vector& a, const Vector& b) { + return equal_with_abs_tol(a, b, 1e-5); } typedef NonlinearFactorGraph NLGraph; typedef boost::shared_ptr > shared; typedef boost::shared_ptr > shared_c; -typedef boost::shared_ptr > shared_eq; + +typedef NonlinearEquality NLE; +typedef boost::shared_ptr shared_eq; typedef boost::shared_ptr shared_cfg; typedef NonlinearOptimizer Optimizer; @@ -268,24 +265,26 @@ TEST (SQP, two_pose_truth ) { // position (1, 1) constraint for x1 // position (5, 6) constraint for x2 VectorConfig feas; - feas.insert("x1", Vector_(2, 1.0, 1.0)); - feas.insert("x2", Vector_(2, 5.0, 6.0)); + Vector feas1 = Vector_(2, 1.0, 1.0); + Vector feas2 = Vector_(2, 5.0, 6.0); + feas.insert("x1", feas1); + feas.insert("x2", feas2); // constant constraint on x1 - shared_eq ef1(new NonlinearEquality("x1", feas, 2, *vector_compare)); + shared_eq ef1(new NLE("x1", feas1, vector_compare)); // constant constraint on x2 - shared_eq ef2(new NonlinearEquality("x2", feas, 2, *vector_compare)); + shared_eq ef2(new NLE("x2", feas2, vector_compare)); // measurement from x1 to l1 Vector z1 = Vector_(2, 0.0, 5.0); double sigma1 = 0.1; - shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1")); + shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1")); // measurement from x2 to l1 Vector z2 = Vector_(2, -4.0, 0.0); double sigma2 = 0.1; - shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l1")); + shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l1")); // construct the graph shared_ptr graph(new NLGraph()); @@ -377,12 +376,12 @@ TEST (SQP, two_pose ) { // measurement from x1 to l1 Vector z1 = Vector_(2, 0.0, 5.0); double sigma1 = 0.1; - shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1")); + shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1")); // measurement from x2 to l2 Vector z2 = Vector_(2, -4.0, 0.0); double sigma2 = 0.1; - shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2")); + shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l2")); // equality constraint between l1 and l2 list keys2; keys2 += "l1", "l2"; diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index 7d55e732e..f19fc58af 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -102,12 +102,12 @@ NLGraph linearMapWarpGraph() { // measurement from x1 to l1 Vector z1 = Vector_(2, 0.0, 5.0); double sigma1 = 0.1; - shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1")); + shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1")); // measurement from x2 to l2 Vector z2 = Vector_(2, -4.0, 0.0); double sigma2 = 0.1; - shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2")); + shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l2")); // equality constraint between l1 and l2 list keys; keys += "l1", "l2"; @@ -208,20 +208,15 @@ TEST ( SQPOptimizer, map_warp ) { // states, which enforces a minimum distance. /* ********************************************************************* */ -bool vector_compare(const std::string& key, - const VectorConfig& feasible, - const VectorConfig& input) { - Vector feas, lin; - feas = feasible[key]; - lin = input[key]; - return equal_with_abs_tol(lin, feas, 1e-5); +bool vector_compare(const Vector& a, const Vector& b) { + return equal_with_abs_tol(a, b, 1e-5); } typedef NonlinearConstraint1 NLC1; typedef boost::shared_ptr shared_NLC1; typedef NonlinearConstraint2 NLC2; typedef boost::shared_ptr shared_NLC2; -typedef NonlinearEquality NLE; +typedef NonlinearEquality NLE; typedef boost::shared_ptr shared_NLE; namespace sqp_avoid1 { @@ -255,22 +250,24 @@ Matrix jac_g2(const VectorConfig& config, const list& keys) { pair obstacleAvoidGraph() { // fix start, end, obstacle positions VectorConfig feasible; - feasible.insert("x1", Vector_(2, 0.0, 0.0)); - feasible.insert("x3", Vector_(2, 10.0, 0.0)); - feasible.insert("obs", Vector_(2, 5.0, -0.5)); - shared_NLE e1(new NLE("x1", feasible, 2, *vector_compare)); - shared_NLE e2(new NLE("x3", feasible, 2, *vector_compare)); - shared_NLE e3(new NLE("obs", feasible, 2, *vector_compare)); + Vector feas1 = Vector_(2, 0.0, 0.0), feas2 = Vector_(2, 10.0, 0.0), feas3 = + Vector_(2, 5.0, -0.5); + feasible.insert("x1", feas1); + feasible.insert("x3", feas2); + feasible.insert("obs", feas3); + shared_NLE e1(new NLE("x1", feas1, vector_compare)); + shared_NLE e2(new NLE("x3", feas2, vector_compare)); + shared_NLE e3(new NLE("obs", feas3, vector_compare)); // measurement from x1 to x2 Vector x1x2 = Vector_(2, 5.0, 0.0); double sigma1 = 0.1; - shared f1(new Simulated2DOdometry(x1x2, sigma1, "x1", "x2")); + shared f1(new simulated2D::Simulated2DOdometry(x1x2, sigma1, "x1", "x2")); // measurement from x2 to x3 Vector x2x3 = Vector_(2, 5.0, 0.0); double sigma2 = 0.1; - shared f2(new Simulated2DOdometry(x2x3, sigma2, "x2", "x3")); + shared f2(new simulated2D::Simulated2DOdometry(x2x3, sigma2, "x2", "x3")); // create a binary inequality constraint that forces the middle point away from // the obstacle @@ -385,22 +382,24 @@ Matrix jac_g2(const VectorConfig& config, const list& keys) { pair obstacleAvoidGraphGeneral() { // fix start, end, obstacle positions VectorConfig feasible; - feasible.insert("x1", Vector_(2, 0.0, 0.0)); - feasible.insert("x3", Vector_(2, 10.0, 0.0)); - feasible.insert("obs", Vector_(2, 5.0, -0.5)); - shared_NLE e1(new NLE("x1", feasible, 2, *vector_compare)); - shared_NLE e2(new NLE("x3", feasible, 2, *vector_compare)); - shared_NLE e3(new NLE("obs", feasible, 2, *vector_compare)); + Vector feas1 = Vector_(2, 0.0, 0.0), feas2 = Vector_(2, 10.0, 0.0), feas3 = + Vector_(2, 5.0, -0.5); + feasible.insert("x1", feas1); + feasible.insert("x3", feas2); + feasible.insert("obs", feas3); + shared_NLE e1(new NLE("x1", feas1,vector_compare)); + shared_NLE e2(new NLE("x3", feas2, vector_compare)); + shared_NLE e3(new NLE("obs", feas3, vector_compare)); // measurement from x1 to x2 Vector x1x2 = Vector_(2, 5.0, 0.0); double sigma1 = 0.1; - shared f1(new Simulated2DOdometry(x1x2, sigma1, "x1", "x2")); + shared f1(new simulated2D::Simulated2DOdometry(x1x2, sigma1, "x1", "x2")); // measurement from x2 to x3 Vector x2x3 = Vector_(2, 5.0, 0.0); double sigma2 = 0.1; - shared f2(new Simulated2DOdometry(x2x3, sigma2, "x2", "x3")); + shared f2(new simulated2D::Simulated2DOdometry(x2x3, sigma2, "x2", "x3")); double radius = 1.0; diff --git a/cpp/testSimulated2D.cpp b/cpp/testSimulated2D.cpp index 3229acba1..54f1cc60c 100644 --- a/cpp/testSimulated2D.cpp +++ b/cpp/testSimulated2D.cpp @@ -13,6 +13,7 @@ using namespace gtsam; using namespace std; +using namespace simulated2D; /* ************************************************************************* */ TEST( simulated2D, Dprior ) diff --git a/cpp/testSimulated3D.cpp b/cpp/testSimulated3D.cpp index ef62a722c..b4e2b5e1c 100644 --- a/cpp/testSimulated3D.cpp +++ b/cpp/testSimulated3D.cpp @@ -12,14 +12,15 @@ #include "Simulated3D.h" using namespace gtsam; +using namespace simulated3D; /* ************************************************************************* */ -TEST( simulated3D, Dprior_3D ) +TEST( simulated3D, Dprior ) { Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); Vector v = logmap(x1); - Matrix numerical = numericalDerivative11(prior_3D,v); - Matrix computed = Dprior_3D(v); + Matrix numerical = numericalDerivative11(prior,v); + Matrix computed = Dprior(v); CHECK(assert_equal(numerical,computed,1e-9)); } @@ -30,8 +31,8 @@ TEST( simulated3D, DOdo1 ) Vector v1 = logmap(x1); Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); Vector v2 = logmap(x2); - Matrix numerical = numericalDerivative21(odo_3D,v1,v2); - Matrix computed = Dodo1_3D(v1,v2); + Matrix numerical = numericalDerivative21(odo,v1,v2); + Matrix computed = Dodo1(v1,v2); CHECK(assert_equal(numerical,computed,1e-9)); } @@ -42,8 +43,8 @@ TEST( simulated3D, DOdo2 ) Vector v1 = logmap(x1); Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); Vector v2 = logmap(x2); - Matrix numerical = numericalDerivative22(odo_3D,v1,v2); - Matrix computed = Dodo2_3D(v1,v2); + Matrix numerical = numericalDerivative22(odo,v1,v2); + Matrix computed = Dodo2(v1,v2); CHECK(assert_equal(numerical,computed,1e-9)); } diff --git a/cpp/testVSLAMConfig.cpp b/cpp/testVSLAMConfig.cpp index 78bff37bb..0e6a4b60d 100644 --- a/cpp/testVSLAMConfig.cpp +++ b/cpp/testVSLAMConfig.cpp @@ -5,7 +5,8 @@ */ #include -#include +#include "VectorConfig.h" +#include "VSLAMConfig.h" using namespace std; using namespace gtsam; @@ -18,17 +19,17 @@ TEST( VSLAMConfig, update_with_large_delta) { init.addCameraPose(1, Pose3()); init.addLandmarkPoint(1, Point3(1.0, 2.0, 3.0)); - VectorConfig delta; - delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1)); - delta.insert("l1", Vector_(3, 0.1, 0.1, 0.1)); - delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1)); - - VSLAMConfig actual = expmap(init, delta); VSLAMConfig expected; expected.addCameraPose(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); expected.addLandmarkPoint(1, Point3(1.1, 2.1, 3.1)); - CHECK(assert_equal(actual, expected)); + VectorConfig delta; + delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1)); + delta.insert("l1", Vector_(3, 0.1, 0.1, 0.1)); + delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1)); + VSLAMConfig actual = expmap(init, delta); + + CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index d193e167f..c1e3bdd82 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -38,7 +38,7 @@ TEST( VSLAMFactor, error ) VSLAMConfig config; Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.addCameraPose(1, x1); Point3 l1; config.addLandmarkPoint(1, l1); - CHECK(assert_equal(Vector_(2,320.,240.),factor->predict(config))); + CHECK(assert_equal(Point2(320.,240.),factor->predict(x1,l1))); // Which yields an error of 3^2/2 = 4.5 DOUBLES_EQUAL(4.5,factor->error(config),1e-9);