diff --git a/gtsam.h b/gtsam.h index e211068ad..8ff9056f9 100644 --- a/gtsam.h +++ b/gtsam.h @@ -895,6 +895,7 @@ class SymbolicFactorGraph { void print(string s) const; bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; size_t size() const; + bool exists(size_t i) const; // Standard interface // FIXME: Must wrap FastSet for this to work @@ -1282,6 +1283,7 @@ class GaussianFactorGraph { bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; size_t size() const; gtsam::GaussianFactor* at(size_t idx) const; + bool exists(size_t idx) const; // Inference pair eliminateFrontals(size_t nFrontals) const; @@ -1508,7 +1510,8 @@ class NonlinearFactorGraph { bool empty() const; void remove(size_t i); size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; + gtsam::NonlinearFactor* at(size_t idx) const; + bool exists(size_t idx) const; void push_back(const gtsam::NonlinearFactorGraph& factors); // NonlinearFactorGraph diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 70954152f..0d3b10e31 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -165,6 +165,9 @@ class VariableIndex; const sharedFactor operator[](size_t i) const { return at(i); } sharedFactor& operator[](size_t i) { return at(i); } + /** Checks whether a valid factor exists at the given index */ + inline bool exists(size_t i) const { return i