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;