#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

SparseOptimizer *	optimizer ()
	returns the optimizer

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

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

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

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

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

Public Attributes
int	_level
	level of the star

SparseOptimizer *	_optimizer
	optimizer

HyperGraph::EdgeSet	_lowLevelEdges
	edges in the lower level

HyperGraph::EdgeSet	_starEdges
	edges in the star

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

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

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

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 54 of file star.h.

Constructor & Destructor Documentation

◆ Star()

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 34 of file star.cpp.

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

g2o::Star::_level

int _level

level of the star

Definition star.h:83

g2o::Star::optimizer

SparseOptimizer * optimizer()

returns the optimizer

Definition star.h:70

g2o::Star::_optimizer

SparseOptimizer * _optimizer

optimizer

Definition star.h:85

g2o::Star::level

int level() const

returns the level of the lower edges in the star

Definition star.h:68

Member Function Documentation

◆ gauge()

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

inline

set of nodes to keep fixed in the optimization

Definition at line 78 of file star.h.

78{ return _gauge; }

g2o::Star::_gauge

HyperGraph::VertexSet _gauge

vertices that are fixed (center of the star)

Definition star.h:93

References _gauge.

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

◆ labelStarEdges()

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 37 of file star.cpp.

                                                              {
  // 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;
}