Classes | Public Types | Public Member Functions | Static Public Member Functions | Protected Attributes | List of all members
g2o::OptimizableGraph Struct Reference

#include <optimizable_graph.h>

Inheritance diagram for g2o::OptimizableGraph:
Inheritance graph
[legend]

Classes

class  Data
 data packet for a vertex. Extend this class to store in the vertices the potential additional information you need (e.g. images, laser scans, ...). More...
 
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::vector< OptimizableGraph::Edge * > EdgeContainer
 vector container for edges More...
 
typedef std::set< HyperGraphAction * > HyperGraphActionSet
 
typedef std::vector< OptimizableGraph::Vertex * > VertexContainer
 vector container for vertices More...
 
- Public Types inherited from g2o::HyperGraph
typedef std::set< Edge * > EdgeSet
 
typedef std::bitset< HyperGraph::HGET_NUM_ELEMSGraphElemBitset
 
enum  HyperGraphElementType {
  HGET_VERTEX, HGET_EDGE, HGET_PARAMETER, HGET_CACHE,
  HGET_DATA, HGET_NUM_ELEMS
}
 enum of all the types we have in our graphs More...
 
typedef std::vector< Vertex * > VertexContainer
 
typedef std::tr1::unordered_map< int, Vertex * > VertexIDMap
 
typedef std::set< Vertex * > VertexSet
 

Public Member Functions

virtual bool addEdge (HyperGraph::Edge *e)
 
void addGraph (OptimizableGraph *g)
 adds all edges and vertices of the graph g to this graph. More...
 
bool addParameter (Parameter *p)
 
bool addPostIterationAction (HyperGraphAction *action)
 add an action to be executed after each iteration More...
 
bool addPreIterationAction (HyperGraphAction *action)
 add an action to be executed before each iteration More...
 
virtual bool addVertex (HyperGraph::Vertex *v, Data *userData)
 
virtual bool addVertex (HyperGraph::Vertex *v)
 
double chi2 () const
 returns the chi2 of the current configuration More...
 
virtual void clearParameters ()
 
std::set< int > dimensions () const
 
virtual void discardTop ()
 discard the last backup of the estimate for all variables by removing it 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...
 
bool isSolverSuitable (const OptimizationAlgorithmProperty &solverProperty, const std::set< int > &vertDims=std::set< int >()) const
 
JacobianWorkspacejacobianWorkspace ()
 the workspace for storing the Jacobians of the graph More...
 
const JacobianWorkspacejacobianWorkspace () const
 
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)
 
int maxDimension () const
 return the maximum dimension of all vertices in the graph More...
 
 OptimizableGraph ()
 empty constructor More...
 
virtual int optimize (int iterations, bool online=false)
 
Parameterparameter (int id)
 
virtual void pop ()
 pop (restore) the estimate of all variables from the stack More...
 
virtual void pop (HyperGraph::VertexSet &vset)
 pop (restore) the estimate a subset of the variables from the stack More...
 
virtual void postIteration (int)
 called at the end of an iteration (argument is the number of the iteration) More...
 
virtual void preIteration (int)
 called at the beginning of an iteration (argument is the number of the iteration) More...
 
virtual void push ()
 push the estimate of all variables onto a stack More...
 
virtual void push (HyperGraph::VertexSet &vset)
 push the estimate of a subset of the variables onto a stack More...
 
bool removePostIterationAction (HyperGraphAction *action)
 remove an action that should no longer be execured after each iteration More...
 
bool removePreIterationAction (HyperGraphAction *action)
 remove an action that should no longer be execured before each iteration More...
 
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 saveEdge (std::ostream &os, Edge *e) const
 
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...
 
bool saveVertex (std::ostream &os, Vertex *v) const
 
virtual void setFixed (HyperGraph::VertexSet &vset, bool fixed)
 fixes/releases a set of vertices More...
 
void setRenamedTypesFromString (const std::string &types)
 
bool verifyInformationMatrices (bool verbose=false) const
 
Vertexvertex (int id)
 returns the vertex number id appropriately casted More...
 
const Vertexvertex (int id) const
 returns the vertex number id appropriately casted More...
 
virtual ~OptimizableGraph ()
 
- Public Member Functions inherited from g2o::HyperGraph
virtual bool changeId (Vertex *v, int newId)
 
virtual void clear ()
 clears the graph and empties all structures. More...
 
const EdgeSetedges () const
 
EdgeSetedges ()
 
 HyperGraph ()
 constructs an empty hyper graph More...
 
virtual bool removeEdge (Edge *e)
 removes a vertex from the graph. Returns true on success (edge was present) More...
 
virtual bool removeVertex (Vertex *v)
 removes a vertex from the graph. Returns true on success (vertex was present) More...
 
Vertexvertex (int id)
 returns a vertex id in the hyper-graph, or 0 if the vertex id is not present More...
 
const Vertexvertex (int id) const
 returns a vertex id in the hyper-graph, or 0 if the vertex id is not present More...
 
const VertexIDMapvertices () const
 
VertexIDMapvertices ()
 
virtual ~HyperGraph ()
 destroys the hyper-graph and all the vertices of the graph More...
 

Static Public Member Functions

static bool initMultiThreading ()
 

Protected Attributes

bool _edge_has_id
 
std::vector< HyperGraphActionSet_graphActions
 
JacobianWorkspace _jacobianWorkspace
 
long long _nextEdgeId
 
ParameterContainer _parameters
 
std::map< std::string, std::string > _renamedTypesLookup
 
- Protected Attributes inherited from g2o::HyperGraph
EdgeSet _edges
 
VertexIDMap _vertices
 

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

Member Typedef Documentation

vector container for edges

Definition at line 122 of file optimizable_graph.h.

Definition at line 71 of file optimizable_graph.h.

vector container for vertices

Definition at line 120 of file optimizable_graph.h.

Member Enumeration Documentation

Enumerator
AT_PREITERATION 
AT_POSTITERATION 
AT_NUM_ELEMENTS 

Definition at line 66 of file optimizable_graph.h.

Constructor & Destructor Documentation

g2o::OptimizableGraph::OptimizableGraph ( )

empty constructor

Definition at line 231 of file optimizable_graph.cpp.

g2o::OptimizableGraph::~OptimizableGraph ( )
virtual

Definition at line 237 of file optimizable_graph.cpp.

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

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

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

Definition at line 681 of file optimizable_graph.cpp.

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

Definition at line 637 of file optimizable_graph.h.

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

add an action to be executed after each iteration

Definition at line 795 of file optimizable_graph.cpp.

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

add an action to be executed before each iteration

Definition at line 801 of file optimizable_graph.cpp.

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

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

double g2o::OptimizableGraph::chi2 ( ) const

returns the chi2 of the current configuration

Definition at line 289 of file optimizable_graph.cpp.

void g2o::OptimizableGraph::clearParameters ( )
virtual

remove the parameters of the graph

Definition at line 862 of file optimizable_graph.cpp.

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

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

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

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

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
solverPropertythe solver property to evaluate.
vertDimsshould equal to the set returned by dimensions() to avoid re-evaluating.

Definition at line 740 of file optimizable_graph.cpp.

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

the workspace for storing the Jacobians of the graph

Definition at line 659 of file optimizable_graph.h.

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

Definition at line 660 of file optimizable_graph.h.

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.

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

Definition at line 571 of file optimizable_graph.cpp.

int g2o::OptimizableGraph::maxDimension ( ) const

return the maximum dimension of all vertices in the graph

Definition at line 705 of file optimizable_graph.cpp.

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

carry out n iterations

Returns
the number of performed iterations

Reimplemented in g2o::SparseOptimizer.

Definition at line 287 of file optimizable_graph.cpp.

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

Definition at line 641 of file optimizable_graph.h.

void g2o::OptimizableGraph::pop ( )
virtual

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

Reimplemented in g2o::SparseOptimizer.

Definition at line 307 of file optimizable_graph.cpp.

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

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

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

Definition at line 784 of file optimizable_graph.cpp.

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

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

Definition at line 773 of file optimizable_graph.cpp.

void g2o::OptimizableGraph::push ( )
virtual

push the estimate of all variables onto a stack

Reimplemented in g2o::SparseOptimizer.

Definition at line 299 of file optimizable_graph.cpp.

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

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

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

Definition at line 812 of file optimizable_graph.cpp.

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

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

Definition at line 807 of file optimizable_graph.cpp.

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

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

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

function provided for convenience, see save() above

Definition at line 581 of file optimizable_graph.cpp.

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

Definition at line 843 of file optimizable_graph.cpp.

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

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

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

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

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

Definition at line 817 of file optimizable_graph.cpp.

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

fixes/releases a set of vertices

Definition at line 347 of file optimizable_graph.cpp.

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

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
verboseoutput edges with not PSD information matrix on cerr
Returns
true if all edges have PSD information matrix

Definition at line 867 of file optimizable_graph.cpp.

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

returns the vertex number id appropriately casted

Definition at line 528 of file optimizable_graph.h.

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

returns the vertex number id appropriately casted

Definition at line 531 of file optimizable_graph.h.

Member Data Documentation

bool g2o::OptimizableGraph::_edge_has_id
protected

Definition at line 676 of file optimizable_graph.h.

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

Definition at line 673 of file optimizable_graph.h.

JacobianWorkspace g2o::OptimizableGraph::_jacobianWorkspace
protected

Definition at line 679 of file optimizable_graph.h.

long long g2o::OptimizableGraph::_nextEdgeId
protected

Definition at line 672 of file optimizable_graph.h.

ParameterContainer g2o::OptimizableGraph::_parameters
protected

Definition at line 678 of file optimizable_graph.h.

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

Definition at line 671 of file optimizable_graph.h.


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


orb_slam2_ros
Author(s):
autogenerated on Wed Apr 21 2021 02:53:06