3D edge between two VertexSE3 More...
#include <edge_se3_quat.h>
Public Member Functions | |
void | computeError () |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | EdgeSE3 () |
virtual bool | getMeasurementData (double *d) const |
virtual void | initialEstimate (const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to) |
virtual double | initialEstimatePossible (const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *) |
virtual void | linearizeOplus () |
virtual int | measurementDimension () const |
virtual bool | read (std::istream &is) |
read the vertex from a stream, i.e., the internal state of the vertex | |
virtual void | setMeasurement (const SE3Quat &m) |
virtual bool | setMeasurementData (const double *d) |
virtual bool | setMeasurementFromState () |
virtual bool | write (std::ostream &os) const |
write the vertex to a stream |
3D edge between two VertexSE3
Definition at line 38 of file edge_se3_quat.h.
Definition at line 38 of file edge_se3_quat.cpp.
void g2o::EdgeSE3::computeError | ( | ) | [inline, virtual] |
Implements g2o::OptimizableGraph::Edge.
Definition at line 43 of file edge_se3_quat.h.
virtual bool g2o::EdgeSE3::getMeasurementData | ( | double * | m | ) | const [inline, virtual] |
writes the measurement to an array of double
Reimplemented from g2o::OptimizableGraph::Edge.
Definition at line 71 of file edge_se3_quat.h.
void g2o::EdgeSE3::initialEstimate | ( | const OptimizableGraph::VertexSet & | from, |
OptimizableGraph::Vertex * | to | ||
) | [virtual] |
set the estimate of the to vertex, based on the estimate of the from vertices in the edge.
Reimplemented from g2o::BaseEdge< D, SE3Quat >.
Reimplemented in g2o::OnlineEdgeSE3.
Definition at line 79 of file edge_se3_quat.cpp.
virtual double g2o::EdgeSE3::initialEstimatePossible | ( | const OptimizableGraph::VertexSet & | from, |
OptimizableGraph::Vertex * | to | ||
) | [inline, virtual] |
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 from g2o::OptimizableGraph::Edge.
Definition at line 85 of file edge_se3_quat.h.
void g2o::EdgeSE3::linearizeOplus | ( | ) | [virtual] |
Linearizes the oplus operator in the vertex, and stores the result in temporary variables _jacobianOplusXi and _jacobianOplusXj
Reimplemented from g2o::BaseBinaryEdge< 6, SE3Quat, VertexSE3, VertexSE3 >.
Definition at line 90 of file edge_se3_quat.cpp.
virtual int g2o::EdgeSE3::measurementDimension | ( | ) | const [inline, virtual] |
returns the dimension of the measurement in the extended representation which is used by get/setMeasurement;
Reimplemented from g2o::OptimizableGraph::Edge.
Definition at line 78 of file edge_se3_quat.h.
bool g2o::EdgeSE3::read | ( | std::istream & | is | ) | [virtual] |
read the vertex from a stream, i.e., the internal state of the vertex
Implements g2o::OptimizableGraph::Edge.
Definition at line 52 of file edge_se3_quat.cpp.
virtual void g2o::EdgeSE3::setMeasurement | ( | const SE3Quat & | m | ) | [inline, virtual] |
Reimplemented from g2o::BaseEdge< D, SE3Quat >.
Definition at line 56 of file edge_se3_quat.h.
virtual bool g2o::EdgeSE3::setMeasurementData | ( | const double * | m | ) | [inline, virtual] |
sets the measurement from an array of double
Reimplemented from g2o::OptimizableGraph::Edge.
Definition at line 61 of file edge_se3_quat.h.
bool g2o::EdgeSE3::setMeasurementFromState | ( | ) | [virtual] |
sets the estimate to have a zero error, based on the current value of the state variables returns false if not supported.
Reimplemented from g2o::OptimizableGraph::Edge.
Definition at line 44 of file edge_se3_quat.cpp.
bool g2o::EdgeSE3::write | ( | std::ostream & | os | ) | const [virtual] |
write the vertex to a stream
Implements g2o::OptimizableGraph::Edge.
Definition at line 68 of file edge_se3_quat.cpp.