diff --git a/gtsam.h b/gtsam.h index 2a1e5c0b3..3db42c90f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -764,6 +764,8 @@ class GaussianFactor { void print(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* negate() const; + size_t size() const; }; class JacobianFactor { @@ -775,12 +777,15 @@ class JacobianFactor { const gtsam::noiseModel::Diagonal* model); JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(const gtsam::GaussianFactor& factor); void print(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool empty() const; + size_t size() const; Vector getb() const; double error(const gtsam::VectorValues& c) const; gtsam::GaussianConditional* eliminateFirst(); + gtsam::GaussianFactor* negate() const; }; class HessianFactor { @@ -795,9 +800,11 @@ class HessianFactor { double f); HessianFactor(const gtsam::GaussianConditional& cg); HessianFactor(const gtsam::GaussianFactor& factor); + size_t size() const; void print(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* negate() const; }; class GaussianFactorGraph { @@ -809,6 +816,7 @@ class GaussianFactorGraph { void print(string s) const; bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; size_t size() const; + gtsam::GaussianFactor* at(size_t idx) const; // Building the graph void add(gtsam::JacobianFactor* factor); @@ -917,7 +925,7 @@ class NonlinearFactor { void equals(const gtsam::NonlinearFactor& other, double tol) const; gtsam::KeyVector keys() const; size_t size() const; -// size_t dim() const; // FIXME: Doesn't link + size_t dim() const; // FIXME: Doesn't link }; class Values { @@ -928,24 +936,68 @@ class Values { }; // Actually a FastList -#include +#include class KeyList { KeyList(); KeyList(const gtsam::KeyList& other); // Note: no print function + + // common STL methods size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t front() const; + size_t back() const; + void push_back(size_t key); + void push_front(size_t key); + void sort(); + void remove(size_t key); }; -// Actually a KeyVector -#include +// Actually a FastSet +#include +class KeySet { + KeySet(); + KeySet(const gtsam::KeySet& other); + + // Testable + void print(string s) const; + bool equals(const gtsam::KeySet& other) const; + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(size_t key); + bool erase(size_t key); // returns true if value was removed + bool count(size_t key) const; // returns true if value exists +}; + +// Actually a FastVector +#include class KeyVector { KeyVector(); KeyVector(const gtsam::KeyVector& other); + KeyVector(const gtsam::KeySet& other); + KeyVector(const gtsam::KeyList& other); // Note: no print function + + // common STL methods size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t key) const; }; class Marginals { diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index e91bd1813..c3b8ca761 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -24,6 +24,7 @@ #include #include +#include namespace gtsam { @@ -69,6 +70,12 @@ public: Base::assign(x.begin(), x.end()); } + /** Copy constructor from a FastSet */ + FastVector(const FastSet& x) { + if(x.size() > 0) + Base::assign(x.begin(), x.end()); + } + /** Copy constructor from the base class */ FastVector(const Base& x) : Base(x) {} diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h index 65a65c6d4..ab63fda6d 100644 --- a/gtsam/nonlinear/Key.h +++ b/gtsam/nonlinear/Key.h @@ -20,6 +20,10 @@ #include #include +#include +#include +#include + namespace gtsam { /// Integer nonlinear key type @@ -36,5 +40,10 @@ namespace gtsam { /// and Symbol keys. static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; + /// Useful typedefs for operations with Values - allow for matlab interfaces + typedef FastList KeyList; + typedef FastVector KeyVector; + typedef FastSet KeySet; + } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 8ade8ff06..a01718da4 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -26,8 +26,8 @@ #include #include -#include #include +#include #include #include @@ -44,10 +44,6 @@ namespace gtsam { - // Useful typedefs for operations with Values - allow for matlab interfaces - typedef FastList KeyList; - typedef FastVector KeyVector; - // Forward declarations / utilities class ValueCloneAllocator; class ValueAutomaticCasting;