#include <optimizable_graph.h>

Inheritance diagram for g2o::OptimizableGraph:

[legend]

Collaboration diagram for g2o::OptimizableGraph:

Classes
class	Edge

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...

struct	VertexIDCompare
	order vertices based on their ID More...

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 More...

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

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

typedef std::set< Edge * >	EdgeSet

typedef std::set< Vertex * >	VertexSet

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

typedef std::vector< Vertex * >	VertexContainer

Public Member Functions
Vertex *	vertex (int id)
	returns the vertex number id appropriately casted More...

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

	OptimizableGraph ()
	empty constructor More...

virtual	~OptimizableGraph ()

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

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

virtual bool	addVertex (HyperGraph::Vertex *v)

virtual bool	addEdge (HyperGraph::Edge *e)

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

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

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

std::set< int >	dimensions () const

virtual int	optimize (int iterations, bool online=false)

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

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

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

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

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

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

virtual void	push ()
	push the estimate of all variables onto a stack More...

virtual void	pop ()
	pop (restore) the estimate of all variables from the stack More...

virtual void	discardTop ()
	discard the last backup of the estimate for all variables by removing it from the stack More...

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

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

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

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

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

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

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

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

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

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

void	setRenamedTypesFromString (const std::string &types)

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

virtual void	clearParameters ()

bool	addParameter (Parameter *p)

Parameter *	parameter (int id)

bool	verifyInformationMatrices (bool verbose=false) const

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

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

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

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

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

const JacobianWorkspace &	jacobianWorkspace () const

ParameterContainer &	parameters ()

const ParameterContainer &	parameters () const

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

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

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

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

virtual bool	removeVertex (Vertex *v, bool detach=false)
	removes a vertex from the graph. Returns true on success (vertex was present) More...

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

virtual void	clear ()
	clears the graph and empties all structures. More...

const VertexIDMap &	vertices () const

VertexIDMap &	vertices ()

const EdgeSet &	edges () const

EdgeSet &	edges ()

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

virtual bool	detachVertex (Vertex *v)

virtual bool	changeId (Vertex *v, int newId)

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 Attributes
std::map< std::string, std::string >	_renamedTypesLookup

long long	_nextEdgeId

std::vector< HyperGraphActionSet >	_graphActions

bool	_edge_has_id

ParameterContainer	_parameters

JacobianWorkspace	_jacobianWorkspace

Protected Attributes inherited from g2o::HyperGraph
VertexIDMap	_vertices

EdgeSet	_edges

Additional Inherited Members
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 65 of file optimizable_graph.h.

Member Typedef Documentation

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

vector container for edges

Definition at line 102 of file optimizable_graph.h.

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

Definition at line 72 of file optimizable_graph.h.

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

vector container for vertices

Definition at line 100 of file optimizable_graph.h.

Member Enumeration Documentation

enum g2o::OptimizableGraph::ActionType

Enumerator
AT_PREITERATION
AT_POSTITERATION
AT_NUM_ELEMENTS

Definition at line 67 of file optimizable_graph.h.

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

Constructor & Destructor Documentation

g2o::OptimizableGraph::OptimizableGraph ( )

empty constructor

Definition at line 219 of file optimizable_graph.cpp.

References _edge_has_id, _graphActions, _nextEdgeId, and AT_NUM_ELEMENTS.

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

g2o::OptimizableGraph::~OptimizableGraph ( )

virtual

Definition at line 225 of file optimizable_graph.cpp.

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

   {
     clear();
     clearParameters();
   }

Member Function Documentation

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 257 of file optimizable_graph.cpp.

References g2o::OptimizableGraph::Edge::_internalId, _jacobianWorkspace, _nextEdgeId, g2o::HyperGraph::addEdge(), g2o::HyperGraph::Edge::numUndefinedVertices(), g2o::OptimizableGraph::Edge::resolveCaches(), g2o::OptimizableGraph::Edge::resolveParameters(), and g2o::JacobianWorkspace::updateSize().

   {
     OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(e_);
     assert(e && "Edge does not inherit from OptimizableGraph::Edge");
     //    std::cerr << "subclass of OptimizableGraph::Edge confirmed";
     if (! e)
       return false;
     bool eresult = HyperGraph::addEdge(e);
     if (! eresult)
       return false;
     //    std::cerr << "called HyperGraph::addEdge" << std::endl;
     e->_internalId = _nextEdgeId++;
     if (e->numUndefinedVertices())
       return true;
     //    std::cerr << "internalId set" << std::endl;
     if (! e->resolveParameters()){
       cerr << __FUNCTION__ << ": FATAL, cannot resolve parameters for edge " << e << endl;
       return false;
     }
     //    std::cerr << "parameters set" << std::endl;
     if (! e->resolveCaches()){
       cerr << __FUNCTION__ << ": FATAL, cannot resolve caches for edge " << e << endl;
       return false;
     }
     //    std::cerr << "updating jacobian size" << std::endl;
     _jacobianWorkspace.updateSize(e);
 
     //    std::cerr << "about to return true" << std::endl;
 
     return true;
   }

void g2o::OptimizableGraph::addGraph ( OptimizableGraph * g )

adds all edges and vertices of the graph g to this graph.

Definition at line 741 of file optimizable_graph.cpp.

References addEdge(), addVertex(), g2o::OptimizableGraph::Vertex::clone(), g2o::OptimizableGraph::Edge::clone(), g2o::HyperGraph::Vertex::edges(), g2o::HyperGraph::edges(), g2o::HyperGraph::Vertex::id(), g2o::HyperGraph::Edge::resize(), g2o::OptimizableGraph::Vertex::setHessianIndex(), g2o::HyperGraph::Edge::setVertex(), g2o::HyperGraph::Edge::vertex(), g2o::HyperGraph::Edge::vertices(), and g2o::HyperGraph::vertices().

                                                   {
   for (HyperGraph::VertexIDMap::iterator it=g->vertices().begin(); it!=g->vertices().end(); ++it){
     OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*)(it->second);
     if (vertex(v->id()))
       continue;
     OptimizableGraph::Vertex* v2=v->clone();
     v2->edges().clear();
     v2->setHessianIndex(-1);
     addVertex(v2);
   }
   for (HyperGraph::EdgeSet::iterator it=g->edges().begin(); it!=g->edges().end(); ++it){
     OptimizableGraph::Edge* e = (OptimizableGraph::Edge*)(*it);
     OptimizableGraph::Edge* en = e->clone();
     en->resize(e->vertices().size());
     int cnt = 0;
     for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
       OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) vertex((*it)->id());
       assert(v);
       en->setVertex(cnt++, v);
     }
     addEdge(en);
   }
 }

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

inline

Definition at line 611 of file optimizable_graph.h.

Referenced by main().

                                     {
       return _parameters.addParameter(p);
     }

bool g2o::OptimizableGraph::addPostIterationAction ( HyperGraphAction * action )

add an action to be executed after each iteration

Definition at line 855 of file optimizable_graph.cpp.

References _graphActions, and AT_POSTITERATION.

Referenced by main().

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

bool g2o::OptimizableGraph::addPreIterationAction ( HyperGraphAction * action )

add an action to be executed before each iteration

Definition at line 861 of file optimizable_graph.cpp.

References _graphActions, and AT_PREITERATION.

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

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

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 231 of file optimizable_graph.cpp.

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

Referenced by addGraph(), g2o::addOdometryCalibLinksDifferential(), g2o::G2oSlamInterface::addVertex(), main(), g2o::Gm2dlIO::readGm2dl(), and setFixed().

   {
     if (v->id() <0){
       cerr << __FUNCTION__ << ": FATAL, a vertex with (negative) ID " << v->id() << " cannot be inserted in the graph" << endl;
       assert(0 && "Invalid vertex id");
       return false;
     }
     Vertex* inserted = vertex(v->id());
     if (inserted) {
       cerr << __FUNCTION__ << ": FATAL, a vertex with ID " << v->id() << " has already been registered with this graph" << endl;
       assert(0 && "Vertex with this ID already contained in the graph");
       return false;
     }
     OptimizableGraph::Vertex* ov=dynamic_cast<OptimizableGraph::Vertex*>(v);
     assert(ov && "Vertex does not inherit from OptimizableGraph::Vertex");
     if (ov->_graph != 0 && ov->_graph != this) {
       cerr << __FUNCTION__ << ": FATAL, vertex with ID " << v->id() << " has already registered with another graph " << ov->_graph << endl;
       assert(0 && "Vertex already registered with another graph");
       return false;
     }
     if (userData)
       ov->setUserData(userData);
     ov->_graph=this;
     return HyperGraph::addVertex(v);
   }

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 513 of file optimizable_graph.h.

References addVertex(), and types.

Referenced by addVertex().

513 { return addVertex(v, 0);}

g2o::OptimizableGraph::addVertex

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

Definition: optimizable_graph.cpp:231

double g2o::OptimizableGraph::chi2 ( ) const

returns the chi2 of the current configuration

Definition at line 315 of file optimizable_graph.cpp.

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

Referenced by main().

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

void g2o::OptimizableGraph::clearParameters ( )

virtual

remove the parameters of the graph

Definition at line 940 of file optimizable_graph.cpp.

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

Referenced by ~OptimizableGraph().

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

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 823 of file optimizable_graph.cpp.

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

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

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

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 341 of file optimizable_graph.cpp.

References g2o::HyperGraph::Edge::_vertices, and g2o::OptimizableGraph::Vertex::discardTop().

Referenced by g2o::SparseOptimizer::solver().

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

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

Definition at line 365 of file optimizable_graph.cpp.

References g2o::OptimizableGraph::Vertex::discardTop().

 {
   for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
     OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
     v->discardTop();
   }
 }

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 980 of file optimizable_graph.cpp.

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

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 800 of file optimizable_graph.cpp.

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

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

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

JacobianWorkspace& g2o::OptimizableGraph::jacobianWorkspace ( )

inline

the workspace for storing the Jacobians of the graph

Definition at line 640 of file optimizable_graph.h.

Referenced by g2o::G2oSlamInterface::addEdge(), g2o::BlockSolver< Traits >::buildSystem(), and g2o::SparseOptimizerOnline::optimize().

640 { return _jacobianWorkspace;}

g2o::OptimizableGraph::_jacobianWorkspace

JacobianWorkspace _jacobianWorkspace

Definition: optimizable_graph.h:663

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

inline

Definition at line 641 of file optimizable_graph.h.

641 { return _jacobianWorkspace;}

g2o::OptimizableGraph::_jacobianWorkspace

JacobianWorkspace _jacobianWorkspace

Definition: optimizable_graph.h:663

virtual bool g2o::OptimizableGraph::load	(	std::istream &	is,
		bool	createEdges = `true`
	)

virtual

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

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

bool g2o::OptimizableGraph::load	(	const char *	filename,
		bool	createEdges = `true`
	)

Definition at line 629 of file optimizable_graph.cpp.

References __PRETTY_FUNCTION__, and load().

 {
   ifstream ifs(filename);
   if (!ifs) {
     cerr << __PRETTY_FUNCTION__ << " unable to open file " << filename << endl;
     return false;
   }
   return load(ifs, createEdges);
 }

int g2o::OptimizableGraph::maxDimension ( ) const

return the maximum dimension of all vertices in the graph

Definition at line 765 of file optimizable_graph.cpp.

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

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

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::SparseOptimizerOnline, and g2o::SparseOptimizerIncremental.

Definition at line 313 of file optimizable_graph.cpp.

313 {return 0;}

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

inline

Definition at line 615 of file optimizable_graph.h.

Referenced by main(), and g2o::OptimizableGraph::Edge::resolveParameters().

                                  {
       return _parameters.getParameter(id);
     }

ParameterContainer& g2o::OptimizableGraph::parameters ( )

inline

Definition at line 651 of file optimizable_graph.h.

651 {return _parameters;}

g2o::OptimizableGraph::_parameters

ParameterContainer _parameters

Definition: optimizable_graph.h:662

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

inline

Definition at line 652 of file optimizable_graph.h.

652 {return _parameters;}

g2o::OptimizableGraph::_parameters

ParameterContainer _parameters

Definition: optimizable_graph.h:662

void g2o::OptimizableGraph::pop ( )

virtual

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

Reimplemented in g2o::SparseOptimizer.

Definition at line 333 of file optimizable_graph.cpp.

References g2o::HyperGraph::Edge::_vertices, and g2o::OptimizableGraph::Vertex::pop().

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

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 357 of file optimizable_graph.cpp.

References g2o::OptimizableGraph::Vertex::pop().

 {
   for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
     OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
     v->pop();
   }
 }

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

virtual

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

Definition at line 844 of file optimizable_graph.cpp.

References _graphActions, actions, and AT_POSTITERATION.

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

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

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

virtual

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

Definition at line 833 of file optimizable_graph.cpp.

References _graphActions, actions, and AT_PREITERATION.

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

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

void g2o::OptimizableGraph::push ( )

virtual

push the estimate of all variables onto a stack

Reimplemented in g2o::SparseOptimizer.

Definition at line 325 of file optimizable_graph.cpp.

References g2o::HyperGraph::Edge::_vertices, and g2o::OptimizableGraph::Vertex::push().

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

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 349 of file optimizable_graph.cpp.

References g2o::OptimizableGraph::Vertex::push().

 {
   for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
     OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
     v->push();
   }
 }

bool g2o::OptimizableGraph::removePostIterationAction ( HyperGraphAction * action )

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

Definition at line 872 of file optimizable_graph.cpp.

References _graphActions, and AT_POSTITERATION.

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

bool g2o::OptimizableGraph::removePreIterationAction ( HyperGraphAction * action )

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

Definition at line 867 of file optimizable_graph.cpp.

References _graphActions, and AT_PREITERATION.

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

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

virtual

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

Referenced by main(), and save().

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

function provided for convenience, see save() above

Definition at line 639 of file optimizable_graph.cpp.

References g2o::OptimizableGraph::Edge::_parameters, g2o::HyperGraph::edges(), g2o::OptimizableGraph::Edge::level(), save(), saveEdge(), saveSubset(), saveVertex(), and g2o::HyperGraph::Edge::vertices().

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

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

Definition at line 920 of file optimizable_graph.cpp.

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

Referenced by save().

 {
   Factory* factory = Factory::instance();
   string tag = factory->tag(e);
   if (tag.size() > 0) {
     os << tag << " ";
     if (_edge_has_id)
       os << e->id() << " ";
     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;
 }

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

Definition at line 908 of file optimizable_graph.cpp.

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

 {
   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();
 }

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::assignHierarchicalEdges(), g2o::computeSimpleStars(), main(), and save().

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

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

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

Definition at line 877 of file optimizable_graph.cpp.

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

Referenced by saveEdge(), and saveVertex().

                                                                            {
   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();
 }

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

Definition at line 891 of file optimizable_graph.cpp.

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().

Referenced by save().

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

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

virtual

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

Returns: false if something goes wriong.

Reimplemented from g2o::HyperGraph.

Definition at line 289 of file optimizable_graph.cpp.

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

Referenced by anonymizeLandmarkEdge(), and anonymizePoseEdge().

                                                                                      {
     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()){
         cerr << __FUNCTION__ << ": FATAL, cannot resolve parameters for edge " << e << endl;
         return false;
       }
       if (! ee->resolveCaches()){
         cerr << __FUNCTION__ << ": FATAL, cannot resolve caches for edge " << e << endl;
         return false;
       }
       _jacobianWorkspace.updateSize(e);
     }
     return true;
   }

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

virtual

fixes/releases a set of vertices

Definition at line 373 of file optimizable_graph.cpp.

References __PRETTY_FUNCTION__, _edge_has_id, g2o::OptimizableGraph::Edge::_parameters, _renamedTypesLookup, addEdge(), addVertex(), CL_RED, g2o::Factory::construct(), g2o::OptimizableGraph::Edge::createVertex(), g2o::HyperGraph::Data::dataContainer(), HGET_PARAMETER, g2o::HyperGraph::Vertex::id(), g2o::HyperGraph::Edge::id(), g2o::OptimizableGraph::Edge::initialEstimate(), g2o::Factory::instance(), g2o::Factory::knowsTag(), load(), g2o::HyperGraph::Data::read(), g2o::OptimizableGraph::Vertex::read(), g2o::OptimizableGraph::Edge::read(), g2o::readLine(), g2o::HyperGraph::Edge::resize(), g2o::HyperGraph::Data::setDataContainer(), g2o::OptimizableGraph::Vertex::setFixed(), g2o::HyperGraph::Edge::setId(), g2o::OptimizableGraph::Vertex::setId(), g2o::HyperGraph::Data::setNext(), g2o::HyperGraph::DataContainer::setUserData(), g2o::HyperGraph::Edge::setVertex(), g2o::HyperGraph::UnassignedId, g2o::HyperGraph::Edge::vertex(), and g2o::HyperGraph::Edge::vertices().

Referenced by g2o::computeSimpleStars().

 {
   for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
     OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
     v->setFixed(fixed);
   }
 }

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 occurance of VERTEX_CAM in the file to VERTEX_SE3:EXPMAP

Definition at line 774 of file optimizable_graph.cpp.

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

Referenced by main().

 {
   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) {
       cerr << __PRETTY_FUNCTION__ << ": unable to extract type map from " << typesMap[i] << endl;
       continue;
     }
     string typeInFile = trim(m[0]);
     string loadedType = trim(m[1]);
     if (! factory->knowsTag(loadedType)) {
       cerr << __PRETTY_FUNCTION__ << ": unknown type " << loadedType << endl;
       continue;
     }
 
     _renamedTypesLookup[typeInFile] = loadedType;
   }
 
   cerr << "# load look up table" << endl;
   for (std::map<std::string, std::string>::const_iterator it = _renamedTypesLookup.begin(); it != _renamedTypesLookup.end(); ++it) {
     cerr << "#\t" << it->first << " -> " << it->second << endl;
   }
 }

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 on cerr

Returns: true if all edges have PSD information matrix

Definition at line 946 of file optimizable_graph.cpp.

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

 {
   bool allEdgeOk = true;
   Eigen::SelfAdjointEigenSolver<MatrixXD> eigenSolver;
   for (OptimizableGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
     OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
     MatrixXD::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) {
         if (! isSymmetric)
           cerr << "Information Matrix for an edge is not symmetric:";
         else
           cerr << "Information Matrix for an edge is not SPD:";
         for (size_t i = 0; i < e->vertices().size(); ++i)
           cerr << " " << e->vertex(i)->id();
         if (isSymmetric)
           cerr << "\teigenvalues: " << eigenSolver.eigenvalues().transpose();
         cerr << endl;
       }
     }
   }
   return allEdgeOk;
 }