diff --git a/examples/matlab/VisualISAMExample.m b/examples/matlab/VisualISAMExample.m index e1116552d..0e338340c 100644 --- a/examples/matlab/VisualISAMExample.m +++ b/examples/matlab/VisualISAMExample.m @@ -26,6 +26,7 @@ ALWAYS_RELINEARIZE = false; %% Display Options SAVE_GRAPH = false; +PRINT_STATS = true; DRAW_INTERVAL = 20; CAMERA_INTERVAL = 1; DRAW_TRUE_POSES = false; @@ -140,6 +141,9 @@ for i=2:NCAMERAS if SAVE_GRAPH isam.saveGraph(sprintf('VisualiSAM.dot',i)); end + if PRINT_STATS + isam.printStats(); + end if mod(i,DRAW_INTERVAL)==0 %% Plot results tic diff --git a/gtsam.h b/gtsam.h index 447b9f7ca..0e2367cc6 100644 --- a/gtsam.h +++ b/gtsam.h @@ -935,6 +935,7 @@ class ISAM { ISAM(); ISAM(int reorderInterval); void print(string s) const; + void printStats() const; void saveGraph(string s) const; visualSLAM::Values estimate() const; Matrix marginalCovariance(size_t key) const; diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 55181731c..ea62f1713 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -17,16 +17,15 @@ #pragma once -#include - -#include - #include #include #include -using namespace std; -using namespace gtsam; +#include + +#include + +namespace gtsam { /* ************************************************************************* */ template @@ -71,7 +70,7 @@ void NonlinearISAM::update(const Factors& newFactors, template void NonlinearISAM::reorder_relinearize() { -// cout << "Reordering, relinearizing..." << endl; +// std::cout << "Reordering, relinearizing..." << std::endl; if(factors_.size() > 0) { // Obtain the new linearization point @@ -112,10 +111,26 @@ Matrix NonlinearISAM::marginalCovariance(Key key) const { /* ************************************************************************* */ template void NonlinearISAM::print(const std::string& s) const { - cout << "ISAM - " << s << ":" << endl; - cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; + std::cout << "ISAM - " << s << ":" << std::endl; + std::cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << std::endl; isam_.print("GaussianISAM"); linPoint_.print("Linearization Point"); ordering_.print("System Ordering"); factors_.print("Nonlinear Graph"); } + +/* ************************************************************************* */ +template +void NonlinearISAM::printStats() const { + gtsam::GaussianISAM::CliqueData data = isam_.getCliqueData(); + gtsam::GaussianISAM::CliqueStats stats = data.getStats(); + std::cout << "\navg Conditional Size: " << stats.avgConditionalSize; + std::cout << "\nmax Conditional Size: " << stats.maxConditionalSize; + std::cout << "\navg Separator Size: " << stats.avgSeparatorSize; + std::cout << "\nmax Separator Size: " << stats.maxSeparatorSize; + std::cout << std::endl; +} + +/* ************************************************************************* */ + +}///\ namespace gtsam diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 54dc92a23..145af7c17 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -93,6 +93,9 @@ public: /** prints out all contents of the system */ void print(const std::string& s="") const; + /** prints out clique statistics */ + void printStats() const; + /** saves the Tree to a text file in GraphViz format */ void saveGraph(const std::string& s) const;