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

printStats

release/4.3a0
Frank Dellaert 2012-06-07 14:29:18 +00:00
parent 39aec060ae
commit bf21239e41
4 changed files with 32 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/matlab/VisualISAMExample.m
View File

@ -26,6 +26,7 @@ ALWAYS_RELINEARIZE = false;
%% Display Options %% Display Options
SAVE_GRAPH = false; SAVE_GRAPH = false;
PRINT_STATS = true;
DRAW_INTERVAL = 20; DRAW_INTERVAL = 20;
CAMERA_INTERVAL = 1; CAMERA_INTERVAL = 1;
DRAW_TRUE_POSES = false; DRAW_TRUE_POSES = false;
@ -140,6 +141,9 @@ for i=2:NCAMERAS
if SAVE_GRAPH if SAVE_GRAPH
isam.saveGraph(sprintf('VisualiSAM.dot',i)); isam.saveGraph(sprintf('VisualiSAM.dot',i));
end end
if PRINT_STATS
isam.printStats();
end
if mod(i,DRAW_INTERVAL)==0 if mod(i,DRAW_INTERVAL)==0
%% Plot results %% Plot results
tic tic

1
gtsam.h
View File

@ -935,6 +935,7 @@ class ISAM {
ISAM(); ISAM();
ISAM(int reorderInterval); ISAM(int reorderInterval);
void print(string s) const; void print(string s) const;
void printStats() const;
void saveGraph(string s) const; void saveGraph(string s) const;
visualSLAM::Values estimate() const; visualSLAM::Values estimate() const;
Matrix marginalCovariance(size_t key) const; Matrix marginalCovariance(size_t key) const;

33
gtsam/nonlinear/NonlinearISAM-inl.h
View File

@ -17,16 +17,15 @@
#pragma once #pragma once
#include <iostream>
#include <boost/foreach.hpp>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/ISAM-inl.h> #include <gtsam/inference/ISAM-inl.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
using namespace std; #include <boost/foreach.hpp>
using namespace gtsam;
#include <iostream>
namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class GRAPH> template<class GRAPH>
@ -71,7 +70,7 @@ void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
template<class GRAPH> template<class GRAPH>
void NonlinearISAM<GRAPH>::reorder_relinearize() { void NonlinearISAM<GRAPH>::reorder_relinearize() {
// cout << "Reordering, relinearizing..." << endl; // std::cout << "Reordering, relinearizing..." << std::endl;
if(factors_.size() > 0) { if(factors_.size() > 0) {
// Obtain the new linearization point // Obtain the new linearization point
@ -112,10 +111,26 @@ Matrix NonlinearISAM<GRAPH>::marginalCovariance(Key key) const {
/* ************************************************************************* */ /* ************************************************************************* */
template<class GRAPH> template<class GRAPH>
void NonlinearISAM<GRAPH>::print(const std::string& s) const { void NonlinearISAM<GRAPH>::print(const std::string& s) const {
cout << "ISAM - " << s << ":" << endl; std::cout << "ISAM - " << s << ":" << std::endl;
cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; std::cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << std::endl;
isam_.print("GaussianISAM"); isam_.print("GaussianISAM");
linPoint_.print("Linearization Point"); linPoint_.print("Linearization Point");
ordering_.print("System Ordering"); ordering_.print("System Ordering");
factors_.print("Nonlinear Graph"); factors_.print("Nonlinear Graph");
} }
/* ************************************************************************* */
template<class GRAPH>
void NonlinearISAM<GRAPH>::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

3
gtsam/nonlinear/NonlinearISAM.h
View File

@ -93,6 +93,9 @@ public:
/** prints out all contents of the system */ /** prints out all contents of the system */
void print(const std::string& s="") const; void print(const std::string& s="") const;
/** prints out clique statistics */
void printStats() const;
/** saves the Tree to a text file in GraphViz format */ /** saves the Tree to a text file in GraphViz format */
void saveGraph(const std::string& s) const; void saveGraph(const std::string& s) const;