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

edge from a track to a depth camera node using a disparity measurement More...

#include <edge_se3_pointxyz_disparity.h>

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

Public Member Functions

 EdgeSE3PointXYZDisparity ()
 
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
 
void computeError ()
 
virtual void linearizeOplus ()
 
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 &from, OptimizableGraph::Vertex *to)
 
virtual void initialEstimate (const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to)
 
const ParameterCameracameraParameter () const
 
- Public Member Functions inherited from g2o::BaseBinaryEdge< 3, Vector3, VertexSE3, VertexPointXYZ >
 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::VertexcreateVertex (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... >)
 
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 ()
 
BaseEdgeoperator= (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 ErrorVectorerror () const
 
ErrorVectorerror ()
 
EIGEN_STRONG_INLINE const InformationTypeinformation () const
 information matrix of the constraint
 
EIGEN_STRONG_INLINE InformationTypeinformation ()
 
void setInformation (const InformationType &information)
 
virtual const double * informationData () const
 
virtual double * informationData ()
 
EIGEN_STRONG_INLINE const Measurementmeasurement () const
 accessor functions for the measurement represented by the edge
 
virtual void setMeasurement (const Measurement &m)
 
virtual int rank () const
 
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 ()
 
RobustKernelrobustKernel () const
 if NOT NULL, error of this edge will be robustifed with the kernel
 
void setRobustKernel (RobustKernel *ptr)
 
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 VertexcreateVertex (int)
 
long long internalId () const
 the internal ID of the edge
 
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
 
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 ()
 
- Public Member Functions inherited from g2o::HyperGraph::DataContainer
 DataContainer ()
 
virtual ~DataContainer ()
 
const DatauserData () const
 the user data associated with this vertex
 
DatauserData ()
 
void setUserData (Data *obs)
 
void addUserData (Data *obs)
 

Public Attributes

 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 

Private Member Functions

virtual bool resolveCaches ()
 

Private Attributes

Eigen::Matrix< double, 3, 9, Eigen::ColMajor > J
 
ParameterCameraparams
 
CacheCameracache
 

Additional Inherited Members

- Public Types inherited from g2o::BaseBinaryEdge< 3, Vector3, VertexSE3, VertexPointXYZ >
using VertexXiType = VertexSE3
 
using VertexXjType = VertexPointXYZ
 
- 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 ()
 
- Protected Attributes inherited from g2o::BaseBinaryEdge< 3, Vector3, VertexSE3, VertexPointXYZ >
BaseFixedSizedEdge< D, Vector3, VertexSE3, VertexPointXYZ >::template JacobianType< D, VertexXi::Dimension > & _jacobianOplusXi
 
BaseFixedSizedEdge< D, Vector3, VertexSE3, VertexPointXYZ >::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

edge from a track to a depth camera node using a disparity measurement

the disparity measurement is normalized: disparity / (focal_x * baseline)

Definition at line 45 of file edge_se3_pointxyz_disparity.h.

Constructor & Destructor Documentation

◆ EdgeSE3PointXYZDisparity()

g2o::EdgeSE3PointXYZDisparity::EdgeSE3PointXYZDisparity ( )

Definition at line 40 of file edge_se3_pointxyz_disparity.cpp.

41 : BaseBinaryEdge<3, Vector3, VertexSE3, VertexPointXYZ>(),
42 params(nullptr),
43 cache(nullptr) {
46 information().setIdentity();
47 information()(2, 2) = 1000.;
48 J.fill(0);
49 J.block<3, 3>(0, 0) = -Matrix3::Identity();
50}
EIGEN_STRONG_INLINE const InformationType & information() const
information matrix of the constraint
Definition base_edge.h:107
Eigen::Matrix< double, 3, 9, Eigen::ColMajor > J
bool installParameter(ParameterType *&p, size_t argNo, int paramId=-1)
void resizeParameters(size_t newSize)

References g2o::BaseEdge< D, E >::information(), g2o::OptimizableGraph::Edge::installParameter(), J, params, and g2o::OptimizableGraph::Edge::resizeParameters().

Member Function Documentation

◆ cameraParameter()

const ParameterCamera * g2o::EdgeSE3PointXYZDisparity::cameraParameter ( ) const
inline

Definition at line 85 of file edge_se3_pointxyz_disparity.h.

85{ return params; }

◆ computeError()

void g2o::EdgeSE3PointXYZDisparity::computeError ( )
virtual

Implements g2o::OptimizableGraph::Edge.

Definition at line 72 of file edge_se3_pointxyz_disparity.cpp.

72 {
73 // VertexSE3 *cam = static_cast<VertexSE3*>(_vertices[0]);
74 VertexPointXYZ* point = static_cast<VertexPointXYZ*>(_vertices[1]);
75 const Vector3& pt = point->estimate();
76
77 Vector3 p = cache->w2i() * pt;
78
79 Vector3 perr;
80 perr.head<2>() = p.head<2>() / p(2);
81 perr(2) = 1 / p(2);
82
83 // error, which is backwards from the normal observed - calculated
84 // _measurement is the measured projection
85 _error = perr - _measurement;
86}
Measurement _measurement
the measurement of the edge
Definition base_edge.h:146
ErrorVector _error
Definition base_edge.h:149
const Affine3 & w2i() const
return the world to image transform
VertexContainer _vertices
VectorN< 3 > Vector3
Definition eigen_types.h:51

References g2o::BaseEdge< D, E >::_error, g2o::BaseEdge< D, E >::_measurement, g2o::HyperGraph::Edge::_vertices, cache, g2o::BaseVertex< D, T >::estimate(), and g2o::CacheCamera::w2i().

◆ getMeasurementData()

virtual bool g2o::EdgeSE3PointXYZDisparity::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_pointxyz_disparity.h.

66 {
67 Eigen::Map<Vector3> v(d);
68 v = _measurement;
69 return true;
70 }

◆ initialEstimate()

void g2o::EdgeSE3PointXYZDisparity::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::BaseEdge< D, E >.

Definition at line 148 of file edge_se3_pointxyz_disparity.cpp.

149 {
150 (void)from;
151 assert(from.size() == 1 && from.count(_vertices[0]) == 1 &&
152 "Can not initialize VertexDepthCam position by VertexTrackXYZ");
153 VertexSE3* cam = dynamic_cast<VertexSE3*>(_vertices[0]);
154 VertexPointXYZ* point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
155
156 const Eigen::Matrix<double, 3, 3, Eigen::ColMajor>& invKcam =
157 params->invKcam();
158 Vector3 p;
159 double w = 1. / _measurement(2);
160 p.head<2>() = _measurement.head<2>() * w;
161 p(2) = w;
162 p = invKcam * p;
163 p = cam->estimate() * (params->offset() * p);
164 point->setEstimate(p);
165}
const Matrix3 & invKcam() const
const Isometry3 & offset() const
rotation of the offset as 3x3 rotation matrix

References g2o::BaseEdge< D, E >::_measurement, g2o::HyperGraph::Edge::_vertices, g2o::BaseVertex< D, T >::estimate(), g2o::ParameterCamera::invKcam(), g2o::ParameterSE3Offset::offset(), params, and g2o::BaseVertex< D, T >::setEstimate().

◆ initialEstimatePossible()

virtual double g2o::EdgeSE3PointXYZDisparity::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_pointxyz_disparity.h.

77 {
78 (void)to;
79 return (from.count(_vertices[0]) == 1 ? 1.0 : -1.0);
80 }

◆ linearizeOplus()

void g2o::EdgeSE3PointXYZDisparity::linearizeOplus ( )
virtual

Linearizes the oplus operator in the vertex, and stores the result in temporary variables _jacobianOplus

Reimplemented from g2o::BaseFixedSizedEdge< D, E, VertexTypes >.

Definition at line 90 of file edge_se3_pointxyz_disparity.cpp.

90 {
91 // VertexSE3 *cam = static_cast<VertexSE3 *>(_vertices[0]);
92 VertexPointXYZ* vp = static_cast<VertexPointXYZ*>(_vertices[1]);
93
94 const Vector3& pt = vp->estimate();
95
96 Vector3 Zcam = cache->w2l() * vp->estimate();
97
98 // J(0,3) = -0.0;
99 J(0, 4) = -2 * Zcam(2);
100 J(0, 5) = 2 * Zcam(1);
101
102 J(1, 3) = 2 * Zcam(2);
103 // J(1,4) = -0.0;
104 J(1, 5) = -2 * Zcam(0);
105
106 J(2, 3) = -2 * Zcam(1);
107 J(2, 4) = 2 * Zcam(0);
108 // J(2,5) = -0.0;
109
110 J.block<3, 3>(0, 6) = cache->w2l().rotation();
111
112 // Eigen::Matrix<double,3,9,Eigen::ColMajor> Jprime =
113 // vcache->params->Kcam_inverseOffsetR * J;
114 Eigen::Matrix<double, 3, 9, Eigen::ColMajor> Jprime =
116 Eigen::Matrix<double, 3, 9, Eigen::ColMajor> Jhom;
117 Vector3 Zprime = cache->w2i() * pt;
118
119 Jhom.block<2, 9>(0, 0) = 1 / (Zprime(2) * Zprime(2)) *
120 (Jprime.block<2, 9>(0, 0) * Zprime(2) -
121 Zprime.head<2>() * Jprime.block<1, 9>(2, 0));
122 Jhom.block<1, 9>(2, 0) =
123 -1 / (Zprime(2) * Zprime(2)) * Jprime.block<1, 9>(2, 0);
124
125 _jacobianOplusXi = Jhom.block<3, 6>(0, 0);
126 _jacobianOplusXj = Jhom.block<3, 3>(0, 6);
127}
BaseFixedSizedEdge< D, Vector3, VertexSE3, VertexPointXYZ >::template JacobianType< D, VertexXj::Dimension > & _jacobianOplusXj
BaseFixedSizedEdge< D, Vector3, VertexSE3, VertexPointXYZ >::template JacobianType< D, VertexXi::Dimension > & _jacobianOplusXi
const Isometry3 & w2l() const
const Matrix3 & Kcam_inverseOffsetR() const

References g2o::BaseBinaryEdge< 3, Vector3, VertexSE3, VertexPointXYZ >::_jacobianOplusXi, g2o::BaseBinaryEdge< 3, Vector3, VertexSE3, VertexPointXYZ >::_jacobianOplusXj, g2o::HyperGraph::Edge::_vertices, cache, g2o::BaseVertex< D, T >::estimate(), J, g2o::ParameterCamera::Kcam_inverseOffsetR(), params, g2o::CacheCamera::w2i(), and g2o::CacheSE3Offset::w2l().

◆ measurementDimension()

virtual int g2o::EdgeSE3PointXYZDisparity::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_pointxyz_disparity.h.

72{ return 3; }

◆ read()

bool g2o::EdgeSE3PointXYZDisparity::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 60 of file edge_se3_pointxyz_disparity.cpp.

60 {
61 readParamIds(is);
63 return readInformationMatrix(is);
64}
bool readInformationMatrix(std::istream &is)
Definition base_edge.h:173
bool readParamIds(std::istream &is)
reads the param IDs from the stream
Definition base_edge.h:187
bool readVector(std::istream &is, Eigen::DenseBase< Derived > &b)
Definition io_helper.h:42

References g2o::BaseEdge< D, E >::_measurement, g2o::BaseEdge< D, E >::readInformationMatrix(), g2o::BaseEdge< D, E >::readParamIds(), and g2o::internal::readVector().

◆ resolveCaches()

bool g2o::EdgeSE3PointXYZDisparity::resolveCaches ( )
privatevirtual

Reimplemented from g2o::OptimizableGraph::Edge.

Definition at line 52 of file edge_se3_pointxyz_disparity.cpp.

52 {
53 ParameterVector pv(1);
54 pv[0] = params;
55 resolveCache(cache, (OptimizableGraph::Vertex*)_vertices[0], "CACHE_CAMERA",
56 pv);
57 return cache != 0;
58}
void resolveCache(CacheType *&cache, OptimizableGraph::Vertex *, const std::string &_type, const ParameterVector &parameters)
Definition cache.h:125
std::vector< Parameter * > ParameterVector
Definition parameter.h:54

References g2o::HyperGraph::Edge::_vertices, cache, params, and g2o::OptimizableGraph::Edge::resolveCache().

◆ setMeasurementData()

virtual bool g2o::EdgeSE3PointXYZDisparity::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 60 of file edge_se3_pointxyz_disparity.h.

60 {
61 Eigen::Map<const Vector3> v(d);
62 _measurement = v;
63 return true;
64 }

◆ setMeasurementFromState()

bool g2o::EdgeSE3PointXYZDisparity::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 131 of file edge_se3_pointxyz_disparity.cpp.

131 {
132 // VertexSE3 *cam = static_cast< VertexSE3*>(_vertices[0]);
133 VertexPointXYZ* point = static_cast<VertexPointXYZ*>(_vertices[1]);
134 const Vector3& pt = point->estimate();
135
136 Vector3 p = cache->w2i() * pt;
137
138 Vector3 perr;
139 perr.head<2>() = p.head<2>() / p(2);
140 perr(2) = 1 / p(2);
141
142 // error, which is backwards from the normal observed - calculated
143 // _measurement is the measured projection
144 _measurement = perr;
145 return true;
146}

References g2o::BaseEdge< D, E >::_measurement, g2o::HyperGraph::Edge::_vertices, cache, g2o::BaseVertex< D, T >::estimate(), and g2o::CacheCamera::w2i().

◆ write()

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

write the vertex to a stream

Implements g2o::OptimizableGraph::Edge.

Definition at line 66 of file edge_se3_pointxyz_disparity.cpp.

66 {
67 writeParamIds(os);
69 return writeInformationMatrix(os);
70}
bool writeInformationMatrix(std::ostream &os) const
write the upper trinagular part of the information matrix into the stream
Definition base_edge.h:165
EIGEN_STRONG_INLINE const Measurement & measurement() const
accessor functions for the measurement represented by the edge
Definition base_edge.h:119
bool writeParamIds(std::ostream &os) const
write the param IDs that are potentially used by the edge
Definition base_edge.h:182
bool writeVector(std::ostream &os, const Eigen::DenseBase< Derived > &b)
Definition io_helper.h:36

References g2o::BaseEdge< D, E >::measurement(), g2o::BaseEdge< D, E >::writeInformationMatrix(), g2o::BaseEdge< D, E >::writeParamIds(), and g2o::internal::writeVector().

Member Data Documentation

◆ cache

CacheCamera* g2o::EdgeSE3PointXYZDisparity::cache
private

◆ EIGEN_MAKE_ALIGNED_OPERATOR_NEW

g2o::EdgeSE3PointXYZDisparity::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 48 of file edge_se3_pointxyz_disparity.h.

◆ J

Eigen::Matrix<double, 3, 9, Eigen::ColMajor> g2o::EdgeSE3PointXYZDisparity::J
private

Definition at line 88 of file edge_se3_pointxyz_disparity.h.

Referenced by EdgeSE3PointXYZDisparity(), and linearizeOplus().

◆ params

ParameterCamera* g2o::EdgeSE3PointXYZDisparity::params
private

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