From 7d0a30c20f93ac71ea0a9dbd8406074e39138f2e Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 14 Oct 2009 20:39:59 +0000 Subject: [PATCH] Renamed FGConfig to VectorConfig in gtsam, easylib, EasySLAM, and mast. --- cpp/ChordalBayesNet.cpp | 8 ++-- cpp/ChordalBayesNet.h | 6 +-- cpp/ConditionalGaussian.cpp | 2 +- cpp/ConditionalGaussian.h | 4 +- cpp/ConstrainedConditionalGaussian.cpp | 2 +- cpp/ConstrainedConditionalGaussian.h | 2 +- cpp/ConstrainedLinearFactorGraph.cpp | 4 +- cpp/ConstrainedLinearFactorGraph.h | 2 +- cpp/ConstrainedNonlinearFactorGraph.h | 4 +- cpp/Factor.h | 2 +- cpp/FactorGraph.h | 2 +- cpp/LinearFactor.cpp | 2 +- cpp/LinearFactor.h | 4 +- cpp/LinearFactorGraph.cpp | 4 +- cpp/LinearFactorGraph.h | 8 ++-- cpp/Makefile.am | 8 ++-- cpp/NonlinearFactor.cpp | 16 +++---- cpp/NonlinearFactor.h | 16 +++---- cpp/NonlinearFactorGraph.h | 2 +- cpp/NonlinearOptimizer-inl.h | 6 +-- cpp/NonlinearOptimizer.h | 6 +-- cpp/VSLAMFactor.h | 2 +- cpp/{FGConfig.cpp => VectorConfig.cpp} | 22 +++++----- cpp/{FGConfig.h => VectorConfig.h} | 18 ++++---- cpp/gtsam.h | 36 ++++++++-------- cpp/smallExample.cpp | 42 +++++++++---------- cpp/smallExample.h | 22 +++++----- cpp/testChordalBayesNet.cpp | 4 +- cpp/testConditionalGaussian.cpp | 2 +- cpp/testConstrainedConditionalGaussian.cpp | 10 ++--- cpp/testConstrainedLinearFactorGraph.cpp | 30 ++++++------- cpp/testConstrainedNonlinearFactorGraph.cpp | 12 +++--- cpp/testLinearConstraint.cpp | 6 +-- cpp/testLinearFactor.cpp | 4 +- cpp/testLinearFactorGraph.cpp | 6 +-- cpp/testNonlinearFactor.cpp | 14 +++---- cpp/testNonlinearFactorGraph.cpp | 8 ++-- cpp/testNonlinearOptimizer.cpp | 16 +++---- ...{testFGConfig.cpp => testVectorConfig.cpp} | 34 +++++++-------- 39 files changed, 199 insertions(+), 199 deletions(-) rename cpp/{FGConfig.cpp => VectorConfig.cpp} (78%) rename cpp/{FGConfig.h => VectorConfig.h} (80%) rename cpp/{testFGConfig.cpp => testVectorConfig.cpp} (81%) diff --git a/cpp/ChordalBayesNet.cpp b/cpp/ChordalBayesNet.cpp index 47e6b8e57..caaa408b6 100644 --- a/cpp/ChordalBayesNet.cpp +++ b/cpp/ChordalBayesNet.cpp @@ -37,17 +37,17 @@ void ChordalBayesNet::erase(const string& key) /* ************************************************************************* */ // optimize, i.e. return x = inv(R)*d /* ************************************************************************* */ -boost::shared_ptr ChordalBayesNet::optimize() const +boost::shared_ptr ChordalBayesNet::optimize() const { - boost::shared_ptr result(new FGConfig); + boost::shared_ptr result(new VectorConfig); result = optimize(result); return result; } /* ************************************************************************* */ -boost::shared_ptr ChordalBayesNet::optimize(const boost::shared_ptr &c) const +boost::shared_ptr ChordalBayesNet::optimize(const boost::shared_ptr &c) const { - boost::shared_ptr result(new FGConfig); + boost::shared_ptr result(new VectorConfig); result = c; /** solve each node in turn in topological sort order (parents first)*/ diff --git a/cpp/ChordalBayesNet.h b/cpp/ChordalBayesNet.h index f1a3bde4c..15795a2d3 100644 --- a/cpp/ChordalBayesNet.h +++ b/cpp/ChordalBayesNet.h @@ -14,7 +14,7 @@ #include #include "ConditionalGaussian.h" -#include "FGConfig.h" +#include "VectorConfig.h" namespace gtsam { @@ -72,8 +72,8 @@ public: const_iterator const end() const {return nodes.end();} /** optimize */ - boost::shared_ptr optimize() const; - boost::shared_ptr optimize(const boost::shared_ptr &c) const; + boost::shared_ptr optimize() const; + boost::shared_ptr optimize(const boost::shared_ptr &c) const; /** print */ void print() const; diff --git a/cpp/ConditionalGaussian.cpp b/cpp/ConditionalGaussian.cpp index e1647daed..056a40927 100644 --- a/cpp/ConditionalGaussian.cpp +++ b/cpp/ConditionalGaussian.cpp @@ -62,7 +62,7 @@ void ConditionalGaussian::print(const string &s) const } /* ************************************************************************* */ -Vector ConditionalGaussian::solve(const FGConfig& x) const { +Vector ConditionalGaussian::solve(const VectorConfig& x) const { Vector rhs = d_; for (map::const_iterator it = parents_.begin(); it != parents_.end(); it++) { diff --git a/cpp/ConditionalGaussian.h b/cpp/ConditionalGaussian.h index b31c9bd27..9ecf00990 100644 --- a/cpp/ConditionalGaussian.h +++ b/cpp/ConditionalGaussian.h @@ -15,7 +15,7 @@ #include #include #include "Matrix.h" -#include "FGConfig.h" +#include "VectorConfig.h" #include "Ordering.h" namespace gtsam { @@ -115,7 +115,7 @@ namespace gtsam { * @param x configuration in which the parents values (y,z,...) are known * @return solution x = R \ (d - Sy - Tz - ...) */ - virtual Vector solve(const FGConfig& x) const; + virtual Vector solve(const VectorConfig& x) const; /** * adds a parent diff --git a/cpp/ConstrainedConditionalGaussian.cpp b/cpp/ConstrainedConditionalGaussian.cpp index 33de46b25..0abec005a 100644 --- a/cpp/ConstrainedConditionalGaussian.cpp +++ b/cpp/ConstrainedConditionalGaussian.cpp @@ -48,7 +48,7 @@ ConstrainedConditionalGaussian::ConstrainedConditionalGaussian( const ConstrainedConditionalGaussian& df) { } -Vector ConstrainedConditionalGaussian::solve(const FGConfig& x) const { +Vector ConstrainedConditionalGaussian::solve(const VectorConfig& x) const { // sum the RHS Vector rhs = d_; for (map::const_iterator it = parents_.begin(); it diff --git a/cpp/ConstrainedConditionalGaussian.h b/cpp/ConstrainedConditionalGaussian.h index 2227fa267..fff94ca17 100644 --- a/cpp/ConstrainedConditionalGaussian.h +++ b/cpp/ConstrainedConditionalGaussian.h @@ -100,7 +100,7 @@ public: * @param config contains the values for all of the parents * @return the value for this node */ - Vector solve(const FGConfig& x) const; + Vector solve(const VectorConfig& x) const; }; diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp index c360677fd..9df6bdf8b 100644 --- a/cpp/ConstrainedLinearFactorGraph.cpp +++ b/cpp/ConstrainedLinearFactorGraph.cpp @@ -217,9 +217,9 @@ void ConstrainedLinearFactorGraph::update_constraints(const std::string& key, } /* ************************************************************************* */ -FGConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering) { +VectorConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering) { ChordalBayesNet::shared_ptr cbn = eliminate(ordering); - boost::shared_ptr newConfig = cbn->optimize(); + boost::shared_ptr newConfig = cbn->optimize(); return *newConfig; } diff --git a/cpp/ConstrainedLinearFactorGraph.h b/cpp/ConstrainedLinearFactorGraph.h index ce29cea69..7b782baf6 100644 --- a/cpp/ConstrainedLinearFactorGraph.h +++ b/cpp/ConstrainedLinearFactorGraph.h @@ -114,7 +114,7 @@ public: * optimize a linear factor graph * @param ordering fg in order */ - FGConfig optimize(const Ordering& ordering); + VectorConfig optimize(const Ordering& ordering); /** * Determines if a node has any constraints attached to it diff --git a/cpp/ConstrainedNonlinearFactorGraph.h b/cpp/ConstrainedNonlinearFactorGraph.h index 86454fae0..9dde141c1 100644 --- a/cpp/ConstrainedNonlinearFactorGraph.h +++ b/cpp/ConstrainedNonlinearFactorGraph.h @@ -19,8 +19,8 @@ namespace gtsam { * A nonlinear factor graph with the addition of equality constraints. * * Templated on factor and configuration type. - * TODO FD: this is totally wrong: it can only work if Config==FGConfig - * as LinearConstraint is only defined for FGConfig + * TODO FD: this is totally wrong: it can only work if Config==VectorConfig + * as LinearConstraint is only defined for VectorConfig * To fix it, we need to think more deeply about this. */ template diff --git a/cpp/Factor.h b/cpp/Factor.h index 238a79d11..440e4b1a4 100644 --- a/cpp/Factor.h +++ b/cpp/Factor.h @@ -40,7 +40,7 @@ namespace gtsam { * conflicts with efficiency as well, esp. in the case of incomplete * QR factorization. A solution is still being sought. * - * A Factor is templated on a Config, for example FGConfig is a configuration of + * A Factor is templated on a Config, for example VectorConfig is a configuration of * labeled vectors. This way, we can have factors that might be defined on discrete * variables, continuous ones, or a combination of both. It is up to the config to * provide the appropriate values at the appropriate time. diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index c16f4dc2b..0055c83ac 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -16,7 +16,7 @@ namespace gtsam { class Ordering; - class FGConfig; + class VectorConfig; class LinearFactor; class LinearFactorGraph; class Ordering; diff --git a/cpp/LinearFactor.cpp b/cpp/LinearFactor.cpp index 0bee6053b..72134546d 100644 --- a/cpp/LinearFactor.cpp +++ b/cpp/LinearFactor.cpp @@ -22,7 +22,7 @@ typedef pair& mypair; /* ************************************************************************* */ // we might have multiple As, so iterate and subtract from b -double LinearFactor::error(const FGConfig& c) const { +double LinearFactor::error(const VectorConfig& c) const { if (empty()) return 0; Vector e = b; string j; Matrix Aj; diff --git a/cpp/LinearFactor.h b/cpp/LinearFactor.h index 8d3a70888..3d71337e2 100644 --- a/cpp/LinearFactor.h +++ b/cpp/LinearFactor.h @@ -32,7 +32,7 @@ class MutableLinearFactor; * LinearFactor is non-mutable (all methods const!). * The factor value is exp(-0.5*||Ax-b||^2) */ -class LinearFactor: public Factor { +class LinearFactor: public Factor { public: typedef boost::shared_ptr shared_ptr; @@ -98,7 +98,7 @@ public: // Implementing Factor virtual functions - double error(const FGConfig& c) const; /** 0.5*(A*x-b)'*(A*x-b) */ + double error(const VectorConfig& c) const; /** 0.5*(A*x-b)'*(A*x-b) */ void print(const std::string& s = "") const; bool equals(const LinearFactor& lf, double tol = 1e-9) const; std::string dump() const { return "";} diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index 684fa7b26..478a122a1 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -142,13 +142,13 @@ LinearFactorGraph::eliminate(const Ordering& ordering) /* ************************************************************************* */ /** optimize the linear factor graph */ /* ************************************************************************* */ -FGConfig LinearFactorGraph::optimize(const Ordering& ordering) +VectorConfig LinearFactorGraph::optimize(const Ordering& ordering) { // eliminate all nodes in the given ordering -> chordal Bayes net ChordalBayesNet::shared_ptr chordalBayesNet = eliminate(ordering); // calculate new configuration (using backsubstitution) - boost::shared_ptr newConfig = chordalBayesNet->optimize(); + boost::shared_ptr newConfig = chordalBayesNet->optimize(); return *newConfig; } diff --git a/cpp/LinearFactorGraph.h b/cpp/LinearFactorGraph.h index 476a54d50..cbab2cc3a 100644 --- a/cpp/LinearFactorGraph.h +++ b/cpp/LinearFactorGraph.h @@ -16,7 +16,7 @@ #include "LinearFactor.h" //#include "Ordering.h" -#include "FGConfig.h" +#include "VectorConfig.h" #include "FactorGraph.h" namespace gtsam { @@ -26,10 +26,10 @@ namespace gtsam { /** * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. * Factor == LinearFactor - * FGConfig = A configuration of vectors + * VectorConfig = A configuration of vectors * Most of the time, linear factor graphs arise by linearizing a non-linear factor graph. */ - class LinearFactorGraph : public FactorGraph { + class LinearFactorGraph : public FactorGraph { public: /** @@ -92,7 +92,7 @@ namespace gtsam { * optimize a linear factor graph * @param ordering fg in order */ - FGConfig optimize(const Ordering& ordering); + VectorConfig optimize(const Ordering& ordering); /** * static function that combines two factor graphs diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 0ed00606f..c985af857 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -57,18 +57,18 @@ testVector_LDADD = libgtsam.la testMatrix_LDADD = libgtsam.la # nodes -sources += FGConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp +sources += VectorConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp sources += ConditionalGaussian.cpp LinearConstraint.cpp ConstrainedConditionalGaussian.cpp -check_PROGRAMS += testFGConfig testLinearFactor testConditionalGaussian testNonlinearFactor testLinearConstraint testConstrainedConditionalGaussian +check_PROGRAMS += testVectorConfig testLinearFactor testConditionalGaussian testNonlinearFactor testLinearConstraint testConstrainedConditionalGaussian example = smallExample.cpp -testFGConfig_SOURCES = testFGConfig.cpp +testVectorConfig_SOURCES = testVectorConfig.cpp testLinearFactor_SOURCES = $(example) testLinearFactor.cpp testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp testLinearConstraint_SOURCES = $(example) testLinearConstraint.cpp testConstrainedConditionalGaussian_SOURCES = testConstrainedConditionalGaussian.cpp -testFGConfig_LDADD = libgtsam.la +testVectorConfig_LDADD = libgtsam.la testLinearFactor_LDADD = libgtsam.la testConditionalGaussian_LDADD = libgtsam.la testNonlinearFactor_LDADD = libgtsam.la diff --git a/cpp/NonlinearFactor.cpp b/cpp/NonlinearFactor.cpp index c4dae68af..f041ae536 100644 --- a/cpp/NonlinearFactor.cpp +++ b/cpp/NonlinearFactor.cpp @@ -19,7 +19,7 @@ NonlinearFactor1::NonlinearFactor1(const Vector& z, Vector (*h)(const Vector&), const string& key1, Matrix (*H)(const Vector&)) - : NonlinearFactor(z, sigma), h_(h), key1_(key1), H_(H) + : NonlinearFactor(z, sigma), h_(h), key1_(key1), H_(H) { keys_.push_front(key1); } @@ -30,7 +30,7 @@ void NonlinearFactor1::print(const string& s) const { } /* ************************************************************************* */ -LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const { +LinearFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) const { // get argument 1 from config Vector x1 = c[key1_]; @@ -47,10 +47,10 @@ LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const { /* ************************************************************************* */ /** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html */ /* ************************************************************************* */ -bool NonlinearFactor1::equals(const NonlinearFactor& f, double tol) const { +bool NonlinearFactor1::equals(const NonlinearFactor& f, double tol) const { const NonlinearFactor1* p = dynamic_cast (&f); if (p == NULL) return false; - return NonlinearFactor::equals(*p, tol) + return NonlinearFactor::equals(*p, tol) && (h_ == p->h_) && (key1_== p->key1_) && (H_ == p->H_); @@ -65,7 +65,7 @@ NonlinearFactor2::NonlinearFactor2(const Vector& z, const string& key2, Matrix (*H2)(const Vector&, const Vector&) ) -: NonlinearFactor(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2) +: NonlinearFactor(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2) { keys_.push_front(key1); keys_.push_front(key2); @@ -77,7 +77,7 @@ void NonlinearFactor2::print(const string& s) const { } /* ************************************************************************* */ -LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& c) const { +LinearFactor::shared_ptr NonlinearFactor2::linearize(const VectorConfig& c) const { // get arguments from config Vector x1 = c[key1_]; Vector x2 = c[key2_]; @@ -94,10 +94,10 @@ LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& c) const { } /* ************************************************************************* */ -bool NonlinearFactor2::equals(const NonlinearFactor& f, double tol) const { +bool NonlinearFactor2::equals(const NonlinearFactor& f, double tol) const { const NonlinearFactor2* p = dynamic_cast (&f); if (p == NULL) return false; - return NonlinearFactor::equals(*p, tol) + return NonlinearFactor::equals(*p, tol) && (h_ == p->h_) && (key1_ == p->key1_) && (H2_ == p->H1_) diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h index c3ef2a385..343f30e8f 100644 --- a/cpp/NonlinearFactor.h +++ b/cpp/NonlinearFactor.h @@ -113,7 +113,7 @@ namespace gtsam { * Note: cannot be serialized as contains function pointers * Specialized derived classes could do this */ - class NonlinearFactor1 : public NonlinearFactor { + class NonlinearFactor1 : public NonlinearFactor { private: std::string key1_; @@ -133,15 +133,15 @@ namespace gtsam { void print(const std::string& s = "") const; /** error function */ - inline Vector error_vector(const FGConfig& c) const { + inline Vector error_vector(const VectorConfig& c) const { return z_ - h_(c[key1_]); } /** linearize a non-linearFactor1 to get a linearFactor1 */ - boost::shared_ptr linearize(const FGConfig& c) const; + boost::shared_ptr linearize(const VectorConfig& c) const; /** Check if two factors are equal */ - bool equals(const NonlinearFactor& f, double tol=1e-9) const; + bool equals(const NonlinearFactor& f, double tol=1e-9) const; std::string dump() const {return "";} }; @@ -151,7 +151,7 @@ namespace gtsam { * Note: cannot be serialized as contains function pointers * Specialized derived classes could do this */ - class NonlinearFactor2 : public NonlinearFactor { + class NonlinearFactor2 : public NonlinearFactor { private: @@ -176,15 +176,15 @@ namespace gtsam { void print(const std::string& s = "") const; /** error function */ - inline Vector error_vector(const FGConfig& c) const { + inline Vector error_vector(const VectorConfig& c) const { return z_ - h_(c[key1_], c[key2_]); } /** Linearize a non-linearFactor2 to get a linearFactor2 */ - boost::shared_ptr linearize(const FGConfig& c) const; + boost::shared_ptr linearize(const VectorConfig& c) const; /** Check if two factors are equal */ - bool equals(const NonlinearFactor& f, double tol=1e-9) const; + bool equals(const NonlinearFactor& f, double tol=1e-9) const; std::string dump() const{return "";}; }; diff --git a/cpp/NonlinearFactorGraph.h b/cpp/NonlinearFactorGraph.h index 11bb5c5f7..085c5e828 100644 --- a/cpp/NonlinearFactorGraph.h +++ b/cpp/NonlinearFactorGraph.h @@ -21,7 +21,7 @@ namespace gtsam { * than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds. * Linearizing the non-linear factor graph creates a linear factor graph on the * tangent vector space at the linearization point. Because the tangent space is a true - * vector space, the config type will be an FGConfig in that linearized + * vector space, the config type will be an VectorConfig in that linearized */ template class NonlinearFactorGraph: public FactorGraph ,Config> { diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index 90dee895a..ab1e4786e 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -54,7 +54,7 @@ namespace gtsam { // linearize and optimize /* ************************************************************************* */ template - FGConfig NonlinearOptimizer::delta() const { + VectorConfig NonlinearOptimizer::delta() const { LinearFactorGraph linear = graph_->linearize(*config_); return linear.optimize(*ordering_); } @@ -67,7 +67,7 @@ namespace gtsam { verbosityLevel verbosity) const { // linearize and optimize - FGConfig delta = this->delta(); + VectorConfig delta = this->delta(); // maybe show output if (verbosity >= DELTA) @@ -121,7 +121,7 @@ namespace gtsam { damped.print("damped"); // solve - FGConfig delta = damped.optimize(*ordering_); + VectorConfig delta = damped.optimize(*ordering_); if (verbosity >= TRYDELTA) delta.print("delta"); diff --git a/cpp/NonlinearOptimizer.h b/cpp/NonlinearOptimizer.h index f6fe2b682..21d003645 100644 --- a/cpp/NonlinearOptimizer.h +++ b/cpp/NonlinearOptimizer.h @@ -10,7 +10,7 @@ #include #include "NonlinearFactorGraph.h" -#include "FGConfig.h" +#include "VectorConfig.h" namespace gtsam { @@ -96,9 +96,9 @@ namespace gtsam { /** * linearize and optimize - * Thi returns an FGConfig, i.e., vectors in tangent space of Config + * Thi returns an VectorConfig, i.e., vectors in tangent space of Config */ - FGConfig delta() const; + VectorConfig delta() const; /** * Do one Gauss-Newton iteration and return next state diff --git a/cpp/VSLAMFactor.h b/cpp/VSLAMFactor.h index bbbdf0178..76cab455a 100644 --- a/cpp/VSLAMFactor.h +++ b/cpp/VSLAMFactor.h @@ -8,7 +8,7 @@ #include "NonlinearFactor.h" #include "LinearFactor.h" -#include "FGConfig.h" +#include "VectorConfig.h" #include "Cal3_S2.h" #include "Pose3.h" diff --git a/cpp/FGConfig.cpp b/cpp/VectorConfig.cpp similarity index 78% rename from cpp/FGConfig.cpp rename to cpp/VectorConfig.cpp index fd34bad35..090e27662 100644 --- a/cpp/FGConfig.cpp +++ b/cpp/VectorConfig.cpp @@ -1,14 +1,14 @@ /** - * @file FGConfig.cpp + * @file VectorConfig.cpp * @brief Factor Graph Configuration - * @brief fgConfig + * @brief VectorConfig * @author Carlos Nieto * @author Christian Potthast */ #include #include -#include "FGConfig.h" +#include "VectorConfig.h" // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) @@ -22,14 +22,14 @@ void check_size(const string& key, const Vector & vj, const Vector & dj) { cout << "For key \"" << key << "\"" << endl; cout << "vj.size = " << vj.size() << endl; cout << "dj.size = " << dj.size() << endl; - throw(std::invalid_argument("FGConfig::+ mismatched dimensions")); + throw(std::invalid_argument("VectorConfig::+ mismatched dimensions")); } } /* ************************************************************************* */ -FGConfig FGConfig::exmap(const FGConfig & delta) const +VectorConfig VectorConfig::exmap(const VectorConfig & delta) const { - FGConfig newConfig; + VectorConfig newConfig; for (const_iterator it = values.begin(); it!=values.end(); it++) { string j = it->first; const Vector &vj = it->second; @@ -41,19 +41,19 @@ FGConfig FGConfig::exmap(const FGConfig & delta) const } /* ************************************************************************* */ -Vector FGConfig::get(const std::string& name) const { +Vector VectorConfig::get(const std::string& name) const { const_iterator it = values.find(name); if (it==values.end()) { print(); cout << "asked for key " << name << endl; - throw(std::invalid_argument("FGConfig::[] invalid key")); + throw(std::invalid_argument("VectorConfig::[] invalid key")); } return it->second; } /* ************************************************************************* */ -void FGConfig::print(const std::string& name) const { - odprintf("FGConfig %s\n", name.c_str()); +void VectorConfig::print(const std::string& name) const { + odprintf("VectorConfig %s\n", name.c_str()); odprintf("size: %d\n", values.size()); string j; Vector v; FOREACH_PAIR(j, v, values) { @@ -63,7 +63,7 @@ void FGConfig::print(const std::string& name) const { } /* ************************************************************************* */ -bool FGConfig::equals(const FGConfig& expected, double tol) const { +bool VectorConfig::equals(const VectorConfig& expected, double tol) const { string j; Vector vActual; if( values.size() != expected.size() ) goto fail; diff --git a/cpp/FGConfig.h b/cpp/VectorConfig.h similarity index 80% rename from cpp/FGConfig.h rename to cpp/VectorConfig.h index 12e98ff37..c8cd31aa6 100644 --- a/cpp/FGConfig.h +++ b/cpp/VectorConfig.h @@ -1,5 +1,5 @@ /** - * @file FGConfig.h + * @file VectorConfig.h * @brief Factor Graph Configuration * @author Carlos Nieto * @author Christian Potthast @@ -18,7 +18,7 @@ namespace gtsam { /** Factor Graph Configuration */ - class FGConfig : public Testable { + class VectorConfig : public Testable { protected: /** Map from string indices to values */ @@ -28,10 +28,10 @@ namespace gtsam { typedef std::map::iterator iterator; typedef std::map::const_iterator const_iterator; - FGConfig():Testable() {} - FGConfig(const FGConfig& cfg_in) : Testable(), values(cfg_in.values) {} + VectorConfig():Testable() {} + VectorConfig(const VectorConfig& cfg_in) : Testable(), values(cfg_in.values) {} - virtual ~FGConfig() {} + virtual ~VectorConfig() {} /** return all the nodes in the graph **/ std::vector get_names() const { @@ -42,16 +42,16 @@ namespace gtsam { } /** Insert a value into the configuration with a given index */ - FGConfig& insert(const std::string& name, const Vector& val) { + VectorConfig& insert(const std::string& name, const Vector& val) { values.insert(std::make_pair(name,val)); return *this; } /** * Add a delta config, needed for use in NonlinearOptimizer - * For FGConfig, this is just addition. + * For VectorConfig, this is just addition. */ - FGConfig exmap(const FGConfig & delta) const; + VectorConfig exmap(const VectorConfig & delta) const; const_iterator begin() const {return values.begin();} const_iterator end() const {return values.end();} @@ -78,7 +78,7 @@ namespace gtsam { void print(const std::string& name = "") const; /** equals, for unit testing */ - bool equals(const FGConfig& expected, double tol=1e-9) const; + bool equals(const VectorConfig& expected, double tol=1e-9) const; void clear() {values.clear();} diff --git a/cpp/gtsam.h b/cpp/gtsam.h index c3cf30acb..b43a0da2c 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -1,11 +1,11 @@ -class FGConfig { - FGConfig(); +class VectorConfig { + VectorConfig(); Vector get(string name) const; bool contains(string name) const; size_t size() const; void insert(string name, Vector val); void print() const; - bool equals(const FGConfig& expected, double tol) const; + bool equals(const VectorConfig& expected, double tol) const; void clear(); }; @@ -33,7 +33,7 @@ class LinearFactor { bool empty() const; Vector get_b() const; Matrix get_A(string key) const; - double error(const FGConfig& c) const; + double error(const VectorConfig& c) const; bool involves(string key) const; void print() const; bool equals(const LinearFactor& lf, double tol) const; @@ -55,7 +55,7 @@ class ConditionalGaussian { string name2, Matrix T); void print() const; - Vector solve(const FGConfig& x); + Vector solve(const VectorConfig& x); void add(string key, Matrix S); bool equals(const ConditionalGaussian &cg) const; }; @@ -70,7 +70,7 @@ class ChordalBayesNet { ChordalBayesNet(); void insert(string name, ConditionalGaussian* node); ConditionalGaussian* get(string name); - FGConfig* optimize(); + VectorConfig* optimize(); void print() const; bool equals(const ChordalBayesNet& cbn) const; pair matrix() const; @@ -81,12 +81,12 @@ class LinearFactorGraph { size_t size() const; void push_back(LinearFactor* ptr_f); - double error(const FGConfig& c) const; - double probPrime(const FGConfig& c) const; + double error(const VectorConfig& c) const; + double probPrime(const VectorConfig& c) const; void print() const; bool equals(const LinearFactorGraph& lfgraph) const; - FGConfig optimize(const Ordering& ordering); + VectorConfig optimize(const Ordering& ordering); LinearFactor* combine_factors(string key); ConditionalGaussian* eliminate_one(string key); ChordalBayesNet* eliminate(const Ordering& ordering); @@ -117,31 +117,31 @@ class Point3 { class Point2Prior { Point2Prior(Vector mu, double sigma, string key); - Vector error_vector(const FGConfig& c) const; - LinearFactor* linearize(const FGConfig& c) const; + Vector error_vector(const VectorConfig& c) const; + LinearFactor* linearize(const VectorConfig& c) const; double get_sigma(); Vector get_measurement(); - double error(const FGConfig& c) const; + double error(const VectorConfig& c) const; void print() const; }; class Simulated2DOdometry { Simulated2DOdometry(Vector odo, double sigma, string key, string key2); - Vector error_vector(const FGConfig& c) const; - LinearFactor* linearize(const FGConfig& c) const; + Vector error_vector(const VectorConfig& c) const; + LinearFactor* linearize(const VectorConfig& c) const; double get_sigma(); Vector get_measurement(); - double error(const FGConfig& c) const; + double error(const VectorConfig& c) const; void print() const; }; class Simulated2DMeasurement { Simulated2DMeasurement(Vector odo, double sigma, string key, string key2); - Vector error_vector(const FGConfig& c) const; - LinearFactor* linearize(const FGConfig& c) const; + Vector error_vector(const VectorConfig& c) const; + LinearFactor* linearize(const VectorConfig& c) const; double get_sigma(); Vector get_measurement(); - double error(const FGConfig& c) const; + double error(const VectorConfig& c) const; void print() const; }; diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index 384d31234..80ac96f42 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -26,7 +26,7 @@ using namespace std; namespace gtsam { -typedef boost::shared_ptr > shared; +typedef boost::shared_ptr > shared; /* ************************************************************************* */ boost::shared_ptr sharedNonlinearFactorGraph() { @@ -66,12 +66,12 @@ ExampleNonlinearFactorGraph createNonlinearFactorGraph() { } /* ************************************************************************* */ -FGConfig createConfig() +VectorConfig createConfig() { Vector v_x1(2); v_x1(0) = 0.; v_x1(1) = 0.; Vector v_x2(2); v_x2(0) = 1.5; v_x2(1) = 0.; Vector v_l1(2); v_l1(0) = 0.; v_l1(1) = -1.; - FGConfig c; + VectorConfig c; c.insert("x1", v_x1); c.insert("x2", v_x2); c.insert("l1", v_l1); @@ -79,12 +79,12 @@ FGConfig createConfig() } /* ************************************************************************* */ -boost::shared_ptr sharedNoisyConfig() +boost::shared_ptr sharedNoisyConfig() { Vector v_x1(2); v_x1(0) = 0.1; v_x1(1) = 0.1; Vector v_x2(2); v_x2(0) = 1.4; v_x2(1) = 0.2; Vector v_l1(2); v_l1(0) = 0.1; v_l1(1) = -1.1; - boost::shared_ptr c(new FGConfig); + boost::shared_ptr c(new VectorConfig); c->insert("x1", v_x1); c->insert("x2", v_x2); c->insert("l1", v_l1); @@ -92,16 +92,16 @@ boost::shared_ptr sharedNoisyConfig() } /* ************************************************************************* */ -FGConfig createNoisyConfig() { +VectorConfig createNoisyConfig() { return *sharedNoisyConfig(); } /* ************************************************************************* */ -FGConfig createCorrectDelta() { +VectorConfig createCorrectDelta() { Vector v_x1(2); v_x1(0) = -0.1; v_x1(1) = -0.1; Vector v_x2(2); v_x2(0) = 0.1; v_x2(1) = -0.2; Vector v_l1(2); v_l1(0) = -0.1; v_l1(1) = 0.1; - FGConfig c; + VectorConfig c; c.insert("x1", v_x1); c.insert("x2", v_x2); c.insert("l1", v_l1); @@ -109,11 +109,11 @@ FGConfig createCorrectDelta() { } /* ************************************************************************* */ -FGConfig createZeroDelta() { +VectorConfig createZeroDelta() { Vector v_x1(2); v_x1(0) = 0; v_x1(1) = 0; Vector v_x2(2); v_x2(0) = 0; v_x2(1) = 0; Vector v_l1(2); v_l1(0) = 0; v_l1(1) = 0; - FGConfig c; + VectorConfig c; c.insert("x1", v_x1); c.insert("x2", v_x2); c.insert("l1", v_l1); @@ -123,7 +123,7 @@ FGConfig createZeroDelta() { /* ************************************************************************* */ LinearFactorGraph createLinearFactorGraph() { - FGConfig c = createNoisyConfig(); + VectorConfig c = createNoisyConfig(); // Create LinearFactorGraph fg; @@ -333,9 +333,9 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() { //} /* ************************************************************************* */ -// ConstrainedNonlinearFactorGraph , FGConfig> createConstrainedNonlinearFactorGraph() { -// ConstrainedNonlinearFactorGraph , FGConfig> graph; -// FGConfig c = createConstrainedConfig(); +// ConstrainedNonlinearFactorGraph , VectorConfig> createConstrainedNonlinearFactorGraph() { +// ConstrainedNonlinearFactorGraph , VectorConfig> graph; +// VectorConfig c = createConstrainedConfig(); // // // equality constraint for initial pose // LinearConstraint::shared_ptr f1(new LinearConstraint(c["x0"], "x0")); @@ -349,9 +349,9 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() { // } /* ************************************************************************* */ -//FGConfig createConstrainedConfig() +//VectorConfig createConstrainedConfig() //{ -// FGConfig config; +// VectorConfig config; // // Vector x0(2); x0(0)=1.0; x0(1)=2.0; // config.insert("x0", x0); @@ -363,9 +363,9 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() { //} /* ************************************************************************* */ -//FGConfig createConstrainedLinConfig() +//VectorConfig createConstrainedLinConfig() //{ -// FGConfig config; +// VectorConfig config; // // Vector x0(2); x0(0)=1.0; x0(1)=2.0; // value doesn't actually matter // config.insert("x0", x0); @@ -377,9 +377,9 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() { //} /* ************************************************************************* */ -//FGConfig createConstrainedCorrectDelta() +//VectorConfig createConstrainedCorrectDelta() //{ -// FGConfig config; +// VectorConfig config; // // Vector x0(2); x0(0)=0.; x0(1)=0.; // config.insert("x0", x0); @@ -394,7 +394,7 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() { //ConstrainedChordalBayesNet createConstrainedChordalBayesNet() //{ // ConstrainedChordalBayesNet cbn; -// FGConfig c = createConstrainedConfig(); +// VectorConfig c = createConstrainedConfig(); // // // add regular conditional gaussian - no parent // Matrix R = eye(2); diff --git a/cpp/smallExample.h b/cpp/smallExample.h index 3014471e1..ae55603d7 100644 --- a/cpp/smallExample.h +++ b/cpp/smallExample.h @@ -16,13 +16,13 @@ #include "NonlinearFactorGraph.h" #include "ChordalBayesNet.h" #include "LinearFactorGraph.h" -#include "FGConfig.h" +#include "VectorConfig.h" // \namespace namespace gtsam { - typedef NonlinearFactorGraph ExampleNonlinearFactorGraph; + typedef NonlinearFactorGraph ExampleNonlinearFactorGraph; /** * Create small example for non-linear factor graph @@ -34,23 +34,23 @@ namespace gtsam { * Create configuration to go with it * The ground truth configuration for the example above */ - FGConfig createConfig(); + VectorConfig createConfig(); /** * create a noisy configuration for a nonlinear factor graph */ - boost::shared_ptr sharedNoisyConfig(); - FGConfig createNoisyConfig(); + boost::shared_ptr sharedNoisyConfig(); + VectorConfig createNoisyConfig(); /** * Zero delta config */ - FGConfig createZeroDelta(); + VectorConfig createZeroDelta(); /** * Delta config that, when added to noisyConfig, returns the ground truth */ - FGConfig createCorrectDelta(); + VectorConfig createCorrectDelta(); /** * create a linear factor graph @@ -93,17 +93,17 @@ namespace gtsam { * Create configuration for constrained example * This is the ground truth version */ - //FGConfig createConstrainedConfig(); + //VectorConfig createConstrainedConfig(); /** * Create a noisy configuration for linearization */ - //FGConfig createConstrainedLinConfig(); + //VectorConfig createConstrainedLinConfig(); /** * Create the correct delta configuration */ - //FGConfig createConstrainedCorrectDelta(); + //VectorConfig createConstrainedCorrectDelta(); /** * Create small example constrained factor graph @@ -113,6 +113,6 @@ namespace gtsam { /** * Create small example constrained nonlinear factor graph */ -// ConstrainedNonlinearFactorGraph,FGConfig> +// ConstrainedNonlinearFactorGraph,VectorConfig> // createConstrainedNonlinearFactorGraph(); } diff --git a/cpp/testChordalBayesNet.cpp b/cpp/testChordalBayesNet.cpp index 142d13fa6..d35548800 100644 --- a/cpp/testChordalBayesNet.cpp +++ b/cpp/testChordalBayesNet.cpp @@ -69,9 +69,9 @@ TEST( ChordalBayesNet, optimize ) { // optimize small Bayes Net ChordalBayesNet cbn = createSmallChordalBayesNet(); - boost::shared_ptr actual = cbn.optimize(); + boost::shared_ptr actual = cbn.optimize(); - FGConfig expected; + VectorConfig expected; Vector x(1), y(1); x(0) = 4; y(0) = 5; expected.insert("x",x); diff --git a/cpp/testConditionalGaussian.cpp b/cpp/testConditionalGaussian.cpp index 892af2f0d..6e0833d58 100644 --- a/cpp/testConditionalGaussian.cpp +++ b/cpp/testConditionalGaussian.cpp @@ -80,7 +80,7 @@ TEST( ConditionalGaussian, solve ) Vector sl1(2); sl1(0) = 0.5; sl1(1) = 0.8; - FGConfig solution; + VectorConfig solution; solution.insert("x1", sx1); solution.insert("l1", sl1); diff --git a/cpp/testConstrainedConditionalGaussian.cpp b/cpp/testConstrainedConditionalGaussian.cpp index f74433272..6e6dfdb4a 100644 --- a/cpp/testConstrainedConditionalGaussian.cpp +++ b/cpp/testConstrainedConditionalGaussian.cpp @@ -18,7 +18,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary1 ) // check unary constructor that doesn't require an R matrix // assumed identity matrix ConstrainedConditionalGaussian unary(v); - FGConfig fg; + VectorConfig fg; fg.insert("x1", v); CHECK(assert_equal(v, unary.solve(fg))); @@ -33,7 +33,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary2 ) Matrix A = eye(2) * 10; ConstrainedConditionalGaussian unary(10*v, A); - FGConfig fg; + VectorConfig fg; fg.insert("x1", v); CHECK(assert_equal(v, unary.solve(fg))); @@ -52,7 +52,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary3 ) Vector rhs = A*v; ConstrainedConditionalGaussian unary(rhs, A); - FGConfig fg; + VectorConfig fg; fg.insert("x1", v); CHECK(assert_equal(v, unary.solve(fg))); @@ -79,7 +79,7 @@ TEST (ConstrainedConditionalGaussian, basic_binary1 ) Vector y = Vector_(2, 1.0, 2.0); - FGConfig fg; + VectorConfig fg; fg.insert("x1", y); Vector expected = Vector_(2, -3.3333, 0.6667); @@ -113,7 +113,7 @@ TEST (ConstrainedConditionalGaussian, basic_ternary1 ) Vector y = Vector_(2, 1.0, 2.0); Vector z = Vector_(2, 1.0, -1.0); - FGConfig fg; + VectorConfig fg; fg.insert("x1", y); fg.insert("x2", z); diff --git a/cpp/testConstrainedLinearFactorGraph.cpp b/cpp/testConstrainedLinearFactorGraph.cpp index e49414b0d..4370c4412 100644 --- a/cpp/testConstrainedLinearFactorGraph.cpp +++ b/cpp/testConstrainedLinearFactorGraph.cpp @@ -67,9 +67,9 @@ TEST( ConstrainedLinearFactorGraph, optimize ) Ordering ord; ord.push_back("y"); ord.push_back("x"); - FGConfig actual = fg.optimize(ord); + VectorConfig actual = fg.optimize(ord); - FGConfig expected; + VectorConfig expected; expected.insert("x", Vector_(2, 1.0, -1.0)); expected.insert("y", Vector_(2, 0.2, 0.1)); @@ -88,9 +88,9 @@ TEST( ConstrainedLinearFactorGraph, optimize2 ) Ordering ord; ord.push_back("x"); ord.push_back("y"); - FGConfig actual = fg.optimize(ord); + VectorConfig actual = fg.optimize(ord); - FGConfig expected; + VectorConfig expected; expected.insert("x", Vector_(2, 1.0, -1.0)); expected.insert("y", Vector_(2, 0.2, 0.1)); @@ -239,7 +239,7 @@ TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint ) CHECK(cg3->size() == 0); // solve piecewise - FGConfig actual; + VectorConfig actual; Vector act_z = cg3->solve(actual); actual.insert("z", act_z); CHECK(assert_equal(act_z, Vector_(2, -4.0, 5.0), 1e-4)); @@ -261,10 +261,10 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint ) ord.push_back("y"); ord.push_back("z"); - FGConfig actual = fg.optimize(ord); + VectorConfig actual = fg.optimize(ord); // verify - FGConfig expected; + VectorConfig expected; expected.insert("x", Vector_(2, -2.0, 2.0)); expected.insert("y", Vector_(2, -0.1, 0.4)); expected.insert("z", Vector_(2, -4.0, 5.0)); @@ -363,7 +363,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint ) // ConstrainedLinearFactorGraph fg1 = createConstrainedLinearFactorGraph(); // ConstrainedLinearFactorGraph fg2 = createConstrainedLinearFactorGraph(); // -// FGConfig expected = createConstrainedConfig(); +// VectorConfig expected = createConstrainedConfig(); // // Ordering ord1; // ord1.push_back("x0"); @@ -373,8 +373,8 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint ) // ord2.push_back("x1"); // ord2.push_back("x0"); // -// FGConfig actual1 = fg1.optimize(ord1); -// FGConfig actual2 = fg2.optimize(ord2); +// VectorConfig actual1 = fg1.optimize(ord1); +// VectorConfig actual2 = fg2.optimize(ord2); // // CHECK(actual1.equals(expected)); // CHECK(actual1.equals(actual2)); @@ -383,7 +383,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint ) //TEST (ConstrainedLinearFactorGraph, eliminate ) //{ // ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); -// FGConfig c = createConstrainedConfig(); +// VectorConfig c = createConstrainedConfig(); // // Ordering ord1; // ord1.push_back("x0"); @@ -417,9 +417,9 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint ) // ord.push_back("x1"); // ord.push_back("x2"); // -// FGConfig actual = clfg.optimize(ord); +// VectorConfig actual = clfg.optimize(ord); // -// FGConfig expected = lfg.optimize(ord); // should be identical to regular lfg optimize +// VectorConfig expected = lfg.optimize(ord); // should be identical to regular lfg optimize // // CHECK(actual.equals(expected)); //} @@ -455,7 +455,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint ) //// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); //// ConstrainedConditionalGaussian::shared_ptr actual = fg.eliminate_constraint("x0"); //// -//// FGConfig c = createConstrainedConfig(); +//// VectorConfig c = createConstrainedConfig(); //// ConstrainedConditionalGaussian::shared_ptr expected(new ConstrainedConditionalGaussian);//(c["x0"], "x0")); //// //// CHECK(assert_equal(*actual, *expected)); // check output for correct delta function @@ -496,7 +496,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint ) // CHECK(actual->size() == 1); // remaining factor will be unary // // // verify values -// FGConfig c = createConstrainedConfig(); +// VectorConfig c = createConstrainedConfig(); // Vector exp_v = c["x1"]; // Matrix A = actual->get_A("x1"); // Vector b = actual->get_b(); diff --git a/cpp/testConstrainedNonlinearFactorGraph.cpp b/cpp/testConstrainedNonlinearFactorGraph.cpp index 99caf1717..22b14968b 100644 --- a/cpp/testConstrainedNonlinearFactorGraph.cpp +++ b/cpp/testConstrainedNonlinearFactorGraph.cpp @@ -12,8 +12,8 @@ using namespace gtsam; -typedef boost::shared_ptr > shared; -typedef ConstrainedNonlinearFactorGraph,FGConfig> TestGraph; +typedef boost::shared_ptr > shared; +typedef ConstrainedNonlinearFactorGraph,VectorConfig> TestGraph; //TEST( TestGraph, equals ) //{ @@ -47,7 +47,7 @@ typedef ConstrainedNonlinearFactorGraph,FGConfig> Test // ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); // TestGraph cfg(nfg); // -// FGConfig initial = createNoisyConfig(); +// VectorConfig initial = createNoisyConfig(); // ConstrainedLinearFactorGraph linearized = cfg.linearize(initial); // LinearFactorGraph lfg = createLinearFactorGraph(); // ConstrainedLinearFactorGraph expected(lfg); @@ -68,11 +68,11 @@ typedef ConstrainedNonlinearFactorGraph,FGConfig> Test //TEST( TestGraph, linearize_and_solve ) //{ // TestGraph nfg = createConstrainedNonlinearFactorGraph(); -// FGConfig lin = createConstrainedLinConfig(); +// VectorConfig lin = createConstrainedLinConfig(); // ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin); -// FGConfig actual = actual_lfg.optimize(actual_lfg.getOrdering()); +// VectorConfig actual = actual_lfg.optimize(actual_lfg.getOrdering()); // -// FGConfig expected = createConstrainedCorrectDelta(); +// VectorConfig expected = createConstrainedCorrectDelta(); // CHECK(actual.equals(expected)); //} diff --git a/cpp/testLinearConstraint.cpp b/cpp/testLinearConstraint.cpp index fbd653a9a..c828f088c 100644 --- a/cpp/testLinearConstraint.cpp +++ b/cpp/testLinearConstraint.cpp @@ -151,7 +151,7 @@ TEST ( LinearConstraint, eliminate1 ) ConstrainedConditionalGaussian::shared_ptr ccg = lc.eliminate(key); // solve the ccg to get a value - FGConfig fg; + VectorConfig fg; CHECK(assert_equal(ccg->solve(fg), v)); } @@ -176,7 +176,7 @@ TEST ( LinearConstraint, eliminate2 ) Vector y = Vector_(2, 1.0, 2.0); - FGConfig fg1; + VectorConfig fg1; fg1.insert("y", y); Vector expected = Vector_(2, -3.3333, 0.6667); @@ -186,7 +186,7 @@ TEST ( LinearConstraint, eliminate2 ) CHECK(assert_equal(expected, actual->solve(fg1), 1e-4)); // eliminate y to test thrown error - FGConfig fg2; + VectorConfig fg2; fg2.insert("x", expected); actual = lc.eliminate("y"); try { diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp index d274949e2..b3225a1ad 100644 --- a/cpp/testLinearFactor.cpp +++ b/cpp/testLinearFactor.cpp @@ -172,7 +172,7 @@ TEST( LinearFactor, error ) LinearFactor::shared_ptr lf = fg[0]; // check the error of the first factor with nosiy config - FGConfig cfg = createZeroDelta(); + VectorConfig cfg = createZeroDelta(); // calculate the error from the factor "f1" // note the error is the same as in testNonlinearFactor @@ -330,7 +330,7 @@ TEST( LinearFactor, eliminate2 ) TEST( LinearFactor, default_error ) { MutableLinearFactor f; - FGConfig c; + VectorConfig c; double actual = f.error(c); CHECK(actual==0.0); } diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index 216441fc4..5e9518f26 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -31,7 +31,7 @@ TEST( LinearFactorGraph, equals ){ TEST( LinearFactorGraph, error ) { LinearFactorGraph fg = createLinearFactorGraph(); - FGConfig cfg = createZeroDelta(); + VectorConfig cfg = createZeroDelta(); // note the error is the same as in testNonlinearFactorGraph as a // zero delta config in the linear graph is equivalent to noisy in @@ -484,10 +484,10 @@ TEST( LinearFactorGraph, OPTIMIZE ) Ordering ord = fg.getOrdering(); // optimize the graph - FGConfig actual = fg.optimize(ord); + VectorConfig actual = fg.optimize(ord); // verify - FGConfig expected = createCorrectDelta(); + VectorConfig expected = createCorrectDelta(); CHECK(actual.equals(expected)); } diff --git a/cpp/testNonlinearFactor.cpp b/cpp/testNonlinearFactor.cpp index a057f85b8..a747fbfb5 100644 --- a/cpp/testNonlinearFactor.cpp +++ b/cpp/testNonlinearFactor.cpp @@ -17,7 +17,7 @@ using namespace std; using namespace gtsam; -typedef boost::shared_ptr > shared_nlf; +typedef boost::shared_ptr > shared_nlf; /* ************************************************************************* */ TEST( NonLinearFactor, NonlinearFactor ) @@ -26,7 +26,7 @@ TEST( NonLinearFactor, NonlinearFactor ) ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); // create a configuration for the non linear factor graph - FGConfig cfg = createNoisyConfig(); + VectorConfig cfg = createNoisyConfig(); // get the factor "f1" from the factor graph shared_nlf factor = fg[0]; @@ -55,7 +55,7 @@ TEST( NonLinearFactor, linearize_f1 ) boost::static_pointer_cast(nfg[0]); // We linearize at noisy config from SmallExample - FGConfig c = createNoisyConfig(); + VectorConfig c = createNoisyConfig(); LinearFactor::shared_ptr actual = nlf->linearize(c); LinearFactorGraph lfg = createLinearFactorGraph(); @@ -73,7 +73,7 @@ TEST( NonLinearFactor, linearize_f2 ) boost::static_pointer_cast(nfg[1]); // We linearize at noisy config from SmallExample - FGConfig c = createNoisyConfig(); + VectorConfig c = createNoisyConfig(); LinearFactor::shared_ptr actual = nlf->linearize(c); LinearFactorGraph lfg = createLinearFactorGraph(); @@ -91,7 +91,7 @@ TEST( NonLinearFactor, linearize_f3 ) boost::static_pointer_cast(nfg[2]); // We linearize at noisy config from SmallExample - FGConfig c = createNoisyConfig(); + VectorConfig c = createNoisyConfig(); LinearFactor::shared_ptr actual = nlf->linearize(c); LinearFactorGraph lfg = createLinearFactorGraph(); @@ -109,7 +109,7 @@ TEST( NonLinearFactor, linearize_f4 ) boost::static_pointer_cast(nfg[3]); // We linearize at noisy config from SmallExample - FGConfig c = createNoisyConfig(); + VectorConfig c = createNoisyConfig(); LinearFactor::shared_ptr actual = nlf->linearize(c); LinearFactorGraph lfg = createLinearFactorGraph(); @@ -125,7 +125,7 @@ TEST( NonLinearFactor, size ) ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); // create a configuration for the non linear factor graph - FGConfig cfg = createNoisyConfig(); + VectorConfig cfg = createNoisyConfig(); // get some factors from the graph shared_nlf factor1 = fg[0]; diff --git a/cpp/testNonlinearFactorGraph.cpp b/cpp/testNonlinearFactorGraph.cpp index 251540221..96759c8e7 100644 --- a/cpp/testNonlinearFactorGraph.cpp +++ b/cpp/testNonlinearFactorGraph.cpp @@ -33,11 +33,11 @@ TEST( ExampleNonlinearFactorGraph, error ) { ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); - FGConfig c1 = createConfig(); + VectorConfig c1 = createConfig(); double actual1 = fg.error(c1); DOUBLES_EQUAL( 0.0, actual1, 1e-9 ); - FGConfig c2 = createNoisyConfig(); + VectorConfig c2 = createNoisyConfig(); double actual2 = fg.error(c2); DOUBLES_EQUAL( 5.625, actual2, 1e-9 ); } @@ -46,7 +46,7 @@ TEST( ExampleNonlinearFactorGraph, error ) TEST( ExampleNonlinearFactorGraph, probPrime ) { ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); - FGConfig cfg = createConfig(); + VectorConfig cfg = createConfig(); // evaluate the probability of the factor graph double actual = fg.probPrime(cfg); @@ -58,7 +58,7 @@ TEST( ExampleNonlinearFactorGraph, probPrime ) TEST( ExampleNonlinearFactorGraph, linearize ) { ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); - FGConfig initial = createNoisyConfig(); + VectorConfig initial = createNoisyConfig(); LinearFactorGraph linearized = fg.linearize(initial); LinearFactorGraph expected = createLinearFactorGraph(); CHECK(expected.equals(linearized)); diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index a72e3530a..87730cada 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -17,7 +17,7 @@ using namespace std; using namespace gtsam; -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ TEST( NonlinearOptimizer, delta ) @@ -27,7 +27,7 @@ TEST( NonlinearOptimizer, delta ) // Expected configuration is the difference between the noisy config // and the ground-truth config. One step only because it's linear ! - FGConfig expected; + VectorConfig expected; Vector dl1(2); dl1(0) = -0.1; dl1(1) = 0.1; @@ -47,7 +47,7 @@ TEST( NonlinearOptimizer, delta ) ord1.push_back("l1"); ord1.push_back("x1"); Optimizer optimizer1(fg, ord1, initial); - FGConfig actual1 = optimizer1.delta(); + VectorConfig actual1 = optimizer1.delta(); CHECK(assert_equal(actual1,expected)); // Check another @@ -56,7 +56,7 @@ TEST( NonlinearOptimizer, delta ) ord2.push_back("x2"); ord2.push_back("l1"); Optimizer optimizer2(fg, ord2, initial); - FGConfig actual2 = optimizer2.delta(); + VectorConfig actual2 = optimizer2.delta(); CHECK(assert_equal(actual2,expected)); // And yet another... @@ -65,7 +65,7 @@ TEST( NonlinearOptimizer, delta ) ord3.push_back("x1"); ord3.push_back("x2"); Optimizer optimizer3(fg, ord3, initial); - FGConfig actual3 = optimizer3.delta(); + VectorConfig actual3 = optimizer3.delta(); CHECK(assert_equal(actual3,expected)); } @@ -77,7 +77,7 @@ TEST( NonlinearOptimizer, iterateLM ) // config far from minimum Vector x0 = Vector_(1, 3.0); - boost::shared_ptr config(new FGConfig); + boost::shared_ptr config(new VectorConfig); config->insert("x", x0); // ordering @@ -103,13 +103,13 @@ TEST( NonlinearOptimizer, optimize ) // test error at minimum Vector xstar = Vector_(1, 0.0); - FGConfig cstar; + VectorConfig cstar; cstar.insert("x", xstar); DOUBLES_EQUAL(0.0,fg.error(cstar),0.0); // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = Vector x0 = Vector_(1, 3.0); - boost::shared_ptr c0(new FGConfig); + boost::shared_ptr c0(new VectorConfig); c0->insert("x", x0); DOUBLES_EQUAL(199.0,fg.error(*c0),1e-3); diff --git a/cpp/testFGConfig.cpp b/cpp/testVectorConfig.cpp similarity index 81% rename from cpp/testFGConfig.cpp rename to cpp/testVectorConfig.cpp index fc10a61b7..6760ab34a 100644 --- a/cpp/testFGConfig.cpp +++ b/cpp/testVectorConfig.cpp @@ -1,5 +1,5 @@ /** - * @file testFGConfig.cpp + * @file testVectorConfig.cpp * @brief Unit tests for Factor Graph Configuration * @author Carlos Nieto **/ @@ -17,27 +17,27 @@ #include #include "Matrix.h" -#include "FGConfig.h" +#include "VectorConfig.h" #include "smallExample.cpp" using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST( FGConfig, equals ) +TEST( VectorConfig, equals ) { - FGConfig expected; + VectorConfig expected; Vector v = Vector_(3, 5.0, 6.0, 7.0); expected.insert("a",v); - FGConfig actual; + VectorConfig actual; actual.insert("a",v); CHECK(actual.equals(expected)); } /* ************************************************************************* */ -TEST( FGConfig, contains) +TEST( VectorConfig, contains) { - FGConfig fg; + VectorConfig fg; Vector v = Vector_(3, 5.0, 6.0, 7.0); fg.insert("ali", v); CHECK(fg.contains("ali")); @@ -45,34 +45,34 @@ TEST( FGConfig, contains) } /* ************************************************************************* */ -TEST( FGConfig, plus) +TEST( VectorConfig, plus) { - FGConfig fg; + VectorConfig fg; Vector vx = Vector_(3, 5.0, 6.0, 7.0), vy = Vector_(2, 8.0, 9.0); fg.insert("x", vx).insert("y",vy); - FGConfig delta; + VectorConfig delta; Vector dx = Vector_(3, 1.0, 1.0, 1.0), dy = Vector_(2, -1.0, -1.0); delta.insert("x", dx).insert("y",dy); - FGConfig expected; + VectorConfig expected; Vector wx = Vector_(3, 6.0, 7.0, 8.0), wy = Vector_(2, 7.0, 8.0); expected.insert("x", wx).insert("y",wy); // functional - FGConfig actual = fg.exmap(delta); + VectorConfig actual = fg.exmap(delta); CHECK(actual.equals(expected)); } /* ************************************************************************* */ #ifdef HAVE_BOOST_SERIALIZATION -TEST( FGConfig, serialize) +TEST( VectorConfig, serialize) { //DEBUG: - cout << "FGConfig: Running Serialization Test" << endl; + cout << "VectorConfig: Running Serialization Test" << endl; - //create an FGConfig - FGConfig fg = createConfig(); + //create an VectorConfig + VectorConfig fg = createConfig(); //serialize the config std::ostringstream in_archive_stream; @@ -83,7 +83,7 @@ TEST( FGConfig, serialize) //deserialize the config std::istringstream out_archive_stream(serialized_fgc); boost::archive::text_iarchive out_archive(out_archive_stream); - FGConfig output; + VectorConfig output; out_archive >> output; //check for equality