#include <star.h>

Collaboration diagram for g2o::Star:

[legend]

Public Member Functions
	Star (int level, SparseOptimizer *optimizer)

bool	labelStarEdges (int iterations, EdgeLabeler *labeler)

int	level () const
	returns the level of the lower edges in the star More...

SparseOptimizer *	optimizer ()
	returns the optimizer More...

HyperGraph::EdgeSet &	lowLevelEdges ()
	low level edge set More...

HyperGraph::EdgeSet &	starEdges ()
	high level edge set More...

HyperGraph::EdgeSet &	starFrontierEdges ()
	edges in the high level that lead to some node owned by a different star More...

HyperGraph::VertexSet &	gauge ()
	set of nodes to keep fixed in the optimization More...

HyperGraph::VertexSet &	lowLevelVertices ()
	set of all vertices in the low level More...

Public Attributes
int	_level
	level of the star More...

SparseOptimizer *	_optimizer
	optimizer More...

HyperGraph::EdgeSet	_lowLevelEdges
	edges in the lower level More...

HyperGraph::EdgeSet	_starEdges
	edges in the star More...

HyperGraph::EdgeSet	_starFrontierEdges
	edges in the star that lead to some other star More...

HyperGraph::VertexSet	_gauge
	vertices that are fixed (center of the star) More...

HyperGraph::VertexSet	_lowLevelVertices
	vertices that are fixed (center of the star) More...

Detailed Description

Class that represents a subgraph in the hierarchical optimization. The subgraph is consisting of

a set of "central" nodes forming the gauge
a set of edges and vertices in the lower (denser) level
a set of edges in the higher level, each one going from the gauge to one of the vertices in the lower level These edges form the "star" view at the higher level

Additionally, a star provides a function to compute the parameters for each of the edges in the higher level, based on the actual configuration of the state variables. It does so by using an EdgeLabeler class.

Definition at line 26 of file star.h.

Constructor & Destructor Documentation

g2o::Star::Star	(	int	level,
		SparseOptimizer *	optimizer
	)

constructs a star at level l in the graph of the sparse optimizer passed as argument

Parameters

level	the (higher) level of the star
optimizer	the optimizer

Definition at line 7 of file star.cpp.

7 : _level(level), _optimizer(optimizer) {}

g2o::Star::optimizer

SparseOptimizer * optimizer()

returns the optimizer

Definition: star.h:41

g2o::Star::level

int level() const

returns the level of the lower edges in the star

Definition: star.h:39

g2o::Star::_optimizer

SparseOptimizer * _optimizer

optimizer

Definition: star.h:56

g2o::Star::_level

int _level

level of the star

Definition: star.h:54

Member Function Documentation

HyperGraph::VertexSet& g2o::Star::gauge ( )

inline

set of nodes to keep fixed in the optimization

Definition at line 49 of file star.h.

References _gauge.

Referenced by g2o::computeBorder(), g2o::computeSimpleStars(), and main().

49 {return _gauge;}

g2o::Star::_gauge

HyperGraph::VertexSet _gauge

vertices that are fixed (center of the star)

Definition: star.h:64

bool g2o::Star::labelStarEdges	(	int	iterations,
		EdgeLabeler *	labeler
	)

labels the edges in the star by first optimizing the low level edges, then by calling the labelEdge of the labeler.

Parameters

iterations	the number of iterations of the optimizer
labeler	the labeler

Definition at line 9 of file star.cpp.

References _gauge, _lowLevelEdges, _optimizer, _starEdges, g2o::SparseOptimizer::activeVertices(), g2o::OptimizationAlgorithmWithHessian::buildLinearStructure(), g2o::SparseOptimizer::computeActiveErrors(), g2o::SparseOptimizer::computeInitialGuess(), g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::HyperGraph::Vertex::id(), g2o::SparseOptimizer::indexMapping(), g2o::OptimizationAlgorithm::init(), g2o::SparseOptimizer::initializeOptimization(), g2o::EdgeLabeler::labelEdges(), g2o::SparseOptimizer::optimize(), optimizer(), g2o::OptimizableGraph::Vertex::pop(), g2o::OptimizableGraph::Vertex::push(), g2o::OptimizableGraph::Vertex::setFixed(), g2o::SparseOptimizer::solver(), g2o::OptimizationAlgorithmWithHessian::updateLinearSystem(), and g2o::HyperGraph::Edge::vertices().

Referenced by g2o::assignHierarchicalEdges(), and g2o::computeSimpleStars().

                                                                {
     // mark all vertices in the lowLevelEdges as floating
     bool ok=true;
     std::set<OptimizableGraph::Vertex*> vset;
     for (HyperGraph::EdgeSet::iterator it=_lowLevelEdges.begin(); it!=_lowLevelEdges.end(); it++){
       HyperGraph::Edge* e=*it;
       for (size_t i=0; i<e->vertices().size(); i++){
         OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)e->vertices()[i];
         v->setFixed(false);
         vset.insert(v);
       }
     }
     for (std::set<OptimizableGraph::Vertex*>::iterator it=vset.begin(); it!=vset.end(); it++){
       OptimizableGraph::Vertex* v = *it;
       v->push();
     }
 
     // fix all vertices in the gauge
     //cerr << "fixing gauge: ";
     for (HyperGraph::VertexSet::iterator it = _gauge.begin(); it!=_gauge.end(); it++){
       OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*it;
       //cerr << v->id() << " ";
       v->setFixed(true);
     }
     //cerr << endl;
     if (iterations>0){
       _optimizer->initializeOptimization(_lowLevelEdges);
       _optimizer->computeInitialGuess();
       int result=_optimizer->optimize(iterations);
       if (result<1){
         cerr << "Vertices num: " << _optimizer->activeVertices().size() << "ids: ";
         for (size_t i=0; i<_optimizer->indexMapping().size(); i++){
           cerr << _optimizer->indexMapping()[i]->id() << " " ;
         }
         cerr << endl;
         cerr << "!!! optimization failure" << endl;
         cerr << "star size=" << _lowLevelEdges.size() << endl;
         cerr << "gauge: ";
         for (HyperGraph::VertexSet::iterator it=_gauge.begin(); it!=_gauge.end(); it++){
           OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*)*it;
           cerr << "[" << v->id() << " " << v->hessianIndex() << "] ";
         }
         cerr << endl;
         ok=false;
       }
     }  else {
       optimizer()->initializeOptimization(_lowLevelEdges);
       // cerr << "guess" << endl;
       //optimizer()->computeInitialGuess();
       // cerr << "solver init" << endl;
       optimizer()->solver()->init();
       // cerr << "structure" << endl;
       OptimizationAlgorithmWithHessian* solverWithHessian = dynamic_cast<OptimizationAlgorithmWithHessian*> (optimizer()->solver());
       if (!solverWithHessian->buildLinearStructure())
         cerr << "FATAL: failure while building linear structure" << endl;
       // cerr << "errors" << endl;
       optimizer()->computeActiveErrors();
       // cerr << "system" << endl;
       solverWithHessian->updateLinearSystem();
     }
 
     std::set<OptimizableGraph::Edge*> star;
     for(HyperGraph::EdgeSet::iterator it=_starEdges.begin(); it!=_starEdges.end(); it++){
       star.insert((OptimizableGraph::Edge*)*it);
     }
     if (ok) {
       int result = labeler->labelEdges(star);
       if (result < 0)
         ok=false;
     }
     // release all vertices in the gauge
     for (std::set<OptimizableGraph::Vertex*>::iterator it=vset.begin(); it!=vset.end(); it++){
       OptimizableGraph::Vertex* v = *it;
       v->pop();
     }
     for (HyperGraph::VertexSet::iterator it=_gauge.begin(); it!=_gauge.end(); it++){
       OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*it;
       v->setFixed(false);
     }
 
     return ok;
   }