g2o::OptimizableGraph Struct Reference

#include <optimizable_graph.h>

Inheritance diagram for g2o::OptimizableGraph:

Collaboration diagram for g2o::OptimizableGraph:

Classes
struct	VertexIDCompare
	order vertices based on their ID More...
struct	EdgeIDCompare
	order edges based on the internal ID, which is assigned to the edge in addEdge() More...
class	Vertex
	A general case Vertex for optimization. More...
class	Edge

Public Types
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
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 int	optimize (int iterations, bool online=false)
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 void	push ()
	push the estimate of all variables onto a stack
virtual void	pop ()
	pop (restore) the estimate of all variables from the stack
virtual void	discardTop ()
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	push (HyperGraph::VertexSet &vset)
	push the estimate of a subset of the variables onto a stack
virtual void	pop (HyperGraph::VertexSet &vset)
	pop (restore) the estimate a subset of the variables from the stack
virtual void	discardTop (HyperGraph::VertexSet &vset)
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	removeVertex (Vertex *v, bool detach=false)
virtual bool	removeEdge (Edge *e)
virtual void	clear ()
	clears the graph and empties all structures.
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)

Static Public Member Functions
static bool	initMultiThreading ()

Public Attributes
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 Member Functions
void	performActions (int iter, HyperGraphActionSet &actions)

Protected Attributes
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

Additional Inherited Members
Static Public Attributes inherited from g2o::HyperGraph
static const int	UnassignedId = -1
static const int	InvalidId = -2

Detailed Description

This is an abstract class that represents one optimization problem. It specializes the general graph to contain special vertices and edges. The vertices represent parameters that can be optimized, while the edges represent constraints. This class also provides basic functionalities to handle the backup/restore of portions of the vertices.

Definition at line 62 of file optimizable_graph.h.

Member Typedef Documentation

EdgeContainer

typedef std::vector<OptimizableGraph::Edge*> g2o::OptimizableGraph::EdgeContainer

vector container for edges

Definition at line 102 of file optimizable_graph.h.

HyperGraphActionSet

typedef std::set<HyperGraphAction*> g2o::OptimizableGraph::HyperGraphActionSet

Definition at line 69 of file optimizable_graph.h.

VertexContainer

typedef std::vector<OptimizableGraph::Vertex*> g2o::OptimizableGraph::VertexContainer

vector container for vertices

Definition at line 100 of file optimizable_graph.h.

Member Enumeration Documentation

ActionType

enum g2o::OptimizableGraph::ActionType

Enumerator
AT_PREITERATION
AT_POSTITERATION
AT_NUM_ELEMENTS

Definition at line 63 of file optimizable_graph.h.

                  {
    AT_PREITERATION,
    AT_POSTITERATION,
    AT_NUM_ELEMENTS,  // keep as last element
  };

Constructor & Destructor Documentation

OptimizableGraph()

g2o::OptimizableGraph::OptimizableGraph

(

)

empty constructor

Definition at line 173 of file optimizable_graph.cpp.

                                   {
  _nextEdgeId = 0;
  _graphActions.resize(AT_NUM_ELEMENTS);
}

References _graphActions, _nextEdgeId, and AT_NUM_ELEMENTS.

Referenced by addEdge().

~OptimizableGraph()

g2o::OptimizableGraph::~OptimizableGraph ( )

virtual

Definition at line 178 of file optimizable_graph.cpp.

                                    {
  clear();
  clearParameters();
}

References g2o::HyperGraph::clear(), and clearParameters().

Member Function Documentation

addEdge() [1/2]

bool g2o::OptimizableGraph::addEdge ( HyperGraph::Edge * e )

virtual

adds a new edge. The edge should point to the vertices that it is connecting (setFrom/setTo).

Returns

false if the insertion does not work (incompatible types of the vertices/missing vertex). true otherwise.

Reimplemented from g2o::HyperGraph.

Definition at line 252 of file optimizable_graph.cpp.

                                                 {
  OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(e_);
  assert(e && "Edge does not inherit from OptimizableGraph::Edge");
  if (!e) return false;
  return addEdge(e);
}

References addEdge().

Referenced by addEdge(), g2o::addOdometryCalibLinksDifferential(), g2o::assignHierarchicalEdges(), g2o::computeSimpleStars(), main(), main(), main(), Robot::move(), g2o::Gm2dlIO::readGm2dl(), g2o::BinarySensor< RobotType_, EdgeType_, WorldObjectType_ >::sense(), g2o::SensorOdometry2D::sense(), g2o::SensorOdometry3D::sense(), g2o::SensorOdometry< R, E, O >::sense(), g2o::SensorPointXY::sense(), g2o::SensorPointXYBearing::sense(), g2o::SensorPointXYOffset::sense(), g2o::SensorPointXYZ::sense(), g2o::SensorPointXYZDepth::sense(), g2o::SensorPointXYZDisparity::sense(), g2o::SensorPose2D::sense(), g2o::SensorPose3D::sense(), g2o::SensorPose3DOffset::sense(), g2o::SensorSE3Prior::sense(), g2o::SensorSegment2D::sense(), g2o::SensorSegment2DLine::sense(), g2o::SensorSegment2DPointLine::sense(), g2o::UnarySensor< RobotType_, EdgeType_ >::sense(), LineSensor::sense(), and PlaneSensor::sense().

addEdge() [2/2]

bool g2o::OptimizableGraph::addEdge

(

OptimizableGraph::Edge *

e

)

Definition at line 220 of file optimizable_graph.cpp.

                                                      {
  OptimizableGraph* g = e->graph();
 
  if (g != nullptr && g != this) {
    G2O_ERROR(
        "{}: FATAL, edge with ID {} has already registered with another graph "
        "{}",
        __PRETTY_FUNCTION__, e->id(), static_cast<void*>(g));
    return false;
  }
 
  bool eresult = HyperGraph::addEdge(e);
  if (!eresult) return false;
 
  e->_internalId = _nextEdgeId++;
  if (e->numUndefinedVertices()) return true;
  if (!e->resolveParameters()) {
    G2O_ERROR("{}: FATAL, cannot resolve parameters for edge {}",
              __PRETTY_FUNCTION__, static_cast<void*>(e));
    return false;
  }
  if (!e->resolveCaches()) {
    G2O_ERROR("{}: FATAL, cannot resolve caches for edge {}",
              __PRETTY_FUNCTION__, static_cast<void*>(e));
    return false;
  }
 
  _jacobianWorkspace.updateSize(e);
 
  return true;
}

References __PRETTY_FUNCTION__, g2o::OptimizableGraph::Edge::_internalId, _jacobianWorkspace, _nextEdgeId, g2o::HyperGraph::addEdge(), G2O_ERROR, g2o::OptimizableGraph::Edge::graph(), g2o::HyperGraph::Edge::id(), g2o::HyperGraph::Edge::numUndefinedVertices(), OptimizableGraph(), g2o::OptimizableGraph::Edge::resolveCaches(), and g2o::OptimizableGraph::Edge::resolveParameters().

addParameter()

bool g2o::OptimizableGraph::addParameter ( Parameter * p )

inline

Definition at line 715 of file optimizable_graph.h.

{ return _parameters.addParameter(p); }

References _parameters.

Referenced by g2o::World::addParameter(), main(), main(), and main().

addPostIterationAction()

bool g2o::OptimizableGraph::addPostIterationAction

(

HyperGraphAction *

action

)

add an action to be executed after each iteration

Definition at line 711 of file optimizable_graph.cpp.

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

References _graphActions, and AT_POSTITERATION.

Referenced by main().

addPreIterationAction()

bool g2o::OptimizableGraph::addPreIterationAction

(

HyperGraphAction *

action

)

add an action to be executed before each iteration

Definition at line 717 of file optimizable_graph.cpp.

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

References _graphActions, and AT_PREITERATION.

Referenced by g2o::RunG2OViewer::run().

addVertex() [1/4]

virtual bool g2o::OptimizableGraph::addVertex ( HyperGraph::Vertex * v )

inlinevirtual

adds a vertex to the graph. The id of the vertex should be set before invoking this function. the function fails if another vertex with the same id is already in the graph. returns true, on success, or false on failure.

Reimplemented from g2o::HyperGraph.

Definition at line 595 of file optimizable_graph.h.

{ return addVertex(v, 0); }

References addVertex().

Referenced by addVertex().

addVertex() [2/4]

bool g2o::OptimizableGraph::addVertex ( HyperGraph::Vertex * v, Data * userData )

virtual

adds a new vertex. The new vertex is then "taken".

Returns

false if a vertex with the same id as v is already in the graph, true otherwise.

Definition at line 212 of file optimizable_graph.cpp.

                                                                    {
  OptimizableGraph::Vertex* ov = dynamic_cast<OptimizableGraph::Vertex*>(v);
  assert(ov && "Vertex does not inherit from OptimizableGraph::Vertex");
  if (!ov) return false;
 
  return addVertex(ov, userData);
}

References addVertex().

Referenced by g2o::addOdometryCalibLinksDifferential(), addVertex(), g2o::World::addWorldObject(), LineItem::LineItem(), LineSensor::LineSensor(), main(), main(), main(), Robot::move(), PlaneItem::PlaneItem(), PlaneSensor::PlaneSensor(), and g2o::Gm2dlIO::readGm2dl().

addVertex() [3/4]

bool g2o::OptimizableGraph::addVertex ( OptimizableGraph::Vertex * v )

inline

Definition at line 597 of file optimizable_graph.h.

{ return addVertex(v, 0); }

References addVertex().

Referenced by addVertex().

addVertex() [4/4]

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

Definition at line 183 of file optimizable_graph.cpp.

                                                                           {
  if (ov->id() < 0) {
    G2O_ERROR(
        "{}: FATAL, a vertex with (negative) ID {} cannot be inserted into the "
        "graph",
        __PRETTY_FUNCTION__, ov->id());
    assert(0 && "Invalid vertex id");
    return false;
  }
  Vertex* inserted = vertex(ov->id());
  if (inserted) {
    G2O_WARN(
        "{}: a vertex with ID {} has already been registered with this "
        "graph",
        __PRETTY_FUNCTION__, ov->id());
    return false;
  }
  if (ov->_graph != nullptr && ov->_graph != this) {
    G2O_ERROR(
        "{}: FATAL, vertex with ID {} has already been registered with another "
        "graph {}",
        __PRETTY_FUNCTION__, ov->id(), static_cast<void*>(ov->_graph));
    return false;
  }
  if (userData) ov->setUserData(userData);
  ov->_graph = this;
  return HyperGraph::addVertex(ov);
}

References __PRETTY_FUNCTION__, g2o::OptimizableGraph::Vertex::_graph, g2o::HyperGraph::addVertex(), G2O_ERROR, G2O_WARN, g2o::HyperGraph::Vertex::id(), g2o::HyperGraph::DataContainer::setUserData(), and vertex().

chi2()

double g2o::OptimizableGraph::chi2

(

)

const

returns the chi2 of the current configuration

Definition at line 290 of file optimizable_graph.cpp.

                                    {
  double chi = 0.0;
  for (OptimizableGraph::EdgeSet::const_iterator it = this->edges().begin();
       it != this->edges().end(); ++it) {
    const OptimizableGraph::Edge* e =
        static_cast<const OptimizableGraph::Edge*>(*it);
    chi += e->chi2();
  }
  return chi;
}

References g2o::OptimizableGraph::Edge::chi2(), and g2o::HyperGraph::edges().

Referenced by main(), and main().

clearParameters()

void g2o::OptimizableGraph::clearParameters ( )

virtual

remove the parameters of the graph

Definition at line 793 of file optimizable_graph.cpp.

                                       {
  HyperGraph::clear();
  _parameters.clear();
}

References _parameters, and g2o::HyperGraph::clear().

Referenced by saveSubset(), and ~OptimizableGraph().

dimensions()

std::set< int > g2o::OptimizableGraph::dimensions

(

)

const

iterates over all vertices and returns a set of all the vertex dimensions in the graph

Definition at line 682 of file optimizable_graph.cpp.

                                               {
  std::set<int> auxDims;
  for (VertexIDMap::const_iterator it = vertices().begin();
       it != vertices().end(); ++it) {
    OptimizableGraph::Vertex* v =
        static_cast<OptimizableGraph::Vertex*>(it->second);
    auxDims.insert(v->dimension());
  }
  return auxDims;
}

References g2o::HyperGraph::vertices().

Referenced by isSolverSuitable(), MainWindow::load(), and main().

discardTop() [1/2]

void g2o::OptimizableGraph::discardTop ( )

virtual

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

Reimplemented in g2o::SparseOptimizer.

Definition at line 309 of file optimizable_graph.cpp.

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

References g2o::OptimizableGraph::Vertex::discardTop(), and forEachVertex().

Referenced by saveSubset().

discardTop() [2/2]

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

Definition at line 321 of file optimizable_graph.cpp.

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

References g2o::OptimizableGraph::Vertex::discardTop(), and forEachVertex().

forEachVertex() [1/2]

void g2o::OptimizableGraph::forEachVertex	(	HyperGraph::VertexSet &	vset,
		std::function< void(OptimizableGraph::Vertex *)>	fn )

apply a unary function to the vertices in vset

Definition at line 339 of file optimizable_graph.cpp.

                                                   {
  for (auto it = vset.begin(); it != vset.end(); ++it) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
    fn(v);
  }
}

forEachVertex() [2/2]

void g2o::OptimizableGraph::forEachVertex

(

std::function< void(OptimizableGraph::Vertex *)>

fn

)

apply a unary function to all vertices

Definition at line 330 of file optimizable_graph.cpp.

                                                   {
  for (auto it = _vertices.begin(); it != _vertices.end(); ++it) {
    OptimizableGraph::Vertex* v =
        static_cast<OptimizableGraph::Vertex*>(it->second);
    fn(v);
  }
}

References g2o::HyperGraph::_vertices.

Referenced by discardTop(), discardTop(), pop(), pop(), push(), push(), and setFixed().

initMultiThreading()

bool g2o::OptimizableGraph::initMultiThreading ( )

static

Eigen starting from version 3.1 requires that we call an initialize function, if we perform calls to Eigen from several threads. Currently, this function calls Eigen::initParallel if g2o is compiled with OpenMP support and Eigen's version is at least 3.1

Definition at line 835 of file optimizable_graph.cpp.

                                          {
#if (defined G2O_OPENMP) && EIGEN_VERSION_AT_LEAST(3, 1, 0)
  Eigen::initParallel();
#endif
  return true;
}

Referenced by main().

isSolverSuitable()

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

test whether a solver is suitable for optimizing this graph.

Parameters

solverProperty	the solver property to evaluate.
vertDims	should equal to the set returned by dimensions() to avoid re-evaluating.

Definition at line 656 of file optimizable_graph.cpp.

                                        {
  std::set<int> auxDims;
  if (vertDims_.size() == 0) {
    auxDims = dimensions();
  }
  const set<int>& vertDims = vertDims_.size() == 0 ? auxDims : vertDims_;
  bool suitableSolver = true;
  if (vertDims.size() == 2) {
    if (solverProperty.requiresMarginalize) {
      suitableSolver = vertDims.count(solverProperty.poseDim) == 1 &&
                       vertDims.count(solverProperty.landmarkDim) == 1;
    } else {
      suitableSolver = solverProperty.poseDim == -1;
    }
  } else if (vertDims.size() == 1) {
    suitableSolver = vertDims.count(solverProperty.poseDim) == 1 ||
                     solverProperty.poseDim == -1;
  } else {
    suitableSolver =
        solverProperty.poseDim == -1 && !solverProperty.requiresMarginalize;
  }
  return suitableSolver;
}

References dimensions(), g2o::OptimizationAlgorithmProperty::landmarkDim, g2o::OptimizationAlgorithmProperty::poseDim, and g2o::OptimizationAlgorithmProperty::requiresMarginalize.

Referenced by MainWindow::load(), main(), and saveSubset().

jacobianWorkspace() [1/2]

JacobianWorkspace & g2o::OptimizableGraph::jacobianWorkspace ( )

inline

the workspace for storing the Jacobians of the graph

Definition at line 740 of file optimizable_graph.h.

{ return _jacobianWorkspace; }

References _jacobianWorkspace.

Referenced by g2o::OptimizableGraph::Edge::linearizeOplus(), g2o::SparseOptimizerOnline::optimize(), and g2o::SparseOptimizerIncremental::updateInitialization().

jacobianWorkspace() [2/2]

const JacobianWorkspace & g2o::OptimizableGraph::jacobianWorkspace ( ) const

inline

Definition at line 741 of file optimizable_graph.h.

                                                     {
    return _jacobianWorkspace;
  }

References _jacobianWorkspace.

load() [1/2]

bool g2o::OptimizableGraph::load

(

const char *

filename

)

Definition at line 533 of file optimizable_graph.cpp.

                                                {
  ifstream ifs(filename);
  if (!ifs) {
    G2O_ERROR("Unable to open file {}", filename);
    return false;
  }
  return load(ifs);
}

References G2O_ERROR, and load().

load() [2/2]

virtual bool g2o::OptimizableGraph::load ( std::istream & is )

virtual

load the graph from a stream. Uses the Factory singleton for creating the vertices and edges.

References load().

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

maxDimension()

int g2o::OptimizableGraph::maxDimension

(

)

const

return the maximum dimension of all vertices in the graph

Definition at line 617 of file optimizable_graph.cpp.

                                         {
  int maxDim = 0;
  for (HyperGraph::VertexIDMap::const_iterator it = vertices().begin();
       it != vertices().end(); ++it) {
    const OptimizableGraph::Vertex* v =
        static_cast<const OptimizableGraph::Vertex*>(it->second);
    maxDim = (std::max)(maxDim, v->dimension());
  }
  return maxDim;
}

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

Referenced by g2o::SparseOptimizer::findGauge(), and g2o::SparseOptimizer::gaugeFreedom().

optimize()

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

virtual

carry out n iterations

Returns

the number of performed iterations

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

Definition at line 286 of file optimizable_graph.cpp.

                                            {
  return 0;
}

parameter()

Parameter * g2o::OptimizableGraph::parameter ( int id )

inline

Definition at line 717 of file optimizable_graph.h.

{ return _parameters.getParameter(id); }

References _parameters.

Referenced by main().

parameters() [1/2]

ParameterContainer & g2o::OptimizableGraph::parameters ( )

inline

Definition at line 753 of file optimizable_graph.h.

{ return _parameters; }

References _parameters.

parameters() [2/2]

const ParameterContainer & g2o::OptimizableGraph::parameters ( ) const

inline

Definition at line 754 of file optimizable_graph.h.

{ return _parameters; }

References _parameters.

performActions()

void g2o::OptimizableGraph::performActions ( int iter, HyperGraphActionSet & actions )

protected

Definition at line 693 of file optimizable_graph.cpp.

                                                                            {
  if (actions.size() > 0) {
    HyperGraphAction::ParametersIteration params(iter);
    for (HyperGraphActionSet::iterator it = actions.begin();
         it != actions.end(); ++it) {
      (*(*it))(this, &params);
    }
  }
}

Referenced by postIteration(), and preIteration().

pop() [1/2]

void g2o::OptimizableGraph::pop ( )

virtual

pop (restore) the estimate of all variables from the stack

Reimplemented in g2o::SparseOptimizer.

Definition at line 305 of file optimizable_graph.cpp.

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

References forEachVertex(), and g2o::OptimizableGraph::Vertex::pop().

Referenced by g2o::SparseOptimizer::pop(), and saveSubset().

pop() [2/2]

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

virtual

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

Reimplemented in g2o::SparseOptimizer.

Definition at line 317 of file optimizable_graph.cpp.

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

References forEachVertex(), and g2o::OptimizableGraph::Vertex::pop().

postIteration()

void g2o::OptimizableGraph::postIteration ( int iter )

virtual

called at the end of an iteration (argument is the number of the iteration)

Definition at line 707 of file optimizable_graph.cpp.

                                             {
  performActions(iter, _graphActions[AT_POSTITERATION]);
}

References _graphActions, AT_POSTITERATION, and performActions().

Referenced by g2o::SparseOptimizer::initializeOptimization(), g2o::SparseOptimizer::initializeOptimization(), and g2o::SparseOptimizer::optimize().

preIteration()

void g2o::OptimizableGraph::preIteration ( int iter )

virtual

called at the beginning of an iteration (argument is the number of the iteration)

Definition at line 703 of file optimizable_graph.cpp.

                                            {
  performActions(iter, _graphActions[AT_PREITERATION]);
}

References _graphActions, AT_PREITERATION, and performActions().

Referenced by g2o::SparseOptimizer::initializeOptimization(), g2o::SparseOptimizer::initializeOptimization(), and g2o::SparseOptimizer::optimize().

push() [1/2]

void g2o::OptimizableGraph::push ( )

virtual

push the estimate of all variables onto a stack

Reimplemented in g2o::SparseOptimizer.

Definition at line 301 of file optimizable_graph.cpp.

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

References forEachVertex(), and g2o::OptimizableGraph::Vertex::push().

Referenced by g2o::SparseOptimizer::push(), and saveSubset().

push() [2/2]

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

virtual

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

Reimplemented in g2o::SparseOptimizer.

Definition at line 313 of file optimizable_graph.cpp.

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

References forEachVertex(), and g2o::OptimizableGraph::Vertex::push().

recomputeJacobianWorkspaceSize()

void g2o::OptimizableGraph::recomputeJacobianWorkspaceSize ( )

inline

Recompute the size of the Jacobian workspace from all the edges in the graph.

Definition at line 626 of file optimizable_graph.h.

                                        {
    _jacobianWorkspace.updateSize(*this, true);
  }

References _jacobianWorkspace.

removePostIterationAction()

bool g2o::OptimizableGraph::removePostIterationAction

(

HyperGraphAction *

action

)

remove an action that should no longer be executed after each iteration

Definition at line 727 of file optimizable_graph.cpp.

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

References _graphActions, and AT_POSTITERATION.

removePreIterationAction()

bool g2o::OptimizableGraph::removePreIterationAction

(

HyperGraphAction *

action

)

remove an action that should no longer be executed before each iteration

Definition at line 723 of file optimizable_graph.cpp.

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

References _graphActions, and AT_PREITERATION.

save() [1/2]

bool g2o::OptimizableGraph::save	(	const char *	filename,
		int	level = 0 ) const

function provided for convenience, see save() above

Definition at line 542 of file optimizable_graph.cpp.

                                                                 {
  ofstream ofs(filename);
  if (!ofs) return false;
  return save(ofs, level);
}

References save().

save() [2/2]

virtual bool g2o::OptimizableGraph::save ( std::ostream & os, int level = 0 ) const

virtual

save the graph to a stream. Again uses the Factory system.

References save().

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

saveEdge()

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

Definition at line 774 of file optimizable_graph.cpp.

                                                               {
  Factory* factory = Factory::instance();
  string tag = factory->tag(e);
  if (tag.size() > 0) {
    os << tag << " ";
    for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin();
         it != e->vertices().end(); ++it) {
      int vertexId = (*it) ? (*it)->id() : HyperGraph::UnassignedId;
      os << vertexId << " ";
    }
    e->write(os);
    os << endl;
    saveUserData(os, e->userData());
    return os.good();
  }
  return false;
}

References g2o::Factory::instance(), saveUserData(), g2o::Factory::tag(), g2o::HyperGraph::UnassignedId, g2o::HyperGraph::DataContainer::userData(), g2o::HyperGraph::Edge::vertices(), and g2o::OptimizableGraph::Edge::write().

saveParameter()

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

Definition at line 763 of file optimizable_graph.cpp.

                                                                       {
  Factory* factory = Factory::instance();
  string tag = factory->tag(p);
  if (tag.size() > 0) {
    os << tag << " " << p->id() << " ";
    p->write(os);
    os << endl;
  }
  return os.good();
}

References g2o::Parameter::id(), g2o::Factory::instance(), g2o::Factory::tag(), and g2o::Parameter::write().

saveSubset() [1/2]

bool g2o::OptimizableGraph::saveSubset	(	std::ostream &	os,
		HyperGraph::EdgeSet &	eset )

save a subgraph to a stream. Again uses the Factory system.

References clearParameters(), discardTop(), isSolverSuitable(), pop(), push(), setFixed(), and setRenamedTypesFromString().

saveSubset() [2/2]

bool g2o::OptimizableGraph::saveSubset	(	std::ostream &	os,
		HyperGraph::VertexSet &	vset,
		int	level = 0 )

save a subgraph to a stream. Again uses the Factory system.

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

saveUserData()

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

Definition at line 731 of file optimizable_graph.cpp.

                                                             {
  Factory* factory = Factory::instance();
  while (d) {  // write the data packet for the vertex
    string tag = factory->tag(d);
    if (tag.size() > 0) {
      os << tag << " ";
      d->write(os);
      os << endl;
    }
    d = d->next();
  }
  return os.good();
}

References g2o::Factory::instance(), g2o::HyperGraph::Data::next(), g2o::Factory::tag(), and g2o::HyperGraph::Data::write().

Referenced by saveEdge(), and saveVertex().

saveVertex()

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

Definition at line 746 of file optimizable_graph.cpp.

                                                                   {
  Factory* factory = Factory::instance();
  string tag = factory->tag(v);
  if (tag.size() > 0) {
    os << tag << " " << v->id() << " ";
    v->write(os);
    os << endl;
    saveUserData(os, v->userData());
    if (v->fixed()) {
      os << "FIX " << v->id() << endl;
    }
    return os.good();
  }
  return false;
}

References g2o::OptimizableGraph::Vertex::fixed(), g2o::HyperGraph::Vertex::id(), g2o::Factory::instance(), saveUserData(), g2o::Factory::tag(), g2o::HyperGraph::DataContainer::userData(), and g2o::OptimizableGraph::Vertex::write().

setEdgeVertex()

bool g2o::OptimizableGraph::setEdgeVertex ( HyperGraph::Edge * e, int pos, HyperGraph::Vertex * v )

virtual

overridden from HyperGraph, to maintain the bookkeeping of the caches/parameters and jacobian workspaces consistent upon a change in the vertex.

Returns

false if something goes wrong.

Reimplemented from g2o::HyperGraph.

Definition at line 259 of file optimizable_graph.cpp.

                                                          {
  if (!HyperGraph::setEdgeVertex(e, pos, v)) {
    return false;
  }
  if (!e->numUndefinedVertices()) {
#ifndef NDEBUG
    OptimizableGraph::Edge* ee = dynamic_cast<OptimizableGraph::Edge*>(e);
    assert(ee && "Edge is not a OptimizableGraph::Edge");
#else
    OptimizableGraph::Edge* ee = static_cast<OptimizableGraph::Edge*>(e);
#endif
    if (!ee->resolveParameters()) {
      G2O_ERROR("{}: FATAL, cannot resolve parameters for edge {}",
                __PRETTY_FUNCTION__, static_cast<void*>(e));
      return false;
    }
    if (!ee->resolveCaches()) {
      G2O_ERROR("{}: FATAL, cannot resolve caches for edge {}",
                __PRETTY_FUNCTION__, static_cast<void*>(e));
      return false;
    }
    _jacobianWorkspace.updateSize(e);
  }
  return true;
}

References __PRETTY_FUNCTION__, _jacobianWorkspace, G2O_ERROR, g2o::HyperGraph::Edge::numUndefinedVertices(), g2o::OptimizableGraph::Edge::resolveCaches(), g2o::OptimizableGraph::Edge::resolveParameters(), and g2o::HyperGraph::setEdgeVertex().

Referenced by anonymizeLandmarkEdge(), and anonymizePoseEdge().

setFixed()

void g2o::OptimizableGraph::setFixed ( HyperGraph::VertexSet & vset, bool fixed )

virtual

fixes/releases a set of vertices

Definition at line 325 of file optimizable_graph.cpp.

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

References forEachVertex(), and g2o::OptimizableGraph::Vertex::setFixed().

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

setRenamedTypesFromString()

void g2o::OptimizableGraph::setRenamedTypesFromString

(

const std::string &

types

)

set the renamed types lookup from a string, format is for example: VERTEX_CAM=VERTEX_SE3:EXPMAP,EDGE_PROJECT_P2MC=EDGE_PROJECT_XYZ:EXPMAP This will change the occurrence of VERTEX_CAM in the file to VERTEX_SE3:EXPMAP

Definition at line 628 of file optimizable_graph.cpp.

                                                                       {
  Factory* factory = Factory::instance();
  vector<string> typesMap = strSplit(types, ",");
  for (size_t i = 0; i < typesMap.size(); ++i) {
    vector<string> m = strSplit(typesMap[i], "=");
    if (m.size() != 2) {
      G2O_ERROR("{}: unable to extract type map from {}", __PRETTY_FUNCTION__,
                typesMap[i]);
      continue;
    }
    string typeInFile = trim(m[0]);
    string loadedType = trim(m[1]);
    if (!factory->knowsTag(loadedType)) {
      G2O_ERROR("{}: unknown type {}", __PRETTY_FUNCTION__, loadedType);
      continue;
    }
 
    _renamedTypesLookup[typeInFile] = loadedType;
  }
 
  G2O_DEBUG("Load look up table:");
  for (std::map<std::string, std::string>::const_iterator it =
           _renamedTypesLookup.begin();
       it != _renamedTypesLookup.end(); ++it) {
    G2O_DEBUG("{} -> {}", it->first, it->second);
  }
}

References __PRETTY_FUNCTION__, _renamedTypesLookup, G2O_DEBUG, G2O_ERROR, g2o::Factory::instance(), g2o::Factory::knowsTag(), g2o::strSplit(), and g2o::trim().

Referenced by main(), g2o::RunG2OViewer::run(), and saveSubset().

verifyInformationMatrices()

bool g2o::OptimizableGraph::verifyInformationMatrices

(

bool

verbose = false

)

const

verify that all the information of the edges are semi positive definite, i.e., all Eigenvalues are >= 0.

Parameters

verbose

output edges with not PSD information matrix by logging

Returns

true if all edges have PSD information matrix

Definition at line 798 of file optimizable_graph.cpp.

                                                                   {
  bool allEdgeOk = true;
  Eigen::SelfAdjointEigenSolver<MatrixX> eigenSolver;
  for (OptimizableGraph::EdgeSet::const_iterator it = edges().begin();
       it != edges().end(); ++it) {
    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
    MatrixX::MapType information(e->informationData(), e->dimension(),
                                 e->dimension());
    // test on symmetry
    bool isSymmetric = information.transpose() == information;
    bool okay = isSymmetric;
    if (isSymmetric) {
      // compute the eigenvalues
      eigenSolver.compute(information, Eigen::EigenvaluesOnly);
      bool isSPD = eigenSolver.eigenvalues()(0) >= 0.;
      okay = okay && isSPD;
    }
    allEdgeOk = allEdgeOk && okay;
    if (!okay) {
      if (verbose) {
        vector<int> ids(e->vertices().size());
        for (size_t i = 0; i < e->vertices().size(); ++i)
          ids[i] = e->vertex(i)->id();
        if (!isSymmetric)
          G2O_WARN("Information Matrix for an edge is not symmetric: {}",
                   fmt::join(ids, " "));
        else
          G2O_WARN("Information Matrix for an edge is not SPD: {}",
                   fmt::join(ids, " "));
        if (isSymmetric)
          G2O_WARN("eigenvalues: {}", eigenSolver.eigenvalues().transpose());
      }
    }
  }
  return allEdgeOk;
}

References g2o::OptimizableGraph::Edge::dimension(), g2o::HyperGraph::edges(), G2O_WARN, g2o::HyperGraph::Vertex::id(), g2o::OptimizableGraph::Edge::informationData(), g2o::HyperGraph::Edge::vertex(), and g2o::HyperGraph::Edge::vertices().

vertex() [1/2]

Vertex * g2o::OptimizableGraph::vertex ( int id )

inline

returns the vertex number id appropriately casted

Definition at line 576 of file optimizable_graph.h.

                                {
    return reinterpret_cast<Vertex*>(HyperGraph::vertex(id));
  }

References g2o::HyperGraph::vertex().

Referenced by g2o::SparseOptimizer::ActivePathCostFunction, addVertex(), g2o::SparseOptimizer::computeMarginals(), main(), main(), g2o::Gm2dlIO::readGm2dl(), and g2o::Gm2dlIO::updateLaserData().

vertex() [2/2]

const Vertex * g2o::OptimizableGraph::vertex ( int id ) const

inline

returns the vertex number id appropriately casted

Definition at line 581 of file optimizable_graph.h.

                                            {
    return reinterpret_cast<const Vertex*>(HyperGraph::vertex(id));
  }

References g2o::HyperGraph::vertex().

Member Data Documentation

_graphActions

std::vector<HyperGraphActionSet> g2o::OptimizableGraph::_graphActions

protected

Definition at line 765 of file optimizable_graph.h.

Referenced by g2o::SparseOptimizer::addComputeErrorAction(), addPostIterationAction(), addPreIterationAction(), g2o::SparseOptimizer::computeActiveErrors(), OptimizableGraph(), postIteration(), preIteration(), g2o::SparseOptimizer::removeComputeErrorAction(), removePostIterationAction(), removePreIterationAction(), and g2o::SparseOptimizer::SparseOptimizer().

_jacobianWorkspace

JacobianWorkspace g2o::OptimizableGraph::_jacobianWorkspace

protected

Definition at line 768 of file optimizable_graph.h.

Referenced by addEdge(), g2o::SparseOptimizer::initializeOptimization(), g2o::SparseOptimizer::initializeOptimization(), jacobianWorkspace(), jacobianWorkspace(), recomputeJacobianWorkspaceSize(), and setEdgeVertex().

_nextEdgeId

long long g2o::OptimizableGraph::_nextEdgeId

protected

Definition at line 764 of file optimizable_graph.h.

Referenced by addEdge(), and OptimizableGraph().

_parameters

ParameterContainer g2o::OptimizableGraph::_parameters

protected

Definition at line 767 of file optimizable_graph.h.

Referenced by addParameter(), clearParameters(), parameter(), parameters(), and parameters().

_renamedTypesLookup

std::map<std::string, std::string> g2o::OptimizableGraph::_renamedTypesLookup

protected

Definition at line 763 of file optimizable_graph.h.

Referenced by setRenamedTypesFromString().

Edge

class G2O_CORE_API g2o::OptimizableGraph::Edge

Definition at line 73 of file optimizable_graph.h.

Vertex

class G2O_CORE_API g2o::OptimizableGraph::Vertex

Definition at line 72 of file optimizable_graph.h.

---

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

• /build/g2o-YEvA6N/g2o-0~20230806/g2o/core/optimizable_graph.h
• /build/g2o-YEvA6N/g2o-0~20230806/g2o/core/optimizable_graph.cpp