12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Put NonlinearISAM into the gtsam namespace

release/4.3a0
Alex Cunningham 2010-12-15 17:01:04 +00:00
parent f4babac85c
commit 000e58d4b2
1 changed files with 35 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
gtsam/nonlinear/NonlinearISAM.h
View File

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