From 68e20eec2c21ebc1b1746955924b3fdc3192aae7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 13 Sep 2009 04:13:03 +0000 Subject: [PATCH] 2 BIG changes: (1) FactorGraph and NonlinearOptimizer now no longer have a .cpp file, but a -inl.h file as in [http://google-styleguide.googlecode.com/svn/trunk/cppguide.xml Google's C++ Style Guide]. This means if you expect to instantiate one of the functions in a cpp file, you have to include the -inl.h file. (1) getOrdering is now in FactorGraph, and the non-linear version does *not* take a config anymore. Long version: I made this change because colamd works on the graph structure alone, and should not depend on the type of graph. Instead, because getOrdering happened to implemented in LinearFactorGraph first, the non-linear version converted to a linear factor graph (at the cost of an unnecessary linearization), and then threw all that away to call colamd. To implement this in a key-neutral way (a hidden agenda), i had to modify the keys_ type to a list, so a lot of changes resulted from that. --- .cproject | 22 +++-- cpp/ConstrainedLinearFactorGraph.cpp | 13 +-- cpp/DeltaFunction.cpp | 10 +- cpp/DeltaFunction.h | 8 +- cpp/EqualityFactor.cpp | 18 ++-- cpp/EqualityFactor.h | 13 ++- cpp/Factor.h | 6 ++ cpp/FactorGraph-inl.h | 97 +++++++++++++++++++ cpp/FactorGraph.cpp | 9 -- cpp/FactorGraph.h | 11 ++- cpp/LinearFactor.cpp | 6 +- cpp/LinearFactor.h | 2 +- cpp/LinearFactorGraph.cpp | 73 -------------- cpp/LinearFactorGraph.h | 6 -- cpp/Makefile.am | 8 +- cpp/NonlinearFactor.cpp | 7 +- cpp/NonlinearFactor.h | 36 ++++--- cpp/NonlinearFactorGraph.cpp | 7 -- cpp/NonlinearFactorGraph.h | 5 - ...Optimizer.cpp => NonlinearOptimizer-inl.h} | 6 +- cpp/Point2Prior.h | 10 +- cpp/VSLAMFactor.cpp | 5 +- cpp/smallExample.cpp | 1 + cpp/testLinearFactor.cpp | 6 +- cpp/testLinearFactorGraph.cpp | 1 + cpp/testNonlinearOptimizer.cpp | 3 +- 26 files changed, 213 insertions(+), 176 deletions(-) create mode 100644 cpp/FactorGraph-inl.h delete mode 100644 cpp/FactorGraph.cpp rename cpp/{NonlinearOptimizer.cpp => NonlinearOptimizer-inl.h} (97%) diff --git a/.cproject b/.cproject index 10af82bcb..fe4d4d232 100644 --- a/.cproject +++ b/.cproject @@ -300,6 +300,7 @@ make + check true true @@ -307,7 +308,6 @@ make - testSimpleCamera.run true true @@ -315,7 +315,6 @@ make - testCal3_S2.run true true @@ -323,6 +322,7 @@ make + testVSLAMFactor.run true true @@ -330,7 +330,6 @@ make - testCalibratedCamera.run true true @@ -338,6 +337,7 @@ make + testConditionalGaussian.run true true @@ -345,7 +345,6 @@ make - testPose2.run true true @@ -353,6 +352,7 @@ make + testFGConfig.run true true @@ -360,7 +360,6 @@ make - testRot3.run true true @@ -368,6 +367,7 @@ make + testNonlinearOptimizer.run true true @@ -375,14 +375,22 @@ make - testLinearFactor.run true true true + +make + +testLinearFactorGraph.run +true +true +true + make + install true true @@ -390,6 +398,7 @@ make + clean true true @@ -397,6 +406,7 @@ make + check true true diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp index 25e36f413..54e1de877 100644 --- a/cpp/ConstrainedLinearFactorGraph.cpp +++ b/cpp/ConstrainedLinearFactorGraph.cpp @@ -7,6 +7,7 @@ #include #include "ConstrainedLinearFactorGraph.h" +#include "FactorGraph-inl.h" // for getOrdering using namespace std; @@ -181,14 +182,10 @@ void ConstrainedLinearFactorGraph::eq_combine_and_eliminate( lf->set_b(b); string j; Matrix A; LinearFactor::const_iterator it = joint_factor.begin(); - for (; it != joint_factor.end(); it++) - { + for (; it != joint_factor.end(); it++) { j = it->first; A = it->second; - if (j != key) - { - lf->insert(j, A); - } + if (j != key) lf->insert(j, A); } // insert factor @@ -199,9 +196,7 @@ Ordering ConstrainedLinearFactorGraph::getOrdering() const { Ordering ord = LinearFactorGraph::getOrdering(); BOOST_FOREACH(EqualityFactor::shared_ptr e, eq_factors) - { ord.push_back(e->get_key()); - } return ord; } @@ -209,9 +204,7 @@ LinearFactorGraph ConstrainedLinearFactorGraph::convert() const { LinearFactorGraph ret; BOOST_FOREACH(LinearFactor::shared_ptr f, factors_) - { ret.push_back(f); - } return ret; } diff --git a/cpp/DeltaFunction.cpp b/cpp/DeltaFunction.cpp index 8f5d07341..233595083 100644 --- a/cpp/DeltaFunction.cpp +++ b/cpp/DeltaFunction.cpp @@ -18,12 +18,12 @@ DeltaFunction::DeltaFunction() { } DeltaFunction::DeltaFunction(const Vector& v, const std::string& id) -: value(v), key(id) +: value_(v), key_(id) { } DeltaFunction::DeltaFunction(const DeltaFunction& df) -: boost::noncopyable(), value(df.value), key(df.key) +: boost::noncopyable(), value_(df.value_), key_(df.key_) { } @@ -33,13 +33,13 @@ DeltaFunction::~DeltaFunction() { bool DeltaFunction::equals(const DeltaFunction &df) const { - return equal_with_abs_tol(value, df.value) && key == df.key; + return equal_with_abs_tol(value_, df.value_) && key_ == df.key_; } void DeltaFunction::print() const { - cout << "DeltaFunction: [" << key << "]"; - gtsam::print(value); + cout << "DeltaFunction: [" << key_ << "]"; + gtsam::print(value_); cout << endl; } diff --git a/cpp/DeltaFunction.h b/cpp/DeltaFunction.h index 6e5d25588..12abd6129 100644 --- a/cpp/DeltaFunction.h +++ b/cpp/DeltaFunction.h @@ -17,8 +17,8 @@ namespace gtsam { class DeltaFunction : boost::noncopyable { protected: - Vector value; /// location of the delta function - std::string key; /// id of node with delta function + Vector value_; /// location of the delta function + std::string key_; /// id of node with delta function public: typedef boost::shared_ptr shared_ptr; @@ -43,8 +43,8 @@ public: /** * basic get functions */ - Vector get_value() const {return value;} - std::string get_key() const {return key;} + Vector get_value() const {return value_;} + std::string get_key() const {return key_;} /** equals function */ diff --git a/cpp/EqualityFactor.cpp b/cpp/EqualityFactor.cpp index 66c8e2d64..e0a0b76a6 100644 --- a/cpp/EqualityFactor.cpp +++ b/cpp/EqualityFactor.cpp @@ -12,15 +12,21 @@ namespace gtsam { using namespace std; EqualityFactor::EqualityFactor() -: key(""), value(Vector(0)) +: key_(""), value_(Vector(0)) { } EqualityFactor::EqualityFactor(const Vector& constraint, const std::string& id) -: key(id), value(constraint) +: key_(id), value_(constraint) { } +list EqualityFactor::keys() const { + list keys; + keys.push_back(key_); + return keys; +} + double EqualityFactor::error(const FGConfig& c) const { return 0.0; @@ -33,7 +39,7 @@ void EqualityFactor::print(const string& s) const bool EqualityFactor::equals(const EqualityFactor& f, double tol) const { - return equal_with_abs_tol(value, f.get_value(), tol) && key == f.get_key(); + return equal_with_abs_tol(value_, f.get_value(), tol) && key_ == f.get_key(); } bool EqualityFactor::equals(const Factor& f, double tol) const @@ -45,19 +51,19 @@ bool EqualityFactor::equals(const Factor& f, double tol) const string EqualityFactor::dump() const { - string ret = "[" + key + "] " + gtsam::dump(value); + string ret = "[" + key_ + "] " + gtsam::dump(value_); return ret; } DeltaFunction::shared_ptr EqualityFactor::getDeltaFunction() const { - DeltaFunction::shared_ptr ret(new DeltaFunction(value, key)); + DeltaFunction::shared_ptr ret(new DeltaFunction(value_, key_)); return ret; } EqualityFactor::shared_ptr EqualityFactor::linearize() const { - EqualityFactor::shared_ptr ret(new EqualityFactor(zero(value.size()), key)); + EqualityFactor::shared_ptr ret(new EqualityFactor(zero(value_.size()), key_)); return ret; } diff --git a/cpp/EqualityFactor.h b/cpp/EqualityFactor.h index f5d68d7ff..691d92f64 100644 --- a/cpp/EqualityFactor.h +++ b/cpp/EqualityFactor.h @@ -19,8 +19,8 @@ public: protected: - Vector value; /// forces a variable be equal to this value - std::string key; /// name of variable factor is attached to + Vector value_; /// forces a variable be equal to this value + std::string key_; /// name of variable factor is attached to public: /** @@ -68,8 +68,13 @@ public: std::string dump() const; // get functions - std::string get_key() const {return key;} - Vector get_value() const {return value;} + std::string get_key() const {return key_;} + Vector get_value() const {return value_;} + + /** + * return keys in preferred order + */ + std::list keys() const; /** * @return the number of nodes the factor connects diff --git a/cpp/Factor.h b/cpp/Factor.h index 621428eb3..ac948aa51 100644 --- a/cpp/Factor.h +++ b/cpp/Factor.h @@ -10,6 +10,7 @@ #pragma once #include +#include #include "FGConfig.h" namespace gtsam { @@ -61,6 +62,11 @@ namespace gtsam { virtual bool equals(const Factor& f, double tol=1e-9) const = 0; virtual std::string dump() const = 0; + + /** + * return keys in preferred order + */ + virtual std::list keys() const = 0; /** * @return the number of nodes the factor connects diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h new file mode 100644 index 000000000..ac4810041 --- /dev/null +++ b/cpp/FactorGraph-inl.h @@ -0,0 +1,97 @@ +/** + * @file FactorGraph-inl.h + * This is a template definition file, include it where needed (only!) + * so that the appropriate code is generated and link errors avoided. + * @brief Factor Graph Base Class + * @author Carlos Nieto + * @author Frank Dellaert + * @author Alireza Fathi + */ + +#pragma once + +#include +#include +#include "Ordering.h" +#include "FactorGraph.h" + +#include + +namespace gtsam { + +/* ************************************************************************* */ + +/** + * Call colamd given a column-major symbolic matrix A + * @param n_col colamd arg 1: number of rows in A + * @param n_row colamd arg 2: number of columns in A + * @param nrNonZeros number of non-zero entries in A + * @param columns map from keys to a sparse column of non-zero row indices + */ +template +Ordering colamd(int n_col, int n_row, int nrNonZeros, const std::map >& columns) { + + // Convert to compressed column major format colamd wants it in (== MATLAB format!) + std::vector initialOrder; + int Alen = nrNonZeros*30; /* colamd arg 3: size of the array A TODO: use Tim's function ! */ + int * A = new int[Alen]; /* colamd arg 4: row indices of A, of size Alen */ + int * p = new int[n_col + 1]; /* colamd arg 5: column pointers of A, of size n_col+1 */ + + p[0] = 0; + int j = 1; + int count = 0; + typedef typename std::map >::const_iterator iterator; + for(iterator it = columns.begin(); it != columns.end(); it++) { + const Key& key = it->first; + const std::vector& column = it->second; + initialOrder.push_back(key); + BOOST_FOREACH(int i, column) A[count++] = i; // copy sparse column + p[j] = count; // column j (base 1) goes from A[j-1] to A[j]-1 + j+=1; + } + + double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ + int stats[COLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ + + // call colamd, result will be in p ************************************************* + /* TODO: returns (1) if successful, (0) otherwise*/ + ::colamd(n_row, n_col, Alen, A, p, knobs, stats); + // ********************************************************************************** + delete [] A; // delete symbolic A + + // Convert elimination ordering in p to an ordering + Ordering result; + for(int j = 0; j < n_col; j++) + result.push_back(initialOrder[j]); + delete [] p; // delete colamd result vector + + return result; +} + +/* ************************************************************************* */ +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. + typedef std::string Key; // default case with string keys + std::map > columns; // map from keys to a sparse column of non-zero row indices + int nrNonZeros = 0; // number of non-zero entries + int n_row = factors_.size(); /* colamd arg 1: number of rows in A */ + + // loop over all factors = rows + for (int i = 0; i < n_row; i++) { + std::list keys = factors_[i]->keys(); + BOOST_FOREACH(Key key, keys) columns[key].push_back(i); + nrNonZeros+= keys.size(); + } + int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */ + + if(n_col == 0) + return Ordering(); // empty ordering + else + return colamd(n_col, n_row, nrNonZeros, columns); +} + +/* ************************************************************************* */ +} diff --git a/cpp/FactorGraph.cpp b/cpp/FactorGraph.cpp deleted file mode 100644 index 8b4628599..000000000 --- a/cpp/FactorGraph.cpp +++ /dev/null @@ -1,9 +0,0 @@ -/** - * @file FactorGraph.cpp - * @brief Factor Graph Base Class - * @author Carlos Nieto - */ - -#include "FactorGraph.h" - - diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index fdf935981..33aeb55b2 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -18,13 +18,15 @@ namespace gtsam { + 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. */ - template class FactorGraph { + template class FactorGraph { public: - typedef typename boost::shared_ptr shared_factor; + typedef typename boost::shared_ptr shared_factor; typedef typename std::vector::iterator iterator; typedef typename std::vector::const_iterator const_iterator; @@ -103,6 +105,11 @@ namespace gtsam { return false; } + /** + * Compute colamd ordering + */ + Ordering getOrdering() const; + private: /** Serialization function */ diff --git a/cpp/LinearFactor.cpp b/cpp/LinearFactor.cpp index e0d233e9f..43a6c1182 100644 --- a/cpp/LinearFactor.cpp +++ b/cpp/LinearFactor.cpp @@ -77,11 +77,11 @@ bool LinearFactor::equals(const Factor& f, double tol) const { } /* ************************************************************************* */ -set LinearFactor::keys() const { - set result; +list LinearFactor::keys() const { + list result; string j; Matrix A; FOREACH_PAIR(j,A,As) - result.insert(j); + result.push_back(j); return result; } diff --git a/cpp/LinearFactor.h b/cpp/LinearFactor.h index c1ee42b42..b2a9c7bfe 100644 --- a/cpp/LinearFactor.h +++ b/cpp/LinearFactor.h @@ -148,7 +148,7 @@ public: * Find all variables * @return The set of all variable keys */ - std::set keys() const; + std::list keys() const; /** * Find all variables and their dimensions diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index bc730bcee..684fa7b26 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -228,77 +228,4 @@ pair LinearFactorGraph::matrix(const Ordering& ordering) const { return lf.matrix(ordering); } -/* ************************************************************************* */ - -/** - * Call colamd given a column-major symbolic matrix A - * @param n_col colamd arg 1: number of rows in A - * @param n_row colamd arg 2: number of columns in A - * @param nrNonZeros number of non-zero entries in A - * @param columns map from keys to a sparse column of non-zero row indices - */ -template -Ordering colamd(int n_col, int n_row, int nrNonZeros, const map >& columns) { - - // Convert to compressed column major format colamd wants it in (== MATLAB format!) - vector initialOrder; - int Alen = nrNonZeros*30; /* colamd arg 3: size of the array A TODO: use Tim's function ! */ - int * A = new int[Alen]; /* colamd arg 4: row indices of A, of size Alen */ - int * p = new int[n_col + 1]; /* colamd arg 5: column pointers of A, of size n_col+1 */ - - p[0] = 0; - int j = 1; - int count = 0; - typedef typename map >::const_iterator iterator; - for(iterator it = columns.begin(); it != columns.end(); it++) { - const Key& key = it->first; - const vector& column = it->second; - initialOrder.push_back(key); - BOOST_FOREACH(int i, column) A[count++] = i; // copy sparse column - p[j] = count; // column j (base 1) goes from A[j-1] to A[j]-1 - j+=1; - } - - double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ - int stats[COLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ - - // call colamd, result will be in p ************************************************* - /* TODO: returns (1) if successful, (0) otherwise*/ - colamd(n_row, n_col, Alen, A, p, knobs, stats); - // ********************************************************************************** - delete [] A; // delete symbolic A - - // Convert elimination ordering in p to an ordering - Ordering result; - for(int j = 0; j < n_col; j++) - result.push_back(initialOrder[j]); - delete [] p; // delete colamd result vector - - return result; -} - -/* ************************************************************************* */ -Ordering LinearFactorGraph::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. - typedef string Key; // default case with string keys - map > columns; // map from keys to a sparse column of non-zero row indices - int nrNonZeros = 0; // number of non-zero entries - int n_row = factors_.size(); /* colamd arg 1: number of rows in A */ - - // loop over all factors = rows - for (int i = 0; i < n_row; i++) { - set keys = factors_[i]->keys(); - BOOST_FOREACH(Key key, keys) columns[key].push_back(i); - nrNonZeros+= keys.size(); - } - int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */ - - if(n_col == 0) - return Ordering(); // empty ordering - else - return colamd(n_col, n_row, nrNonZeros, columns); -} - /* ************************************************************************* */ diff --git a/cpp/LinearFactorGraph.h b/cpp/LinearFactorGraph.h index 39ac54ee3..21f749cd5 100644 --- a/cpp/LinearFactorGraph.h +++ b/cpp/LinearFactorGraph.h @@ -40,12 +40,6 @@ namespace gtsam { */ void setCBN(const ChordalBayesNet& CBN); - /** - * This function returns the best ordering for this linear factor - * graph, computed using colamd - */ - Ordering getOrdering() const; - /** * find the separator, i.e. all the nodes that have at least one * common factor with the given node. FD: not used AFAIK. diff --git a/cpp/Makefile.am b/cpp/Makefile.am index cbbc8604a..df7a287a4 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -80,7 +80,7 @@ timeLinearFactor: CXXFLAGS += -I /opt/local/include timeLinearFactor: LDFLAGS += -L.libs -lgtsam # graphs -sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp FactorGraph.cpp +sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp sources += LinearFactorGraph.cpp NonlinearFactorGraph.cpp sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp check_PROGRAMS += testChordalBayesNet testConstrainedChordalBayesNet testFactorgraph @@ -146,9 +146,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 += NonlinearOptimizer.h NonlinearOptimizer.cpp # template ! +headers += Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h headers += $(sources:.cpp=.h) +# templates: +headers += FactorGraph.h FactorGraph-inl.h +headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h # create both dynamic and static libraries AM_CXXFLAGS = -I$(boost) -fPIC diff --git a/cpp/NonlinearFactor.cpp b/cpp/NonlinearFactor.cpp index fc4ec4a97..1dc4308c6 100644 --- a/cpp/NonlinearFactor.cpp +++ b/cpp/NonlinearFactor.cpp @@ -14,11 +14,8 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -NonlinearFactor::NonlinearFactor(const Vector& z, - const double sigma) -{ - z_ = z; - sigma_ = sigma; +NonlinearFactor::NonlinearFactor(const Vector& z, const double sigma) : + z_(z), sigma_(sigma) { } /* ************************************************************************* */ diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h index 229df2285..8374940a3 100644 --- a/cpp/NonlinearFactor.h +++ b/cpp/NonlinearFactor.h @@ -15,7 +15,7 @@ #include #include -#include +#include #include "Factor.h" #include "Matrix.h" @@ -63,10 +63,9 @@ namespace gtsam { virtual void print(const std::string& s = "") const = 0; /** get functions */ - double get_sigma() const {return sigma_;} - Vector get_measurement() const {return z_;} - const std::list& get_keys() const {return keys_;} - void set_keys(std::list keys) {keys_ = keys;} + double sigma() const {return sigma_;} + Vector measurement() const {return z_;} + std::list keys() const {return keys_;} /** calculate the error of the factor */ double error(const FGConfig& c) const { @@ -106,8 +105,15 @@ namespace gtsam { * Specialized derived classes could do this */ class NonlinearFactor1 : public NonlinearFactor { + private: + + std::string key1_; + public: + Vector (*h_)(const Vector&); + Matrix (*H_)(const Vector&); + /** Constructor */ NonlinearFactor1(const Vector& z, // measurement const double sigma, // variance @@ -117,10 +123,6 @@ namespace gtsam { void print(const std::string& s = "") const; - Vector (*h_)(const Vector&); - std::string key1_; - Matrix (*H_)(const Vector&); - /** error function */ inline Vector error_vector(const FGConfig& c) const { return z_ - h_(c[key1_]); @@ -141,8 +143,18 @@ namespace gtsam { * Specialized derived classes could do this */ class NonlinearFactor2 : public NonlinearFactor { + + private: + + std::string key1_; + std::string key2_; + public: + Vector (*h_)(const Vector&, const Vector&); + Matrix (*H1_)(const Vector&, const Vector&); + Matrix (*H2_)(const Vector&, const Vector&); + /** Constructor */ NonlinearFactor2(const Vector& z, // the measurement const double sigma, // the variance @@ -154,12 +166,6 @@ namespace gtsam { void print(const std::string& s = "") const; - Vector (*h_)(const Vector&, const Vector&); - std::string key1_; - Matrix (*H1_)(const Vector&, const Vector&); - std::string key2_; - Matrix (*H2_)(const Vector&, const Vector&); - /** error function */ inline Vector error_vector(const FGConfig& c) const { return z_ - h_(c[key1_], c[key2_]); diff --git a/cpp/NonlinearFactorGraph.cpp b/cpp/NonlinearFactorGraph.cpp index 6dbd002f8..148860009 100644 --- a/cpp/NonlinearFactorGraph.cpp +++ b/cpp/NonlinearFactorGraph.cpp @@ -66,13 +66,6 @@ namespace gtsam { return converged; } - /* ************************************************************************* */ - Ordering NonlinearFactorGraph::getOrdering(const FGConfig& config) const { - // TODO: FD: Whoa! This is crazy !!!!! re-linearizing just to get ordering ? - LinearFactorGraph lfg = linearize(config); - return lfg.getOrdering(); - } - /* ************************************************************************* */ bool NonlinearFactorGraph::check_convergence(const FGConfig& config1, const FGConfig& config2, double relativeErrorTreshold, diff --git a/cpp/NonlinearFactorGraph.h b/cpp/NonlinearFactorGraph.h index 6283671b3..b118b2c31 100644 --- a/cpp/NonlinearFactorGraph.h +++ b/cpp/NonlinearFactorGraph.h @@ -24,11 +24,6 @@ typedef FactorGraph BaseFactorGraph; /** Factor Graph consisting of non-linear factors */ class NonlinearFactorGraph : public BaseFactorGraph { -public: - // internal, exposed for testing only, doc in .cpp file - - Ordering getOrdering(const FGConfig& config) const; - public: // these you will probably want to use /** * linearize a non linear factor diff --git a/cpp/NonlinearOptimizer.cpp b/cpp/NonlinearOptimizer-inl.h similarity index 97% rename from cpp/NonlinearOptimizer.cpp rename to cpp/NonlinearOptimizer-inl.h index 372b130a1..f1628f43e 100644 --- a/cpp/NonlinearOptimizer.cpp +++ b/cpp/NonlinearOptimizer-inl.h @@ -1,10 +1,14 @@ /** - * NonlinearOptimizer.cpp + * NonlinearOptimizer-inl.h + * This is a template definition file, include it where needed (only!) + * so that the appropriate code is generated and link errors avoided. * @brief: Encapsulates nonlinear optimization state * @Author: Frank Dellaert * Created on: Sep 7, 2009 */ +#pragma once + #include #include #include "NonlinearOptimizer.h" diff --git a/cpp/Point2Prior.h b/cpp/Point2Prior.h index 886c2223e..3a8310ab5 100644 --- a/cpp/Point2Prior.h +++ b/cpp/Point2Prior.h @@ -6,9 +6,11 @@ namespace gtsam { - struct Point2Prior : public NonlinearFactor1 { - Point2Prior(const Vector& mu, double sigma, const std::string& key): - NonlinearFactor1(mu, sigma, prior, key, Dprior) {} - }; + class Point2Prior: public NonlinearFactor1 { + public: + Point2Prior(const Vector& mu, double sigma, const std::string& key) : + NonlinearFactor1(mu, sigma, prior, key, Dprior) { + } + }; } // namespace gtsam diff --git a/cpp/VSLAMFactor.cpp b/cpp/VSLAMFactor.cpp index 82358d162..dba27a165 100644 --- a/cpp/VSLAMFactor.cpp +++ b/cpp/VSLAMFactor.cpp @@ -85,11 +85,10 @@ string VSLAMFactor::dump() const { int i = getCameraFrameNumber(); int j = getLandmarkNumber(); - double sigma = get_sigma(); - Vector z = get_measurement(); + 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(), 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/smallExample.cpp b/cpp/smallExample.cpp index d6af2d69a..8316f4efd 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -14,6 +14,7 @@ using namespace std; +#include "Ordering.h" #include "Matrix.h" #include "NonlinearFactor.h" #include "EqualityFactor.h" diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp index 74abc5d1b..d274949e2 100644 --- a/cpp/testLinearFactor.cpp +++ b/cpp/testLinearFactor.cpp @@ -48,9 +48,9 @@ TEST( LinearFactor, keys ) // get the factor "f2" from the small linear factor graph LinearFactorGraph fg = createLinearFactorGraph(); LinearFactor::shared_ptr lf = fg[1]; - set expected; - expected.insert("x1"); - expected.insert("x2"); + list expected; + expected.push_back("x1"); + expected.push_back("x2"); CHECK(lf->keys() == expected); } diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index 6adf192ce..216441fc4 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -13,6 +13,7 @@ using namespace std; #include #include "Matrix.h" #include "smallExample.h" +#include "FactorGraph-inl.h" // template definitions using namespace gtsam; diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index 0abe59937..6764ab791 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -11,7 +11,8 @@ using namespace std; #include "Matrix.h" #include "smallExample.h" -#include "NonlinearOptimizer.cpp" +// template definitions +#include "NonlinearOptimizer-inl.h" using namespace gtsam;