diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index da486db83..a40380f86 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -23,6 +23,7 @@ #include #include +namespace gtsam { /** * Wrapper class to manage ISAM in a nonlinear context */ @@ -30,55 +31,57 @@ template class NonlinearISAM { public: - typedef gtsam::NonlinearFactorGraph Factors; + typedef gtsam::NonlinearFactorGraph Factors; protected: - /** The internal iSAM object */ - gtsam::GaussianISAM isam_; + /** The internal iSAM object */ + gtsam::GaussianISAM isam_; - /** The current linearization point */ - Values linPoint_; + /** The current linearization point */ + Values linPoint_; - /** The ordering */ - gtsam::Ordering ordering_; + /** The ordering */ + gtsam::Ordering ordering_; - /** The original factors, used when relinearizing */ - Factors factors_; + /** The original factors, used when relinearizing */ + Factors factors_; - /** The reordering interval and counter */ - int reorderInterval_; - int reorderCounter_; + /** The reordering interval and counter */ + int reorderInterval_; + int reorderCounter_; public: - /** default constructor will disable periodic reordering */ - NonlinearISAM() : reorderInterval_(0), reorderCounter_(0) {} + /** default constructor will disable periodic reordering */ + NonlinearISAM() : reorderInterval_(0), reorderCounter_(0) {} - /** Periodically reorder and relinearize */ - NonlinearISAM(int reorderInterval) : reorderInterval_(reorderInterval), reorderCounter_(0) {} + /** Periodically reorder and relinearize */ + NonlinearISAM(int reorderInterval) : reorderInterval_(reorderInterval), reorderCounter_(0) {} - /** Add new factors along with their initial linearization points */ - void update(const Factors& newFactors, const Values& initialValues); + /** Add new factors along with their initial linearization points */ + void update(const Factors& newFactors, const Values& initialValues); - /** Return the current solution estimate */ - Values estimate(); + /** Return the current solution estimate */ + Values estimate(); - /** Relinearization and reordering of variables */ - void reorder_relinearize(); + /** Relinearization and reordering of variables */ + void reorder_relinearize(); - // access + // access - /** Return the current linearization point */ - const Values& getLinearizationPoint() { return linPoint_; } + /** Return the current linearization point */ + const Values& getLinearizationPoint() { return linPoint_; } - /** Get the ordering */ - const gtsam::Ordering& getOrdering() const { return ordering_; } + /** Get the ordering */ + const gtsam::Ordering& getOrdering() const { return ordering_; } - /** get underlying nonlinear graph */ - const Factors& getFactorsUnsafe() { return factors_; } + /** get underlying nonlinear graph */ + const Factors& getFactorsUnsafe() { return factors_; } - /** get counters */ - int reorderInterval() const { return reorderInterval_; } - int reorderCounter() const { return reorderCounter_; } + /** get counters */ + int reorderInterval() const { return reorderInterval_; } + int reorderCounter() const { return reorderCounter_; } }; + +} // \namespace gtsam