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

prior for an SE3 element More...

#include <edge_se3_prior.h>

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

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::VertexcreateVertex (int i)
 
const JacobianXiOplusTypejacobianOplusXi () 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 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 int rank () const
 
- Public Member Functions inherited from g2o::OptimizableGraph::Edge
 Edge ()
 
virtual ~Edge ()
 
virtual Edgeclone () const
 
RobustKernelrobustKernel () 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...
 
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)
 

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 &parameters)
 
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
 

Detailed Description

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.

Constructor & Destructor Documentation

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().

35  : BaseUnaryEdge<6, Isometry3D, VertexSE3>() {
36  setMeasurement(Isometry3D::Identity());
37  information().setIdentity();
38  _cache = 0;
39  _offsetParam = 0;
42  }
bool installParameter(ParameterType *&p, size_t argNo, int paramId=-1)
ParameterSE3Offset * _offsetParam
void resizeParameters(size_t newSize)
EIGEN_STRONG_INLINE const InformationType & information() const
information matrix of the constraint
Definition: base_edge.h:67
virtual void setMeasurement(const Isometry3D &m)
CacheSE3Offset * _cache

Member Function Documentation

void g2o::EdgeSE3Prior::computeError ( )
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().

94  {
97  }
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3D
Definition: eigen_types.h:66
Vector6d toVectorMQT(const Isometry3D &t)
const Isometry3D & n2w() const
Isometry3D _inverseMeasurement
CacheSE3Offset * _cache
virtual bool g2o::EdgeSE3Prior::getMeasurementData ( double *  m) const
inlinevirtual

writes the measurement to an array of double

Returns
true on success

Reimplemented from g2o::OptimizableGraph::Edge.

Definition at line 66 of file edge_se3_prior.h.

References g2o::internal::toVectorQT().

66  {
67  Eigen::Map<Vector7d> v(d);
69  return true;
70  }
Vector7d toVectorQT(const Isometry3D &t)
void g2o::EdgeSE3Prior::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::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().

116  {
117  VertexSE3 *v = static_cast<VertexSE3*>(_vertices[0]);
118  assert(v && "Vertex for the Prior edge is not set");
119 
120  Isometry3D newEstimate = _offsetParam->offset().inverse() * measurement();
121  if (_information.block<3,3>(0,0).array().abs().sum() == 0){ // do not set translation, as that part of the information is all zero
122  newEstimate.translation()=v->estimate().translation();
123  }
124  if (_information.block<3,3>(3,3).array().abs().sum() == 0){ // do not set rotation, as that part of the information is all zero
125  newEstimate.matrix().block<3,3>(0,0) = internal::extractRotation(v->estimate());
126  }
127  v->setEstimate(newEstimate);
128  }
const Isometry3D & offset() const
rotation of the offset as 3x3 rotation matrix
ParameterSE3Offset * _offsetParam
InformationType _information
Definition: base_edge.h:88
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3D
Definition: eigen_types.h:66
Isometry3D::ConstLinearPart extractRotation(const Isometry3D &A)
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
virtual double g2o::EdgeSE3Prior::initialEstimatePossible ( const OptimizableGraph::VertexSet from,
OptimizableGraph::Vertex to 
)
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.

77  {
78  return 1.;
79  }
void g2o::EdgeSE3Prior::linearizeOplus ( )
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().

99  {
100  VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
101  Isometry3D E;
102  Isometry3D Z, X, P;
103  X=from->estimate();
104  P=_cache->offsetParam()->offset();
105  Z=_measurement;
107  }
const ParameterSE3Offset * offsetParam() const
const Isometry3D & offset() const
rotation of the offset as 3x3 rotation matrix
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3D
Definition: eigen_types.h:66
void computeEdgeSE3PriorGradient(Isometry3D &E, const Eigen::MatrixBase< Derived > &JConstRef, const Isometry3D &Z, const Isometry3D &X, const Isometry3D &P=Isometry3D())
CacheSE3Offset * _cache
VertexContainer _vertices
Definition: hyper_graph.h:202
virtual int g2o::EdgeSE3Prior::measurementDimension ( ) const
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.

72 {return 7;}
bool g2o::EdgeSE3Prior::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 55 of file edge_se3_prior.cpp.

References g2o::internal::fromVectorQT(), g2o::BaseEdge< D, Isometry3D >::information(), setMeasurement(), and g2o::OptimizableGraph::Edge::setParameterId().

55  {
56  int pid;
57  is >> pid;
58  if (!setParameterId(0, pid))
59  return false;
60  // measured keypoint
61  Vector7d meas;
62  for (int i=0; i<7; i++) is >> meas[i];
64  // don't need this if we don't use it in error calculation (???)
65  // information matrix is the identity for features, could be changed to allow arbitrary covariances
66  if (is.bad()) {
67  return false;
68  }
69  for ( int i=0; i<information().rows() && is.good(); i++)
70  for (int j=i; j<information().cols() && is.good(); j++){
71  is >> information()(i,j);
72  if (i!=j)
73  information()(j,i)=information()(i,j);
74  }
75  if (is.bad()) {
76  // we overwrite the information matrix
77  information().setIdentity();
78  }
79  return true;
80  }
Isometry3D fromVectorQT(const Vector7d &v)
Eigen::Matrix< double, 7, 1 > Vector7d
Definition: sim3.h:35
bool setParameterId(int argNum, int paramId)
EIGEN_STRONG_INLINE const InformationType & information() const
information matrix of the constraint
Definition: base_edge.h:67
virtual void setMeasurement(const Isometry3D &m)
bool g2o::EdgeSE3Prior::resolveCaches ( )
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().

45  {
46  assert(_offsetParam);
47  ParameterVector pv(1);
48  pv[0]=_offsetParam;
49  resolveCache(_cache, (OptimizableGraph::Vertex*)_vertices[0],"CACHE_SE3_OFFSET",pv);
50  return _cache != 0;
51  }
ParameterSE3Offset * _offsetParam
class G2O_CORE_API Vertex
void resolveCache(CacheType *&cache, OptimizableGraph::Vertex *, const std::string &_type, const ParameterVector &parameters)
Definition: cache.h:122
std::vector< Parameter * > ParameterVector
Definition: parameter.h:52
CacheSE3Offset * _cache
VertexContainer _vertices
Definition: hyper_graph.h:202
virtual void g2o::EdgeSE3Prior::setMeasurement ( const Isometry3D m)
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().

54  {
55  _measurement = m;
56  _inverseMeasurement = m.inverse();
57  }
Isometry3D _inverseMeasurement
virtual bool g2o::EdgeSE3Prior::setMeasurementData ( const double *  m)
inlinevirtual

sets the measurement from an array of double

Returns
true on success

Reimplemented from g2o::OptimizableGraph::Edge.

Definition at line 59 of file edge_se3_prior.h.

References g2o::internal::fromVectorQT().

59  {
60  Eigen::Map<const Vector7d> v(d);
63  return true;
64  }
Isometry3D fromVectorQT(const Vector7d &v)
Isometry3D _inverseMeasurement
bool g2o::EdgeSE3Prior::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 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().

110  {
112  return true;
113  }
const Isometry3D & n2w() const
virtual void setMeasurement(const Isometry3D &m)
CacheSE3Offset * _cache
bool g2o::EdgeSE3Prior::write ( std::ostream &  os) const
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().

82  {
83  os << _offsetParam->id() << " ";
85  for (int i=0; i<7; i++) os << meas[i] << " ";
86  for (int i=0; i<information().rows(); i++)
87  for (int j=i; j<information().cols(); j++) {
88  os << information()(i,j) << " ";
89  }
90  return os.good();
91  }
ParameterSE3Offset * _offsetParam
Eigen::Matrix< double, 7, 1 > Vector7d
Definition: sim3.h:35
int id() const
Definition: parameter.h:45
EIGEN_STRONG_INLINE const InformationType & information() const
information matrix of the constraint
Definition: base_edge.h:67
Vector7d toVectorQT(const Isometry3D &t)

Member Data Documentation

CacheSE3Offset* g2o::EdgeSE3Prior::_cache
protected
Isometry3D g2o::EdgeSE3Prior::_inverseMeasurement
protected

Definition at line 83 of file edge_se3_prior.h.

Referenced by computeError().

ParameterSE3Offset* g2o::EdgeSE3Prior::_offsetParam
protected

Definition at line 85 of file edge_se3_prior.h.

Referenced by EdgeSE3Prior(), initialEstimate(), resolveCaches(), and write().


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