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

#include <edge_project_p2sc.h>

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

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeProjectP2SC ()
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 ()
 return the error estimate as a 2-vector
virtual void linearizeOplus ()
 Jacobian for stereo projection.
Public Member Functions inherited from g2o::BaseBinaryEdge< 3, Vector3, VertexPointXYZ, VertexCam >
 BaseBinaryEdge ()
Public Member Functions inherited from g2o::BaseFixedSizedEdge< D, Vector3, VertexPointXYZ, VertexCam >
bool allVerticesFixedNs (std::index_sequence< Ints... >) const
void constructQuadraticFormNs (const InformationType &omega, const ErrorVector &weightedError, std::index_sequence< Ints... >)
void constructOffDiagonalQuadraticFormMs (const AtOType &, std::index_sequence<>)
void constructOffDiagonalQuadraticFormMs (const AtOType &AtO, std::index_sequence< Ints... >)
void constructOffDiagonalQuadraticFormM (const AtOType &AtO)
void constructQuadraticFormN (const InformationType &omega, const ErrorVector &weightedError)
void linearizeOplus_allocate (JacobianWorkspace &jacobianWorkspace, std::index_sequence< Ints... >)
void linearizeOplusN ()
void linearizeOplusNs (std::index_sequence< Ints... >)
constexpr std::enable_if< VertexXnType< VertexN >::Dimension!=-1, int >::type vertexDimension () const
const VertexXnType< VertexN > * vertexXn () const
 BaseFixedSizedEdge ()
OptimizableGraph::Vertex * createVertex (int i, CtorArgs... args)
 create an instance of the Nth VertexType
virtual void resize (size_t size)
virtual bool allVerticesFixed () const
virtual void linearizeOplus (JacobianWorkspace &jacobianWorkspace)
const std::tuple_element< N, std::tuple< JacobianType< D, VertexTypes::Dimension >... > >::type & jacobianOplusXn () const
virtual void constructQuadraticForm ()
virtual void mapHessianMemory (double *d, int i, int j, bool rowMajor)
Public Member Functions inherited from g2o::BaseEdge< D, Vector3 >
 BaseEdge ()
BaseEdge & operator= (const BaseEdge &)=delete
virtual ~BaseEdge ()
virtual double chi2 () const
virtual const double * errorData () const
 returns the error vector cached after calling the computeError;
const ErrorVector & error () const
EIGEN_STRONG_INLINE const InformationType & information () const
 information matrix of the constraint
void setInformation (const InformationType &information)
virtual const double * informationData () const
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 *)
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 * errorData ()=0
virtual double * informationData ()=0
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)

Additional Inherited Members

Public Types inherited from g2o::BaseBinaryEdge< 3, Vector3, VertexPointXYZ, VertexCam >
using VertexXiType
using VertexXjType
Public Types inherited from g2o::BaseFixedSizedEdge< D, Vector3, VertexPointXYZ, VertexCam >
using NthType
using VertexXnType
 The type of the N-th vertex.
typedef BaseEdge< D, Vector3 >::Measurement Measurement
typedef BaseEdge< D, Vector3 >::ErrorVector ErrorVector
typedef BaseEdge< D, Vector3 >::InformationType InformationType
using JacobianType
using HessianBlockType
 it requires quite some ugly code to get the type of hessians...
using HessianBlockTypeK
using HessianBlockTypeKTransposed
using HessianTuple
using HessianTupleTransposed
using HessianRowMajorStorage
Public Types inherited from g2o::BaseEdge< D, Vector3 >
typedef Vector3 Measurement
typedef internal::BaseEdgeTraits< D >::ErrorVector ErrorVector
typedef internal::BaseEdgeTraits< D >::InformationType InformationType
Static Public Member Functions inherited from g2o::BaseFixedSizedEdge< D, Vector3, VertexPointXYZ, VertexCam >
static constexpr int VertexDimension ()
 Size of the N-th vertex at compile time.
Static Public Attributes inherited from g2o::BaseFixedSizedEdge< D, Vector3, VertexPointXYZ, VertexCam >
static const int Dimension
static const std::size_t _nr_of_vertices
static const std::size_t _nr_of_vertex_pairs
Static Public Attributes inherited from g2o::BaseEdge< D, Vector3 >
static constexpr int Dimension
Protected Member Functions inherited from g2o::BaseEdge< D, Vector3 >
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, Vector3, VertexPointXYZ, VertexCam >
BaseFixedSizedEdge< D, Vector3, VertexPointXYZ, VertexCam >::template JacobianType< D, VertexPointXYZ::Dimension > & _jacobianOplusXi
BaseFixedSizedEdge< D, Vector3, VertexPointXYZ, VertexCam >::template JacobianType< D, VertexCam::Dimension > & _jacobianOplusXj
Protected Attributes inherited from g2o::BaseFixedSizedEdge< D, Vector3, VertexPointXYZ, VertexCam >
HessianRowMajorStorage _hessianRowMajor
HessianTuple _hessianTuple
HessianTupleTransposed _hessianTupleTransposed
std::tuple< JacobianType< D, VertexTypes::Dimension >... > _jacobianOplus
Protected Attributes inherited from g2o::BaseEdge< D, Vector3 >
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 39 of file edge_project_p2sc.h.

Constructor & Destructor Documentation

◆ EdgeProjectP2SC()

g2o::EdgeProjectP2SC::EdgeProjectP2SC ( )

Definition at line 32 of file edge_project_p2sc.cpp.

33 : BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexCam>() {}

References g2o::BaseBinaryEdge< 3, Vector3, VertexPointXYZ, VertexCam >::BaseBinaryEdge().

Member Function Documentation

◆ computeError()

void g2o::EdgeProjectP2SC::computeError ( )
virtual

return the error estimate as a 2-vector

Implements g2o::OptimizableGraph::Edge.

Definition at line 47 of file edge_project_p2sc.cpp.

47 {
48 // from <Point> to <Cam>
49 const VertexPointXYZ* point =
50 static_cast<const VertexPointXYZ*>(_vertices[0]);
51 VertexCam* cam = static_cast<VertexCam*>(_vertices[1]);
52
53 // calculate the projection
54 Vector3 kp;
55 Vector4 pt;
56 pt.head<3>() = point->estimate();
57 pt(3) = 1;
58 const SBACam& nd = cam->estimate();
59 // these should be already ok
60 /* nd.setTransform(); */
61 /* nd.setProjection(); */
62 /* nd.setDr(); */
63
64 Vector3 p1 = nd.w2i * pt;
65 Vector3 p2 = nd.w2n * pt;
66 Vector3 pb(nd.baseline, 0, 0);
67
68 double invp1 = cst(1.0) / p1(2);
69 kp.head<2>() = p1.head<2>() * invp1;
70
71 // right camera px
72 p2 = nd.Kcam * (p2 - pb);
73 kp(2) = p2(0) / p2(2);
74
75 // std::cout << std::endl << "CAM " << cam->estimate() << std::endl;
76 // std::cout << "POINT " << pt.transpose() << std::endl;
77 // std::cout << "PROJ " << p1.transpose() << std::endl;
78 // std::cout << "PROJ " << p2.transpose() << std::endl;
79 // std::cout << "CPROJ " << kp.transpose() << std::endl;
80 // std::cout << "MEAS " << _measurement.transpose() << std::endl;
81
82 // error, which is backwards from the normal observed - calculated
83 // _measurement is the measured projection
84 _error = kp - _measurement;
85}
g2o::HyperGraph::Edge::_vertices
VertexContainer _vertices
Definition hyper_graph.h:221
g2o::Vector3
VectorN< 3 > Vector3
Definition eigen_types.h:51
g2o::cst
constexpr double cst(long double v)
Definition misc.h:60
g2o::Vector4
VectorN< 4 > Vector4
Definition eigen_types.h:52

References g2o::BaseEdge< D, Vector3 >::_error, g2o::BaseEdge< D, Vector3 >::_measurement, g2o::HyperGraph::Edge::_vertices, g2o::SBACam::baseline, g2o::cst(), g2o::BaseVertex< D, T >::estimate(), g2o::SBACam::Kcam, g2o::SBACam::w2i, and g2o::SBACam::w2n.

◆ linearizeOplus()

void g2o::EdgeProjectP2SC::linearizeOplus ( )
virtual

Jacobian for stereo projection.

Definition at line 87 of file edge_project_p2sc.cpp.

87 {
88 VertexCam* vc = static_cast<VertexCam*>(_vertices[1]);
89 const SBACam& cam = vc->estimate();
90
91 VertexPointXYZ* vp = static_cast<VertexPointXYZ*>(_vertices[0]);
92 Vector4 pt, trans;
93 pt.head<3>() = vp->estimate();
94 pt(3) = 1.0;
95 trans.head<3>() = cam.translation();
96 trans(3) = 1.0;
97
98 // first get the world point in camera coords
99 Vector3 pc = cam.w2n * pt;
100
101 // Jacobians wrt camera parameters
102 // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
103 double px = pc(0);
104 double py = pc(1);
105 double pz = pc(2);
106 double ipz2 = 1.0 / (pz * pz);
107 if (g2o_isnan(ipz2)) {
108 std::cout << "[SetJac] infinite jac" << std::endl;
109 abort();
110 }
111
112 double ipz2fx = ipz2 * cam.Kcam(0, 0); // Fx
113 double ipz2fy = ipz2 * cam.Kcam(1, 1); // Fy
114 double b = cam.baseline; // stereo baseline
115
116 // check for local vars
117 Vector3 pwt =
118 (pt - trans)
119 .head<3>(); // transform translations, use differential rotation
120
121 // dx
122 Vector3 dp = cam.dRdx * pwt; // dR'/dq * [pw - t]
123 _jacobianOplusXj(0, 3) = (pz * dp(0) - px * dp(2)) * ipz2fx;
124 _jacobianOplusXj(1, 3) = (pz * dp(1) - py * dp(2)) * ipz2fy;
125 _jacobianOplusXj(2, 3) =
126 (pz * dp(0) - (px - b) * dp(2)) * ipz2fx; // right image px
127 // dy
128 dp = cam.dRdy * pwt; // dR'/dq * [pw - t]
129 _jacobianOplusXj(0, 4) = (pz * dp(0) - px * dp(2)) * ipz2fx;
130 _jacobianOplusXj(1, 4) = (pz * dp(1) - py * dp(2)) * ipz2fy;
131 _jacobianOplusXj(2, 4) =
132 (pz * dp(0) - (px - b) * dp(2)) * ipz2fx; // right image px
133 // dz
134 dp = cam.dRdz * pwt; // dR'/dq * [pw - t]
135 _jacobianOplusXj(0, 5) = (pz * dp(0) - px * dp(2)) * ipz2fx;
136 _jacobianOplusXj(1, 5) = (pz * dp(1) - py * dp(2)) * ipz2fy;
137 _jacobianOplusXj(2, 5) =
138 (pz * dp(0) - (px - b) * dp(2)) * ipz2fx; // right image px
139
140 // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
141 dp = -cam.w2n.col(0); // dpc / dx
142 _jacobianOplusXj(0, 0) = (pz * dp(0) - px * dp(2)) * ipz2fx;
143 _jacobianOplusXj(1, 0) = (pz * dp(1) - py * dp(2)) * ipz2fy;
144 _jacobianOplusXj(2, 0) =
145 (pz * dp(0) - (px - b) * dp(2)) * ipz2fx; // right image px
146 dp = -cam.w2n.col(1); // dpc / dy
147 _jacobianOplusXj(0, 1) = (pz * dp(0) - px * dp(2)) * ipz2fx;
148 _jacobianOplusXj(1, 1) = (pz * dp(1) - py * dp(2)) * ipz2fy;
149 _jacobianOplusXj(2, 1) =
150 (pz * dp(0) - (px - b) * dp(2)) * ipz2fx; // right image px
151 dp = -cam.w2n.col(2); // dpc / dz
152 _jacobianOplusXj(0, 2) = (pz * dp(0) - px * dp(2)) * ipz2fx;
153 _jacobianOplusXj(1, 2) = (pz * dp(1) - py * dp(2)) * ipz2fy;
154 _jacobianOplusXj(2, 2) =
155 (pz * dp(0) - (px - b) * dp(2)) * ipz2fx; // right image px
156
157 // Jacobians wrt point parameters
158 // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
159 dp = cam.w2n.col(0); // dpc / dx
160 _jacobianOplusXi(0, 0) = (pz * dp(0) - px * dp(2)) * ipz2fx;
161 _jacobianOplusXi(1, 0) = (pz * dp(1) - py * dp(2)) * ipz2fy;
162 _jacobianOplusXi(2, 0) =
163 (pz * dp(0) - (px - b) * dp(2)) * ipz2fx; // right image px
164 dp = cam.w2n.col(1); // dpc / dy
165 _jacobianOplusXi(0, 1) = (pz * dp(0) - px * dp(2)) * ipz2fx;
166 _jacobianOplusXi(1, 1) = (pz * dp(1) - py * dp(2)) * ipz2fy;
167 _jacobianOplusXi(2, 1) =
168 (pz * dp(0) - (px - b) * dp(2)) * ipz2fx; // right image px
169 dp = cam.w2n.col(2); // dpc / dz
170 _jacobianOplusXi(0, 2) = (pz * dp(0) - px * dp(2)) * ipz2fx;
171 _jacobianOplusXi(1, 2) = (pz * dp(1) - py * dp(2)) * ipz2fy;
172 _jacobianOplusXi(2, 2) =
173 (pz * dp(0) - (px - b) * dp(2)) * ipz2fx; // right image px
174}
g2o::BaseBinaryEdge< 3, Vector3, VertexPointXYZ, VertexCam >::_jacobianOplusXj
BaseFixedSizedEdge< D, Vector3, VertexPointXYZ, VertexCam >::template JacobianType< D, VertexCam::Dimension > & _jacobianOplusXj
Definition base_binary_edge.h:48
g2o::BaseBinaryEdge< 3, Vector3, VertexPointXYZ, VertexCam >::_jacobianOplusXi
BaseFixedSizedEdge< D, Vector3, VertexPointXYZ, VertexCam >::template JacobianType< D, VertexPointXYZ::Dimension > & _jacobianOplusXi
Definition base_binary_edge.h:45
g2o_isnan
#define g2o_isnan(x)
Definition macros.h:100

References g2o::BaseBinaryEdge< 3, Vector3, VertexPointXYZ, VertexCam >::_jacobianOplusXi, g2o::BaseBinaryEdge< 3, Vector3, VertexPointXYZ, VertexCam >::_jacobianOplusXj, g2o::HyperGraph::Edge::_vertices, g2o::SBACam::baseline, g2o::SBACam::dRdx, g2o::SBACam::dRdy, g2o::SBACam::dRdz, g2o::BaseVertex< D, T >::estimate(), g2o_isnan, g2o::SBACam::Kcam, g2o::SE3Quat::translation(), and g2o::SBACam::w2n.

◆ read()

bool g2o::EdgeProjectP2SC::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 35 of file edge_project_p2sc.cpp.

35 {
36 internal::readVector(is, _measurement);
37 return readInformationMatrix(is);
38}
g2o::BaseEdge< D, Vector3 >::readInformationMatrix
bool readInformationMatrix(std::istream &is)
Definition base_edge.h:173
g2o::internal::readVector
bool readVector(std::istream &is, Eigen::DenseBase< Derived > &b)
Definition io_helper.h:42

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

◆ write()

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

write the vertex to a stream

Implements g2o::OptimizableGraph::Edge.

Definition at line 40 of file edge_project_p2sc.cpp.

40 {
41 internal::writeVector(os, measurement());
42 writeInformationMatrix(os);
43 return os.good();
44}
g2o::BaseEdge< D, Vector3 >::writeInformationMatrix
bool writeInformationMatrix(std::ostream &os) const
Definition base_edge.h:165
g2o::BaseEdge< D, Vector3 >::measurement
EIGEN_STRONG_INLINE const Measurement & measurement() const
Definition base_edge.h:119
g2o::internal::writeVector
bool writeVector(std::ostream &os, const Eigen::DenseBase< Derived > &b)
Definition io_helper.h:36

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

The documentation for this class was generated from the following files:
Generated on for g2o by doxygen 1.15.0