g2o
Public Member Functions | Public Attributes | List of all members
g2o::EdgeSE2OdomDifferentialCalib Class Reference

#include <edge_se2_odom_differential_calib.h>

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

Public Member Functions

 EdgeSE2OdomDifferentialCalib ()
 
void computeError ()
 
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...
 
- Public Member Functions inherited from g2o::BaseMultiEdge< 3, VelocityMeasurement >
 BaseMultiEdge ()
 
virtual void linearizeOplus (JacobianWorkspace &jacobianWorkspace)
 
virtual void linearizeOplus ()
 
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, VelocityMeasurement >
 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 ErrorVectorerror () const
 
ErrorVectorerror ()
 
EIGEN_STRONG_INLINE const InformationTypeinformation () const
 information matrix of the constraint More...
 
EIGEN_STRONG_INLINE InformationTypeinformation ()
 
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 Measurementmeasurement () const
 accessor functions for the measurement represented by the edge More...
 
virtual void setMeasurement (const Measurement &m)
 
virtual int rank () const
 
virtual void initialEstimate (const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
 
- Public Member Functions inherited from g2o::OptimizableGraph::Edge
 Edge ()
 
virtual ~Edge ()
 
virtual Edgeclone () const
 
virtual bool setMeasurementData (const double *m)
 
virtual bool getMeasurementData (double *m) const
 
virtual int measurementDimension () const
 
virtual bool setMeasurementFromState ()
 
RobustKernelrobustKernel () const
 if NOT NULL, error of this edge will be robustifed with the kernel More...
 
void setRobustKernel (RobustKernel *ptr)
 
virtual double initialEstimatePossible (const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to)
 
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())
 
virtual VertexcreateVertex (int)
 
long long internalId () const
 the internal ID of the edge More...
 
OptimizableGraphgraph ()
 
const OptimizableGraphgraph () const
 
bool setParameterId (int argNum, int paramId)
 
const Parameterparameter (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 VertexContainervertices () const
 
VertexContainervertices ()
 
const Vertexvertex (size_t i) const
 
Vertexvertex (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 ()
 
HyperGraphElementclone () const
 
- Public Member Functions inherited from g2o::HyperGraph::DataContainer
 DataContainer ()
 
virtual ~DataContainer ()
 
const DatauserData () const
 the user data associated with this vertex More...
 
DatauserData ()
 
void setUserData (Data *obs)
 
void addUserData (Data *obs)
 

Public Attributes

 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 

Additional Inherited Members

- Public Types inherited from g2o::BaseMultiEdge< 3, VelocityMeasurement >
typedef BaseEdge< D, VelocityMeasurement >::Measurement Measurement
 
typedef MatrixXD::MapType JacobianType
 
typedef BaseEdge< D, VelocityMeasurement >::ErrorVector ErrorVector
 
typedef BaseEdge< D, VelocityMeasurement >::InformationType InformationType
 
typedef Eigen::Map< MatrixXD, MatrixXD::Flags &Eigen::AlignedBit?Eigen::Aligned:Eigen::Unaligned > HessianBlockType
 
- Public Types inherited from g2o::BaseEdge< D, VelocityMeasurement >
typedef VelocityMeasurement 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::BaseMultiEdge< 3, VelocityMeasurement >
static const int Dimension
 
- Static Public Attributes inherited from g2o::BaseEdge< D, VelocityMeasurement >
static const int Dimension
 
- Protected Member Functions inherited from g2o::BaseMultiEdge< 3, VelocityMeasurement >
void computeQuadraticForm (const InformationType &omega, const ErrorVector &weightedError)
 
- Protected Member Functions inherited from g2o::BaseEdge< D, VelocityMeasurement >
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 &parameters)
 
bool resolveParameters ()
 
virtual bool resolveCaches ()
 
- Protected Attributes inherited from g2o::BaseMultiEdge< 3, VelocityMeasurement >
std::vector< HessianHelper > _hessian
 
std::vector< JacobianType, Eigen::aligned_allocator< JacobianType > > _jacobianOplus
 jacobians of the edge (w.r.t. oplus) More...
 
- Protected Attributes inherited from g2o::BaseEdge< D, VelocityMeasurement >
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
 

Detailed Description

Definition at line 39 of file edge_se2_odom_differential_calib.h.

Constructor & Destructor Documentation

g2o::EdgeSE2OdomDifferentialCalib::EdgeSE2OdomDifferentialCalib ( )

Definition at line 35 of file edge_se2_odom_differential_calib.cpp.

References g2o::BaseMultiEdge< 3, VelocityMeasurement >::resize().

35  :
36  BaseMultiEdge<3, VelocityMeasurement>()
37  {
38  resize(3);
39  }

Member Function Documentation

void g2o::EdgeSE2OdomDifferentialCalib::computeError ( )
inlinevirtual

Implements g2o::OptimizableGraph::Edge.

Definition at line 45 of file edge_se2_odom_differential_calib.h.

References g2o::OdomConvert::convertToMotion(), g2o::BaseVertex< D, T >::estimate(), g2o::SE2::fromVector(), G2O_TYPES_SCLAM2D_API, g2o::SE2::inverse(), and g2o::SE2::toVector().

46  {
47  const VertexSE2* v1 = dynamic_cast<const VertexSE2*>(_vertices[0]);
48  const VertexSE2* v2 = dynamic_cast<const VertexSE2*>(_vertices[1]);
49  const VertexOdomDifferentialParams* params = dynamic_cast<const VertexOdomDifferentialParams*>(_vertices[2]);
50  const SE2& x1 = v1->estimate();
51  const SE2& x2 = v2->estimate();
52 
53  // get the calibrated motion given by the odometry
54  VelocityMeasurement calibratedVelocityMeasurment(measurement().vl() * params->estimate()(0),
55  measurement().vr() * params->estimate()(1),
56  measurement().dt());
57  MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, params->estimate()(2));
58  SE2 Ku_ij;
59  Ku_ij.fromVector(mm.measurement());
60 
61  SE2 delta = Ku_ij.inverse() * x1.inverse() * x2 ;
62  _error = delta.toVector();
63  }
static MotionMeasurement convertToMotion(const VelocityMeasurement &vi, double l=1.0)
EIGEN_STRONG_INLINE const Measurement & measurement() const
accessor functions for the measurement represented by the edge
Definition: base_edge.h:75
VertexContainer _vertices
Definition: hyper_graph.h:202
bool g2o::EdgeSE2OdomDifferentialCalib::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 41 of file edge_se2_odom_differential_calib.cpp.

References g2o::BaseEdge< D, VelocityMeasurement >::information(), and g2o::BaseEdge< D, VelocityMeasurement >::setMeasurement().

42  {
43  double vl, vr, dt;
44  is >> vl >> vr >> dt;
45  VelocityMeasurement vm(vl, vr, dt);
46  setMeasurement(vm);
47  for (int i = 0; i < information().rows(); ++i)
48  for (int j = i; j < information().cols(); ++j) {
49  is >> information()(i, j);
50  if (i != j)
51  information()(j, i) = information()(i, j);
52  }
53  return true;
54  }
virtual void setMeasurement(const Measurement &m)
Definition: base_edge.h:76
EIGEN_STRONG_INLINE const InformationType & information() const
information matrix of the constraint
Definition: base_edge.h:67
bool g2o::EdgeSE2OdomDifferentialCalib::write ( std::ostream &  os) const
virtual

write the vertex to a stream

Implements g2o::OptimizableGraph::Edge.

Definition at line 56 of file edge_se2_odom_differential_calib.cpp.

References g2o::VelocityMeasurement::dt(), g2o::BaseVertex< D, T >::estimate(), g2o::BaseEdge< D, VelocityMeasurement >::information(), g2o::BaseEdge< D, VelocityMeasurement >::measurement(), g2o::SE2::translation(), g2o::HyperGraph::Edge::vertex(), g2o::VelocityMeasurement::vl(), and g2o::VelocityMeasurement::vr().

57  {
58  os << measurement().vl() << " " << measurement().vr() << " " << measurement().dt();
59  for (int i = 0; i < information().rows(); ++i)
60  for (int j = i; j < information().cols(); ++j)
61  os << " " << information()(i, j);
62  return os.good();
63  }
EIGEN_STRONG_INLINE const InformationType & information() const
information matrix of the constraint
Definition: base_edge.h:67
EIGEN_STRONG_INLINE const Measurement & measurement() const
accessor functions for the measurement represented by the edge
Definition: base_edge.h:75

Member Data Documentation

g2o::EdgeSE2OdomDifferentialCalib::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 42 of file edge_se2_odom_differential_calib.h.


The documentation for this class was generated from the following files: