From 989f290c992dd7ea8c89ce21dc11724c8f920b21 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 6 Oct 2009 18:25:04 +0000 Subject: [PATCH] '''BIG CHANGE''': avoid converting back and to FGConfigs by templating on configuration type. Details: * Factors are now templated on the configuration type. Factor Graphs are now templated on the factor type and configuration type. * LinearFactor is a factor on an FGConfig. * LinearFactorGraph uses LinearFactor and FGConfig. * NonLinearFactor is still templated on Config. * NonLinearFactorGraph uses NonLinearFactors, but is still templated on Config. * Tests and VSLAMFactor have been updated to reflect those changes. --- .cproject | 46 ++++++--- cpp/ConstrainedNonlinearFactorGraph.cpp | 45 --------- cpp/ConstrainedNonlinearFactorGraph.h | 55 +++++++++-- cpp/EqualityFactor.cpp | 7 -- cpp/EqualityFactor.h | 4 +- cpp/Factor.h | 14 ++- cpp/FactorGraph-inl.h | 5 +- cpp/FactorGraph.h | 16 +-- cpp/LinearFactor.cpp | 16 +-- cpp/LinearFactor.h | 7 +- cpp/LinearFactorGraph.h | 18 +++- cpp/Makefile.am | 10 +- cpp/NonlinearFactor.cpp | 102 +++++++++----------- cpp/NonlinearFactor.h | 37 ++++--- cpp/NonlinearFactorGraph-inl.h | 39 ++++++++ cpp/NonlinearFactorGraph.cpp | 82 ---------------- cpp/NonlinearFactorGraph.h | 70 ++++---------- cpp/NonlinearOptimizer-inl.h | 23 +++++ cpp/NonlinearOptimizer.h | 10 +- cpp/VSLAMFactor.cpp | 6 +- cpp/VSLAMFactor.h | 16 +-- cpp/smallExample.cpp | 41 ++++---- cpp/smallExample.h | 13 ++- cpp/testConstrainedNonlinearFactorGraph.cpp | 37 +++---- cpp/testNonlinearFactor.cpp | 22 +++-- cpp/testNonlinearFactorGraph.cpp | 20 ++-- cpp/testNonlinearOptimizer.cpp | 17 ++-- 27 files changed, 386 insertions(+), 392 deletions(-) create mode 100644 cpp/NonlinearFactorGraph-inl.h delete mode 100644 cpp/NonlinearFactorGraph.cpp diff --git a/.cproject b/.cproject index fe4d4d232..bf6c64a8b 100644 --- a/.cproject +++ b/.cproject @@ -1,4 +1,4 @@ - + @@ -300,7 +300,6 @@ make - check true true @@ -308,6 +307,7 @@ make + testSimpleCamera.run true true @@ -315,14 +315,14 @@ make +-f local.mk testCal3_S2.run true -true +false true make - testVSLAMFactor.run true true @@ -330,6 +330,7 @@ make + testCalibratedCamera.run true true @@ -337,7 +338,6 @@ make - testConditionalGaussian.run true true @@ -345,6 +345,7 @@ make + testPose2.run true true @@ -352,14 +353,15 @@ make - +-f local.mk testFGConfig.run true -true +false true make + testRot3.run true true @@ -367,7 +369,6 @@ make - testNonlinearOptimizer.run true true @@ -375,22 +376,45 @@ make + testLinearFactor.run true true true + +make + +testConstrainedNonlinearFactorGraph.run +true +true +true + make - + testLinearFactorGraph.run true true true + +make +testNonlinearFactorGraph.run +true +true +true + + +make + +testPose3.run +true +true +true + make - install true true @@ -398,7 +422,6 @@ make - clean true true @@ -406,7 +429,6 @@ make - check true true diff --git a/cpp/ConstrainedNonlinearFactorGraph.cpp b/cpp/ConstrainedNonlinearFactorGraph.cpp index dc00abce3..e68470523 100644 --- a/cpp/ConstrainedNonlinearFactorGraph.cpp +++ b/cpp/ConstrainedNonlinearFactorGraph.cpp @@ -7,48 +7,3 @@ #include "ConstrainedNonlinearFactorGraph.h" -namespace gtsam { - -ConstrainedNonlinearFactorGraph::ConstrainedNonlinearFactorGraph() -{ -} - -ConstrainedNonlinearFactorGraph::ConstrainedNonlinearFactorGraph( - const NonlinearFactorGraph& nfg) -: NonlinearFactorGraph(nfg) -{ -} - -ConstrainedNonlinearFactorGraph::~ConstrainedNonlinearFactorGraph() -{ -} - -ConstrainedLinearFactorGraph ConstrainedNonlinearFactorGraph::linearize(const FGConfig& config) const -{ - ConstrainedLinearFactorGraph ret; - - // linearize all nonlinear factors - for(const_iterator factor=factors_.begin(); factorlinearize(config); - ret.push_back(lf); - } - - // linearize the equality factors (set to zero because they are now in delta space) - for(eq_const_iterator e_factor=eq_factors.begin(); e_factorlinearize(); - ret.push_back_eq(eq); - } - - return ret; -} - -NonlinearFactorGraph ConstrainedNonlinearFactorGraph::convert() const -{ - NonlinearFactorGraph ret; - BOOST_FOREACH(boost::shared_ptr f, factors_) - ret.push_back(f); - - return ret; -} - -} diff --git a/cpp/ConstrainedNonlinearFactorGraph.h b/cpp/ConstrainedNonlinearFactorGraph.h index d907c5b0a..b11c0ae02 100644 --- a/cpp/ConstrainedNonlinearFactorGraph.h +++ b/cpp/ConstrainedNonlinearFactorGraph.h @@ -8,39 +8,78 @@ #ifndef CONSTRAINEDNONLINEARFACTORGRAPH_H_ #define CONSTRAINEDNONLINEARFACTORGRAPH_H_ +#include #include "NonlinearFactorGraph.h" #include "EqualityFactor.h" #include "ConstrainedLinearFactorGraph.h" namespace gtsam { -class ConstrainedNonlinearFactorGraph: public NonlinearFactorGraph { +/** + * 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 EqualityFactor is only defined for FGConfig + * To fix it, we need to think more deeply about this. + */ +template +class ConstrainedNonlinearFactorGraph: public FactorGraph { protected: /** collection of equality factors */ std::vector eq_factors; public: // iterators over equality factors - typedef std::vector::const_iterator eq_const_iterator; + typedef std::vector::const_iterator eq_const_iterator; typedef std::vector::iterator eq_iterator; /** * Default constructor */ - ConstrainedNonlinearFactorGraph(); + ConstrainedNonlinearFactorGraph() { + } /** * Copy constructor from regular NLFGs */ - ConstrainedNonlinearFactorGraph(const NonlinearFactorGraph& nfg); + ConstrainedNonlinearFactorGraph(const NonlinearFactorGraph& nfg) : + FactorGraph (nfg) { + } - virtual ~ConstrainedNonlinearFactorGraph(); + typedef typename boost::shared_ptr shared_factor; + typedef typename std::vector::const_iterator const_iterator; /** * Linearize a nonlinear graph to produce a linear graph * Note that equality constraints will just pass through */ - ConstrainedLinearFactorGraph linearize(const FGConfig& initial) const; + ConstrainedLinearFactorGraph linearize(const Config& config) const { + ConstrainedLinearFactorGraph ret; + + // linearize all nonlinear factors + // TODO uncomment + for (const_iterator factor = this->factors_.begin(); factor < this->factors_.end(); factor++) { + LinearFactor::shared_ptr lf = (*factor)->linearize(config); + ret.push_back(lf); + } + + // linearize the equality factors (set to zero because they are now in delta space) + for (eq_const_iterator e_factor = eq_factors.begin(); e_factor + < eq_factors.end(); e_factor++) { + EqualityFactor::shared_ptr eq = (*e_factor)->linearize(); + ret.push_back_eq(eq); + } + + return ret; + } + + /** + * Insert a factor into the graph + */ + void push_back(const shared_factor& f) { + FactorGraph::push_back(f); + } /** * Insert a equality factor into the graph @@ -49,10 +88,6 @@ public: eq_factors.push_back(eq); } - /** - * converts the graph to a regular nonlinear graph - removes equality constraints - */ - NonlinearFactorGraph convert() const; }; } diff --git a/cpp/EqualityFactor.cpp b/cpp/EqualityFactor.cpp index e0a0b76a6..dd0e7c073 100644 --- a/cpp/EqualityFactor.cpp +++ b/cpp/EqualityFactor.cpp @@ -42,13 +42,6 @@ bool EqualityFactor::equals(const EqualityFactor& f, double tol) const return equal_with_abs_tol(value_, f.get_value(), tol) && key_ == f.get_key(); } -bool EqualityFactor::equals(const Factor& f, double tol) const -{ - const EqualityFactor* p = dynamic_cast(&f); - if (p == NULL) return false; - return equals(f, tol); -} - string EqualityFactor::dump() const { string ret = "[" + key_ + "] " + gtsam::dump(value_); diff --git a/cpp/EqualityFactor.h b/cpp/EqualityFactor.h index 691d92f64..e1a5ba05a 100644 --- a/cpp/EqualityFactor.h +++ b/cpp/EqualityFactor.h @@ -9,11 +9,12 @@ #define EQUALITYFACTOR_H_ #include "Factor.h" +#include "FGConfig.h" #include "DeltaFunction.h" namespace gtsam { -class EqualityFactor: public Factor { +class EqualityFactor: public Factor { public: typedef boost::shared_ptr shared_ptr; @@ -54,7 +55,6 @@ public: /** * equality up to tolerance */ - bool equals(const Factor& f, double tol=1e-9) const; bool equals(const EqualityFactor& f, double tol=1e-9) const; /** diff --git a/cpp/Factor.h b/cpp/Factor.h index ac948aa51..238a79d11 100644 --- a/cpp/Factor.h +++ b/cpp/Factor.h @@ -11,10 +11,11 @@ #include #include -#include "FGConfig.h" +#include // for noncopyable namespace gtsam { + /** A combination of a key with a dimension - TODO FD: move, vector specific */ struct Variable { private: std::string key_; @@ -26,6 +27,7 @@ namespace gtsam { std::size_t dim() const { return dim_;} }; + /** A set of variables, used to eliminate linear factor factor graphs. TODO FD: move */ class VariableSet : public std::set { }; @@ -37,7 +39,13 @@ namespace gtsam { * immutable, i.e., practicing functional programming. However, this * 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 + * 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. */ + template class Factor : boost::noncopyable { public: @@ -47,7 +55,7 @@ namespace gtsam { /** * negative log probability */ - virtual double error(const FGConfig& c) const = 0; + virtual double error(const Config& c) const = 0; /** * print @@ -58,8 +66,8 @@ namespace gtsam { /** * equality up to tolerance * tricky to implement, see NonLinearFactor1 for an example - */ virtual bool equals(const Factor& f, double tol=1e-9) const = 0; + */ virtual std::string dump() const = 0; diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index ac4810041..82e2843cc 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -10,6 +10,7 @@ #pragma once +#include #include #include #include "Ordering.h" @@ -69,8 +70,8 @@ Ordering colamd(int n_col, int n_row, int nrNonZeros, const std::map -Ordering FactorGraph::getOrdering() const { +template +Ordering FactorGraph::getOrdering() const { // A factor graph is really laid out in row-major format, each factor a row // Below, we compute a symbolic matrix stored in sparse columns. diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 33aeb55b2..c16f4dc2b 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -13,18 +13,21 @@ #include #include -#include "Factor.h" -#include "FGConfig.h" - namespace gtsam { class Ordering; + class FGConfig; + class LinearFactor; + class LinearFactorGraph; + class Ordering; /** * A factor graph is a bipartite graph with factor nodes connected to variable nodes. * In this class, however, only factor nodes are kept around. + * + * Templated on the type of factors and configuration. */ - template class FactorGraph { + template class FactorGraph { public: typedef typename boost::shared_ptr shared_factor; typedef typename std::vector::iterator iterator; @@ -67,7 +70,7 @@ namespace gtsam { } /** unnormalized error */ - double error(const FGConfig& c) const { + double error(const Config& c) const { double total_error = 0.; /** iterate over all the factors_ to accumulate the log probabilities */ for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++) @@ -77,7 +80,7 @@ namespace gtsam { } /** Unnormalized probability. O(n) */ - double probPrime(const FGConfig& c) const { + double probPrime(const Config& c) const { return exp(-0.5 * error(c)); } @@ -120,3 +123,4 @@ namespace gtsam { } }; // FactorGraph } // namespace gtsam + diff --git a/cpp/LinearFactor.cpp b/cpp/LinearFactor.cpp index 43a6c1182..0bee6053b 100644 --- a/cpp/LinearFactor.cpp +++ b/cpp/LinearFactor.cpp @@ -44,15 +44,15 @@ void LinearFactor::print(const string& s) const { /* ************************************************************************* */ // Check if two linear factors are equal -bool LinearFactor::equals(const Factor& f, double tol) const { +bool LinearFactor::equals(const LinearFactor& lf, double tol) const { - const LinearFactor* lf = dynamic_cast(&f); - if (lf == NULL) return false; + //const LinearFactor* lf = dynamic_cast(&f); + //if (lf == NULL) return false; - if (empty()) return (lf->empty()); + if (empty()) return (lf.empty()); - const_iterator it1 = As.begin(), it2 = lf->As.begin(); - if(As.size() != lf->As.size()) goto fail; + const_iterator it1 = As.begin(), it2 = lf.As.begin(); + if(As.size() != lf.As.size()) goto fail; for(; it1 != As.end(); it1++, it2++){ const string& j1 = it1->first, j2 = it2->first; @@ -63,7 +63,7 @@ bool LinearFactor::equals(const Factor& f, double tol) const { goto fail; } } - if( !(::equal_with_abs_tol(b, (lf->b),tol)) ) { + if( !(::equal_with_abs_tol(b, (lf.b),tol)) ) { cout << "RHS disagree" << endl; goto fail; } @@ -72,7 +72,7 @@ bool LinearFactor::equals(const Factor& f, double tol) const { fail: // they don't match, print out and fail print(); - lf->print(); + lf.print(); return false; } diff --git a/cpp/LinearFactor.h b/cpp/LinearFactor.h index b2a9c7bfe..8d3a70888 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; @@ -100,7 +100,7 @@ public: double error(const FGConfig& c) const; /** 0.5*(A*x-b)'*(A*x-b) */ void print(const std::string& s = "") const; - bool equals(const Factor& lf, double tol = 1e-9) const; + bool equals(const LinearFactor& lf, double tol = 1e-9) const; std::string dump() const { return "";} std::size_t size() const { return As.size();} @@ -234,8 +234,7 @@ public: * @param key the key of the node to be eliminated * @return a new factor and a conditional gaussian on the eliminated variable */ - std::pair eliminate( - const std::string& key); + std::pair eliminate(const std::string& key); /** * Take the factor f, and append to current matrices. Not very general. diff --git a/cpp/LinearFactorGraph.h b/cpp/LinearFactorGraph.h index 21f749cd5..476a54d50 100644 --- a/cpp/LinearFactorGraph.h +++ b/cpp/LinearFactorGraph.h @@ -15,13 +15,21 @@ #include #include "LinearFactor.h" +//#include "Ordering.h" +#include "FGConfig.h" #include "FactorGraph.h" -#include "ChordalBayesNet.h" namespace gtsam { - /** Linear Factor Graph where all factors are Gaussians */ - class LinearFactorGraph : public FactorGraph { + class ChordalBayesNet; + + /** + * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. + * Factor == LinearFactor + * FGConfig = A configuration of vectors + * Most of the time, linear factor graphs arise by linearizing a non-linear factor graph. + */ + class LinearFactorGraph : public FactorGraph { public: /** @@ -72,13 +80,13 @@ namespace gtsam { * eliminate factor graph in place(!) in the given order, yielding * a chordal Bayes net */ - ChordalBayesNet::shared_ptr eliminate(const Ordering& ordering); + boost::shared_ptr eliminate(const Ordering& ordering); /** * Same as eliminate but allows for passing an incomplete ordering * that does not completely eliminate the graph */ - ChordalBayesNet::shared_ptr eliminate_partially(const Ordering& ordering); + boost::shared_ptr eliminate_partially(const Ordering& ordering); /** * optimize a linear factor graph diff --git a/cpp/Makefile.am b/cpp/Makefile.am index df7a287a4..f3ead9a64 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -57,7 +57,8 @@ testVector_LDADD = libgtsam.la testMatrix_LDADD = libgtsam.la # nodes -sources += FGConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp ConditionalGaussian.cpp EqualityFactor.cpp DeltaFunction.cpp +sources += FGConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp +sources += ConditionalGaussian.cpp EqualityFactor.cpp DeltaFunction.cpp check_PROGRAMS += testFGConfig testLinearFactor testConditionalGaussian testNonlinearFactor testEqualityFactor testDeltaFunction example = smallExample.cpp testFGConfig_SOURCES = testFGConfig.cpp @@ -80,8 +81,8 @@ timeLinearFactor: CXXFLAGS += -I /opt/local/include timeLinearFactor: LDFLAGS += -L.libs -lgtsam # graphs -sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp -sources += LinearFactorGraph.cpp NonlinearFactorGraph.cpp +sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp +sources += LinearFactorGraph.cpp sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp check_PROGRAMS += testChordalBayesNet testConstrainedChordalBayesNet testFactorgraph check_PROGRAMS += testLinearFactorGraph testNonlinearFactorGraph testNonlinearOptimizer @@ -146,10 +147,11 @@ testVSLAMFactor_LDADD = libgtsam.la # The header files will be installed in ~/include/gtsam headers = gtsam.h Value.h Testable.h Factor.h LinearFactorSet.h Point2Prior.h -headers += Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h +headers += Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h headers += $(sources:.cpp=.h) # templates: headers += FactorGraph.h FactorGraph-inl.h +headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h # create both dynamic and static libraries diff --git a/cpp/NonlinearFactor.cpp b/cpp/NonlinearFactor.cpp index 1dc4308c6..c4dae68af 100644 --- a/cpp/NonlinearFactor.cpp +++ b/cpp/NonlinearFactor.cpp @@ -7,105 +7,97 @@ * @author Christian Potthast * @author Frank Dellaert */ - + #include "NonlinearFactor.h" using namespace std; using namespace gtsam; -/* ************************************************************************* */ -NonlinearFactor::NonlinearFactor(const Vector& z, const double sigma) : - z_(z), sigma_(sigma) { -} - /* ************************************************************************* */ NonlinearFactor1::NonlinearFactor1(const Vector& z, const double sigma, 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); } /* ************************************************************************* */ void NonlinearFactor1::print(const string& s) const { - cout << "NonLinearFactor1 " << s << endl; + cout << "NonLinearFactor1 " << s << endl; } /* ************************************************************************* */ -LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const -{ - // get argument 1 from config - Vector x1 = c[key1_]; +LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const { + // get argument 1 from config + Vector x1 = c[key1_]; - // Jacobian A = H(x1)/sigma - Matrix A = H_(x1)/sigma_; + // Jacobian A = H(x1)/sigma + Matrix A = H_(x1) / sigma_; - // Right-hand-side b = error(c) = (z - h(x1))/sigma - Vector b = (z_ - h_(x1))/sigma_; + // Right-hand-side b = error(c) = (z - h(x1))/sigma + Vector b = (z_ - h_(x1)) / sigma_; - LinearFactor::shared_ptr p(new LinearFactor(key1_,A,b)); - return p; + LinearFactor::shared_ptr p(new LinearFactor(key1_, A, b)); + 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_) - && (key1_ == p->key1_) - && (H_ == p->H_); +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) + && (h_ == p->h_) + && (key1_== p->key1_) + && (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); +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 << "NonLinearFactor2 " << s << endl; } /* ************************************************************************* */ -LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& c) const -{ - // get arguments from config - Vector x1 = c[key1_]; - Vector x2 = c[key2_]; +LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& c) const { + // get arguments from config + Vector x1 = c[key1_]; + Vector x2 = c[key2_]; - // Jacobian A = H(x)/sigma - Matrix A1 = H1_(x1,x2)/sigma_; - Matrix A2 = H2_(x1,x2)/sigma_; + // Jacobian A = H(x)/sigma + Matrix A1 = H1_(x1, x2) / sigma_; + Matrix A2 = H2_(x1, x2) / sigma_; - // Right-hand-side b = (z - h(x))/sigma - Vector b = (z_ - h_(x1,x2))/sigma_; + // Right-hand-side b = (z - h(x))/sigma + Vector b = (z_ - h_(x1, x2)) / sigma_; - LinearFactor::shared_ptr p(new LinearFactor(key1_,A1,key2_,A2,b)); - return p; + LinearFactor::shared_ptr p(new LinearFactor(key1_, A1, key2_, A2, b)); + 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) +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) && (h_ == p->h_) && (key1_ == p->key1_) && (H2_ == p->H1_) diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h index 8374940a3..c3ef2a385 100644 --- a/cpp/NonlinearFactor.h +++ b/cpp/NonlinearFactor.h @@ -35,8 +35,12 @@ namespace gtsam { /** * 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. */ - class NonlinearFactor : public Factor + template + class NonlinearFactor : public Factor { protected: @@ -49,15 +53,21 @@ namespace gtsam { /** Default constructor, with easily identifiable bogus values */ NonlinearFactor():z_(Vector_(2,888.0,999.0)),sigma_(0.1234567) {} - /** Constructor */ - NonlinearFactor(const Vector& z, // the measurement - const double sigma); // the variance + /** + * Constructor + * @param z the measurement + * @param sigma the standard deviation + */ + NonlinearFactor(const Vector& z, const double sigma) { + z_ = z; + sigma_ = sigma; + } /** Vector of errors */ - virtual Vector error_vector(const FGConfig& c) const = 0; + virtual Vector error_vector(const Config& c) const = 0; /** linearize to a LinearFactor */ - virtual boost::shared_ptr linearize(const FGConfig& c) const = 0; + virtual boost::shared_ptr linearize(const Config& c) const = 0; /** print to cout */ virtual void print(const std::string& s = "") const = 0; @@ -68,7 +78,7 @@ namespace gtsam { std::list keys() const {return keys_;} /** calculate the error of the factor */ - double error(const FGConfig& c) const { + double error(const Config& c) const { Vector e = error_vector(c) / sigma_; return 0.5 * inner_prod(trans(e),e); }; @@ -90,7 +100,6 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - // ar & boost::serialization::base_object(*this); // TODO: needed ? ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(sigma_); ar & BOOST_SERIALIZATION_NVP(keys_); @@ -104,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_; @@ -124,7 +133,7 @@ namespace gtsam { void print(const std::string& s = "") const; /** error function */ - inline Vector error_vector(const FGConfig& c) const { + inline Vector error_vector(const FGConfig& c) const { return z_ - h_(c[key1_]); } @@ -132,7 +141,7 @@ namespace gtsam { boost::shared_ptr linearize(const FGConfig& c) const; /** Check if two factors are equal */ - bool equals(const Factor& f, double tol=1e-9) const; + bool equals(const NonlinearFactor& f, double tol=1e-9) const; std::string dump() const {return "";} }; @@ -142,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: @@ -167,7 +176,7 @@ namespace gtsam { void print(const std::string& s = "") const; /** error function */ - inline Vector error_vector(const FGConfig& c) const { + inline Vector error_vector(const FGConfig& c) const { return z_ - h_(c[key1_], c[key2_]); } @@ -175,7 +184,7 @@ namespace gtsam { boost::shared_ptr linearize(const FGConfig& c) const; /** Check if two factors are equal */ - bool equals(const Factor& 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-inl.h b/cpp/NonlinearFactorGraph-inl.h new file mode 100644 index 000000000..d91504313 --- /dev/null +++ b/cpp/NonlinearFactorGraph-inl.h @@ -0,0 +1,39 @@ +/** + * @file NonlinearFactorGraph-inl.h + * @brief Factor Graph Consisting of non-linear factors + * @author Frank Dellaert + * @author Carlos Nieto + * @author Christian Potthast + */ + +#pragma once + +#include "LinearFactorGraph.h" +#include "NonlinearFactorGraph.h" + +namespace gtsam { + +/* ************************************************************************* */ +template +LinearFactorGraph NonlinearFactorGraph::linearize(const Config& config) const { + // TODO speed up the function either by returning a pointer or by + // returning the linearisation as a second argument and returning + // the reference + + // create an empty linear FG + LinearFactorGraph linearFG; + + typedef typename FactorGraph ,Config>:: const_iterator const_iterator; + // linearize all factors + for (const_iterator factor = this->factors_.begin(); factor + < this->factors_.end(); factor++) { + boost::shared_ptr lf = (*factor)->linearize(config); + linearFG.push_back(lf); + } + + return linearFG; +} + +/* ************************************************************************* */ + +} diff --git a/cpp/NonlinearFactorGraph.cpp b/cpp/NonlinearFactorGraph.cpp deleted file mode 100644 index 148860009..000000000 --- a/cpp/NonlinearFactorGraph.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/** - * @file NonlinearFactorGraph.cpp - * @brief Factor Graph Constsiting of non-linear factors - * @brief nonlinearFactorGraph - * @author Frank Dellaert - * @author Carlos Nieto - * @author Christian Potthast - */ - -#include -#include -#include -#include -#include "NonlinearFactorGraph.h" - -using namespace std; -namespace gtsam { - - /* ************************************************************************* */ - LinearFactorGraph NonlinearFactorGraph::linearize(const FGConfig& config) const { - // TODO speed up the function either by returning a pointer or by - // returning the linearisation as a second argument and returning - // the reference - - // create an empty linear FG - LinearFactorGraph linearFG; - - // linearize all factors - for (const_iterator factor = factors_.begin(); factor < factors_.end(); factor++) { - LinearFactor::shared_ptr lf = (*factor)->linearize(config); - linearFG.push_back(lf); - } - - return linearFG; - } - - /* ************************************************************************* */ - double calculate_error(const NonlinearFactorGraph& fg, - const FGConfig& config, int verbosity) { - double newError = fg.error(config); - if (verbosity >= 1) - cout << "error: " << newError << endl; - return newError; - } - - /* ************************************************************************* */ - bool check_convergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double currentError, double newError, - int verbosity) { - // check if diverges - double absoluteDecrease = currentError - newError; - if (verbosity >= 2) - cout << "absoluteDecrease: " << absoluteDecrease << endl; - if (absoluteDecrease < 0) - throw overflow_error( - "NonlinearFactorGraph::optimize: error increased, diverges."); - - // calculate relative error decrease and update currentError - double relativeDecrease = absoluteDecrease / currentError; - if (verbosity >= 2) - cout << "relativeDecrease: " << relativeDecrease << endl; - bool converged = (relativeDecrease < relativeErrorTreshold) - || (absoluteDecrease < absoluteErrorTreshold); - if (verbosity >= 1 && converged) - cout << "converged" << endl; - return converged; - } - - /* ************************************************************************* */ - bool NonlinearFactorGraph::check_convergence(const FGConfig& config1, - const FGConfig& config2, double relativeErrorTreshold, - double absoluteErrorTreshold, int verbosity) const { - double currentError = calculate_error(*this, config1, verbosity); - double newError = calculate_error(*this, config2, verbosity); - return gtsam::check_convergence(relativeErrorTreshold, - absoluteErrorTreshold, currentError, newError, verbosity); - - } - -/* ************************************************************************* */ - -} // namespace gtsam diff --git a/cpp/NonlinearFactorGraph.h b/cpp/NonlinearFactorGraph.h index b118b2c31..11bb5c5f7 100644 --- a/cpp/NonlinearFactorGraph.h +++ b/cpp/NonlinearFactorGraph.h @@ -10,65 +10,29 @@ #pragma once -#include -#include -#include "FactorGraph.h" #include "NonlinearFactor.h" -#include "LinearFactorGraph.h" -#include "ChordalBayesNet.h" +#include "FactorGraph.h" namespace gtsam { -typedef FactorGraph BaseFactorGraph; - -/** Factor Graph consisting of non-linear factors */ -class NonlinearFactorGraph : public BaseFactorGraph -{ -public: // these you will probably want to use /** - * linearize a non linear factor - */ - LinearFactorGraph linearize(const FGConfig& config) const; + * A non-linear factor graph is templated on a configuration, but the factor type + * is fixed as a NonLinearFactor. The configurations are typically (in SAM) more general + * 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 + */ + template + class NonlinearFactorGraph: public FactorGraph ,Config> { - /** - * Given two configs, check whether the error of config2 is - * different enough from the error for config1, or whether config1 - * is essentially optimal - * - * @param config1 Reference to first configuration - * @param config2 Reference to second configuration - * @param relativeErrorTreshold - * @param absoluteErrorTreshold - * @param verbosity Integer specifying how much output to provide - */ - bool check_convergence(const FGConfig& config1, - const FGConfig& config2, - double relativeErrorTreshold, double absoluteErrorTreshold, - int verbosity = 0) const; + public: -private: + /** + * linearize a nonlinear factor graph + */ + LinearFactorGraph linearize(const Config& config) const; - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - // do not use BOOST_SERIALIZATION_NVP for this name-value-pair ! It will crash. - ar & boost::serialization::make_nvp("BaseFactorGraph", - boost::serialization::base_object(*this)); - } + }; -}; - -/** - * Check convergence - */ -bool check_convergence (double relativeErrorTreshold, - double absoluteErrorTreshold, - double currentError, double newError, - int verbosity); - -/** - * calculate error for current configuration - */ -double calculate_error (const NonlinearFactorGraph& fg, const FGConfig& config, int verbosity); -} +} // namespace diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index f1628f43e..90dee895a 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -17,6 +17,29 @@ using namespace std; namespace gtsam { + /* ************************************************************************* */ + bool check_convergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double currentError, double newError, + int verbosity) { + // check if diverges + double absoluteDecrease = currentError - newError; + if (verbosity >= 2) + cout << "absoluteDecrease: " << absoluteDecrease << endl; + if (absoluteDecrease < 0) + throw overflow_error( + "NonlinearFactorGraph::optimize: error increased, diverges."); + + // calculate relative error decrease and update currentError + double relativeDecrease = absoluteDecrease / currentError; + if (verbosity >= 2) + cout << "relativeDecrease: " << relativeDecrease << endl; + bool converged = (relativeDecrease < relativeErrorTreshold) + || (absoluteDecrease < absoluteErrorTreshold); + if (verbosity >= 1 && converged) + cout << "converged" << endl; + return converged; + } + /* ************************************************************************* */ // Constructors /* ************************************************************************* */ diff --git a/cpp/NonlinearOptimizer.h b/cpp/NonlinearOptimizer.h index 6d5beaebd..f6fe2b682 100644 --- a/cpp/NonlinearOptimizer.h +++ b/cpp/NonlinearOptimizer.h @@ -142,6 +142,14 @@ namespace gtsam { }; -} + /** + * Check convergence + */ + bool check_convergence (double relativeErrorTreshold, + double absoluteErrorTreshold, + double currentError, double newError, + int verbosity); + +} // gtsam #endif /* NONLINEAROPTIMIZER_H_ */ diff --git a/cpp/VSLAMFactor.cpp b/cpp/VSLAMFactor.cpp index dba27a165..1d7002306 100644 --- a/cpp/VSLAMFactor.cpp +++ b/cpp/VSLAMFactor.cpp @@ -12,7 +12,7 @@ using namespace gtsam; /* ************************************************************************* */ VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K) - : NonlinearFactor(z, sigma) + : NonlinearFactor(z, sigma) { cameraFrameNumber_ = cn; landmarkNumber_ = ln; @@ -67,7 +67,7 @@ LinearFactor::shared_ptr VSLAMFactor::linearize(const FGConfig& c) const } /* ************************************************************************* */ -bool VSLAMFactor::equals(const Factor& f, double tol) const { +bool VSLAMFactor::equals(const NonlinearFactor& f, double tol) const { const VSLAMFactor* p = dynamic_cast(&f); if (p == NULL) goto fail; if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail; @@ -88,7 +88,7 @@ string VSLAMFactor::dump() const Vector z = measurement(); char buffer[200]; buffer[0] = 0; - sprintf(buffer, "1 %d %d %f %d", i, j , sigma(), z.size()); + sprintf(buffer, "1 %d %d %f %d", i, j , sigma(), (int)z.size()); for(size_t i = 0; i < z.size(); i++) sprintf(buffer, "%s %f", buffer, z(i)); sprintf(buffer, "%s %s", buffer, K_.dump().c_str()); diff --git a/cpp/VSLAMFactor.h b/cpp/VSLAMFactor.h index ae281fd63..f0a2a115d 100644 --- a/cpp/VSLAMFactor.h +++ b/cpp/VSLAMFactor.h @@ -8,20 +8,23 @@ #include "NonlinearFactor.h" #include "LinearFactor.h" +#include "FGConfig.h" #include "Cal3_S2.h" #include "Pose3.h" +namespace gtsam { + /** * Non-linear factor for a constraint derived from a 2D measurement, * i.e. the main building block for visual SLAM. */ -class VSLAMFactor : public gtsam::NonlinearFactor +class VSLAMFactor : public NonlinearFactor { private: int cameraFrameNumber_, landmarkNumber_; std::string cameraFrameName_, landmarkName_; - gtsam::Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this. + Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this. public: @@ -35,7 +38,7 @@ class VSLAMFactor : public gtsam::NonlinearFactor * @param landmarkNumber is the index of the landmark * @param K the constant calibration */ - VSLAMFactor(const Vector& z, double sigma, int cameraFrameNumber, int landmarkNumber, const gtsam::Cal3_S2& K); + VSLAMFactor(const Vector& z, double sigma, int cameraFrameNumber, int landmarkNumber, const Cal3_S2& K); /** @@ -47,17 +50,17 @@ class VSLAMFactor : public gtsam::NonlinearFactor /** * calculate the error of the factor */ - Vector error_vector(const gtsam::FGConfig&) const; + Vector error_vector(const FGConfig&) const; /** * linerarization */ - gtsam::LinearFactor::shared_ptr linearize(const gtsam::FGConfig&) const; + LinearFactor::shared_ptr linearize(const FGConfig&) const; /** * equals */ - bool equals(const gtsam::Factor&, double tol=1e-9) const; + bool equals(const NonlinearFactor&, double tol=1e-9) const; int getCameraFrameNumber() const { return cameraFrameNumber_; } int getLandmarkNumber() const { return landmarkNumber_; } @@ -68,3 +71,4 @@ class VSLAMFactor : public gtsam::NonlinearFactor std::string dump() const; }; +} diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index 8316f4efd..5c2a32830 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -27,12 +27,12 @@ using namespace std; namespace gtsam { -typedef boost::shared_ptr shared; +typedef boost::shared_ptr > shared; /* ************************************************************************* */ -boost::shared_ptr sharedNonlinearFactorGraph() { +boost::shared_ptr sharedNonlinearFactorGraph() { // Create - boost::shared_ptr nlfg(new NonlinearFactorGraph); + boost::shared_ptr nlfg(new ExampleNonlinearFactorGraph); // prior on x1 double sigma1=0.1; @@ -61,7 +61,7 @@ boost::shared_ptr sharedNonlinearFactorGraph() { return nlfg; } -NonlinearFactorGraph createNonlinearFactorGraph() { +ExampleNonlinearFactorGraph createNonlinearFactorGraph() { return *sharedNonlinearFactorGraph(); } @@ -92,24 +92,23 @@ ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph() } /* ************************************************************************* */ -ConstrainedNonlinearFactorGraph createConstrainedNonlinearFactorGraph() -{ - ConstrainedNonlinearFactorGraph graph; - FGConfig c = createConstrainedConfig(); + ConstrainedNonlinearFactorGraph , FGConfig> createConstrainedNonlinearFactorGraph() { + ConstrainedNonlinearFactorGraph , FGConfig> graph; + FGConfig c = createConstrainedConfig(); - // equality constraint for initial pose - EqualityFactor::shared_ptr f1(new EqualityFactor(c["x0"], "x0")); - graph.push_back_eq(f1); + // equality constraint for initial pose + EqualityFactor::shared_ptr f1(new EqualityFactor(c["x0"], "x0")); + graph.push_back_eq(f1); - // odometry between x0 and x1 - double sigma=0.1; - shared f2(new Simulated2DOdometry(c["x1"]-c["x0"], sigma, "x0", "x1")); - graph.push_back(f2); + // odometry between x0 and x1 + double sigma = 0.1; + shared f2(new Simulated2DOdometry(c["x1"] - c["x0"], sigma, "x0", "x1")); + graph.push_back(f2); // TODO - return graph; -} + return graph; + } -/* ************************************************************************* */ + /* ************************************************************************* */ FGConfig createConfig() { Vector v_x1(2); v_x1(0) = 0.; v_x1(1) = 0.; @@ -328,9 +327,9 @@ namespace optimize { } /* ************************************************************************* */ -boost::shared_ptr sharedReallyNonlinearFactorGraph() +boost::shared_ptr sharedReallyNonlinearFactorGraph() { - boost::shared_ptr fg(new NonlinearFactorGraph); + boost::shared_ptr fg(new ExampleNonlinearFactorGraph); Vector z = Vector_(2,1.0,0.0); double sigma = 0.1; boost::shared_ptr @@ -339,7 +338,7 @@ boost::shared_ptr sharedReallyNonlinearFactorGraph() return fg; } -NonlinearFactorGraph createReallyNonlinearFactorGraph() { +ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() { return *sharedReallyNonlinearFactorGraph(); } diff --git a/cpp/smallExample.h b/cpp/smallExample.h index 0a4572a84..3a6604ea6 100644 --- a/cpp/smallExample.h +++ b/cpp/smallExample.h @@ -21,11 +21,13 @@ namespace gtsam { + typedef NonlinearFactorGraph ExampleNonlinearFactorGraph; + /** * Create small example for non-linear factor graph */ - boost::shared_ptr sharedNonlinearFactorGraph(); - NonlinearFactorGraph createNonlinearFactorGraph(); + boost::shared_ptr sharedNonlinearFactorGraph(); + ExampleNonlinearFactorGraph createNonlinearFactorGraph(); /** * Create small example constrained factor graph @@ -35,7 +37,8 @@ namespace gtsam { /** * Create small example constrained nonlinear factor graph */ - ConstrainedNonlinearFactorGraph createConstrainedNonlinearFactorGraph(); + ConstrainedNonlinearFactorGraph,FGConfig> + createConstrainedNonlinearFactorGraph(); /** * Create configuration to go with it @@ -84,8 +87,8 @@ namespace gtsam { /** * Create really non-linear factor graph (cos/sin) */ - boost::shared_ptr sharedReallyNonlinearFactorGraph(); - NonlinearFactorGraph createReallyNonlinearFactorGraph(); + boost::shared_ptr sharedReallyNonlinearFactorGraph(); + ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph(); /** * Create a noisy configuration for linearization diff --git a/cpp/testConstrainedNonlinearFactorGraph.cpp b/cpp/testConstrainedNonlinearFactorGraph.cpp index eb0e67e16..cc73242e5 100644 --- a/cpp/testConstrainedNonlinearFactorGraph.cpp +++ b/cpp/testConstrainedNonlinearFactorGraph.cpp @@ -12,26 +12,27 @@ using namespace gtsam; -typedef boost::shared_ptr shared; +typedef boost::shared_ptr > shared; +typedef ConstrainedNonlinearFactorGraph,FGConfig> TestGraph; -TEST( ConstrainedNonlinearFactorGraph, equals ) +TEST( TestGraph, equals ) { - ConstrainedNonlinearFactorGraph fg = createConstrainedNonlinearFactorGraph(); - ConstrainedNonlinearFactorGraph fg2 = createConstrainedNonlinearFactorGraph(); + TestGraph fg = createConstrainedNonlinearFactorGraph(); + TestGraph fg2 = createConstrainedNonlinearFactorGraph(); CHECK( fg.equals(fg2) ); } -TEST( ConstrainedNonlinearFactorGraph, copy ) +TEST( TestGraph, copy ) { - NonlinearFactorGraph nfg = createNonlinearFactorGraph(); - ConstrainedNonlinearFactorGraph actual(nfg); + ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); + TestGraph actual(nfg); shared f1 = nfg[0]; shared f2 = nfg[1]; shared f3 = nfg[2]; shared f4 = nfg[3]; - ConstrainedNonlinearFactorGraph expected; + TestGraph expected; expected.push_back(f1); expected.push_back(f2); expected.push_back(f3); @@ -40,11 +41,11 @@ TEST( ConstrainedNonlinearFactorGraph, copy ) CHECK(actual.equals(expected)); } -TEST( ConstrainedNonlinearFactorGraph, baseline ) +TEST( TestGraph, baseline ) { // use existing examples - NonlinearFactorGraph nfg = createNonlinearFactorGraph(); - ConstrainedNonlinearFactorGraph cfg(nfg); + ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); + TestGraph cfg(nfg); FGConfig initial = createNoisyConfig(); ConstrainedLinearFactorGraph linearized = cfg.linearize(initial); @@ -54,17 +55,19 @@ TEST( ConstrainedNonlinearFactorGraph, baseline ) CHECK(expected.equals(linearized)); } -TEST( ConstrainedNonlinearFactorGraph, convert ) +/* +TEST( TestGraph, convert ) { - NonlinearFactorGraph expected = createNonlinearFactorGraph(); - ConstrainedNonlinearFactorGraph cfg(expected); - NonlinearFactorGraph actual = cfg.convert(); + ExampleNonlinearFactorGraph expected = createNonlinearFactorGraph(); + TestGraph cfg(expected); + ExampleNonlinearFactorGraph actual = cfg.convert(); CHECK(actual.equals(expected)); } +*/ -TEST( ConstrainedNonlinearFactorGraph, linearize_and_solve ) +TEST( TestGraph, linearize_and_solve ) { - ConstrainedNonlinearFactorGraph nfg = createConstrainedNonlinearFactorGraph(); + TestGraph nfg = createConstrainedNonlinearFactorGraph(); FGConfig lin = createConstrainedLinConfig(); ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin); FGConfig actual = actual_lfg.optimize(actual_lfg.getOrdering()); diff --git a/cpp/testNonlinearFactor.cpp b/cpp/testNonlinearFactor.cpp index 17a222adc..a057f85b8 100644 --- a/cpp/testNonlinearFactor.cpp +++ b/cpp/testNonlinearFactor.cpp @@ -17,17 +17,19 @@ using namespace std; using namespace gtsam; +typedef boost::shared_ptr > shared_nlf; + /* ************************************************************************* */ TEST( NonLinearFactor, NonlinearFactor ) { // create a non linear factor graph - NonlinearFactorGraph fg = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); // create a configuration for the non linear factor graph FGConfig cfg = createNoisyConfig(); // get the factor "f1" from the factor graph - boost::shared_ptr factor = fg[0]; + shared_nlf factor = fg[0]; // calculate the error_vector from the factor "f1" Vector actual_e = factor->error_vector(cfg); @@ -48,7 +50,7 @@ TEST( NonLinearFactor, NonlinearFactor ) TEST( NonLinearFactor, linearize_f1 ) { // Grab a non-linear factor - NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[0]); @@ -66,7 +68,7 @@ TEST( NonLinearFactor, linearize_f1 ) TEST( NonLinearFactor, linearize_f2 ) { // Grab a non-linear factor - NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[1]); @@ -84,7 +86,7 @@ TEST( NonLinearFactor, linearize_f2 ) TEST( NonLinearFactor, linearize_f3 ) { // Grab a non-linear factor - NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[2]); @@ -102,7 +104,7 @@ TEST( NonLinearFactor, linearize_f3 ) TEST( NonLinearFactor, linearize_f4 ) { // Grab a non-linear factor - NonlinearFactorGraph nfg = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[3]); @@ -120,15 +122,15 @@ TEST( NonLinearFactor, linearize_f4 ) TEST( NonLinearFactor, size ) { // create a non linear factor graph - NonlinearFactorGraph fg = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); // create a configuration for the non linear factor graph FGConfig cfg = createNoisyConfig(); // get some factors from the graph - boost::shared_ptr factor1 = fg[0]; - boost::shared_ptr factor2 = fg[1]; - boost::shared_ptr factor3 = fg[2]; + shared_nlf factor1 = fg[0]; + shared_nlf factor2 = fg[1]; + shared_nlf factor3 = fg[2]; CHECK(factor1->size() == 1); CHECK(factor2->size() == 2); diff --git a/cpp/testNonlinearFactorGraph.cpp b/cpp/testNonlinearFactorGraph.cpp index 60393b357..251540221 100644 --- a/cpp/testNonlinearFactorGraph.cpp +++ b/cpp/testNonlinearFactorGraph.cpp @@ -14,22 +14,24 @@ using namespace std; #include "Matrix.h" #include "smallExample.h" +#include "FactorGraph-inl.h" +#include "NonlinearFactorGraph-inl.h" using namespace gtsam; /* ************************************************************************* */ -TEST( NonlinearFactorGraph, equals ) +TEST( ExampleNonlinearFactorGraph, equals ) { - NonlinearFactorGraph fg = createNonlinearFactorGraph(); - NonlinearFactorGraph fg2 = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph fg2 = createNonlinearFactorGraph(); CHECK( fg.equals(fg2) ); } /* ************************************************************************* */ -TEST( NonlinearFactorGraph, error ) +TEST( ExampleNonlinearFactorGraph, error ) { - NonlinearFactorGraph fg = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); FGConfig c1 = createConfig(); double actual1 = fg.error(c1); @@ -41,9 +43,9 @@ TEST( NonlinearFactorGraph, error ) } /* ************************************************************************* */ -TEST( NonlinearFactorGraph, probPrime ) +TEST( ExampleNonlinearFactorGraph, probPrime ) { - NonlinearFactorGraph fg = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); FGConfig cfg = createConfig(); // evaluate the probability of the factor graph @@ -53,9 +55,9 @@ TEST( NonlinearFactorGraph, probPrime ) } /* ************************************************************************* */ -TEST( NonlinearFactorGraph, linearize ) +TEST( ExampleNonlinearFactorGraph, linearize ) { - NonlinearFactorGraph fg = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); FGConfig initial = createNoisyConfig(); LinearFactorGraph linearized = fg.linearize(initial); LinearFactorGraph expected = createLinearFactorGraph(); diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index 6764ab791..a72e3530a 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -12,16 +12,17 @@ using namespace std; #include "Matrix.h" #include "smallExample.h" // template definitions +#include "NonlinearFactorGraph-inl.h" #include "NonlinearOptimizer-inl.h" using namespace gtsam; -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ -TEST( NonlinearFactorGraph, delta ) +TEST( NonlinearOptimizer, delta ) { - NonlinearFactorGraph fg = createNonlinearFactorGraph(); + ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); Optimizer::shared_config initial = sharedNoisyConfig(); // Expected configuration is the difference between the noisy config @@ -69,10 +70,10 @@ TEST( NonlinearFactorGraph, delta ) } /* ************************************************************************* */ -TEST( NonlinearFactorGraph, iterateLM ) +TEST( NonlinearOptimizer, iterateLM ) { // really non-linear factor graph - NonlinearFactorGraph fg = createReallyNonlinearFactorGraph(); + ExampleNonlinearFactorGraph fg = createReallyNonlinearFactorGraph(); // config far from minimum Vector x0 = Vector_(1, 3.0); @@ -92,13 +93,13 @@ TEST( NonlinearFactorGraph, iterateLM ) // LM iterate with lambda 0 should be the same Optimizer iterated2 = optimizer.iterateLM(); - CHECK(assert_equal(*(iterated1.config()), *(iterated2.config()), 1e-9)); + CHECK(assert_equal(*iterated1.config(), *iterated2.config(), 1e-9)); } /* ************************************************************************* */ -TEST( NonlinearFactorGraph, optimize ) +TEST( NonlinearOptimizer, optimize ) { - NonlinearFactorGraph fg = createReallyNonlinearFactorGraph(); + ExampleNonlinearFactorGraph fg = createReallyNonlinearFactorGraph(); // test error at minimum Vector xstar = Vector_(1, 0.0);