#include <optimizable_graph.h>
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_ELEMS > | GraphElemBitset |
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 |
JacobianWorkspace & | jacobianWorkspace () |
the workspace for storing the Jacobians of the graph More... | |
const JacobianWorkspace & | jacobianWorkspace () 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) |
Parameter * | parameter (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 |
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... | |
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 EdgeSet & | edges () const |
EdgeSet & | edges () |
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... | |
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... | |
const VertexIDMap & | vertices () const |
VertexIDMap & | vertices () |
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 |
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.
typedef std::vector<OptimizableGraph::Edge*> g2o::OptimizableGraph::EdgeContainer |
vector container for edges
Definition at line 122 of file optimizable_graph.h.
Definition at line 71 of file optimizable_graph.h.
typedef std::vector<OptimizableGraph::Vertex*> g2o::OptimizableGraph::VertexContainer |
vector container for vertices
Definition at line 120 of file optimizable_graph.h.
Enumerator | |
---|---|
AT_PREITERATION | |
AT_POSTITERATION | |
AT_NUM_ELEMENTS |
Definition at line 66 of file optimizable_graph.h.
g2o::OptimizableGraph::OptimizableGraph | ( | ) |
empty constructor
Definition at line 231 of file optimizable_graph.cpp.
|
virtual |
Definition at line 237 of file optimizable_graph.cpp.
|
virtual |
adds a new edge. The edge should point to the vertices that it is connecting (setFrom/setTo).
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.
|
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.
|
virtual |
adds a new vertex. The new vertex is then "taken".
Definition at line 243 of file optimizable_graph.cpp.
|
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.
|
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.
|
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.
|
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.
|
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.
solverProperty | the solver property to evaluate. |
vertDims | should equal to the set returned by dimensions() to avoid re-evaluating. |
Definition at line 740 of file optimizable_graph.cpp.
|
inline |
the workspace for storing the Jacobians of the graph
Definition at line 659 of file optimizable_graph.h.
|
inline |
Definition at line 660 of file optimizable_graph.h.
|
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.
|
virtual |
carry out n iterations
Reimplemented in g2o::SparseOptimizer.
Definition at line 287 of file optimizable_graph.cpp.
|
inline |
Definition at line 641 of file optimizable_graph.h.
|
virtual |
pop (restore) the estimate of all variables from the stack
Reimplemented in g2o::SparseOptimizer.
Definition at line 307 of file optimizable_graph.cpp.
|
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.
|
virtual |
called at the end of an iteration (argument is the number of the iteration)
Definition at line 784 of file optimizable_graph.cpp.
|
virtual |
called at the beginning of an iteration (argument is the number of the iteration)
Definition at line 773 of file optimizable_graph.cpp.
|
virtual |
push the estimate of all variables onto a stack
Reimplemented in g2o::SparseOptimizer.
Definition at line 299 of file optimizable_graph.cpp.
|
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 |
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.
|
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.
verbose | output edges with not PSD information matrix on cerr |
Definition at line 867 of file optimizable_graph.cpp.
|
inline |
returns the vertex number id appropriately casted
Definition at line 528 of file optimizable_graph.h.
|
inline |
returns the vertex number id appropriately casted
Definition at line 531 of file optimizable_graph.h.
|
protected |
Definition at line 676 of file optimizable_graph.h.
|
protected |
Definition at line 673 of file optimizable_graph.h.
|
protected |
Definition at line 679 of file optimizable_graph.h.
|
protected |
Definition at line 672 of file optimizable_graph.h.
|
protected |
Definition at line 678 of file optimizable_graph.h.
|
protected |
Definition at line 671 of file optimizable_graph.h.