#include <optimizable_graph.h>
Public Member Functions | |
virtual bool | allVerticesFixed () const =0 |
virtual double | chi2 () const =0 |
computes the chi2 based on the cached error value, only valid after computeError has been called. More... | |
virtual Edge * | clone () const |
virtual void | computeError ()=0 |
virtual void | constructQuadraticForm ()=0 |
virtual Vertex * | createFrom () |
virtual Vertex * | createTo () |
int | dimension () const |
returns the dimensions of the error function More... | |
Edge () | |
virtual const double * | errorData () const =0 |
returns the error vector cached after calling the computeError; More... | |
virtual double * | errorData ()=0 |
virtual bool | getMeasurementData (double *m) const |
OptimizableGraph * | graph () |
const OptimizableGraph * | graph () const |
virtual const double * | informationData () const =0 |
returns the memory of the information matrix, usable for example with a Eigen::Map<MatrixXd> More... | |
virtual double * | informationData ()=0 |
virtual void | initialEstimate (const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to)=0 |
virtual double | initialEstimatePossible (const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to) |
long long | internalId () const |
the internal ID of the edge More... | |
int | level () const |
returns the level of the edge More... | |
virtual void | linearizeOplus (JacobianWorkspace &jacobianWorkspace)=0 |
virtual void | mapHessianMemory (double *d, int i, int j, bool rowMajor)=0 |
virtual int | measurementDimension () const |
size_t | numParameters () const |
const Parameter * | parameter (int argNo) const |
virtual bool | read (std::istream &is)=0 |
read the vertex from a stream, i.e., the internal state of the vertex More... | |
void | resizeParameters (size_t newSize) |
RobustKernel * | robustKernel () const |
if NOT NULL, error of this edge will be robustifed with the kernel More... | |
void | setLevel (int l) |
sets the level of the edge More... | |
virtual bool | setMeasurementData (const double *m) |
virtual bool | setMeasurementFromState () |
bool | setParameterId (int argNum, int paramId) |
void | setRobustKernel (RobustKernel *ptr) |
virtual bool | write (std::ostream &os) const =0 |
write the vertex to a stream More... | |
virtual | ~Edge () |
Public Member Functions inherited from g2o::HyperGraph::Edge | |
Edge (int id=-1) | |
creates and empty edge with no vertices More... | |
virtual HyperGraphElementType | elementType () const |
int | id () const |
virtual void | resize (size_t size) |
void | setId (int id) |
void | setVertex (size_t i, Vertex *v) |
const Vertex * | vertex (size_t i) const |
Vertex * | vertex (size_t i) |
const VertexContainer & | vertices () const |
VertexContainer & | vertices () |
Public Member Functions inherited from g2o::HyperGraph::HyperGraphElement | |
virtual | ~HyperGraphElement () |
Protected Member Functions | |
template<typename ParameterType > | |
bool | installParameter (ParameterType *&p, size_t argNo, int paramId=-1) |
template<typename CacheType > | |
void | resolveCache (CacheType *&cache, OptimizableGraph::Vertex *, const std::string &_type, const ParameterVector ¶meters) |
virtual bool | resolveCaches () |
bool | resolveParameters () |
Protected Attributes | |
std::vector< int > | _cacheIds |
int | _dimension |
long long | _internalId |
int | _level |
std::vector< int > | _parameterIds |
std::vector< Parameter ** > | _parameters |
std::vector< std::string > | _parameterTypes |
RobustKernel * | _robustKernel |
Protected Attributes inherited from g2o::HyperGraph::Edge | |
int | _id |
unique id More... | |
VertexContainer | _vertices |
Friends | |
struct | OptimizableGraph |
Definition at line 382 of file optimizable_graph.h.
g2o::OptimizableGraph::Edge::Edge | ( | ) |
Definition at line 132 of file optimizable_graph.cpp.
|
virtual |
Reimplemented from g2o::HyperGraph::Edge.
Definition at line 138 of file optimizable_graph.cpp.
|
pure virtual |
Implemented in g2o::BaseMultiEdge< D, E >, g2o::BaseBinaryEdge< D, E, VertexXi, VertexXj >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap >, g2o::BaseBinaryEdge< 7, Sim3, VertexSim3Expmap, VertexSim3Expmap >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap >, g2o::BaseBinaryEdge< 3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap >, g2o::BaseUnaryEdge< D, E, VertexXi >, g2o::BaseUnaryEdge< 2, Vector2d, VertexSE3Expmap >, and g2o::BaseUnaryEdge< 3, Vector3d, VertexSE3Expmap >.
|
pure virtual |
computes the chi2 based on the cached error value, only valid after computeError has been called.
Implemented in g2o::BaseEdge< D, E >, g2o::BaseEdge< D, Vector2d >, g2o::BaseEdge< D, Vector3d >, and g2o::BaseEdge< D, Sim3 >.
|
virtual |
Definition at line 224 of file optimizable_graph.cpp.
|
pure virtual |
|
pure virtual |
Linearizes the constraint in the edge. Makes side effect on the vertices of the graph by changing the parameter vector b and the hessian blocks ii and jj. The off diagoinal block is accesed via _hessian.
Implemented in g2o::BaseBinaryEdge< D, E, VertexXi, VertexXj >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap >, g2o::BaseBinaryEdge< 7, Sim3, VertexSim3Expmap, VertexSim3Expmap >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap >, g2o::BaseBinaryEdge< 3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap >, g2o::BaseMultiEdge< D, E >, g2o::BaseUnaryEdge< D, E, VertexXi >, g2o::BaseUnaryEdge< 2, Vector2d, VertexSE3Expmap >, and g2o::BaseUnaryEdge< 3, Vector3d, VertexSE3Expmap >.
|
inlinevirtual |
Reimplemented in g2o::BaseBinaryEdge< D, E, VertexXi, VertexXj >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap >, g2o::BaseBinaryEdge< 7, Sim3, VertexSim3Expmap, VertexSim3Expmap >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap >, and g2o::BaseBinaryEdge< 3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap >.
Definition at line 474 of file optimizable_graph.h.
|
inlinevirtual |
Reimplemented in g2o::BaseBinaryEdge< D, E, VertexXi, VertexXj >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap >, g2o::BaseBinaryEdge< 7, Sim3, VertexSim3Expmap, VertexSim3Expmap >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap >, and g2o::BaseBinaryEdge< 3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap >.
Definition at line 475 of file optimizable_graph.h.
|
inline |
returns the dimensions of the error function
Definition at line 472 of file optimizable_graph.h.
|
pure virtual |
returns the error vector cached after calling the computeError;
Implemented in g2o::BaseEdge< D, E >, g2o::BaseEdge< D, Vector2d >, g2o::BaseEdge< D, Vector3d >, and g2o::BaseEdge< D, Sim3 >.
|
pure virtual |
Implemented in g2o::BaseEdge< D, E >, g2o::BaseEdge< D, Vector2d >, g2o::BaseEdge< D, Vector3d >, and g2o::BaseEdge< D, Sim3 >.
|
virtual |
writes the measurement to an array of double
Definition at line 209 of file optimizable_graph.cpp.
OptimizableGraph * g2o::OptimizableGraph::Edge::graph | ( | ) |
Definition at line 143 of file optimizable_graph.cpp.
const OptimizableGraph * g2o::OptimizableGraph::Edge::graph | ( | ) | const |
Definition at line 152 of file optimizable_graph.cpp.
|
pure virtual |
returns the memory of the information matrix, usable for example with a Eigen::Map<MatrixXd>
Implemented in g2o::BaseEdge< D, E >, g2o::BaseEdge< D, Vector2d >, g2o::BaseEdge< D, Vector3d >, and g2o::BaseEdge< D, Sim3 >.
|
pure virtual |
Implemented in g2o::BaseEdge< D, E >, g2o::BaseEdge< D, Vector2d >, g2o::BaseEdge< D, Vector3d >, and g2o::BaseEdge< D, Sim3 >.
|
pure virtual |
set the estimate of the to vertex, based on the estimate of the from vertices in the edge.
Implemented in g2o::EdgeSim3, g2o::BaseEdge< D, E >, g2o::BaseEdge< D, Vector2d >, g2o::BaseEdge< D, Vector3d >, g2o::BaseEdge< D, Sim3 >, g2o::BaseUnaryEdge< D, E, VertexXi >, g2o::BaseUnaryEdge< 2, Vector2d, VertexSE3Expmap >, and g2o::BaseUnaryEdge< 3, Vector3d, VertexSE3Expmap >.
|
inlinevirtual |
override in your class if it's possible to initialize the vertices in certain combinations. The return value may correspond to the cost for initiliaizng the vertex but should be positive if the initialization is possible and negative if not possible.
Reimplemented in g2o::EdgeSim3.
Definition at line 464 of file optimizable_graph.h.
|
inlineprotected |
Definition at line 505 of file optimizable_graph.h.
|
inline |
the internal ID of the edge
Definition at line 483 of file optimizable_graph.h.
|
inline |
returns the level of the edge
Definition at line 467 of file optimizable_graph.h.
|
pure virtual |
Linearizes the constraint in the edge in the manifold space, and store the result in the given workspace
Implemented in g2o::BaseBinaryEdge< D, E, VertexXi, VertexXj >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap >, g2o::BaseBinaryEdge< 7, Sim3, VertexSim3Expmap, VertexSim3Expmap >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap >, g2o::BaseBinaryEdge< 3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap >, g2o::BaseMultiEdge< D, E >, g2o::BaseUnaryEdge< D, E, VertexXi >, g2o::BaseUnaryEdge< 2, Vector2d, VertexSE3Expmap >, and g2o::BaseUnaryEdge< 3, Vector3d, VertexSE3Expmap >.
|
pure virtual |
maps the internal matrix to some external memory location, you need to provide the memory before calling constructQuadraticForm
d | the memory location to which we map |
i | index of the vertex i |
j | index of the vertex j (j > i, upper triangular fashion) |
rowMajor | if true, will write in rowMajor order to the block. Since EIGEN is columnMajor by default, this results in writing the transposed |
Implemented in g2o::BaseBinaryEdge< D, E, VertexXi, VertexXj >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap >, g2o::BaseBinaryEdge< 7, Sim3, VertexSim3Expmap, VertexSim3Expmap >, g2o::BaseBinaryEdge< 2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap >, g2o::BaseBinaryEdge< 3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap >, g2o::BaseMultiEdge< D, E >, g2o::BaseUnaryEdge< D, E, VertexXi >, g2o::BaseUnaryEdge< 2, Vector2d, VertexSE3Expmap >, and g2o::BaseUnaryEdge< 3, Vector3d, VertexSE3Expmap >.
|
virtual |
returns the dimension of the measurement in the extended representation which is used by get/setMeasurement;
Definition at line 214 of file optimizable_graph.cpp.
|
inline |
Definition at line 490 of file optimizable_graph.h.
|
inline |
Definition at line 489 of file optimizable_graph.h.
|
pure virtual |
read the vertex from a stream, i.e., the internal state of the vertex
Implemented in g2o::EdgeStereoSE3ProjectXYZOnlyPose, g2o::EdgeInverseSim3ProjectXYZ, g2o::EdgeSE3ProjectXYZOnlyPose, g2o::EdgeSim3ProjectXYZ, g2o::EdgeStereoSE3ProjectXYZ, g2o::EdgeSim3, and g2o::EdgeSE3ProjectXYZ.
|
inline |
Definition at line 491 of file optimizable_graph.h.
|
protected |
|
protectedvirtual |
Definition at line 200 of file optimizable_graph.cpp.
|
protected |
Definition at line 171 of file optimizable_graph.cpp.
|
inline |
if NOT NULL, error of this edge will be robustifed with the kernel
Definition at line 415 of file optimizable_graph.h.
|
inline |
sets the level of the edge
Definition at line 469 of file optimizable_graph.h.
|
virtual |
sets the measurement from an array of double
Definition at line 204 of file optimizable_graph.cpp.
|
virtual |
sets the estimate to have a zero error, based on the current value of the state variables returns false if not supported.
Definition at line 219 of file optimizable_graph.cpp.
bool g2o::OptimizableGraph::Edge::setParameterId | ( | int | argNum, |
int | paramId | ||
) |
Definition at line 161 of file optimizable_graph.cpp.
void g2o::OptimizableGraph::Edge::setRobustKernel | ( | RobustKernel * | ptr | ) |
specify the robust kernel to be used in this edge
Definition at line 193 of file optimizable_graph.cpp.
|
pure virtual |
write the vertex to a stream
Implemented in g2o::EdgeStereoSE3ProjectXYZOnlyPose, g2o::EdgeInverseSim3ProjectXYZ, g2o::EdgeSE3ProjectXYZOnlyPose, g2o::EdgeSim3ProjectXYZ, g2o::EdgeStereoSE3ProjectXYZ, g2o::EdgeSim3, and g2o::EdgeSE3ProjectXYZ.
|
friend |
Definition at line 384 of file optimizable_graph.h.
|
protected |
Definition at line 502 of file optimizable_graph.h.
|
protected |
Definition at line 497 of file optimizable_graph.h.
|
protected |
Definition at line 500 of file optimizable_graph.h.
|
protected |
Definition at line 498 of file optimizable_graph.h.
|
protected |
Definition at line 524 of file optimizable_graph.h.
|
protected |
Definition at line 523 of file optimizable_graph.h.
|
protected |
Definition at line 522 of file optimizable_graph.h.
|
protected |
Definition at line 499 of file optimizable_graph.h.