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

edge representing the observation of a world feature by a camera More...

Inheritance diagram for EdgeObservationBAL:
Inheritance graph
[legend]
Collaboration diagram for EdgeObservationBAL:
Collaboration graph
[legend]

Public Member Functions

 EdgeObservationBAL ()
 
bool read (std::istream &) override
 read the vertex from a stream, i.e., the internal state of the vertex
 
bool write (std::ostream &) const override
 write the vertex to a stream
 
template<typename T >
bool operator() (const T *p_camera, const T *p_point, T *p_error) const
 
- Public Member Functions inherited from g2o::BaseBinaryEdge< 2, g2o::Vector2, VertexCameraBAL, VertexPointBAL >
 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 void computeError ()=0
 
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< 2, g2o::Vector2, VertexCameraBAL, VertexPointBAL >
using VertexXiType = VertexCameraBAL
 
using VertexXjType = VertexPointBAL
 
- 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< 2, g2o::Vector2, VertexCameraBAL, VertexPointBAL >
BaseFixedSizedEdge< D, g2o::Vector2, VertexCameraBAL, VertexPointBAL >::template JacobianType< D, VertexXi::Dimension > & _jacobianOplusXi
 
BaseFixedSizedEdge< D, g2o::Vector2, VertexCameraBAL, VertexPointBAL >::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 representing the observation of a world feature by a camera

see: http://grail.cs.washington.edu/projects/bal/ We use a pinhole camera model; the parameters we estimate for each camera area rotation R, a translation t, a focal length f and two radial distortion parameters k1 and k2. The formula for projecting a 3D point X into a camera R,t,f,k1,k2 is: P = R * X + t (conversion from world to camera coordinates) p = -P / P.z (perspective division) p' = f * r(p) * p (conversion to pixel coordinates) where P.z is the third (z) coordinate of P.

In the last equation, r(p) is a function that computes a scaling factor to undo the radial distortion: r(p) = 1.0 + k1 * ||p||^2 + k2 * ||p||^4.

This gives a projection in pixels, where the origin of the image is the center of the image, the positive x-axis points right, and the positive y-axis points up (in addition, in the camera coordinate system, the positive z-axis points backwards, so the camera is looking down the negative z-axis, as in OpenGL).

Definition at line 146 of file bal_example.cpp.

Constructor & Destructor Documentation

◆ EdgeObservationBAL()

EdgeObservationBAL::EdgeObservationBAL ( )
inline

Definition at line 151 of file bal_example.cpp.

151{}

Member Function Documentation

◆ operator()()

template<typename T >
bool EdgeObservationBAL::operator() ( const T *  p_camera,
const T *  p_point,
T *  p_error 
) const
inline

templatized function to compute the error as described in the comment above

Definition at line 165 of file bal_example.cpp.

165 {
166 typename g2o::VectorN<9, T>::ConstMapType camera(p_camera);
167 typename g2o::VectorN<3, T>::ConstMapType point(p_point);
168
169 typename g2o::VectorN<3, T> p;
170
171 // Rodrigues' formula for the rotation
172 T theta = camera.template head<3>().norm();
173 if (theta > T(0)) {
174 g2o::VectorN<3, T> v = camera.template head<3>() / theta;
175 T cth = cos(theta);
176 T sth = sin(theta);
177
178 g2o::VectorN<3, T> vXp = v.cross(point);
179 T vDotp = v.dot(point);
180 T oneMinusCth = T(1) - cth;
181
182 p = point * cth + vXp * sth + v * vDotp * oneMinusCth;
183 } else {
184 // taylor expansion for theta close to zero
185 p = point + camera.template head<3>().cross(point);
186 }
187
188 // translation of the camera
189 p += camera.template segment<3>(3);
190
191 // perspective division
192 g2o::VectorN<2, T> projectedPoint = -p.template head<2>() / p(2);
193
194 // conversion to pixel coordinates
195 T radiusSqr = projectedPoint.squaredNorm();
196 const T& f = camera(6);
197 const T& k1 = camera(7);
198 const T& k2 = camera(8);
199 T r_p = T(1) + k1 * radiusSqr + k2 * radiusSqr * radiusSqr;
200 g2o::VectorN<2, T> prediction = f * r_p * projectedPoint;
201
202 // compute the error
203 typename g2o::VectorN<2, T>::MapType error(p_error);
204 error = prediction - measurement().cast<T>();
205 (void)error;
206 return true;
207 }
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
const ErrorVector & error() const
Definition base_edge.h:103
g2o::ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition jet.h:452
g2o::ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition jet.h:465
g2o::VectorN
Eigen::Matrix< T, N, 1, Eigen::ColMajor > VectorN
Definition eigen_types.h:49

References g2o::BaseEdge< D, E >::error(), and g2o::BaseEdge< D, E >::measurement().

◆ read()

bool EdgeObservationBAL::read ( std::istream &  is)
inlineoverridevirtual

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

Implements g2o::OptimizableGraph::Edge.

Definition at line 152 of file bal_example.cpp.

152 {
153 cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
154 return false;
155 }
__PRETTY_FUNCTION__
#define __PRETTY_FUNCTION__
Definition macros.h:90

References __PRETTY_FUNCTION__.

◆ write()

bool EdgeObservationBAL::write ( std::ostream &  os) const
inlineoverridevirtual

write the vertex to a stream

Implements g2o::OptimizableGraph::Edge.

Definition at line 156 of file bal_example.cpp.

156 {
157 cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
158 return false;
159 }

References __PRETTY_FUNCTION__.

Member Data Documentation

◆ EIGEN_MAKE_ALIGNED_OPERATOR_NEW

EdgeObservationBAL::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 150 of file bal_example.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