diff --git a/examples/vSLAMexample/Makefile.am b/examples/vSLAMexample/Makefile.am index cfa60931e..442186b4e 100644 --- a/examples/vSLAMexample/Makefile.am +++ b/examples/vSLAMexample/Makefile.am @@ -17,7 +17,6 @@ vSFMexample_SOURCES = vSFMexample.cpp Feature2D.cpp vSLAMutils.cpp noinst_PROGRAMS += vISAMexample vISAMexample_SOURCES = vISAMexample.cpp Feature2D.cpp vSLAMutils.cpp -headers += ISAMLoop.h ISAMLoop-inl.h headers += Feature2D.h vSLAMutils.h noinst_HEADERS = $(headers) diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 512ff56ad..e522f79c3 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -26,11 +26,9 @@ using namespace boost; #include #include #include - #include #include -#include "ISAMLoop.h" -#include "ISAMLoop-inl.h" +#include #include "vSLAMutils.h" #include "Feature2D.h" @@ -124,9 +122,9 @@ int main(int argc, char* argv[]) { g_dataFolder = string(argv[1]) + "/"; readAllDataISAM(); - // Create an ISAMLoop which will be relinearized and reordered after every "relinearizeInterval" updates + // Create an NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates int relinearizeInterval = 3; - ISAMLoop isam(relinearizeInterval); + NonlinearISAM isam(relinearizeInterval); // At each frame i with new camera pose and new set of measurements associated with it, // create a graph of new factors and update ISAM diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index e07c5e63e..331f2758a 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -27,6 +27,9 @@ headers += NonlinearFactor.h sources += NonlinearOptimizer.cpp Ordering.cpp check_PROGRAMS += tests/testKey +# Nonlinear iSAM +headers += NonlinearISAM.h NonlinearISAM-inl.h + # Nonlinear constraints headers += NonlinearEquality.h diff --git a/examples/vSLAMexample/ISAMLoop-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h similarity index 82% rename from examples/vSLAMexample/ISAMLoop-inl.h rename to gtsam/nonlinear/NonlinearISAM-inl.h index 7c6ec5cb9..4d6ef59a9 100644 --- a/examples/vSLAMexample/ISAMLoop-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * ISAMLoop.cpp + * NonlinearISAM-inl.h * * Created on: Jan 19, 2010 * Author: Viorela Ila and Richard Roberts @@ -21,15 +21,14 @@ #include #include #include +#include #include -#include "ISAMLoop.h" - using namespace gtsam; /* ************************************************************************* */ template -void ISAMLoop::update(const Factors& newFactors, const Values& initialValues) { +void NonlinearISAM::update(const Factors& newFactors, const Values& initialValues) { if(newFactors.size() > 0) { @@ -55,20 +54,20 @@ void ISAMLoop::update(const Factors& newFactors, const Values& initialVa boost::shared_ptr linearizedNewFactors(newFactors.linearize(linPoint_, ordering_)); // Update ISAM - isam.update(*linearizedNewFactors); + isam_.update(*linearizedNewFactors); } } /* ************************************************************************* */ template -void ISAMLoop::reorder_relinearize() { +void NonlinearISAM::reorder_relinearize() { - cout << "Reordering, relinearizing..." << endl; +// cout << "Reordering, relinearizing..." << endl; // Obtain the new linearization point const Values newLinPoint = estimate(); - isam.clear(); + isam_.clear(); // Compute an ordering ordering_ = *factors_.orderingCOLAMD(newLinPoint); @@ -77,7 +76,7 @@ void ISAMLoop::reorder_relinearize() { boost::shared_ptr gfg(factors_.linearize(newLinPoint, ordering_)); // Just recreate the whole BayesTree - isam.update(*gfg); + isam_.update(*gfg); // Update linearization point linPoint_ = newLinPoint; @@ -85,9 +84,9 @@ void ISAMLoop::reorder_relinearize() { /* ************************************************************************* */ template -Values ISAMLoop::estimate() { - if(isam.size() > 0) - return linPoint_.expmap(optimize(isam), ordering_); +Values NonlinearISAM::estimate() { + if(isam_.size() > 0) + return linPoint_.expmap(optimize(isam_), ordering_); else return linPoint_; } diff --git a/examples/vSLAMexample/ISAMLoop.h b/gtsam/nonlinear/NonlinearISAM.h similarity index 73% rename from examples/vSLAMexample/ISAMLoop.h rename to gtsam/nonlinear/NonlinearISAM.h index e96b0b758..da486db83 100644 --- a/examples/vSLAMexample/ISAMLoop.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * ISAMLoop.h + * NonlinearISAM.h * * Created on: Jan 19, 2010 * Author: Viorela Ila and Richard Roberts @@ -23,18 +23,19 @@ #include #include - +/** + * Wrapper class to manage ISAM in a nonlinear context + */ template -class ISAMLoop { +class NonlinearISAM { public: typedef gtsam::NonlinearFactorGraph Factors; -public: -//protected: +protected: /** The internal iSAM object */ - gtsam::GaussianISAM isam; + gtsam::GaussianISAM isam_; /** The current linearization point */ Values linPoint_; @@ -49,12 +50,13 @@ public: int reorderInterval_; int reorderCounter_; - public: - ISAMLoop() : reorderInterval_(0), reorderCounter_(0) {} + + /** default constructor will disable periodic reordering */ + NonlinearISAM() : reorderInterval_(0), reorderCounter_(0) {} /** Periodically reorder and relinearize */ - ISAMLoop(int reorderInterval) : reorderInterval_(reorderInterval), reorderCounter_(0) {} + NonlinearISAM(int reorderInterval) : reorderInterval_(reorderInterval), reorderCounter_(0) {} /** Add new factors along with their initial linearization points */ void update(const Factors& newFactors, const Values& initialValues); @@ -62,17 +64,21 @@ public: /** Return the current solution estimate */ Values estimate(); + /** Relinearization and reordering of variables */ + void reorder_relinearize(); + + // access + /** Return the current linearization point */ const Values& getLinearizationPoint() { return linPoint_; } /** Get the ordering */ const gtsam::Ordering& getOrdering() const { return ordering_; } + /** get underlying nonlinear graph */ const Factors& getFactorsUnsafe() { return factors_; } - /** - * Relinearization and reordering of variables - */ - void reorder_relinearize(); - + /** get counters */ + int reorderInterval() const { return reorderInterval_; } + int reorderCounter() const { return reorderCounter_; } };