diff --git a/gtsam.h b/gtsam.h index 0e50ac9b6..3222cd2d1 100644 --- a/gtsam.h +++ b/gtsam.h @@ -982,6 +982,7 @@ class NonlinearFactorGraph { void print(string s) const; double error(const gtsam::Values& c) const; double probPrime(const gtsam::Values& c) const; + gtsam::NonlinearFactor* at(size_t i) const; void add(const gtsam::NonlinearFactor* factor); gtsam::Ordering* orderingCOLAMD(const gtsam::Values& c) const; // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; @@ -1132,6 +1133,15 @@ virtual class BetweenFactor : gtsam::NonlinearFactor { }; +template +virtual class NonlinearEquality : gtsam::NonlinearFactor { + // Constructor - forces exact evaluation + NonlinearEquality(size_t j, const T& feasible); + // Constructor - allows inexact evaluation + NonlinearEquality(size_t j, const T& feasible, double error_gain); +}; + + template virtual class RangeFactor : gtsam::NonlinearFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);