#include <sparse_optimizer.h>

Inheritance diagram for g2o::SparseOptimizer:

[legend]

Collaboration diagram for g2o::SparseOptimizer:

[legend]

Public Types
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

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

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

Public Member Functions
	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 bool	updateInitialization (HyperGraph::VertexSet &vset, HyperGraph::EdgeSet &eset)

virtual void	computeInitialGuess ()

virtual void	computeInitialGuess (EstimatePropagatorCost &propagator)

virtual void	setToOrigin ()

int	optimize (int iterations, bool online=false)

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

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

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

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

bool	gaugeFreedom ()

double	activeChi2 () const

double	activeRobustChi2 () const

bool	verbose () const
	verbose information during optimization

void	setVerbose (bool verbose)

void	setForceStopFlag (bool *flag)

bool *	forceStopFlag () const

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

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

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

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

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

OptimizationAlgorithm *	solver ()

void	setAlgorithm (OptimizationAlgorithm *algorithm)

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

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

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

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

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

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

void	discardTop (SparseOptimizer::VertexContainer &vlist)

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

virtual void	clear ()

void	computeActiveErrors ()

	G2O_ATTRIBUTE_DEPRECATED (void linearizeSystem())

void	update (const double *update)

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

bool	removeComputeErrorAction (HyperGraphAction *action)

virtual void	discardTop ()

virtual void	discardTop (HyperGraph::VertexSet &vset)

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

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

	OptimizableGraph ()
	empty constructor

virtual	~OptimizableGraph ()

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

virtual bool	addVertex (HyperGraph::Vertex *v)

bool	addVertex (OptimizableGraph::Vertex v, Data userData)

bool	addVertex (OptimizableGraph::Vertex *v)

virtual bool	addEdge (HyperGraph::Edge *e)

bool	addEdge (OptimizableGraph::Edge *e)

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

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

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

void	recomputeJacobianWorkspaceSize ()

std::set< int >	dimensions () const

virtual void	preIteration (int)

virtual void	postIteration (int)

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

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

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

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

virtual bool	load (std::istream &is)

bool	load (const char *filename)

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

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

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

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

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

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

const JacobianWorkspace &	jacobianWorkspace () const

ParameterContainer &	parameters ()

const ParameterContainer &	parameters () const

void	forEachVertex (std::function< void(OptimizableGraph::Vertex *)> fn)
	apply a unary function to all vertices

void	forEachVertex (HyperGraph::VertexSet &vset, std::function< void(OptimizableGraph::Vertex *)> fn)
	apply a unary function to the vertices in vset

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

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

Vertex *	vertex (int id)

const Vertex *	vertex (int id) const

virtual bool	removeEdge (Edge *e)

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)

Protected Member Functions
void	sortVectorContainers ()

bool	buildIndexMapping (SparseOptimizer::VertexContainer &vlist)

void	clearIndexMapping ()

Protected Member Functions inherited from g2o::OptimizableGraph
void	performActions (int iter, HyperGraphActionSet &actions)

Protected Attributes
bool *	_forceStopFlag

bool	_verbose

VertexContainer	_ivMap

VertexContainer	_activeVertices
	sorted according to VertexIDCompare

EdgeContainer	_activeEdges
	sorted according to EdgeIDCompare

OptimizationAlgorithm *	_algorithm

BatchStatisticsContainer	_batchStatistics

bool	_computeBatchStatistics

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

long long	_nextEdgeId

std::vector< HyperGraphActionSet >	_graphActions

ParameterContainer	_parameters

JacobianWorkspace	_jacobianWorkspace

Protected Attributes inherited from g2o::HyperGraph
VertexIDMap	_vertices

EdgeSet	_edges

Friends
class	ActivePathCostFunction

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

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

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

static const int	InvalidId = -2

Detailed Description

Definition at line 42 of file sparse_optimizer.h.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum

Enumerator
AT_COMPUTEACTIVERROR
AT_NUM_ELEMENTS

Definition at line 44 of file sparse_optimizer.h.

       {
    AT_COMPUTEACTIVERROR = OptimizableGraph::AT_NUM_ELEMENTS,
    AT_NUM_ELEMENTS,  // keep as last element
  };

Constructor & Destructor Documentation

◆ SparseOptimizer()

g2o::SparseOptimizer::SparseOptimizer ( )

Definition at line 52 of file sparse_optimizer.cpp.

    : _forceStopFlag(0),
      _verbose(false),
      _algorithm(nullptr),
      _computeBatchStatistics(false) {
  _graphActions.resize(AT_NUM_ELEMENTS);
}

References g2o::OptimizableGraph::_graphActions, and AT_NUM_ELEMENTS.

◆ ~SparseOptimizer()

g2o::SparseOptimizer::~SparseOptimizer ( )

virtual

Definition at line 60 of file sparse_optimizer.cpp.

                                  {
  release(_algorithm);
  G2OBatchStatistics::setGlobalStats(0);
}

References _algorithm, g2o::release(), and g2o::G2OBatchStatistics::setGlobalStats().

Member Function Documentation

◆ activeChi2()

double g2o::SparseOptimizer::activeChi2 ( ) const

returns the cached chi2 of the active portion of the graph

Definition at line 94 of file sparse_optimizer.cpp.

                                         {
  double chi = 0.0;
  for (EdgeContainer::const_iterator it = _activeEdges.begin();
       it != _activeEdges.end(); ++it) {
    const OptimizableGraph::Edge* e = *it;
    chi += e->chi2();
  }
  return chi;
}

References _activeEdges, and g2o::OptimizableGraph::Edge::chi2().

Referenced by computeInitialGuess(), main(), g2o::SparseOptimizerIncremental::optimize(), and g2o::SparseOptimizerOnline::optimize().

◆ activeEdges()

const EdgeContainer & g2o::SparseOptimizer::activeEdges ( ) const

inline

the edges active in the current optimization

Definition at line 190 of file sparse_optimizer.h.

190{ return _activeEdges; }

Referenced by g2o::activeVertexChi(), g2o::EstimatePropagatorCost::operator()(), g2o::EstimatePropagatorCostOdometry::operator()(), and g2o::SolverSLAM2DLinear::solveOrientation().

◆ activeRobustChi2()

double g2o::SparseOptimizer::activeRobustChi2 ( ) const

returns the cached chi2 of the active portion of the graph. In contrast to activeChi2() this functions considers the weighting of the error according to the robustification of the error functions.

Definition at line 104 of file sparse_optimizer.cpp.

                                               {
  Vector3 rho;
  double chi = 0.0;
  for (EdgeContainer::const_iterator it = _activeEdges.begin();
       it != _activeEdges.end(); ++it) {
    const OptimizableGraph::Edge* e = *it;
    if (e->robustKernel()) {
      e->robustKernel()->robustify(e->chi2(), rho);
      chi += rho[0];
    } else
      chi += e->chi2();
  }
  return chi;
}

References _activeEdges, g2o::OptimizableGraph::Edge::chi2(), g2o::RobustKernel::robustify(), and g2o::OptimizableGraph::Edge::robustKernel().

Referenced by g2o::SparseOptimizerTerminateAction::operator()(), optimize(), g2o::OptimizationAlgorithmDogleg::solve(), and g2o::OptimizationAlgorithmLevenberg::solve().

◆ activeVertices()

const VertexContainer & g2o::SparseOptimizer::activeVertices ( ) const

inline

the vertices active in the current optimization

Definition at line 188 of file sparse_optimizer.h.

188{ return _activeVertices; }

g2o::SparseOptimizer::_activeVertices

VertexContainer _activeVertices

sorted according to VertexIDCompare

Definition sparse_optimizer.h:292

Referenced by g2o::OptimizationAlgorithmWithHessian::init(), g2o::StructureOnlySolver< PointDoF >::init(), g2o::Star::labelStarEdges(), main(), and testMarginals().

◆ addComputeErrorAction()

bool g2o::SparseOptimizer::addComputeErrorAction ( HyperGraphAction * action )

add an action to be executed before the error vectors are computed

Definition at line 614 of file sparse_optimizer.cpp.

                                                                    {
  std::pair<HyperGraphActionSet::iterator, bool> insertResult =
      _graphActions[AT_COMPUTEACTIVERROR].insert(action);
  return insertResult.second;
}

References g2o::OptimizableGraph::_graphActions, and AT_COMPUTEACTIVERROR.

◆ algorithm()

const OptimizationAlgorithm * g2o::SparseOptimizer::algorithm ( ) const

inline

the solver used by the optimizer

Definition at line 214 of file sparse_optimizer.h.

214{ return _algorithm; }

Referenced by setAlgorithm().

◆ batchStatistics() [1/2]

BatchStatisticsContainer & g2o::SparseOptimizer::batchStatistics ( )

inline

returns the set of batch statistics about the optimisation

Definition at line 274 of file sparse_optimizer.h.

274{ return _batchStatistics; }

g2o::SparseOptimizer::_batchStatistics

BatchStatisticsContainer _batchStatistics

Definition sparse_optimizer.h:307

◆ batchStatistics() [2/2]

const BatchStatisticsContainer & g2o::SparseOptimizer::batchStatistics ( ) const

inline

returns the set of batch statistics about the optimisation

Definition at line 268 of file sparse_optimizer.h.

                                                          {
    return _batchStatistics;
  }

Referenced by main().

◆ buildIndexMapping()

bool g2o::SparseOptimizer::buildIndexMapping ( SparseOptimizer::VertexContainer & vlist )

protected

builds the mapping of the active vertices to the (block) row / column in the Hessian

Definition at line 162 of file sparse_optimizer.cpp.

                                           {
  if (!vlist.size()) {
    _ivMap.clear();
    return false;
  }
 
  _ivMap.resize(vlist.size());
  size_t i = 0;
  for (int k = 0; k < 2; k++)
    for (VertexContainer::iterator it = vlist.begin(); it != vlist.end();
         ++it) {
      OptimizableGraph::Vertex* v = *it;
      if (!v->fixed()) {
        if (static_cast<int>(v->marginalized()) == k) {
          v->setHessianIndex(i);
          _ivMap[i] = v;
          i++;
        }
      } else {
        v->setHessianIndex(-1);
      }
    }
  _ivMap.resize(i);
  return true;
}

References _ivMap, g2o::OptimizableGraph::Vertex::fixed(), g2o::OptimizableGraph::Vertex::marginalized(), g2o::HyperGraph::Edge::resize(), and g2o::OptimizableGraph::Vertex::setHessianIndex().

Referenced by initializeOptimization(), and initializeOptimization().

◆ clear()

void g2o::SparseOptimizer::clear ( )

virtual

clears the graph, and polishes some intermediate structures Note that this only removes nodes / edges. Parameters can be removed with clearParameters().

Reimplemented from g2o::HyperGraph.

Definition at line 513 of file sparse_optimizer.cpp.

                            {
  _ivMap.clear();
  _activeVertices.clear();
  _activeEdges.clear();
  OptimizableGraph::clear();
}

References _activeEdges, _activeVertices, _ivMap, and g2o::HyperGraph::clear().

Referenced by g2o::BlockSolver< Traits >::init(), main(), main(), and g2o::Gm2dlIO::readGm2dl().

◆ clearIndexMapping()

void g2o::SparseOptimizer::clearIndexMapping ( )

protected

Definition at line 189 of file sparse_optimizer.cpp.

                                        {
  for (size_t i = 0; i < _ivMap.size(); ++i) {
    _ivMap[i]->setHessianIndex(-1);
    _ivMap[i] = 0;
  }
}

References _ivMap.

Referenced by initializeOptimization(), initializeOptimization(), and removeVertex().

◆ computeActiveErrors()

void g2o::SparseOptimizer::computeActiveErrors ( )

computes the error vectors of all edges in the activeSet, and caches them

Definition at line 65 of file sparse_optimizer.cpp.

                                          {
  // call the callbacks in case there is something registered
  HyperGraphActionSet& actions = _graphActions[AT_COMPUTEACTIVERROR];
  if (actions.size() > 0) {
    for (HyperGraphActionSet::iterator it = actions.begin();
         it != actions.end(); ++it)
      (*(*it))(this);
  }
 
#ifdef G2O_OPENMP
#pragma omp parallel for default(shared) if (_activeEdges.size() > 50)
#endif
  for (int k = 0; k < static_cast<int>(_activeEdges.size()); ++k) {
    OptimizableGraph::Edge* e = _activeEdges[k];
    e->computeError();
  }
 
#ifndef NDEBUG
  for (int k = 0; k < static_cast<int>(_activeEdges.size()); ++k) {
    OptimizableGraph::Edge* e = _activeEdges[k];
    bool hasNan = arrayHasNaN(e->errorData(), e->dimension());
    if (hasNan) {
      G2O_WARN("computeActiveErrors(): found NaN in error for edge {}",
               static_cast<void*>(e));
    }
  }
#endif
}

References _activeEdges, g2o::OptimizableGraph::_graphActions, g2o::arrayHasNaN(), AT_COMPUTEACTIVERROR, g2o::OptimizableGraph::Edge::computeError(), g2o::OptimizableGraph::Edge::dimension(), g2o::OptimizableGraph::Edge::errorData(), and G2O_WARN.

Referenced by computeInitialGuess(), g2o::computeSimpleStars(), g2o::Star::labelStarEdges(), main(), main(), optimize(), g2o::SparseOptimizerIncremental::optimize(), g2o::SparseOptimizerOnline::optimize(), g2o::OptimizationAlgorithmDogleg::solve(), g2o::OptimizationAlgorithmGaussNewton::solve(), and g2o::OptimizationAlgorithmLevenberg::solve().

◆ computeBatchStatistics()

bool g2o::SparseOptimizer::computeBatchStatistics ( ) const

inline

Definition at line 278 of file sparse_optimizer.h.

278{ return _computeBatchStatistics; }

Referenced by setComputeBatchStatistics().

◆ computeInitialGuess() [1/2]

void g2o::SparseOptimizer::computeInitialGuess ( )

virtual

Propagates an initial guess from the vertex specified as origin. It should be called after initializeOptimization(...), as it relies on the _activeVertices/_edges structures. It constructs a set of trees starting from the nodes in the graph which are fixed and eligible for preforming optimization. The trees are constructed by utlizing a cost-function specified.

Parameters

costFunction the cost function used @patam maxDistance: the distance where to stop the search

Definition at line 320 of file sparse_optimizer.cpp.

                                          {
  EstimatePropagator::PropagateCost costFunction(this);
  computeInitialGuess(costFunction);
}

References computeInitialGuess().

Referenced by computeInitialGuess(), g2o::computeSimpleStars(), g2o::Star::labelStarEdges(), and main().

◆ computeInitialGuess() [2/2]

void g2o::SparseOptimizer::computeInitialGuess ( EstimatePropagatorCost & propagator )

virtual

Same as above but using a specific propagator

Definition at line 325 of file sparse_optimizer.cpp.

                                          {
  OptimizableGraph::VertexSet emptySet;
  std::set<Vertex*> backupVertices;
  HyperGraph::VertexSet fixedVertices;  // these are the root nodes where to
                                        // start the initialization
  for (EdgeContainer::iterator it = _activeEdges.begin();
       it != _activeEdges.end(); ++it) {
    OptimizableGraph::Edge* e = *it;
    for (size_t i = 0; i < e->vertices().size(); ++i) {
      OptimizableGraph::Vertex* v =
          static_cast<OptimizableGraph::Vertex*>(e->vertex(i));
      if (!v) continue;
      if (v->fixed())
        fixedVertices.insert(v);
      else {  // check for having a prior which is able to fully initialize a
              // vertex
        for (EdgeSet::const_iterator vedgeIt = v->edges().begin();
             vedgeIt != v->edges().end(); ++vedgeIt) {
          OptimizableGraph::Edge* vedge =
              static_cast<OptimizableGraph::Edge*>(*vedgeIt);
          if (vedge->vertices().size() == 1 &&
              vedge->initialEstimatePossible(emptySet, v) > 0.) {
            vedge->initialEstimate(emptySet, v);
            fixedVertices.insert(v);
          }
        }
      }
      if (v->hessianIndex() == -1) {
        std::set<Vertex*>::const_iterator foundIt = backupVertices.find(v);
        if (foundIt == backupVertices.end()) {
          v->push();
          backupVertices.insert(v);
        }
      }
    }
  }
 
  EstimatePropagator estimatePropagator(this);
  estimatePropagator.propagate(fixedVertices, costFunction);
 
  // restoring the vertices that should not be initialized
  for (std::set<Vertex*>::iterator it = backupVertices.begin();
       it != backupVertices.end(); ++it) {
    Vertex* v = *it;
    v->pop();
  }
  if (verbose()) {
    computeActiveErrors();
    cerr << "iteration= -1\t chi2= " << activeChi2() << "\t time= 0.0"
         << "\t cumTime= 0.0"
         << "\t (using initial guess from " << costFunction.name() << ")"
         << endl;
  }
}

References _activeEdges, activeChi2(), computeActiveErrors(), g2o::HyperGraph::Vertex::edges(), g2o::OptimizableGraph::Vertex::fixed(), g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::OptimizableGraph::Edge::initialEstimate(), g2o::OptimizableGraph::Edge::initialEstimatePossible(), g2o::EstimatePropagatorCost::name(), g2o::EstimatePropagator::propagate(), g2o::OptimizableGraph::Vertex::push(), verbose(), g2o::HyperGraph::Edge::vertex(), and g2o::HyperGraph::Edge::vertices().

◆ computeMarginals() [1/3]

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

computes the blocks of the inverse of the specified pattern. the pattern is given via pairs <row, col> of the blocks in the hessian

Parameters

blockIndices	the pattern
spinv	the sparse block matrix with the result

Returns: false if the operation is not supported by the solver

Definition at line 576 of file sparse_optimizer.cpp.

                                                       {
  return _algorithm->computeMarginals(spinv, blockIndices);
}

References _algorithm, and g2o::OptimizationAlgorithm::computeMarginals().

Referenced by computeMarginals(), computeMarginals(), g2o::EdgeLabeler::computePartialInverse(), main(), main(), and testMarginals().

◆ computeMarginals() [2/3]

bool g2o::SparseOptimizer::computeMarginals	(	SparseBlockMatrix< MatrixX > &	spinv,
		const Vertex *	vertex
	)

computes the inverse of the specified vertex.

Parameters

vertex	the vertex whose state is to be marginalised
spinv	the sparse block matrix with the result

Returns: false if the operation is not supported by the solver

Definition at line 582 of file sparse_optimizer.cpp.

                                                             {
  if (vertex->hessianIndex() < 0) {
    return false;
  }
  std::vector<std::pair<int, int> > index{
      std::make_pair<int, int>(vertex->hessianIndex(), vertex->hessianIndex())};
  return computeMarginals(spinv, index);
}

References computeMarginals(), and g2o::OptimizableGraph::vertex().

◆ computeMarginals() [3/3]

bool g2o::SparseOptimizer::computeMarginals	(	SparseBlockMatrix< MatrixX > &	spinv,
		const VertexContainer &	vertices
	)

computes the inverse of the set specified vertices, assembled into a single covariance matrix.

Parameters

vertex	the pattern
spinv	the sparse block matrix with the result

Returns: false if the operation is not supported by the solver

Definition at line 592 of file sparse_optimizer.cpp.

                                                                        {
  std::vector<std::pair<int, int> > indices;
  indices.reserve(vertices.size());
  for (VertexContainer::const_iterator it = vertices.begin();
       it != vertices.end(); ++it) {
    indices.emplace_back((*it)->hessianIndex(), (*it)->hessianIndex());
  }
  return computeMarginals(spinv, indices);
}

References computeMarginals(), and g2o::HyperGraph::vertices().

◆ discardTop() [1/4]

void g2o::SparseOptimizer::discardTop ( )

virtual

same as above, but for the active vertices

Reimplemented from g2o::OptimizableGraph.

Definition at line 628 of file sparse_optimizer.cpp.

628{ discardTop(_activeVertices); }

g2o::SparseOptimizer::discardTop

void discardTop()

same as above, but for the active vertices

Definition sparse_optimizer.cpp:628

References _activeVertices, and discardTop().

Referenced by discardTop().

◆ discardTop() [2/4]

void g2o::OptimizableGraph::discardTop ( )

virtual

discard the last backup of the estimate for all variables by removing it from the stack

Reimplemented from g2o::OptimizableGraph.

Definition at line 665 of file optimizable_graph.cpp.

                                  {
  forEachVertex([](OptimizableGraph::Vertex* v) { v->discardTop(); });
}

References g2o::OptimizationAlgorithmProperty::landmarkDim, g2o::OptimizationAlgorithmProperty::poseDim, and g2o::OptimizationAlgorithmProperty::requiresMarginalize.

◆ discardTop() [3/4]

void g2o::OptimizableGraph::discardTop ( HyperGraph::VertexSet & vset )

virtual

ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate

Reimplemented from g2o::OptimizableGraph.

Definition at line 688 of file optimizable_graph.cpp.

                                                           {
  forEachVertex(vset, [](OptimizableGraph::Vertex* v) { v->discardTop(); });
}

◆ discardTop() [4/4]

void g2o::SparseOptimizer::discardTop ( SparseOptimizer::VertexContainer & vlist )

ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate

Definition at line 556 of file sparse_optimizer.cpp.

                                                                      {
  for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
    (*it)->discardTop();
}

Referenced by g2o::OptimizationAlgorithmDogleg::solve(), and g2o::OptimizationAlgorithmLevenberg::solve().

◆ findActiveEdge()

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

search for an edge in _activeEdges and return the iterator pointing to it getActiveEdges().end() if not found

Definition at line 529 of file sparse_optimizer.cpp.

                                         {
  EdgeContainer::const_iterator lower =
      lower_bound(_activeEdges.begin(), _activeEdges.end(), e, EdgeIDCompare());
  if (lower == _activeEdges.end()) return _activeEdges.end();
  if ((*lower) == e) return lower;
  return _activeEdges.end();
}

References _activeEdges.

Referenced by g2o::activeVertexChi(), g2o::EstimatePropagatorCost::operator()(), and g2o::EstimatePropagatorCostOdometry::operator()().

◆ findActiveVertex()

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

search for an edge in _activeVertices and return the iterator pointing to it getActiveVertices().end() if not found

Definition at line 521 of file sparse_optimizer.cpp.

                                                                       {
  VertexContainer::const_iterator lower = lower_bound(
      _activeVertices.begin(), _activeVertices.end(), v, VertexIDCompare());
  if (lower == _activeVertices.end()) return _activeVertices.end();
  if ((*lower) == v) return lower;
  return _activeVertices.end();
}

References _activeVertices.

◆ findGauge()

OptimizableGraph::Vertex * g2o::SparseOptimizer::findGauge ( )

virtual

finds a gauge in the graph to remove the undefined dof.

Definition at line 119 of file sparse_optimizer.cpp.

                                                   {
  if (vertices().empty()) return nullptr;
 
  int maxDim = maxDimension();
 
  OptimizableGraph::Vertex* rut = 0;
  for (HyperGraph::VertexIDMap::iterator it = vertices().begin();
       it != vertices().end(); ++it) {
    OptimizableGraph::Vertex* v =
        static_cast<OptimizableGraph::Vertex*>(it->second);
    if (v->dimension() == maxDim) {
      rut = v;
      break;
    }
  }
  return rut;
}

References g2o::OptimizableGraph::Vertex::dimension(), g2o::OptimizableGraph::maxDimension(), and g2o::HyperGraph::vertices().

Referenced by main().

◆ forceStopFlag()

bool * g2o::SparseOptimizer::forceStopFlag ( ) const

inline

Definition at line 180 of file sparse_optimizer.h.

180{ return _forceStopFlag; };

Referenced by g2o::SparseOptimizerTerminateAction::setOptimizerStopFlag().

◆ G2O_ATTRIBUTE_DEPRECATED()

g2o::SparseOptimizer::G2O_ATTRIBUTE_DEPRECATED ( void linearizeSystem() )

inline

Linearizes the system by computing the Jacobians for the nodes and edges in the graph

Definition at line 254 of file sparse_optimizer.h.

                                                   {
    // nothing needed, linearization is now done inside the solver
  }

◆ gaugeFreedom()

bool g2o::SparseOptimizer::gaugeFreedom ( )

Definition at line 137 of file sparse_optimizer.cpp.

                                   {
  if (vertices().empty()) return false;
 
  int maxDim = maxDimension();
 
  for (HyperGraph::VertexIDMap::iterator it = vertices().begin();
       it != vertices().end(); ++it) {
    OptimizableGraph::Vertex* v =
        static_cast<OptimizableGraph::Vertex*>(it->second);
    if (v->dimension() == maxDim) {
      // test for fixed vertex
      if (v->fixed()) {
        return false;
      }
      // test for full dimension prior
      for (HyperGraph::EdgeSet::const_iterator eit = v->edges().begin();
           eit != v->edges().end(); ++eit) {
        OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*eit);
        if (e->vertices().size() == 1 && e->dimension() == maxDim) return false;
      }
    }
  }
  return true;
}

References g2o::OptimizableGraph::Vertex::dimension(), g2o::OptimizableGraph::Edge::dimension(), g2o::HyperGraph::Vertex::edges(), g2o::OptimizableGraph::Vertex::fixed(), g2o::OptimizableGraph::maxDimension(), g2o::HyperGraph::Edge::vertices(), and g2o::HyperGraph::vertices().

Referenced by main().

◆ indexMapping()

const VertexContainer & g2o::SparseOptimizer::indexMapping ( ) const

inline

the index mapping of the vertices

Definition at line 186 of file sparse_optimizer.h.

186{ return _ivMap; }

Referenced by g2o::OptimizationAlgorithmLevenberg::computeLambdaInit(), g2o::Star::labelStarEdges(), g2o::SparseOptimizerIncremental::optimize(), g2o::SparseOptimizerOnline::optimize(), and g2o::SolverSLAM2DLinear::solveOrientation().

◆ initializeOptimization() [1/3]

bool g2o::SparseOptimizer::initializeOptimization ( HyperGraph::EdgeSet & eset )

virtual

Initializes the structures for optimizing a portion of the graph specified by a subset of edges. Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the schur complement or to set as fixed during the optimization.

Parameters

eset	the subgraph to be optimized.

Returns: false if somethings goes wrong

Definition at line 277 of file sparse_optimizer.cpp.

                                                                    {
  preIteration(-1);
  bool workspaceAllocated = _jacobianWorkspace.allocate();
  (void)workspaceAllocated;
  assert(workspaceAllocated &&
         "Error while allocating memory for the Jacobians");
  clearIndexMapping();
  _activeVertices.clear();
  _activeEdges.clear();
  _activeEdges.reserve(eset.size());
  set<Vertex*> auxVertexSet;  // temporary structure to avoid duplicates
  for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end();
       ++it) {
    OptimizableGraph::Edge* e = (OptimizableGraph::Edge*)(*it);
    if (e->numUndefinedVertices()) continue;
    for (vector<HyperGraph::Vertex*>::const_iterator vit =
             e->vertices().begin();
         vit != e->vertices().end(); ++vit) {
      auxVertexSet.insert(static_cast<OptimizableGraph::Vertex*>(*vit));
    }
    _activeEdges.push_back(reinterpret_cast<OptimizableGraph::Edge*>(*it));
  }
 
  _activeVertices.reserve(auxVertexSet.size());
  for (set<Vertex*>::iterator it = auxVertexSet.begin();
       it != auxVertexSet.end(); ++it)
    _activeVertices.push_back(*it);
 
  sortVectorContainers();
  bool indexMappingStatus = buildIndexMapping(_activeVertices);
  postIteration(-1);
  return indexMappingStatus;
}

References _activeEdges, _activeVertices, g2o::OptimizableGraph::_jacobianWorkspace, g2o::JacobianWorkspace::allocate(), buildIndexMapping(), clearIndexMapping(), g2o::HyperGraph::Edge::numUndefinedVertices(), g2o::OptimizableGraph::postIteration(), g2o::OptimizableGraph::preIteration(), sortVectorContainers(), and g2o::HyperGraph::Edge::vertices().

Referenced by g2o::computeSimpleStars(), initializeOptimization(), g2o::Star::labelStarEdges(), main(), main(), main(), and g2o::G2oSlamInterface::solve().

◆ initializeOptimization() [2/3]

bool g2o::SparseOptimizer::initializeOptimization	(	HyperGraph::VertexSet &	vset,
		int	level = `0`
	)

virtual

Initializes the structures for optimizing a portion of the graph specified by a subset of vertices. Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the schur complement or to set as fixed during the optimization.

Parameters

vset	the subgraph to be optimized.
level	is the level (in multilevel optimization)

Returns: false if somethings goes wrong

Definition at line 204 of file sparse_optimizer.cpp.

                                                        {
  if (edges().size() == 0) {
    G2O_WARN("Attempt to initialize an empty graph");
    return false;
  }
  preIteration(-1);
  bool workspaceAllocated = _jacobianWorkspace.allocate();
  (void)workspaceAllocated;
  assert(workspaceAllocated &&
         "Error while allocating memory for the Jacobians");
  clearIndexMapping();
  _activeVertices.clear();
  _activeVertices.reserve(vset.size());
  _activeEdges.clear();
  set<Edge*> auxEdgeSet;  // temporary structure to avoid duplicates
  for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end();
       ++it) {
    OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*)*it;
    const OptimizableGraph::EdgeSet& vEdges = v->edges();
    // count if there are edges in that level. If not remove from the pool
    int levelEdges = 0;
    for (OptimizableGraph::EdgeSet::const_iterator it = vEdges.begin();
         it != vEdges.end(); ++it) {
      OptimizableGraph::Edge* e =
          reinterpret_cast<OptimizableGraph::Edge*>(*it);
      if (level < 0 || e->level() == level) {
        bool allVerticesOK = true;
        for (vector<HyperGraph::Vertex*>::const_iterator vit =
                 e->vertices().begin();
             vit != e->vertices().end(); ++vit) {
          if (vset.find(*vit) == vset.end()) {
            allVerticesOK = false;
            break;
          }
        }
        if (allVerticesOK && !e->allVerticesFixed()) {
          auxEdgeSet.insert(e);
          levelEdges++;
        }
      }
    }
    if (levelEdges) {
      _activeVertices.push_back(v);
 
      // test for NANs in the current estimate if we are debugging
#ifndef NDEBUG
      int estimateDim = v->estimateDimension();
      if (estimateDim > 0) {
        VectorX estimateData(estimateDim);
        if (v->getEstimateData(estimateData.data()) == true) {
          int k;
          bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k);
          if (hasNan)
            G2O_WARN("{}: Vertex {} contains a nan entry at index {}",
                     __PRETTY_FUNCTION__, v->id(), k);
        }
      }
#endif
    }
  }
 
  _activeEdges.reserve(auxEdgeSet.size());
  for (set<Edge*>::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end();
       ++it)
    _activeEdges.push_back(*it);
 
  sortVectorContainers();
  bool indexMappingStatus = buildIndexMapping(_activeVertices);
  postIteration(-1);
  return indexMappingStatus;
}

References __PRETTY_FUNCTION__, _activeEdges, _activeVertices, g2o::OptimizableGraph::_jacobianWorkspace, g2o::JacobianWorkspace::allocate(), g2o::OptimizableGraph::Edge::allVerticesFixed(), g2o::arrayHasNaN(), buildIndexMapping(), clearIndexMapping(), g2o::HyperGraph::Vertex::edges(), g2o::HyperGraph::edges(), g2o::OptimizableGraph::Vertex::estimateDimension(), G2O_WARN, g2o::OptimizableGraph::Vertex::getEstimateData(), g2o::HyperGraph::Vertex::id(), g2o::OptimizableGraph::postIteration(), g2o::OptimizableGraph::preIteration(), sortVectorContainers(), and g2o::HyperGraph::Edge::vertices().

◆ initializeOptimization() [3/3]

bool g2o::SparseOptimizer::initializeOptimization ( int level = 0 )

virtual

Initializes the structures for optimizing the whole graph. Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the schur complement or to set as fixed during the optimization.

Parameters

level is the level (in multilevel optimization)

Returns: false if somethings goes wrong

Definition at line 196 of file sparse_optimizer.cpp.

                                                      {
  HyperGraph::VertexSet vset;
  for (VertexIDMap::iterator it = vertices().begin(); it != vertices().end();
       ++it)
    vset.insert(it->second);
  return initializeOptimization(vset, level);
}

References initializeOptimization(), and g2o::HyperGraph::vertices().

◆ optimize()

int g2o::SparseOptimizer::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::OptimizableGraph.

Reimplemented in g2o::SparseOptimizerIncremental, and g2o::SparseOptimizerOnline.

Definition at line 381 of file sparse_optimizer.cpp.

                                                         {
  if (_ivMap.size() == 0) {
    G2O_WARN(
        "{}: 0 vertices to optimize, maybe forgot to call "
        "initializeOptimization()",
        __PRETTY_FUNCTION__);
    return -1;
  }
 
  int cjIterations = 0;
  double cumTime = 0;
  bool ok = true;
 
  ok = _algorithm->init(online);
  if (!ok) {
    G2O_ERROR("{}: Error while initializing", __PRETTY_FUNCTION__);
    return -1;
  }
 
  _batchStatistics.clear();
  if (_computeBatchStatistics) _batchStatistics.resize(iterations);
 
  OptimizationAlgorithm::SolverResult result = OptimizationAlgorithm::OK;
  for (int i = 0; i < iterations && !terminate() && ok; i++) {
    preIteration(i);
 
    if (_computeBatchStatistics) {
      G2OBatchStatistics& cstat = _batchStatistics[i];
      G2OBatchStatistics::setGlobalStats(&cstat);
      cstat.iteration = i;
      cstat.numEdges = _activeEdges.size();
      cstat.numVertices = _activeVertices.size();
    }
 
    double ts = get_monotonic_time();
    result = _algorithm->solve(i, online);
    ok = (result == OptimizationAlgorithm::OK);
 
    bool errorComputed = false;
    if (_computeBatchStatistics) {
      computeActiveErrors();
      errorComputed = true;
      _batchStatistics[i].chi2 = activeRobustChi2();
      _batchStatistics[i].timeIteration = get_monotonic_time() - ts;
    }
 
    if (verbose()) {
      double dts = get_monotonic_time() - ts;
      cumTime += dts;
      if (!errorComputed) computeActiveErrors();
      cerr << "iteration= " << i << "\t chi2= " << FIXED(activeRobustChi2())
           << "\t time= " << dts << "\t cumTime= " << cumTime
           << "\t edges= " << _activeEdges.size();
      _algorithm->printVerbose(cerr);
      cerr << endl;
    }
    ++cjIterations;
    postIteration(i);
  }
  if (result == OptimizationAlgorithm::Fail) {
    return 0;
  }
  return cjIterations;
}

References __PRETTY_FUNCTION__, _activeEdges, _activeVertices, _algorithm, _batchStatistics, _computeBatchStatistics, _ivMap, activeRobustChi2(), computeActiveErrors(), G2O_ERROR, G2O_WARN, g2o::get_monotonic_time(), g2o::OptimizationAlgorithm::init(), g2o::G2OBatchStatistics::iteration, g2o::G2OBatchStatistics::numEdges, g2o::G2OBatchStatistics::numVertices, g2o::OptimizableGraph::postIteration(), g2o::OptimizableGraph::preIteration(), g2o::OptimizationAlgorithm::printVerbose(), g2o::G2OBatchStatistics::setGlobalStats(), g2o::OptimizationAlgorithm::solve(), terminate(), and verbose().

Referenced by g2o::computeSimpleStars(), g2o::Star::labelStarEdges(), main(), main(), and main().

◆ pop() [1/3]

void g2o::SparseOptimizer::pop ( )

virtual

pop (restore) the estimate of the active vertices from the stack

Reimplemented from g2o::OptimizableGraph.

Definition at line 626 of file sparse_optimizer.cpp.

626{ pop(_activeVertices); }

g2o::SparseOptimizer::pop

void pop()

pop (restore) the estimate of the active vertices from the stack

Definition sparse_optimizer.cpp:626

References _activeVertices, and pop().

Referenced by pop().

◆ pop() [2/3]

void g2o::SparseOptimizer::pop ( HyperGraph::VertexSet & vlist )

virtual

pop (restore) the estimate a subset of the variables from the stack

Reimplemented from g2o::OptimizableGraph.

Definition at line 552 of file sparse_optimizer.cpp.

                                                    {
  OptimizableGraph::pop(vlist);
}

References g2o::OptimizableGraph::pop().

◆ pop() [3/3]

void g2o::SparseOptimizer::pop ( SparseOptimizer::VertexContainer & vlist )

pop (restore) the estimate a subset of the variables from the stack

Definition at line 543 of file sparse_optimizer.cpp.

                                                               {
  for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
    (*it)->pop();
}

Referenced by g2o::computeSimpleStars(), g2o::OptimizationAlgorithmDogleg::solve(), and g2o::OptimizationAlgorithmLevenberg::solve().

◆ push() [1/3]

void g2o::SparseOptimizer::push ( )

virtual

push all the active vertices onto a stack

Reimplemented from g2o::OptimizableGraph.

Definition at line 624 of file sparse_optimizer.cpp.

624{ push(_activeVertices); }

g2o::SparseOptimizer::push

void push()

push all the active vertices onto a stack

Definition sparse_optimizer.cpp:624

References _activeVertices, and push().

Referenced by push().

◆ push() [2/3]

void g2o::SparseOptimizer::push ( HyperGraph::VertexSet & vlist )

virtual

push the estimate of a subset of the variables onto a stack

Reimplemented from g2o::OptimizableGraph.

Definition at line 548 of file sparse_optimizer.cpp.

                                                     {
  OptimizableGraph::push(vlist);
}

References g2o::OptimizableGraph::push().

◆ push() [3/3]

void g2o::SparseOptimizer::push ( SparseOptimizer::VertexContainer & vlist )

push the estimate of a subset of the variables onto a stack

Definition at line 538 of file sparse_optimizer.cpp.

                                                                {
  for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
    (*it)->push();
}

Referenced by g2o::computeSimpleStars(), g2o::OptimizationAlgorithmDogleg::solve(), and g2o::OptimizationAlgorithmLevenberg::solve().

◆ removeComputeErrorAction()

bool g2o::SparseOptimizer::removeComputeErrorAction ( HyperGraphAction * action )

remove an action that should no longer be executed before computing the error vectors

Definition at line 620 of file sparse_optimizer.cpp.

                                                                       {
  return _graphActions[AT_COMPUTEACTIVERROR].erase(action) > 0;
}

References g2o::OptimizableGraph::_graphActions, and AT_COMPUTEACTIVERROR.

◆ removeVertex()

bool g2o::SparseOptimizer::removeVertex	(	HyperGraph::Vertex *	v,
		bool	detach = `false`
	)

virtual

Remove a vertex. If the vertex is contained in the currently active set of vertices, then the internal temporary structures are cleaned, e.g., the index mapping is erased. In case you need the index mapping for manipulating the graph, you have to store it in your own copy.

Reimplemented from g2o::HyperGraph.

Definition at line 605 of file sparse_optimizer.cpp.

                                                                   {
  OptimizableGraph::Vertex* vv = static_cast<OptimizableGraph::Vertex*>(v);
  if (vv->hessianIndex() >= 0) {
    clearIndexMapping();
    _ivMap.clear();
  }
  return HyperGraph::removeVertex(v, detach);
}

References _ivMap, clearIndexMapping(), g2o::OptimizableGraph::Vertex::hessianIndex(), and g2o::HyperGraph::removeVertex().

◆ setAlgorithm()

void g2o::SparseOptimizer::setAlgorithm ( OptimizationAlgorithm * algorithm )

Definition at line 563 of file sparse_optimizer.cpp.

                                                                   {
  if (_algorithm) {
    // reset the optimizer for the formerly used solver in case release() won't
    // delete it
    _algorithm->setOptimizer(nullptr);
    release(_algorithm);
  }
 
  _algorithm = algorithm;
 
  if (_algorithm) _algorithm->setOptimizer(this);
}

References _algorithm, algorithm(), g2o::release(), and g2o::OptimizationAlgorithm::setOptimizer().

Referenced by g2o::allocateSolverForSclam(), g2o::SparseOptimizerIncremental::initSolver(), g2o::SparseOptimizerOnline::initSolver(), main(), main(), and main().

◆ setComputeBatchStatistics()

void g2o::SparseOptimizer::setComputeBatchStatistics ( bool computeBatchStatistics )

Definition at line 461 of file sparse_optimizer.cpp.

                                                                           {
  if ((_computeBatchStatistics == true) && (computeBatchStatistics == false)) {
    G2OBatchStatistics::setGlobalStats(0);
    _batchStatistics.clear();
  }
  _computeBatchStatistics = computeBatchStatistics;
}

References _batchStatistics, _computeBatchStatistics, computeBatchStatistics(), and g2o::G2OBatchStatistics::setGlobalStats().

Referenced by main().

◆ setForceStopFlag()

void g2o::SparseOptimizer::setForceStopFlag ( bool * flag )

sets a variable checked at every iteration to force a user stop. The iteration exits when the variable is true;

Definition at line 603 of file sparse_optimizer.cpp.

603{ _forceStopFlag = flag; }

References _forceStopFlag.

Referenced by main().

◆ setToOrigin()

void g2o::SparseOptimizer::setToOrigin ( )

virtual

sets all vertices to their origin.

Definition at line 311 of file sparse_optimizer.cpp.

                                  {
  for (VertexIDMap::iterator it = vertices().begin(); it != vertices().end();
       ++it) {
    OptimizableGraph::Vertex* v =
        static_cast<OptimizableGraph::Vertex*>(it->second);
    v->setToOrigin();
  }
}

References g2o::OptimizableGraph::Vertex::setToOrigin(), and g2o::HyperGraph::vertices().

◆ setVerbose()

void g2o::SparseOptimizer::setVerbose ( bool verbose )

Definition at line 561 of file sparse_optimizer.cpp.

561{ _verbose = verbose; }

References _verbose, and verbose().

Referenced by main(), main(), and main().

◆ solver()

OptimizationAlgorithm * g2o::SparseOptimizer::solver ( )

inline

Definition at line 215 of file sparse_optimizer.h.

215{ return _algorithm; }

Referenced by g2o::computeSimpleStars(), g2o::SparseOptimizerIncremental::initSolver(), g2o::SparseOptimizerOnline::initSolver(), g2o::Star::labelStarEdges(), main(), g2o::SparseOptimizerIncremental::optimize(), and g2o::SparseOptimizerOnline::optimize().

◆ sortVectorContainers()

void g2o::SparseOptimizer::sortVectorContainers ( )

protected

Definition at line 507 of file sparse_optimizer.cpp.

                                           {
  // sort vector structures to get deterministic ordering based on IDs
  sort(_activeVertices.begin(), _activeVertices.end(), VertexIDCompare());
  sort(_activeEdges.begin(), _activeEdges.end(), EdgeIDCompare());
}

References _activeEdges, and _activeVertices.

Referenced by initializeOptimization(), and initializeOptimization().

◆ terminate()

bool g2o::SparseOptimizer::terminate ( )

inline

if external stop flag is given, return its state. False otherwise

Definition at line 183 of file sparse_optimizer.h.

183{ return _forceStopFlag ? (*_forceStopFlag) : false; }

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

◆ update()

void g2o::SparseOptimizer::update ( const double * update )

update the estimate of the active vertices

Parameters

update the double vector containing the stacked elements of the increments on the vertices.

Definition at line 446 of file sparse_optimizer.cpp.

                                                 {
  // update the graph by calling oplus on the vertices
  for (size_t i = 0; i < _ivMap.size(); ++i) {
    OptimizableGraph::Vertex* v = _ivMap[i];
#ifndef NDEBUG
    bool hasNan = arrayHasNaN(update, v->dimension());
    if (hasNan)
      G2O_WARN("{}: Update contains a nan for vertex {}", __PRETTY_FUNCTION__,
               v->id());
#endif
    v->oplus(update);
    update += v->dimension();
  }
}

References __PRETTY_FUNCTION__, _ivMap, g2o::arrayHasNaN(), g2o::OptimizableGraph::Vertex::dimension(), G2O_WARN, g2o::HyperGraph::Vertex::id(), g2o::OptimizableGraph::Vertex::oplus(), and update().

Referenced by g2o::OptimizationAlgorithmDogleg::solve(), g2o::OptimizationAlgorithmGaussNewton::solve(), g2o::OptimizationAlgorithmLevenberg::solve(), and update().

◆ updateInitialization()

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

virtual

HACK updating the internal structures for online processing

Reimplemented in g2o::SparseOptimizerIncremental, and g2o::SparseOptimizerOnline.

Definition at line 469 of file sparse_optimizer.cpp.

                                                                    {
  std::vector<HyperGraph::Vertex*> newVertices;
  newVertices.reserve(vset.size());
  _activeVertices.reserve(_activeVertices.size() + vset.size());
  _activeEdges.reserve(_activeEdges.size() + eset.size());
  for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end();
       ++it) {
    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
    if (!e->allVerticesFixed()) _activeEdges.push_back(e);
  }
 
  // update the index mapping
  size_t next = _ivMap.size();
  for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end();
       ++it) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
    if (!v->fixed()) {
      if (!v->marginalized()) {
        v->setHessianIndex(next);
        _ivMap.push_back(v);
        newVertices.push_back(v);
        _activeVertices.push_back(v);
        next++;
      } else  // not supported right now
        abort();
    } else {
      v->setHessianIndex(-1);
    }
  }
 
  if (newVertices.size() != vset.size()) {
    G2O_ERROR("{}: something went wrong, size mismatch {} != {}", vset.size(),
              newVertices.size());
  }
  return _algorithm->updateStructure(newVertices, eset);
}

References _activeEdges, _activeVertices, _algorithm, _ivMap, g2o::OptimizableGraph::Edge::allVerticesFixed(), g2o::OptimizableGraph::Vertex::fixed(), G2O_ERROR, g2o::OptimizableGraph::Vertex::marginalized(), g2o::OptimizableGraph::Vertex::setHessianIndex(), and g2o::OptimizationAlgorithm::updateStructure().

Referenced by main(), and g2o::SparseOptimizerOnline::updateInitialization().

◆ verbose()

bool g2o::SparseOptimizer::verbose ( ) const

inline

verbose information during optimization

Definition at line 172 of file sparse_optimizer.h.

172{ return _verbose; }

Referenced by computeInitialGuess(), main(), optimize(), g2o::SparseOptimizerIncremental::optimize(), g2o::SparseOptimizerOnline::optimize(), and setVerbose().

Friends And Related Symbol Documentation

◆ ActivePathCostFunction

friend class ActivePathCostFunction

friend

Definition at line 49 of file sparse_optimizer.h.

Member Data Documentation

◆ _activeEdges

EdgeContainer g2o::SparseOptimizer::_activeEdges

protected

sorted according to EdgeIDCompare

Definition at line 293 of file sparse_optimizer.h.

Referenced by activeChi2(), activeRobustChi2(), clear(), computeActiveErrors(), computeInitialGuess(), findActiveEdge(), initializeOptimization(), initializeOptimization(), optimize(), g2o::SparseOptimizerIncremental::optimize(), g2o::SparseOptimizerOnline::optimize(), sortVectorContainers(), updateInitialization(), and g2o::SparseOptimizerIncremental::updateInitialization().

◆ _activeVertices

VertexContainer g2o::SparseOptimizer::_activeVertices

protected

sorted according to VertexIDCompare

Definition at line 292 of file sparse_optimizer.h.

Referenced by clear(), discardTop(), findActiveVertex(), initializeOptimization(), initializeOptimization(), optimize(), pop(), push(), sortVectorContainers(), updateInitialization(), and g2o::SparseOptimizerIncremental::updateInitialization().

◆ _algorithm

OptimizationAlgorithm* g2o::SparseOptimizer::_algorithm

protected

Definition at line 297 of file sparse_optimizer.h.

Referenced by computeMarginals(), optimize(), g2o::SparseOptimizerIncremental::optimize(), g2o::SparseOptimizerOnline::optimize(), setAlgorithm(), updateInitialization(), g2o::SparseOptimizerIncremental::updateInitialization(), and ~SparseOptimizer().

◆ _batchStatistics

BatchStatisticsContainer g2o::SparseOptimizer::_batchStatistics

protected

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

Definition at line 307 of file sparse_optimizer.h.

Referenced by optimize(), and setComputeBatchStatistics().

◆ _computeBatchStatistics

bool g2o::SparseOptimizer::_computeBatchStatistics

protected

Definition at line 309 of file sparse_optimizer.h.

Referenced by optimize(), and setComputeBatchStatistics().

◆ _forceStopFlag

bool* g2o::SparseOptimizer::_forceStopFlag

protected

Definition at line 288 of file sparse_optimizer.h.

Referenced by setForceStopFlag().

◆ _ivMap

VertexContainer g2o::SparseOptimizer::_ivMap

protected

Definition at line 291 of file sparse_optimizer.h.

Referenced by buildIndexMapping(), clear(), clearIndexMapping(), optimize(), g2o::SparseOptimizerIncremental::optimize(), removeVertex(), update(), g2o::SparseOptimizerOnline::update(), updateInitialization(), and g2o::SparseOptimizerIncremental::updateInitialization().

◆ _verbose

bool g2o::SparseOptimizer::_verbose

protected

Definition at line 289 of file sparse_optimizer.h.

Referenced by setVerbose().

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

/build/reproducible-path/g2o-0~20230806/g2o/core/sparse_optimizer.h
/build/reproducible-path/g2o-0~20230806/g2o/core/sparse_optimizer.cpp

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Friends

Additional Inherited Members

Detailed Description

Member Enumeration Documentation

◆ anonymous enum

Constructor & Destructor Documentation

◆ SparseOptimizer()

◆ ~SparseOptimizer()

Member Function Documentation

◆ activeChi2()

◆ activeEdges()

◆ activeRobustChi2()

◆ activeVertices()

◆ addComputeErrorAction()

◆ algorithm()

◆ batchStatistics() [1/2]

◆ batchStatistics() [2/2]

◆ buildIndexMapping()

◆ clear()

◆ clearIndexMapping()

◆ computeActiveErrors()

◆ computeBatchStatistics()

◆ computeInitialGuess() [1/2]

◆ computeInitialGuess() [2/2]

◆ computeMarginals() [1/3]

◆ computeMarginals() [2/3]

◆ computeMarginals() [3/3]

◆ discardTop() [1/4]

◆ discardTop() [2/4]

◆ discardTop() [3/4]

◆ discardTop() [4/4]

◆ findActiveEdge()

◆ findActiveVertex()

◆ findGauge()

◆ forceStopFlag()

◆ G2O_ATTRIBUTE_DEPRECATED()

◆ gaugeFreedom()

◆ indexMapping()

◆ initializeOptimization() [1/3]

◆ initializeOptimization() [2/3]

◆ initializeOptimization() [3/3]

◆ optimize()

◆ pop() [1/3]

◆ pop() [2/3]

◆ pop() [3/3]

◆ push() [1/3]

◆ push() [2/3]

◆ push() [3/3]

◆ removeComputeErrorAction()

◆ removeVertex()

◆ setAlgorithm()

◆ setComputeBatchStatistics()

◆ setForceStopFlag()

◆ setToOrigin()

◆ setVerbose()

◆ solver()

◆ sortVectorContainers()

◆ terminate()

◆ update()

◆ updateInitialization()

◆ verbose()

Friends And Related Symbol Documentation

◆ ActivePathCostFunction

Member Data Documentation

◆ _activeEdges

◆ _activeVertices

◆ _algorithm

◆ _batchStatistics

◆ _computeBatchStatistics

◆ _forceStopFlag

◆ _ivMap

◆ _verbose