diff --git a/.cproject b/.cproject index b09810484..f5675f5ee 100644 --- a/.cproject +++ b/.cproject @@ -2192,22 +2192,6 @@ true true - - make - -j5 - wrap_gtsam_clean - true - true - true - - - make - -j5 - wrap_gtsam_unstable_clean - true - true - true - make -j5 diff --git a/gtsam.h b/gtsam.h index 8f13ba39c..d15ef08d8 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1063,15 +1063,25 @@ class InvertedOrdering { class NonlinearFactorGraph { NonlinearFactorGraph(); - void print(string s) const; - size_t size() const; - double error(const gtsam::Values& c) const; - double probPrime(const gtsam::Values& c) const; - gtsam::NonlinearFactor* at(size_t i) const; + NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + + // FactorGraph + void print(string s) const; + bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + size_t nrFactors() const; void add(const gtsam::NonlinearFactor* factor); - gtsam::Ordering* orderingCOLAMD(const gtsam::Values& c) const; + gtsam::NonlinearFactor* at(size_t i) const; + + // NonlinearFactorGraph + double error(const gtsam::Values& values) const; + double probPrime(const gtsam::Values& values) const; + gtsam::Ordering* orderingCOLAMD(const gtsam::Values& values) const; // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values, + const gtsam::Ordering& ordering) const; gtsam::NonlinearFactorGraph clone() const; }; @@ -1382,6 +1392,29 @@ class ISAM2 { gtsam::ISAM2Params params() const; }; +#include +class NonlinearISAM { + NonlinearISAM(); + NonlinearISAM(int reorderInterval); + void print(string s) const; + void printStats() const; + void saveGraph(string s) const; + gtsam::Values estimate() const; + Matrix marginalCovariance(size_t key) const; + int reorderInterval() const; + int reorderCounter() const; + void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); + void reorder_relinearize(); + void addKey(size_t key); + void setOrdering(const gtsam::Ordering& new_ordering); + + // These might be expensive as instead of a reference the wrapper will make a copy + gtsam::GaussianISAM bayesTree() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; +}; + //************************************************************************* // Nonlinear factor types //************************************************************************* diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 60bd833ff..1bb6d20e0 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -172,15 +172,21 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { #include #include -template +template virtual class PoseTranslationPrior : gtsam::NonlinearFactor { PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); }; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; + #include -template +template virtual class PoseRotationPrior : gtsam::NonlinearFactor { PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); }; +typedef gtsam::PoseRotationPrior PoseRotationPrior2D; +typedef gtsam::PoseRotationPrior PoseRotationPrior3D; + } //\namespace gtsam