g2o
Loading...
Searching...
No Matches
Public Member Functions | Public Attributes | List of all members
g2o::EdgeSE2TwoPointsXY Class Reference

#include <edge_se2_twopointsxy.h>

Inheritance diagram for g2o::EdgeSE2TwoPointsXY:
Inheritance graph
[legend]
Collaboration diagram for g2o::EdgeSE2TwoPointsXY:
Collaboration graph
[legend]

Public Member Functions

 EdgeSE2TwoPointsXY ()
virtual void computeError ()
virtual bool read (std::istream &is)
 read the vertex from a stream, i.e., the internal state of the vertex
virtual bool write (std::ostream &os) const
 write the vertex to a stream
virtual bool setMeasurementFromState ()
virtual void initialEstimate (const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
virtual double initialEstimatePossible (const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
Public Member Functions inherited from g2o::BaseVariableSizedEdge< 4, Vector4 >
 BaseVariableSizedEdge ()
virtual void linearizeOplus (JacobianWorkspace &jacobianWorkspace)
virtual void resize (size_t size)
virtual bool allVerticesFixed () const
virtual void constructQuadraticForm ()
virtual void mapHessianMemory (double *d, int i, int j, bool rowMajor)
Public Member Functions inherited from g2o::BaseEdge< D, Vector4 >
 BaseEdge ()
BaseEdge & operator= (const BaseEdge &)=delete
virtual ~BaseEdge ()
virtual double chi2 () const
virtual const double * errorData () const
 returns the error vector cached after calling the computeError;
const ErrorVector & error () const
EIGEN_STRONG_INLINE const InformationType & information () const
 information matrix of the constraint
void setInformation (const InformationType &information)
virtual const double * informationData () const
EIGEN_STRONG_INLINE const Measurement & measurement () const
 accessor functions for the measurement represented by the edge
virtual void setMeasurement (const Measurement &m)
virtual int rank () const
std::enable_if< Dim==-1, void >::type setDimension (int dim)
Public Member Functions inherited from g2o::OptimizableGraph::Edge
 Edge ()
virtual ~Edge ()
virtual bool setMeasurementData (const double *m)
virtual bool getMeasurementData (double *m) const
virtual int measurementDimension () const
RobustKernel * robustKernel () const
 if NOT NULL, error of this edge will be robustifed with the kernel
void setRobustKernel (RobustKernel *ptr)
virtual double * errorData ()=0
virtual double * informationData ()=0
int level () const
 returns the level of the edge
void setLevel (int l)
 sets the level of the edge
int dimension () const
 returns the dimensions of the error function
 G2O_ATTRIBUTE_DEPRECATED (virtual Vertex *createFrom())
 G2O_ATTRIBUTE_DEPRECATED (virtual Vertex *createTo())
virtual Vertex * createVertex (int)
long long internalId () const
 the internal ID of the edge
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
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 ()
Public Member Functions inherited from g2o::HyperGraph::DataContainer
 DataContainer ()
virtual ~DataContainer ()
const Data * userData () const
 the user data associated with this vertex
Data * userData ()
void setUserData (Data *obs)
void addUserData (Data *obs)

Public Attributes

 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Additional Inherited Members

Public Types inherited from g2o::BaseVariableSizedEdge< 4, Vector4 >
typedef BaseEdge< D, Vector4 >::Measurement Measurement
typedef MatrixX::MapType JacobianType
typedef BaseEdge< D, Vector4 >::ErrorVector ErrorVector
typedef BaseEdge< D, Vector4 >::InformationType InformationType
typedef Eigen::Map< MatrixX, MatrixX::Flags &Eigen::PacketAccessBit ? Eigen::Aligned :Eigen::Unaligned > HessianBlockType
Public Types inherited from g2o::BaseEdge< D, Vector4 >
typedef Vector4 Measurement
typedef internal::BaseEdgeTraits< D >::ErrorVector ErrorVector
typedef internal::BaseEdgeTraits< D >::InformationType InformationType
Static Public Attributes inherited from g2o::BaseVariableSizedEdge< 4, Vector4 >
static constexpr int Dimension
Static Public Attributes inherited from g2o::BaseEdge< D, Vector4 >
static constexpr int Dimension
Protected Member Functions inherited from g2o::BaseVariableSizedEdge< 4, Vector4 >
void computeQuadraticForm (const InformationType &omega, const ErrorVector &weightedError)
Protected Member Functions inherited from g2o::BaseEdge< D, Vector4 >
InformationType robustInformation (const Vector3 &rho) const
bool writeInformationMatrix (std::ostream &os) const
 write the upper trinagular part of the information matrix into the stream
bool readInformationMatrix (std::istream &is)
bool writeParamIds (std::ostream &os) const
 write the param IDs that are potentially used by the edge
bool readParamIds (std::istream &is)
 reads the param IDs from the stream
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 &parameters)
bool resolveParameters ()
virtual bool resolveCaches ()
Protected Attributes inherited from g2o::BaseVariableSizedEdge< 4, Vector4 >
std::vector< HessianHelper > _hessian
std::vector< JacobianType > _jacobianOplus
 jacobians of the edge (w.r.t. oplus)
Protected Attributes inherited from g2o::BaseEdge< D, Vector4 >
Measurement _measurement
 the measurement of the edge
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
Protected Attributes inherited from g2o::HyperGraph::DataContainer
Data * _userData

Detailed Description

Definition at line 38 of file edge_se2_twopointsxy.h.

Constructor & Destructor Documentation

◆ EdgeSE2TwoPointsXY()

g2o::EdgeSE2TwoPointsXY::EdgeSE2TwoPointsXY ( )

Definition at line 38 of file edge_se2_twopointsxy.cpp.

38 : BaseVariableSizedEdge<4, Vector4>() {
39 resize(3);
40}

References g2o::BaseVariableSizedEdge< 4, Vector4 >::BaseVariableSizedEdge(), and g2o::BaseVariableSizedEdge< 4, Vector4 >::resize().

Member Function Documentation

◆ computeError()

void g2o::EdgeSE2TwoPointsXY::computeError ( )
virtual

Implements g2o::OptimizableGraph::Edge.

Definition at line 42 of file edge_se2_twopointsxy.cpp.

42 {
43 VertexSE2* pose = static_cast<VertexSE2*>(_vertices[0]);
44 VertexPointXY* xy1 = static_cast<VertexPointXY*>(_vertices[1]);
45 VertexPointXY* xy2 = static_cast<VertexPointXY*>(_vertices[2]);
46
47 Vector2 m1 = pose->estimate().inverse() * xy1->estimate();
48 Vector2 m2 = pose->estimate().inverse() * xy2->estimate();
49
50 _error[0] = m1[0] - _measurement[0];
51 _error[1] = m1[1] - _measurement[1];
52 _error[2] = m2[0] - _measurement[2];
53 _error[3] = m2[1] - _measurement[3];
54}
g2o::HyperGraph::Edge::_vertices
VertexContainer _vertices
Definition hyper_graph.h:221
g2o::Vector2
VectorN< 2 > Vector2
Definition eigen_types.h:50

References g2o::BaseEdge< D, Vector4 >::_error, g2o::BaseEdge< D, Vector4 >::_measurement, g2o::HyperGraph::Edge::_vertices, g2o::BaseVertex< D, T >::estimate(), and g2o::SE2::inverse().

◆ initialEstimate()

void g2o::EdgeSE2TwoPointsXY::initialEstimate ( const OptimizableGraph::VertexSet & ,
OptimizableGraph::Vertex *  )
virtual

set the estimate of the to vertex, based on the estimate of the from vertices in the edge.

Reimplemented from g2o::BaseEdge< D, Vector4 >.

Definition at line 83 of file edge_se2_twopointsxy.cpp.

85 {
86 (void)toEstimate;
87
88 assert(initialEstimatePossible(fixed, toEstimate) &&
89 "Bad vertices specified");
90
91 VertexSE2* pose = static_cast<VertexSE2*>(_vertices[0]);
92 VertexPointXY* v1 = static_cast<VertexPointXY*>(_vertices[1]);
93 VertexPointXY* v2 = static_cast<VertexPointXY*>(_vertices[2]);
94
95 bool estimatev1 = true;
96 bool estimatev2 = true;
97
98 for (std::set<HyperGraph::Vertex*>::iterator it = fixed.begin();
99 it != fixed.end(); ++it) {
100 if (v1->id() == (*it)->id()) {
101 estimatev1 = false;
102 } else if (v2->id() == (*it)->id()) {
103 estimatev2 = false;
104 }
105 }
106
107 if (estimatev1) {
108 Vector2 submeas(_measurement[0], _measurement[1]);
109 v1->setEstimate(pose->estimate() * submeas);
110 }
111
112 if (estimatev2) {
113 Vector2 submeas(_measurement[2], _measurement[3]);
114 v2->setEstimate(pose->estimate() * submeas);
115 }
116}
g2o::EdgeSE2TwoPointsXY::initialEstimatePossible
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
Definition edge_se2_twopointsxy.cpp:118

References g2o::BaseEdge< D, Vector4 >::_measurement, g2o::HyperGraph::Edge::_vertices, g2o::BaseVertex< D, T >::estimate(), g2o::HyperGraph::Vertex::id(), initialEstimatePossible(), and g2o::BaseVertex< D, T >::setEstimate().

◆ initialEstimatePossible()

double g2o::EdgeSE2TwoPointsXY::initialEstimatePossible ( const OptimizableGraph::VertexSet & from,
OptimizableGraph::Vertex * to )
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 118 of file edge_se2_twopointsxy.cpp.

120 {
121 (void)toEstimate;
122
123 for (std::set<HyperGraph::Vertex*>::iterator it = fixed.begin();
124 it != fixed.end(); ++it) {
125 if (_vertices[0]->id() == (*it)->id()) {
126 return 1.0;
127 }
128 }
129 return -1.0;
130}

References g2o::HyperGraph::Edge::_vertices.

Referenced by initialEstimate().

◆ read()

bool g2o::EdgeSE2TwoPointsXY::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 56 of file edge_se2_twopointsxy.cpp.

56 {
57 is >> _measurement[0] >> _measurement[1] >> _measurement[2] >>
58 _measurement[3];
59 is >> information()(0, 0) >> information()(0, 1) >> information()(0, 2) >>
60 information()(0, 3) >> information()(1, 1) >> information()(1, 2) >>
61 information()(1, 3) >> information()(2, 2) >> information()(2, 3) >>
62 information()(3, 3);
63 information()(1, 0) = information()(0, 1);
64 information()(2, 0) = information()(0, 2);
65 information()(2, 1) = information()(1, 2);
66 information()(3, 0) = information()(0, 3);
67 information()(3, 1) = information()(1, 3);
68 information()(3, 2) = information()(2, 3);
69 return true;
70}
g2o::BaseEdge< D, Vector4 >::information
EIGEN_STRONG_INLINE const InformationType & information() const
Definition base_edge.h:107

References g2o::BaseEdge< D, Vector4 >::_measurement, and g2o::BaseEdge< D, Vector4 >::information().

◆ setMeasurementFromState()

bool g2o::EdgeSE2TwoPointsXY::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 132 of file edge_se2_twopointsxy.cpp.

132 {
133 VertexSE2* pose = static_cast<VertexSE2*>(_vertices[0]);
134 VertexPointXY* xy1 = static_cast<VertexPointXY*>(_vertices[1]);
135 VertexPointXY* xy2 = static_cast<VertexPointXY*>(_vertices[2]);
136
137 Vector2 m1 = pose->estimate().inverse() * xy1->estimate();
138 Vector2 m2 = pose->estimate().inverse() * xy2->estimate();
139
140 _measurement[0] = m1[0];
141 _measurement[1] = m1[1];
142 _measurement[2] = m2[0];
143 _measurement[3] = m2[1];
144 return true;
145}

References g2o::BaseEdge< D, Vector4 >::_measurement, g2o::HyperGraph::Edge::_vertices, g2o::BaseVertex< D, T >::estimate(), and g2o::SE2::inverse().

◆ write()

bool g2o::EdgeSE2TwoPointsXY::write ( std::ostream & os) const
virtual

write the vertex to a stream

Implements g2o::OptimizableGraph::Edge.

Definition at line 72 of file edge_se2_twopointsxy.cpp.

72 {
73 os << measurement()[0] << " " << measurement()[1] << " " << measurement()[2]
74 << " " << measurement()[3] << " ";
75 os << information()(0, 0) << " " << information()(0, 1) << " "
76 << information()(0, 2) << " " << information()(0, 3) << " "
77 << information()(1, 1) << " " << information()(1, 2) << " "
78 << information()(1, 3) << " " << information()(2, 2) << " "
79 << information()(2, 3) << " " << information()(3, 3);
80 return os.good();
81}
g2o::BaseEdge< D, Vector4 >::measurement
EIGEN_STRONG_INLINE const Measurement & measurement() const
Definition base_edge.h:119

References g2o::BaseEdge< D, Vector4 >::information(), and g2o::BaseEdge< D, Vector4 >::measurement().

Member Data Documentation

◆ EIGEN_MAKE_ALIGNED_OPERATOR_NEW

g2o::EdgeSE2TwoPointsXY::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 41 of file edge_se2_twopointsxy.h.

The documentation for this class was generated from the following files:
Generated on for g2o by doxygen 1.15.0