g2o
|
prior for an SE3 element More...
#include <edge_se3_prior.h>
Public Member Functions | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | EdgeSE3Prior () |
virtual bool | read (std::istream &is) |
read the vertex from a stream, i.e., the internal state of the vertex More... | |
virtual bool | write (std::ostream &os) const |
write the vertex to a stream More... | |
void | computeError () |
virtual void | linearizeOplus () |
virtual void | setMeasurement (const Isometry3D &m) |
virtual bool | setMeasurementData (const double *d) |
virtual bool | getMeasurementData (double *d) const |
virtual int | measurementDimension () const |
virtual bool | setMeasurementFromState () |
virtual double | initialEstimatePossible (const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *) |
virtual void | initialEstimate (const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to) |
Public Member Functions inherited from g2o::BaseUnaryEdge< 6, Isometry3D, VertexSE3 > | |
BaseUnaryEdge () | |
virtual void | resize (size_t size) |
virtual bool | allVerticesFixed () const |
virtual void | linearizeOplus (JacobianWorkspace &jacobianWorkspace) |
virtual OptimizableGraph::Vertex * | createVertex (int i) |
const JacobianXiOplusType & | jacobianOplusXi () const |
returns the result of the linearization in the manifold space for the node xi More... | |
virtual void | constructQuadraticForm () |
virtual void | mapHessianMemory (double *, int, int, bool) |
Public Member Functions inherited from g2o::BaseEdge< D, Isometry3D > | |
BaseEdge () | |
virtual | ~BaseEdge () |
virtual double | chi2 () const |
computes the chi2 based on the cached error value, only valid after computeError has been called. More... | |
virtual const double * | errorData () const |
returns the error vector cached after calling the computeError; More... | |
virtual double * | errorData () |
const ErrorVector & | error () const |
ErrorVector & | error () |
EIGEN_STRONG_INLINE const InformationType & | information () const |
information matrix of the constraint More... | |
EIGEN_STRONG_INLINE InformationType & | information () |
EIGEN_STRONG_INLINE void | setInformation (const InformationType &information) |
virtual const double * | informationData () const |
returns the memory of the information matrix, usable for example with a Eigen::Map<MatrixXD> More... | |
virtual double * | informationData () |
EIGEN_STRONG_INLINE const Measurement & | measurement () const |
accessor functions for the measurement represented by the edge More... | |
virtual int | rank () const |
Public Member Functions inherited from g2o::OptimizableGraph::Edge | |
Edge () | |
virtual | ~Edge () |
virtual Edge * | clone () const |
RobustKernel * | robustKernel () const |
if NOT NULL, error of this edge will be robustifed with the kernel More... | |
void | setRobustKernel (RobustKernel *ptr) |
int | level () const |
returns the level of the edge More... | |
void | setLevel (int l) |
sets the level of the edge More... | |
int | dimension () const |
returns the dimensions of the error function More... | |
G2O_ATTRIBUTE_DEPRECATED (virtual Vertex *createFrom()) | |
G2O_ATTRIBUTE_DEPRECATED (virtual Vertex *createTo()) | |
long long | internalId () const |
the internal ID of the edge More... | |
OptimizableGraph * | graph () |
const OptimizableGraph * | graph () const |
bool | setParameterId (int argNum, int paramId) |
const Parameter * | parameter (int argNo) const |
size_t | numParameters () const |
void | resizeParameters (size_t newSize) |
Public Member Functions inherited from g2o::HyperGraph::Edge | |
Edge (int id=InvalidId) | |
creates and empty edge with no vertices More... | |
const VertexContainer & | vertices () const |
VertexContainer & | vertices () |
const Vertex * | vertex (size_t i) const |
Vertex * | vertex (size_t i) |
void | setVertex (size_t i, Vertex *v) |
int | id () const |
void | setId (int id) |
virtual HyperGraphElementType | elementType () const |
int | numUndefinedVertices () const |
Public Member Functions inherited from g2o::HyperGraph::HyperGraphElement | |
virtual | ~HyperGraphElement () |
HyperGraphElement * | clone () const |
Public Member Functions inherited from g2o::HyperGraph::DataContainer | |
DataContainer () | |
virtual | ~DataContainer () |
const Data * | userData () const |
the user data associated with this vertex More... | |
Data * | userData () |
void | setUserData (Data *obs) |
void | addUserData (Data *obs) |
Protected Member Functions | |
virtual bool | resolveCaches () |
Protected Member Functions inherited from g2o::BaseEdge< D, Isometry3D > | |
InformationType | robustInformation (const Vector3D &rho) |
Protected Member Functions inherited from g2o::OptimizableGraph::Edge | |
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) |
bool | resolveParameters () |
Protected Attributes | |
Isometry3D | _inverseMeasurement |
ParameterSE3Offset * | _offsetParam |
CacheSE3Offset * | _cache |
Protected Attributes inherited from g2o::BaseUnaryEdge< 6, Isometry3D, VertexSE3 > | |
JacobianXiOplusType | _jacobianOplusXi |
Protected Attributes inherited from g2o::BaseEdge< D, Isometry3D > | |
Measurement | _measurement |
InformationType | _information |
ErrorVector | _error |
Protected Attributes inherited from g2o::OptimizableGraph::Edge | |
int | _dimension |
int | _level |
RobustKernel * | _robustKernel |
long long | _internalId |
std::vector< int > | _cacheIds |
std::vector< std::string > | _parameterTypes |
std::vector< Parameter ** > | _parameters |
std::vector< int > | _parameterIds |
Protected Attributes inherited from g2o::HyperGraph::Edge | |
VertexContainer | _vertices |
int | _id |
unique id More... | |
Protected Attributes inherited from g2o::HyperGraph::DataContainer | |
Data * | _userData |
Additional Inherited Members | |
Public Types inherited from g2o::BaseUnaryEdge< 6, Isometry3D, VertexSE3 > | |
typedef BaseEdge< D, Isometry3D >::Measurement | Measurement |
typedef VertexSE3 | VertexXiType |
typedef Eigen::Matrix< double, D, VertexXiType::Dimension, D==1?Eigen::RowMajor:Eigen::ColMajor >::AlignedMapType | JacobianXiOplusType |
typedef BaseEdge< D, Isometry3D >::ErrorVector | ErrorVector |
typedef BaseEdge< D, Isometry3D >::InformationType | InformationType |
Public Types inherited from g2o::BaseEdge< D, Isometry3D > | |
typedef Isometry3D | Measurement |
typedef Eigen::Matrix< double, D, 1, Eigen::ColMajor > | ErrorVector |
typedef Eigen::Matrix< double, D, D, Eigen::ColMajor > | InformationType |
Static Public Attributes inherited from g2o::BaseUnaryEdge< 6, Isometry3D, VertexSE3 > | |
static const int | Dimension |
Static Public Attributes inherited from g2o::BaseEdge< D, Isometry3D > | |
static const int | Dimension |
prior for an SE3 element
Provides a prior for a 3d pose vertex. Again the measurement is represented by an Isometry3D matrix.
Definition at line 41 of file edge_se3_prior.h.
g2o::EdgeSE3Prior::EdgeSE3Prior | ( | ) |
Definition at line 35 of file edge_se3_prior.cpp.
References _cache, _offsetParam, g2o::BaseEdge< D, Isometry3D >::information(), g2o::OptimizableGraph::Edge::installParameter(), g2o::OptimizableGraph::Edge::resizeParameters(), and setMeasurement().
|
virtual |
Implements g2o::OptimizableGraph::Edge.
Definition at line 94 of file edge_se3_prior.cpp.
References _cache, g2o::BaseEdge< D, Isometry3D >::_error, _inverseMeasurement, g2o::CacheSE3Offset::n2w(), and g2o::internal::toVectorMQT().
Referenced by main().
|
inlinevirtual |
writes the measurement to an array of double
Reimplemented from g2o::OptimizableGraph::Edge.
Definition at line 66 of file edge_se3_prior.h.
References g2o::internal::toVectorQT().
|
virtual |
set the estimate of the to vertex, based on the estimate of the from vertices in the edge.
Reimplemented from g2o::BaseUnaryEdge< 6, Isometry3D, VertexSE3 >.
Definition at line 116 of file edge_se3_prior.cpp.
References g2o::BaseEdge< D, Isometry3D >::_information, _offsetParam, g2o::HyperGraph::Edge::_vertices, g2o::BaseVertex< D, T >::estimate(), g2o::internal::extractRotation(), g2o::BaseEdge< D, Isometry3D >::measurement(), g2o::ParameterSE3Offset::offset(), and g2o::BaseVertex< D, T >::setEstimate().
Referenced by main().
|
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 from g2o::OptimizableGraph::Edge.
Definition at line 76 of file edge_se3_prior.h.
|
virtual |
Linearizes the oplus operator in the vertex, and stores the result in temporary variables _jacobianOplusXi and _jacobianOplusXj
Reimplemented from g2o::BaseUnaryEdge< 6, Isometry3D, VertexSE3 >.
Definition at line 99 of file edge_se3_prior.cpp.
References _cache, g2o::BaseUnaryEdge< 6, Isometry3D, VertexSE3 >::_jacobianOplusXi, g2o::BaseEdge< D, Isometry3D >::_measurement, g2o::HyperGraph::Edge::_vertices, g2o::internal::computeEdgeSE3PriorGradient(), g2o::BaseVertex< D, T >::estimate(), g2o::ParameterSE3Offset::offset(), and g2o::CacheSE3Offset::offsetParam().
|
inlinevirtual |
returns the dimension of the measurement in the extended representation which is used by get/setMeasurement;
Reimplemented from g2o::OptimizableGraph::Edge.
Definition at line 72 of file edge_se3_prior.h.
|
virtual |
read the vertex from a stream, i.e., the internal state of the vertex
Implements g2o::OptimizableGraph::Edge.
Definition at line 55 of file edge_se3_prior.cpp.
References g2o::internal::fromVectorQT(), g2o::BaseEdge< D, Isometry3D >::information(), setMeasurement(), and g2o::OptimizableGraph::Edge::setParameterId().
|
protectedvirtual |
Reimplemented from g2o::OptimizableGraph::Edge.
Definition at line 45 of file edge_se3_prior.cpp.
References _cache, _offsetParam, g2o::HyperGraph::Edge::_vertices, and g2o::OptimizableGraph::Edge::resolveCache().
|
inlinevirtual |
Reimplemented from g2o::BaseEdge< D, Isometry3D >.
Definition at line 54 of file edge_se3_prior.h.
Referenced by g2o::SensorSE3Prior::addNoise(), EdgeSE3Prior(), main(), Robot::move(), read(), and setMeasurementFromState().
|
inlinevirtual |
sets the measurement from an array of double
Reimplemented from g2o::OptimizableGraph::Edge.
Definition at line 59 of file edge_se3_prior.h.
References g2o::internal::fromVectorQT().
|
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 110 of file edge_se3_prior.cpp.
References _cache, g2o::CacheSE3Offset::n2w(), and setMeasurement().
Referenced by g2o::SensorSE3Prior::sense(), and g2o::UnarySensor< Robot3D, EdgeSE3Prior >::sense().
|
virtual |
write the vertex to a stream
Implements g2o::OptimizableGraph::Edge.
Definition at line 82 of file edge_se3_prior.cpp.
References g2o::BaseEdge< D, Isometry3D >::_measurement, _offsetParam, g2o::Parameter::id(), g2o::BaseEdge< D, Isometry3D >::information(), and g2o::internal::toVectorQT().
|
protected |
Definition at line 86 of file edge_se3_prior.h.
Referenced by computeError(), EdgeSE3Prior(), linearizeOplus(), resolveCaches(), and setMeasurementFromState().
|
protected |
Definition at line 83 of file edge_se3_prior.h.
Referenced by computeError().
|
protected |
Definition at line 85 of file edge_se3_prior.h.
Referenced by EdgeSE3Prior(), initialEstimate(), resolveCaches(), and write().