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

3D edge between two SBAcam More...

#include <types_sba.h>

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

Public Member Functions

 EdgeSBACam ()
 
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 setMeasurement (const SE3Quat &meas)
 
virtual double initialEstimatePossible (const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
 
virtual void initialEstimate (const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to)
 
virtual bool setMeasurementData (const double *d)
 
virtual bool getMeasurementData (double *d) const
 
virtual int measurementDimension () const
 
virtual bool setMeasurementFromState ()
 
- Public Member Functions inherited from g2o::BaseBinaryEdge< 6, SE3Quat, VertexCam, VertexCam >
 BaseBinaryEdge ()
 
virtual OptimizableGraph::VertexcreateFrom ()
 
virtual OptimizableGraph::VertexcreateTo ()
 
virtual OptimizableGraph::VertexcreateVertex (int i)
 
virtual void resize (size_t size)
 
virtual bool allVerticesFixed () const
 
virtual void linearizeOplus (JacobianWorkspace &jacobianWorkspace)
 
virtual void linearizeOplus ()
 
const JacobianXiOplusTypejacobianOplusXi () const
 returns the result of the linearization in the manifold space for the node xi More...
 
const JacobianXjOplusTypejacobianOplusXj () const
 returns the result of the linearization in the manifold space for the node xj More...
 
virtual void constructQuadraticForm ()
 
virtual void mapHessianMemory (double *d, int i, int j, bool rowMajor)
 
- Public Member Functions inherited from g2o::BaseEdge< D, SE3Quat >
 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)
 

Public Attributes

 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 

Protected Attributes

SE3Quat _inverseMeasurement
 
- Protected Attributes inherited from g2o::BaseBinaryEdge< 6, SE3Quat, VertexCam, VertexCam >
bool _hessianRowMajor
 
HessianBlockType _hessian
 
HessianBlockTransposedType _hessianTransposed
 
JacobianXiOplusType _jacobianOplusXi
 
JacobianXjOplusType _jacobianOplusXj
 
- Protected Attributes inherited from g2o::BaseEdge< D, SE3Quat >
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::BaseBinaryEdge< 6, SE3Quat, VertexCam, VertexCam >
typedef VertexCam VertexXiType
 
typedef VertexCam VertexXjType
 
typedef BaseEdge< D, SE3Quat >::Measurement Measurement
 
typedef Eigen::Matrix< double, D, Di, D==1?Eigen::RowMajor:Eigen::ColMajor >::AlignedMapType JacobianXiOplusType
 
typedef Eigen::Matrix< double, D, Dj, D==1?Eigen::RowMajor:Eigen::ColMajor >::AlignedMapType JacobianXjOplusType
 
typedef BaseEdge< D, SE3Quat >::ErrorVector ErrorVector
 
typedef BaseEdge< D, SE3Quat >::InformationType InformationType
 
typedef Eigen::Map< Eigen::Matrix< double, Di, Dj, Di==1?Eigen::RowMajor:Eigen::ColMajor >, Eigen::Matrix< double, Di, Dj, Di==1?Eigen::RowMajor:Eigen::ColMajor >::Flags &Eigen::AlignedBit?Eigen::Aligned:Eigen::Unaligned > HessianBlockType
 
typedef Eigen::Map< Eigen::Matrix< double, Dj, Di, Dj==1?Eigen::RowMajor:Eigen::ColMajor >, Eigen::Matrix< double, Dj, Di, Dj==1?Eigen::RowMajor:Eigen::ColMajor >::Flags &Eigen::AlignedBit?Eigen::Aligned:Eigen::Unaligned > HessianBlockTransposedType
 
- Public Types inherited from g2o::BaseEdge< D, SE3Quat >
typedef SE3Quat 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::BaseBinaryEdge< 6, SE3Quat, VertexCam, VertexCam >
static const int Di
 
static const int Dj
 
static const int Dimension
 
- Static Public Attributes inherited from g2o::BaseEdge< D, SE3Quat >
static const int Dimension
 
- Protected Member Functions inherited from g2o::BaseEdge< D, SE3Quat >
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 ()
 

Detailed Description

3D edge between two SBAcam

Definition at line 285 of file types_sba.h.

Constructor & Destructor Documentation

g2o::EdgeSBACam::EdgeSBACam ( )

Definition at line 134 of file types_sba.cpp.

134  :
135  BaseBinaryEdge<6, SE3Quat, VertexCam, VertexCam>()
136  {
137  }

Member Function Documentation

void g2o::EdgeSBACam::computeError ( )
inlinevirtual

Implements g2o::OptimizableGraph::Edge.

Definition at line 292 of file types_sba.h.

References g2o::BaseVertex< D, T >::estimate(), g2o::SE3Quat::inverse(), g2o::SE3Quat::rotation(), and g2o::SE3Quat::translation().

293  {
294  const VertexCam* v1 = dynamic_cast<const VertexCam*>(_vertices[0]);
295  const VertexCam* v2 = dynamic_cast<const VertexCam*>(_vertices[1]);
296  SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
297  _error[0]=delta.translation().x();
298  _error[1]=delta.translation().y();
299  _error[2]=delta.translation().z();
300  _error[3]=delta.rotation().x();
301  _error[4]=delta.rotation().y();
302  _error[5]=delta.rotation().z();
303  }
SE3Quat _inverseMeasurement
Definition: types_sba.h:331
VertexContainer _vertices
Definition: hyper_graph.h:202
virtual bool g2o::EdgeSBACam::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 320 of file types_sba.h.

320  {
321  Eigen::Map<Vector7d> v(d);
322  v = _measurement.toVector();
323  return true;
324  }
Vector7d toVector() const
Definition: se3quat.h:135
void g2o::EdgeSBACam::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, SE3Quat >.

Definition at line 166 of file types_sba.cpp.

References _inverseMeasurement, g2o::BaseEdge< D, SE3Quat >::_measurement, g2o::HyperGraph::Edge::_vertices, g2o::BaseVertex< D, T >::estimate(), and g2o::VertexCam::setEstimate().

167  {
168  VertexCam* from = static_cast<VertexCam*>(_vertices[0]);
169  VertexCam* to = static_cast<VertexCam*>(_vertices[1]);
170  if (from_.count(from) > 0)
171  to->setEstimate((SE3Quat) from->estimate() * _measurement);
172  else
173  from->setEstimate((SE3Quat) to->estimate() * _inverseMeasurement);
174  }
SE3Quat _inverseMeasurement
Definition: types_sba.h:331
VertexContainer _vertices
Definition: hyper_graph.h:202
virtual double g2o::EdgeSBACam::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 310 of file types_sba.h.

310 { return 1.;}
virtual int g2o::EdgeSBACam::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 326 of file types_sba.h.

326 {return 7;}
bool g2o::EdgeSBACam::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 139 of file types_sba.cpp.

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

140  {
141  Vector7d meas;
142  for (int i=0; i<7; i++)
143  is >> meas[i];
144  setMeasurement(SE3Quat(meas));
145 
146  for (int i=0; i<6; i++)
147  for (int j=i; j<6; j++) {
148  is >> information()(i,j);
149  if (i!=j)
150  information()(j,i)=information()(i,j);
151  }
152  return true;
153  }
Eigen::Matrix< double, 7, 1 > Vector7d
Definition: sim3.h:35
EIGEN_STRONG_INLINE const InformationType & information() const
information matrix of the constraint
Definition: base_edge.h:67
virtual void setMeasurement(const SE3Quat &meas)
Definition: types_sba.h:305
virtual void g2o::EdgeSBACam::setMeasurement ( const SE3Quat meas)
inlinevirtual

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

Definition at line 305 of file types_sba.h.

References g2o::SE3Quat::inverse().

Referenced by read().

305  {
306  _measurement=meas;
308  }
SE3Quat inverse() const
Definition: se3quat.h:120
SE3Quat _inverseMeasurement
Definition: types_sba.h:331
virtual bool g2o::EdgeSBACam::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 313 of file types_sba.h.

313  {
314  Eigen::Map<const Vector7d> v(d);
317  return true;
318  }
SE3Quat inverse() const
Definition: se3quat.h:120
SE3Quat _inverseMeasurement
Definition: types_sba.h:331
void fromVector(const Vector7d &v)
Definition: se3quat.h:147
bool g2o::EdgeSBACam::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 565 of file types_sba.cpp.

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

566  {
567  const VertexCam* v1 = dynamic_cast<const VertexCam*>(_vertices[0]);
568  const VertexCam* v2 = dynamic_cast<const VertexCam*>(_vertices[1]);
569  _measurement = (v1->estimate().inverse()*v2->estimate());
571  return true;
572  }
SE3Quat inverse() const
Definition: se3quat.h:120
SE3Quat _inverseMeasurement
Definition: types_sba.h:331
VertexContainer _vertices
Definition: hyper_graph.h:202
bool g2o::EdgeSBACam::write ( std::ostream &  os) const
virtual

write the vertex to a stream

Implements g2o::OptimizableGraph::Edge.

Definition at line 155 of file types_sba.cpp.

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

156  {
157  for (int i=0; i<7; i++)
158  os << measurement()[i] << " ";
159  for (int i=0; i<6; i++)
160  for (int j=i; j<6; j++){
161  os << " " << information()(i,j);
162  }
163  return os.good();
164  }
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

SE3Quat g2o::EdgeSBACam::_inverseMeasurement
protected

Definition at line 331 of file types_sba.h.

Referenced by initialEstimate().

g2o::EdgeSBACam::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 288 of file types_sba.h.


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