g2o
Loading...
Searching...
No Matches
Public Member Functions | Public Attributes | List of all members
EdgeCalib Class Reference
Inheritance diagram for EdgeCalib:
Inheritance graph
[legend]
Collaboration diagram for EdgeCalib:
Collaboration graph
[legend]

Public Member Functions

 EdgeCalib ()
 
void computeError ()
 
virtual bool read (std::istream &)
 read the vertex from a stream, i.e., the internal state of the vertex
 
virtual bool write (std::ostream &) const
 write the vertex to a stream
 
- Public Member Functions inherited from g2o::BaseBinaryEdge< 3, OdomAndLaserMotion, VertexSE2, VertexBaseline >
 BaseBinaryEdge ()
 
- Public Member Functions inherited from g2o::BaseFixedSizedEdge< D, E, VertexTypes >
template<int VertexN>
constexpr std::enable_if< VertexXnType< VertexN >::Dimension!=-1, int >::type vertexDimension () const
 
template<int VertexN>
std::enable_if< VertexXnType< VertexN >::Dimension==-1, int >::type vertexDimension () const
 
template<int VertexN>
const VertexXnType< VertexN > * vertexXn () const
 
template<int VertexN>
VertexXnType< VertexN > * vertexXn ()
 
 BaseFixedSizedEdge ()
 
template<typename... CtorArgs>
OptimizableGraph::Vertex * createVertex (int i, CtorArgs... args)
 create an instance of the Nth VertexType
 
virtual void resize (size_t size)
 
template<std::size_t... Ints>
bool allVerticesFixedNs (std::index_sequence< Ints... >) const
 
virtual bool allVerticesFixed () const
 
virtual void linearizeOplus (JacobianWorkspace &jacobianWorkspace)
 
template<std::size_t... Ints>
void linearizeOplus_allocate (JacobianWorkspace &jacobianWorkspace, std::index_sequence< Ints... >)
 
virtual void linearizeOplus ()
 
template<std::size_t... Ints>
void linearizeOplusNs (std::index_sequence< Ints... >)
 
template<int N>
void linearizeOplusN ()
 
template<int N>
const std::tuple_element< N, std::tuple< JacobianType< D, VertexTypes::Dimension >... > >::type & jacobianOplusXn () const
 
template<int N>
std::tuple_element< N, std::tuple< JacobianType< D, VertexTypes::Dimension >... > >::type & jacobianOplusXn ()
 
virtual void constructQuadraticForm ()
 
template<std::size_t... Ints>
void constructQuadraticFormNs (const InformationType &omega, const ErrorVector &weightedError, std::index_sequence< Ints... >)
 
template<int N>
void constructQuadraticFormN (const InformationType &omega, const ErrorVector &weightedError)
 
template<int N, typename AtOType >
void constructOffDiagonalQuadraticFormMs (const AtOType &, std::index_sequence<>)
 
template<int N, std::size_t... Ints, typename AtOType >
void constructOffDiagonalQuadraticFormMs (const AtOType &AtO, std::index_sequence< Ints... >)
 
template<int N, int M, typename AtOType >
void constructOffDiagonalQuadraticFormM (const AtOType &AtO)
 
virtual void mapHessianMemory (double *d, int i, int j, bool rowMajor)
 
template<std::size_t... Ints>
bool allVerticesFixedNs (std::index_sequence< Ints... >) const
 
template<std::size_t... Ints>
void constructQuadraticFormNs (const InformationType &omega, const ErrorVector &weightedError, std::index_sequence< Ints... >)
 
template<int N, typename AtOType >
void constructOffDiagonalQuadraticFormMs (const AtOType &, std::index_sequence<>)
 
template<int N, std::size_t... Ints, typename AtOType >
void constructOffDiagonalQuadraticFormMs (const AtOType &AtO, std::index_sequence< Ints... >)
 
template<int N, int M, typename AtOType >
void constructOffDiagonalQuadraticFormM (const AtOType &AtO)
 
template<int N>
void constructQuadraticFormN (const InformationType &omega, const ErrorVector &weightedError)
 
template<std::size_t... Ints>
void linearizeOplus_allocate (JacobianWorkspace &jacobianWorkspace, std::index_sequence< Ints... >)
 
template<int N>
void linearizeOplusN ()
 
template<std::size_t... Ints>
void linearizeOplusNs (std::index_sequence< Ints... >)
 
- Public Member Functions inherited from g2o::BaseEdge< D, E >
 BaseEdge ()
 
BaseEdge & operator= (const BaseEdge &)=delete
 
 BaseEdge (const BaseEdge &)=delete
 
virtual ~BaseEdge ()
 
virtual double chi2 () const
 
virtual const double * errorData () const
 returns the error vector cached after calling the computeError;
 
virtual double * errorData ()
 
const ErrorVector & error () const
 
ErrorVector & error ()
 
EIGEN_STRONG_INLINE const InformationType & information () const
 information matrix of the constraint
 
EIGEN_STRONG_INLINE InformationType & information ()
 
void setInformation (const InformationType &information)
 
virtual const double * informationData () const
 
virtual double * informationData ()
 
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
 
virtual void initialEstimate (const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
 
template<int Dim = D>
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
 
virtual bool setMeasurementFromState ()
 
RobustKernel * robustKernel () const
 if NOT NULL, error of this edge will be robustifed with the kernel
 
void setRobustKernel (RobustKernel *ptr)
 
virtual double initialEstimatePossible (const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to)
 
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::BaseBinaryEdge< 3, OdomAndLaserMotion, VertexSE2, VertexBaseline >
using VertexXiType = VertexSE2
 
using VertexXjType = VertexBaseline
 
- Public Types inherited from g2o::BaseFixedSizedEdge< D, E, VertexTypes >
template<int N, typename... Types>
using NthType = typename std::tuple_element< N, std::tuple< Types... > >::type
 
template<int VertexN>
using VertexXnType = NthType< VertexN, VertexTypes... >
 The type of the N-th vertex.
 
typedef BaseEdge< D, E >::Measurement Measurement
 
typedef BaseEdge< D, E >::ErrorVector ErrorVector
 
typedef BaseEdge< D, E >::InformationType InformationType
 
template<int EdgeDimension, int VertexDimension>
using JacobianType = typename Eigen::Matrix< double, EdgeDimension, VertexDimension, EdgeDimension==1 ? Eigen::RowMajor :Eigen::ColMajor >::AlignedMapType
 
template<int DN, int DM>
using HessianBlockType = Eigen::Map< Eigen::Matrix< double, DN, DM, DN==1 ? Eigen::RowMajor :Eigen::ColMajor >, Eigen::Matrix< double, DN, DM, DN==1 ? Eigen::RowMajor :Eigen::ColMajor >::Flags &Eigen::PacketAccessBit ? Eigen::Aligned :Eigen::Unaligned >
 it requires quite some ugly code to get the type of hessians...
 
template<int K>
using HessianBlockTypeK = HessianBlockType< VertexXnType< internal::index_to_pair(K).first >::Dimension, VertexXnType< internal::index_to_pair(K).second >::Dimension >
 
template<int K>
using HessianBlockTypeKTransposed = HessianBlockType< VertexXnType< internal::index_to_pair(K).second >::Dimension, VertexXnType< internal::index_to_pair(K).first >::Dimension >
 
using HessianTuple = typename HessianTupleType< std::make_index_sequence< _nr_of_vertex_pairs > >::type
 
using HessianTupleTransposed = typename HessianTupleType< std::make_index_sequence< _nr_of_vertex_pairs > >::typeTransposed
 
using HessianRowMajorStorage = std::array< bool, _nr_of_vertex_pairs >
 
- Public Types inherited from g2o::BaseEdge< D, E >
typedef E Measurement
 
typedef internal::BaseEdgeTraits< D >::ErrorVector ErrorVector
 
typedef internal::BaseEdgeTraits< D >::InformationType InformationType
 
- Static Public Member Functions inherited from g2o::BaseFixedSizedEdge< D, E, VertexTypes >
template<int VertexN>
static constexpr int VertexDimension ()
 Size of the N-th vertex at compile time.
 
- Static Public Attributes inherited from g2o::BaseFixedSizedEdge< D, E, VertexTypes >
static const int Dimension = BaseEdge<D, E>::Dimension
 
static const std::size_t _nr_of_vertices = sizeof...(VertexTypes)
 
static const std::size_t _nr_of_vertex_pairs
 
- Static Public Attributes inherited from g2o::BaseEdge< D, E >
static constexpr int Dimension = internal::BaseEdgeTraits<D>::Dimension
 
- Protected Member Functions inherited from g2o::BaseEdge< D, E >
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::BaseBinaryEdge< 3, OdomAndLaserMotion, VertexSE2, VertexBaseline >
BaseFixedSizedEdge< D, OdomAndLaserMotion, VertexSE2, VertexBaseline >::template JacobianType< D, VertexXi::Dimension > & _jacobianOplusXi
 
BaseFixedSizedEdge< D, OdomAndLaserMotion, VertexSE2, VertexBaseline >::template JacobianType< D, VertexXj::Dimension > & _jacobianOplusXj
 
- Protected Attributes inherited from g2o::BaseFixedSizedEdge< D, E, VertexTypes >
HessianRowMajorStorage _hessianRowMajor
 
HessianTuple _hessianTuple
 
HessianTupleTransposed _hessianTupleTransposed
 
std::tuple< JacobianType< D, VertexTypes::Dimension >... > _jacobianOplus
 
- Protected Attributes inherited from g2o::BaseEdge< D, E >
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 67 of file sclam_pure_calibration.cpp.

Constructor & Destructor Documentation

◆ EdgeCalib()

EdgeCalib::EdgeCalib ( )
inline

Definition at line 71 of file sclam_pure_calibration.cpp.

71{}

Member Function Documentation

◆ computeError()

void EdgeCalib::computeError ( )
inlinevirtual

Implements g2o::OptimizableGraph::Edge.

Definition at line 73 of file sclam_pure_calibration.cpp.

73 {
74 const VertexSE2* laserOffset = static_cast<const VertexSE2*>(_vertices[0]);
75 const VertexBaseline* odomParams =
76 dynamic_cast<const VertexBaseline*>(_vertices[1]);
77
78 // get the calibrated motion given by the odometry
79 double rl = -odomParams->estimate() * linearSolution(0);
80 double rr = odomParams->estimate() * linearSolution(1);
81 VelocityMeasurement calibratedVelocityMeasurement(
82 measurement().velocityMeasurement.vl() * rl,
83 measurement().velocityMeasurement.vr() * rr,
84 measurement().velocityMeasurement.dt());
85 MotionMeasurement mm = OdomConvert::convertToMotion(
86 calibratedVelocityMeasurement, odomParams->estimate());
87 SE2 Ku_ij;
88 Ku_ij.fromVector(mm.measurement());
89
90 SE2 laserMotionInRobotFrame = laserOffset->estimate() *
91 measurement().laserMotion *
92 laserOffset->estimate().inverse();
93 SE2 delta = Ku_ij.inverse() * laserMotionInRobotFrame;
94 _error = delta.toVector();
95 }
g2o::BaseEdge::measurement
EIGEN_STRONG_INLINE const Measurement & measurement() const
accessor functions for the measurement represented by the edge
Definition base_edge.h:119
g2o::BaseEdge::_error
ErrorVector _error
Definition base_edge.h:149
g2o::BaseVertex::estimate
const EstimateType & estimate() const
return the current estimate of the vertex
Definition base_vertex.h:127
g2o::HyperGraph::Edge::_vertices
VertexContainer _vertices
Definition hyper_graph.h:221
g2o::MotionMeasurement
A 2D odometry measurement expressed as a transformation.
Definition odometry_measurement.h:65
g2o::MotionMeasurement::measurement
const Vector3 & measurement() const
Definition odometry_measurement.h:84
g2o::OdomConvert::convertToMotion
static MotionMeasurement convertToMotion(const VelocityMeasurement &vi, double l=1.0)
Definition odometry_measurement.cpp:69
g2o::SE2
represent SE2
Definition se2.h:43
g2o::SE2::toVector
Vector3 toVector() const
convert to a 3D vector (x, y, theta)
Definition se2.h:105
g2o::SE2::inverse
SE2 inverse() const
invert :-)
Definition se2.h:83
g2o::SE2::fromVector
void fromVector(const Vector3 &v)
assign from a 3D vector (x, y, theta)
Definition se2.h:102
g2o::VelocityMeasurement
velocity measurement of a differential robot
Definition odometry_measurement.h:40
g2o::VertexSE2
2D pose Vertex, (x,y,theta)
Definition vertex_se2.h:41
linearSolution
static Eigen::Vector2d linearSolution
Definition sclam_pure_calibration.cpp:55

References g2o::BaseEdge< D, E >::_error, g2o::HyperGraph::Edge::_vertices, g2o::OdomConvert::convertToMotion(), g2o::BaseVertex< D, T >::estimate(), g2o::SE2::fromVector(), g2o::SE2::inverse(), linearSolution, g2o::BaseEdge< D, E >::measurement(), g2o::MotionMeasurement::measurement(), and g2o::SE2::toVector().

◆ read()

virtual bool EdgeCalib::read ( std::istream &  is)
inlinevirtual

read the vertex from a stream, i.e., the internal state of the vertex

Implements g2o::OptimizableGraph::Edge.

Definition at line 97 of file sclam_pure_calibration.cpp.

97{ return false; }

◆ write()

virtual bool EdgeCalib::write ( std::ostream &  os) const
inlinevirtual

write the vertex to a stream

Implements g2o::OptimizableGraph::Edge.

Definition at line 98 of file sclam_pure_calibration.cpp.

98{ return false; }

Member Data Documentation

◆ EIGEN_MAKE_ALIGNED_OPERATOR_NEW

EdgeCalib::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 70 of file sclam_pure_calibration.cpp.

The documentation for this class was generated from the following file:
Generated on Tue Nov 11 2025 05:18:45 for g2o by doxygen 1.9.8