#include <graph_optimizer_sparse_online.h>

Inheritance diagram for g2o::SparseOptimizerOnline:

[legend]

Collaboration diagram for g2o::SparseOptimizerOnline:

[legend]

Public Member Functions
	SparseOptimizerOnline (bool pcg=false)

virtual	~SparseOptimizerOnline ()

int	optimize (int iterations, bool online=false)

virtual bool	updateInitialization (HyperGraph::VertexSet &vset, HyperGraph::EdgeSet &eset)

void	update (double *update)

virtual bool	initSolver (int dimension, int batchEveryN)

virtual void	gnuplotVisualization ()

Public Member Functions inherited from g2o::SparseOptimizer
	SparseOptimizer ()

virtual	~SparseOptimizer ()

virtual bool	initializeOptimization (HyperGraph::EdgeSet &eset)

virtual bool	initializeOptimization (HyperGraph::VertexSet &vset, int level=0)

virtual bool	initializeOptimization (int level=0)

virtual void	computeInitialGuess ()

virtual void	computeInitialGuess (EstimatePropagatorCost &propagator)

virtual void	setToOrigin ()

bool	computeMarginals (SparseBlockMatrix< MatrixXD > &spinv, const std::vector< std::pair< int, int > > &blockIndices)

bool	computeMarginals (SparseBlockMatrix< MatrixXD > &spinv, const Vertex *vertex)

bool	computeMarginals (SparseBlockMatrix< MatrixXD > &spinv, const VertexContainer &vertices)

virtual Vertex *	findGauge ()
	finds a gauge in the graph to remove the undefined dof. More...

bool	gaugeFreedom ()

double	activeChi2 () const

double	activeRobustChi2 () const

bool	verbose () const
	verbose information during optimization More...

void	setVerbose (bool verbose)

void	setForceStopFlag (bool *flag)

bool *	forceStopFlag () const

bool	terminate ()
	if external stop flag is given, return its state. False otherwise More...

const VertexContainer &	indexMapping () const
	the index mapping of the vertices More...

const VertexContainer &	activeVertices () const
	the vertices active in the current optimization More...

const EdgeContainer &	activeEdges () const
	the edges active in the current optimization More...

virtual bool	removeVertex (HyperGraph::Vertex *v, bool detach=false)

VertexContainer::const_iterator	findActiveVertex (const OptimizableGraph::Vertex *v) const

EdgeContainer::const_iterator	findActiveEdge (const OptimizableGraph::Edge *e) const

const OptimizationAlgorithm *	algorithm () const
	the solver used by the optimizer More...

OptimizationAlgorithm *	solver ()

void	setAlgorithm (OptimizationAlgorithm *algorithm)

void	push (SparseOptimizer::VertexContainer &vlist)
	push the estimate of a subset of the variables onto a stack More...

void	push (HyperGraph::VertexSet &vlist)
	push the estimate of a subset of the variables onto a stack More...

void	push ()
	push all the active vertices onto a stack More...

void	pop (SparseOptimizer::VertexContainer &vlist)
	pop (restore) the estimate a subset of the variables from the stack More...

void	pop (HyperGraph::VertexSet &vlist)
	pop (restore) the estimate a subset of the variables from the stack More...

void	pop ()
	pop (restore) the estimate of the active vertices from the stack More...

void	discardTop (SparseOptimizer::VertexContainer &vlist)
	ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate More...

void	discardTop ()
	same as above, but for the active vertices More...

virtual void	clear ()

void	computeActiveErrors ()

const BatchStatisticsContainer &	batchStatistics () const

BatchStatisticsContainer &	batchStatistics ()

void	setComputeBatchStatistics (bool computeBatchStatistics)

bool	computeBatchStatistics () const

bool	addComputeErrorAction (HyperGraphAction *action)
	add an action to be executed before the error vectors are computed More...

bool	removeComputeErrorAction (HyperGraphAction *action)
	remove an action that should no longer be execured before computing the error vectors More...

Public Member Functions inherited from g2o::OptimizableGraph
Vertex *	vertex (int id)
	returns the vertex number id appropriately casted More...

const Vertex *	vertex (int id) const
	returns the vertex number id appropriately casted More...

	OptimizableGraph ()
	empty constructor More...

virtual	~OptimizableGraph ()

void	addGraph (OptimizableGraph *g)
	adds all edges and vertices of the graph g to this graph. More...

virtual bool	addVertex (HyperGraph::Vertex v, Data userData)

virtual bool	addVertex (HyperGraph::Vertex *v)

virtual bool	addEdge (HyperGraph::Edge *e)

virtual bool	setEdgeVertex (HyperGraph::Edge e, int pos, HyperGraph::Vertex v)

double	chi2 () const
	returns the chi2 of the current configuration More...

int	maxDimension () const
	return the maximum dimension of all vertices in the graph More...

std::set< int >	dimensions () const

virtual void	preIteration (int)
	called at the beginning of an iteration (argument is the number of the iteration) More...

virtual void	postIteration (int)
	called at the end of an iteration (argument is the number of the iteration) More...

bool	addPreIterationAction (HyperGraphAction *action)
	add an action to be executed before each iteration More...

bool	addPostIterationAction (HyperGraphAction *action)
	add an action to be executed after each iteration More...

bool	removePreIterationAction (HyperGraphAction *action)
	remove an action that should no longer be execured before each iteration More...

bool	removePostIterationAction (HyperGraphAction *action)
	remove an action that should no longer be execured after each iteration More...

virtual bool	load (std::istream &is, bool createEdges=true)
	load the graph from a stream. Uses the Factory singleton for creating the vertices and edges. More...

bool	load (const char *filename, bool createEdges=true)

virtual bool	save (std::ostream &os, int level=0) const
	save the graph to a stream. Again uses the Factory system. More...

bool	save (const char *filename, int level=0) const
	function provided for convenience, see save() above More...

bool	saveSubset (std::ostream &os, HyperGraph::VertexSet &vset, int level=0)
	save a subgraph to a stream. Again uses the Factory system. More...

bool	saveSubset (std::ostream &os, HyperGraph::EdgeSet &eset)
	save a subgraph to a stream. Again uses the Factory system. More...

virtual void	discardTop (HyperGraph::VertexSet &vset)
	ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate More...

virtual void	setFixed (HyperGraph::VertexSet &vset, bool fixed)
	fixes/releases a set of vertices More...

void	setRenamedTypesFromString (const std::string &types)

bool	isSolverSuitable (const OptimizationAlgorithmProperty &solverProperty, const std::set< int > &vertDims=std::set< int >()) const

virtual void	clearParameters ()

bool	addParameter (Parameter *p)

Parameter *	parameter (int id)

bool	verifyInformationMatrices (bool verbose=false) const

bool	saveVertex (std::ostream &os, Vertex *v) const

bool	saveParameter (std::ostream &os, Parameter *v) const

bool	saveEdge (std::ostream &os, Edge *e) const

bool	saveUserData (std::ostream &os, HyperGraph::Data *v) const

JacobianWorkspace &	jacobianWorkspace ()
	the workspace for storing the Jacobians of the graph More...

const JacobianWorkspace &	jacobianWorkspace () const

ParameterContainer &	parameters ()

const ParameterContainer &	parameters () const

Public Member Functions inherited from g2o::HyperGraph
	HyperGraph ()
	constructs an empty hyper graph More...

virtual	~HyperGraph ()
	destroys the hyper-graph and all the vertices of the graph More...

Vertex *	vertex (int id)
	returns a vertex id in the hyper-graph, or 0 if the vertex id is not present More...

const Vertex *	vertex (int id) const
	returns a vertex id in the hyper-graph, or 0 if the vertex id is not present More...

virtual bool	removeEdge (Edge *e)
	removes a vertex from the graph. Returns true on success (edge was present) More...

const VertexIDMap &	vertices () const

VertexIDMap &	vertices ()

const EdgeSet &	edges () const

EdgeSet &	edges ()

virtual bool	mergeVertices (Vertex vBig, Vertex vSmall, bool erase)

virtual bool	detachVertex (Vertex *v)

virtual bool	changeId (Vertex *v, int newId)

Public Attributes
int	slamDimension

HyperGraph::EdgeSet *	newEdges

bool	batchStep

bool	vizWithGnuplot

Public Attributes inherited from g2o::OptimizableGraph
class G2O_CORE_API	Vertex

class G2O_CORE_API	Edge

Public Attributes inherited from g2o::HyperGraph
class G2O_CORE_API	Data

class G2O_CORE_API	DataContainer

class G2O_CORE_API	Vertex

class G2O_CORE_API	Edge

Protected Attributes
FILE *	_gnuplot

bool	_usePcg

Solver *	_underlyingSolver

Protected Attributes inherited from g2o::SparseOptimizer
bool *	_forceStopFlag

bool	_verbose

VertexContainer	_ivMap

VertexContainer	_activeVertices
	sorted according to VertexIDCompare More...

EdgeContainer	_activeEdges
	sorted according to EdgeIDCompare More...

OptimizationAlgorithm *	_algorithm

BatchStatisticsContainer	_batchStatistics
	global statistics of the optimizer, e.g., timing, num-non-zeros More...

bool	_computeBatchStatistics

Protected Attributes inherited from g2o::OptimizableGraph
std::map< std::string, std::string >	_renamedTypesLookup

long long	_nextEdgeId

std::vector< HyperGraphActionSet >	_graphActions

bool	_edge_has_id

ParameterContainer	_parameters

JacobianWorkspace	_jacobianWorkspace

Protected Attributes inherited from g2o::HyperGraph
VertexIDMap	_vertices

EdgeSet	_edges

Additional Inherited Members
Public Types inherited from g2o::SparseOptimizer
enum	{ AT_COMPUTEACTIVERROR = OptimizableGraph::AT_NUM_ELEMENTS, AT_NUM_ELEMENTS }

Public Types inherited from g2o::OptimizableGraph
enum	ActionType { AT_PREITERATION, AT_POSTITERATION, AT_NUM_ELEMENTS }

typedef std::set< HyperGraphAction * >	HyperGraphActionSet

typedef std::vector< OptimizableGraph::Vertex * >	VertexContainer
	vector container for vertices More...

typedef std::vector< OptimizableGraph::Edge * >	EdgeContainer
	vector container for edges More...

Public Types inherited from g2o::HyperGraph
typedef std::bitset< HyperGraph::HGET_NUM_ELEMS >	GraphElemBitset

typedef std::set< Edge * >	EdgeSet

typedef std::set< Vertex * >	VertexSet

typedef std::unordered_map< int, Vertex * >	VertexIDMap

typedef std::vector< Vertex * >	VertexContainer

Static Public Member Functions inherited from g2o::OptimizableGraph
static bool	initMultiThreading ()

Static Public Attributes inherited from g2o::HyperGraph
static const int	UnassignedId = -1

static const int	InvalidId = -2

Protected Member Functions inherited from g2o::SparseOptimizer
void	sortVectorContainers ()

bool	buildIndexMapping (SparseOptimizer::VertexContainer &vlist)

void	clearIndexMapping ()

Detailed Description

Definition at line 37 of file graph_optimizer_sparse_online.h.

Constructor & Destructor Documentation

g2o::SparseOptimizerOnline::SparseOptimizerOnline ( bool pcg = false )

explicit

Definition at line 62 of file graph_optimizer_sparse_online.cpp.

                                                      :
   SparseOptimizer(),
   slamDimension(3), newEdges(0), batchStep(true), vizWithGnuplot(false),
   _gnuplot(0), _usePcg(pcg), _underlyingSolver(0)
 {
 }

g2o::SparseOptimizerOnline::~SparseOptimizerOnline ( )

virtual

Definition at line 69 of file graph_optimizer_sparse_online.cpp.

References _gnuplot.

 {
   if (_gnuplot) {
 #ifdef WINDOWS
     _pclose(_gnuplot);
 #else
     pclose(_gnuplot);
 #endif
   }
 }

Member Function Documentation

void g2o::SparseOptimizerOnline::gnuplotVisualization ( )

virtual

Definition at line 239 of file graph_optimizer_sparse_online.cpp.

References _gnuplot, g2o::HyperGraph::edges(), slamDimension, g2o::SE2::translation(), g2o::OnlineVertexSE3::updatedEstimate, g2o::OnlineVertexSE2::updatedEstimate, and g2o::HyperGraph::Edge::vertices().

Referenced by optimize().

 {
   if (slamDimension == 3) {
     if (! _gnuplot) {
 #ifdef WINDOWS
       _gnuplot = _popen("gnuplot -persistent", "w");
 #else
       _gnuplot = popen("gnuplot -persistent", "w");
 #endif
       if (_gnuplot == 0)
         return;
       fprintf(_gnuplot, "set terminal X11 noraise\n");
       fprintf(_gnuplot, "set size ratio -1\n");
     }
     fprintf(_gnuplot, "plot \"-\" w l\n");
     for (EdgeSet::iterator it = edges().begin(); it != edges().end(); ++it) {
       OnlineEdgeSE2* e = (OnlineEdgeSE2*) *it;
       OnlineVertexSE2* v1 = (OnlineVertexSE2*) e->vertices()[0];
       OnlineVertexSE2* v2 = (OnlineVertexSE2*) e->vertices()[1];
       fprintf(_gnuplot, "%f %f\n", v1->updatedEstimate.translation().x(), v1->updatedEstimate.translation().y());
       fprintf(_gnuplot, "%f %f\n\n", v2->updatedEstimate.translation().x(), v2->updatedEstimate.translation().y());
     }
     fprintf(_gnuplot, "e\n");
   }
   if (slamDimension == 6) {
     if (! _gnuplot) {
 #ifdef WINDOWS
       _gnuplot = _popen("gnuplot -persistent", "w");
 #else
       _gnuplot = popen("gnuplot -persistent", "w");
 #endif
       if (_gnuplot == 0)
         return;
       fprintf(_gnuplot, "set terminal X11 noraise\n");
     }
     fprintf(_gnuplot, "splot \"-\" w l\n");
     for (EdgeSet::iterator it = edges().begin(); it != edges().end(); ++it) {
       OnlineEdgeSE3* e = (OnlineEdgeSE3*) *it;
       OnlineVertexSE3* v1 = (OnlineVertexSE3*) e->vertices()[0];
       OnlineVertexSE3* v2 = (OnlineVertexSE3*) e->vertices()[1];
       fprintf(_gnuplot, "%f %f %f\n", v1->updatedEstimate.translation().x(), v1->updatedEstimate.translation().y(), v1->updatedEstimate.translation().z());
       fprintf(_gnuplot, "%f %f %f \n\n\n", v2->updatedEstimate.translation().x(), v2->updatedEstimate.translation().y(), v2->updatedEstimate.translation().z());
     }
     fprintf(_gnuplot, "e\n");
   }
 }

bool g2o::SparseOptimizerOnline::initSolver	(	int	dimension,
		int	batchEveryN
	)

virtual

Reimplemented in g2o::SparseOptimizerIncremental.

Definition at line 206 of file graph_optimizer_sparse_online.cpp.

References _underlyingSolver, _usePcg, g2o::OptimizationAlgorithmFactory::construct(), g2o::createSolver(), g2o::OptimizationAlgorithmFactory::instance(), g2o::SparseOptimizer::setAlgorithm(), slamDimension, and g2o::SparseOptimizer::solver().

Referenced by g2o::G2oSlamInterface::addNode().

 {
   slamDimension = dimension;
   OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
   OptimizationAlgorithmProperty solverProperty;
   if (_usePcg) {
     Solver* s = 0;
     if (dimension == 3) {
       s = createSolver("pcg3_2");
     } else {
       s = createSolver("pcg6_3");
     }
     OptimizationAlgorithmGaussNewton* gaussNewton = new OptimizationAlgorithmGaussNewton(s);
     setAlgorithm(gaussNewton);
   }
   else {
     if (dimension == 3) {
       setAlgorithm(solverFactory->construct("gn_fix3_2_cholmod", solverProperty));
     } else {
       setAlgorithm(solverFactory->construct("gn_fix6_3_cholmod", solverProperty));
     }
   }
 
   OptimizationAlgorithmGaussNewton* gaussNewton = dynamic_cast<OptimizationAlgorithmGaussNewton*>(solver());
   _underlyingSolver = gaussNewton->solver();
 
   if (! solver()) {
     cerr << "Error allocating solver. Allocating CHOLMOD solver failed!" << endl;
     return false;
   }
   return true;
 }

int g2o::SparseOptimizerOnline::optimize	(	int	iterations,
		bool	online = `false`
	)

virtual

starts one optimization run given the current configuration of the graph, and the current settings stored in the class instance. It can be called only after initializeOptimization

Reimplemented from g2o::SparseOptimizer.

Definition at line 80 of file graph_optimizer_sparse_online.cpp.

References __PRETTY_FUNCTION__, g2o::SparseOptimizer::_activeEdges, g2o::SparseOptimizer::_algorithm, _underlyingSolver, _usePcg, g2o::SparseOptimizer::activeChi2(), g2o::Solver::b(), batchStep, g2o::Solver::buildStructure(), g2o::Solver::buildSystem(), g2o::OptimizableGraph::Vertex::colInHessian(), g2o::SparseOptimizer::computeActiveErrors(), g2o::OptimizableGraph::Edge::computeError(), g2o::OptimizableGraph::Edge::constructQuadraticForm(), g2o::OptimizableGraph::Vertex::copyB(), gnuplotVisualization(), g2o::SparseOptimizer::indexMapping(), g2o::OptimizationAlgorithm::init(), g2o::OptimizableGraph::jacobianWorkspace(), g2o::OptimizableGraph::Edge::linearizeOplus(), newEdges, g2o::BaseVertex< D, T >::setEstimate(), slamDimension, g2o::Solver::solve(), g2o::SparseOptimizer::solver(), update(), g2o::OnlineVertexSE3::updatedEstimate, g2o::OnlineVertexSE2::updatedEstimate, g2o::SparseOptimizer::verbose(), g2o::HyperGraph::vertices(), vizWithGnuplot, and g2o::Solver::x().

Referenced by g2o::G2oSlamInterface::solve().

 {
   //return SparseOptimizer::optimize(iterations, online);
 
   (void) iterations; // we only do one iteration anyhow
   OptimizationAlgorithm* solver = _algorithm;
 
   int cjIterations=0;
   bool ok=true;
 
   solver->init(online);
   if (!online) {
     ok = _underlyingSolver->buildStructure();
     if (! ok) {
       cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
       return 0;
     }
   }
 
   if (_usePcg)
     batchStep = true;
 
   if (! online || batchStep) {
     //cerr << "BATCH" << endl;
     //_underlyingSolver->buildStructure();
     // copy over the updated estimate as new linearization point
     if (slamDimension == 3) {
       for (size_t i = 0; i < indexMapping().size(); ++i) {
         OnlineVertexSE2* v = static_cast<OnlineVertexSE2*>(indexMapping()[i]);
         v->setEstimate(v->updatedEstimate);
       }
     }
     else if (slamDimension == 6) {
       for (size_t i=0; i < indexMapping().size(); ++i) {
         OnlineVertexSE3* v = static_cast<OnlineVertexSE3*>(indexMapping()[i]);
         v->setEstimate(v->updatedEstimate);
       }
     }
 
     SparseOptimizer::computeActiveErrors();
     //SparseOptimizer::linearizeSystem();
     _underlyingSolver->buildSystem();
   }
   else {
     //cerr << "UPDATE" << endl;
     // compute the active errors for the required edges
     for (HyperGraph::EdgeSet::iterator it = newEdges->begin(); it != newEdges->end(); ++it) {
       OptimizableGraph::Edge * e = static_cast<OptimizableGraph::Edge*>(*it);
       e->computeError();
     }
     // linearize the constraints and update the Hessian
     for (HyperGraph::EdgeSet::iterator it = newEdges->begin(); it != newEdges->end(); ++it) {
       OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
       e->linearizeOplus(jacobianWorkspace());
       e->constructQuadraticForm();
     }
     // update the b vector
     for (int i = 0; i < static_cast<int>(indexMapping().size()); ++i) {
       OptimizableGraph::Vertex* v = indexMapping()[i];
       int iBase = v->colInHessian();
       v->copyB(_underlyingSolver->b() + iBase);
     }
   }
   ok = _underlyingSolver->solve();
   update(_underlyingSolver->x());
 
   ++cjIterations; 
 
   if (verbose()){
     computeActiveErrors();
     cerr
       << "nodes = " << vertices().size()
       << "\t edges= " << _activeEdges.size()
       << "\t chi2= " << FIXED(activeChi2())
       << endl;
   }
 
   if (vizWithGnuplot)
     gnuplotVisualization();
 
   if (! ok)
     return 0;
   return 1;
 }

void g2o::SparseOptimizerOnline::update ( double * update )

Definition at line 165 of file graph_optimizer_sparse_online.cpp.

References g2o::SparseOptimizer::_ivMap, g2o::OnlineVertexSE3::oplusUpdatedEstimate(), g2o::OnlineVertexSE2::oplusUpdatedEstimate(), and slamDimension.

Referenced by optimize().

 {
   if (slamDimension == 3) {
     for (size_t i=0; i < _ivMap.size(); ++i) {
       OnlineVertexSE2* v= static_cast<OnlineVertexSE2*>(_ivMap[i]);
       v->oplusUpdatedEstimate(update);
       update += 3;
     }
   }
   else if (slamDimension == 6) {
     for (size_t i=0; i < _ivMap.size(); ++i) {
       OnlineVertexSE3* v= static_cast<OnlineVertexSE3*>(_ivMap[i]);
       v->oplusUpdatedEstimate(update);
       update += 6;
     }
   }
 }

bool g2o::SparseOptimizerOnline::updateInitialization	(	HyperGraph::VertexSet &	vset,
		HyperGraph::EdgeSet &	eset
	)

virtual

HACK updating the internal structures for online processing

Reimplemented from g2o::SparseOptimizer.

Reimplemented in g2o::SparseOptimizerIncremental.

Definition at line 183 of file graph_optimizer_sparse_online.cpp.

References g2o::OptimizableGraph::Vertex::clearQuadraticForm(), newEdges, and g2o::SparseOptimizer::updateInitialization().

Referenced by g2o::G2oSlamInterface::solve().

 {
   newEdges = &eset;
   bool result = SparseOptimizer::updateInitialization(vset, eset);
   for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) {
     OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
     v->clearQuadraticForm(); // be sure that b is zero for this vertex
   }
   return result;
 }

Member Data Documentation

FILE* g2o::SparseOptimizerOnline::_gnuplot

protected

Definition at line 62 of file graph_optimizer_sparse_online.h.

Referenced by gnuplotVisualization(), and ~SparseOptimizerOnline().

Solver* g2o::SparseOptimizerOnline::_underlyingSolver

protected

Definition at line 64 of file graph_optimizer_sparse_online.h.

Referenced by initSolver(), and optimize().

bool g2o::SparseOptimizerOnline::_usePcg

protected

Definition at line 63 of file graph_optimizer_sparse_online.h.

Referenced by initSolver(), and optimize().

bool g2o::SparseOptimizerOnline::batchStep

Definition at line 56 of file graph_optimizer_sparse_online.h.

Referenced by optimize(), and g2o::G2oSlamInterface::solve().

HyperGraph::EdgeSet* g2o::SparseOptimizerOnline::newEdges

Definition at line 54 of file graph_optimizer_sparse_online.h.

Referenced by optimize(), and updateInitialization().

int g2o::SparseOptimizerOnline::slamDimension

Definition at line 52 of file graph_optimizer_sparse_online.h.

Referenced by gnuplotVisualization(), initSolver(), optimize(), and update().

bool g2o::SparseOptimizerOnline::vizWithGnuplot

Definition at line 57 of file graph_optimizer_sparse_online.h.

Referenced by main(), and optimize().

The documentation for this class was generated from the following files:

/home/xuezhisd/CLionProjects/g2o/g2o/examples/interactive_slam/g2o_interactive/graph_optimizer_sparse_online.h
/home/xuezhisd/CLionProjects/g2o/g2o/examples/interactive_slam/g2o_interactive/graph_optimizer_sparse_online.cpp

Public Member Functions

Public Attributes

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation