Stereo camera vertex, derived from SE3 class. Note that we use the actual pose of the vertex as its parameterization, rather than the transform from RW to camera coords. Uses static vars for camera params, so there is a single camera setup. More...

#include <types_icp.h>

Inheritance diagram for g2o::VertexSCam:

[legend]

Collaboration diagram for g2o::VertexSCam:

[legend]

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	VertexSCam ()

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

virtual void	oplusImpl (const double *update)

void	setTransform ()

void	setProjection ()

void	setDr ()

void	setAll ()

void	mapPoint (Vector3 &res, const Vector3 &pt3)

Public Member Functions inherited from g2o::VertexSE3
	VertexSE3 ()

virtual void	setToOriginImpl ()
	sets the node to the origin (used in the multilevel stuff)

virtual bool	setEstimateDataImpl (const double *est)

virtual bool	getEstimateData (double *est) const

virtual int	estimateDimension () const

virtual bool	setMinimalEstimateDataImpl (const double *est)

virtual bool	getMinimalEstimateData (double *est) const

virtual int	minimalEstimateDimension () const

SE3Quat	G2O_ATTRIBUTE_DEPRECATED (estimateAsSE3Quat() const)
	wrapper function to use the old SE3 type

void	G2O_ATTRIBUTE_DEPRECATED (setEstimateFromSE3Quat(const SE3Quat &se3))
	wrapper function to use the old SE3 type

Public Member Functions inherited from g2o::BaseVertex< 6, Isometry3 >
	BaseVertex ()

	BaseVertex (const BaseVertex &)=delete

BaseVertex &	operator= (const BaseVertex &)=delete

virtual const double &	hessian (int i, int j) const
	get the element from the hessian matrix

virtual double &	hessian (int i, int j)

virtual double	hessianDeterminant () const

virtual double *	hessianData ()

virtual void	mapHessianMemory (double *d)

virtual int	copyB (double *b_) const

virtual const double &	b (int i) const
	get the b vector element

virtual double &	b (int i)

Eigen::Matrix< double, D, 1, Eigen::ColMajor > &	b ()
	return right hand side b of the constructed linear system

const Eigen::Matrix< double, D, 1, Eigen::ColMajor > &	b () const

virtual double *	bData ()
	return a pointer to the b vector associated with this vertex

virtual void	clearQuadraticForm ()

virtual double	solveDirect (double lambda=0)

HessianBlockType &	A ()
	return the hessian block associated with the vertex

const HessianBlockType &	A () const

virtual void	push ()
	backup the position of the vertex to a stack

virtual void	pop ()

virtual void	discardTop ()

virtual int	stackSize () const
	return the stack size

const EstimateType &	estimate () const
	return the current estimate of the vertex

void	setEstimate (const EstimateType &et)
	set the estimate for the vertex also calls updateCache()

Public Member Functions inherited from g2o::OptimizableGraph::Vertex
	Vertex ()

virtual	~Vertex ()

void	setToOrigin ()
	sets the node to the origin (used in the multilevel stuff)

bool	setEstimateData (const double *estimate)

bool	setEstimateData (const std::vector< double > &estimate)

template<typename Derived >
bool	setEstimateData (const Eigen::MatrixBase< Derived > &estimate)

virtual bool	getEstimateData (std::vector< double > &estimate) const

template<typename Derived >
bool	getEstimateData (Eigen::MatrixBase< Derived > &estimate) const

bool	setMinimalEstimateData (const double *estimate)

bool	setMinimalEstimateData (const std::vector< double > &estimate)

template<typename Derived >
bool	setMinimalEstimateData (const Eigen::MatrixBase< Derived > &estimate)

virtual bool	getMinimalEstimateData (std::vector< double > &estimate) const

template<typename Derived >
bool	getMinimalEstimateData (Eigen::MatrixBase< Derived > &estimate) const

void	oplus (const double *v)

int	hessianIndex () const

int	G2O_ATTRIBUTE_DEPRECATED (tempIndex() const)

void	setHessianIndex (int ti)
	set the temporary index of the vertex in the parameter blocks

void	G2O_ATTRIBUTE_DEPRECATED (setTempIndex(int ti))

bool	fixed () const
	true => this node is fixed during the optimization

void	setFixed (bool fixed)
	true => this node should be considered fixed during the optimization

bool	marginalized () const
	true => this node is marginalized out during the optimization

void	setMarginalized (bool marginalized)
	true => this node should be marginalized out during the optimization

int	dimension () const
	dimension of the estimated state belonging to this node

virtual void	setId (int id)

void	setColInHessian (int c)
	set the row of this vertex in the Hessian

int	colInHessian () const
	get the row of this vertex in the Hessian

const OptimizableGraph *	graph () const

OptimizableGraph *	graph ()

void	lockQuadraticForm ()

void	unlockQuadraticForm ()

virtual void	updateCache ()

CacheContainer *	cacheContainer ()

Public Member Functions inherited from g2o::HyperGraph::Vertex
	Vertex (int id=InvalidId)
	creates a vertex having an ID specified by the argument

int	id () const
	returns the id

const EdgeSet &	edges () const
	returns the set of hyper-edges that are leaving/entering in this vertex

EdgeSet &	edges ()
	returns the set of hyper-edges that are leaving/entering in this vertex

virtual HyperGraphElementType	elementType () 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)

Static Public Member Functions
static void	transformW2F (Eigen::Matrix< double, 3, 4, Eigen::ColMajor > &m, const Vector3 &trans, const Quaternion &qrot)

static void	transformF2W (Eigen::Matrix< double, 3, 4, Eigen::ColMajor > &m, const Vector3 &trans, const Quaternion &qrot)

static void	setKcam (double fx, double fy, double cx, double cy, double tx)

Public Attributes
Eigen::Matrix< double, 3, 4, Eigen::ColMajor >	w2n

Eigen::Matrix< double, 3, 4, Eigen::ColMajor >	w2i

Matrix3	dRdx

Matrix3	dRdy

Matrix3	dRdz

Public Attributes inherited from g2o::VertexSE3
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Static Public Attributes
static Matrix3	Kcam

static double	baseline

static Matrix3	dRidx

static Matrix3	dRidy

static Matrix3	dRidz

Static Public Attributes inherited from g2o::VertexSE3
static const int	orthogonalizeAfter

Static Public Attributes inherited from g2o::BaseVertex< 6, Isometry3 >
static const int	Dimension
	dimension of the estimate (minimal) in the manifold space

Additional Inherited Members
Public Types inherited from g2o::BaseVertex< 6, Isometry3 >
using	EstimateType = Isometry3

using	BackupStackType = std::stack< EstimateType, std::vector< EstimateType > >

using	HessianBlockType = Eigen::Map< Eigen::Matrix< double, D, D, Eigen::ColMajor >, Eigen::Matrix< double, D, D, Eigen::ColMajor >::Flags &Eigen::PacketAccessBit ? Eigen::Aligned :Eigen::Unaligned >

Protected Attributes inherited from g2o::VertexSE3
int	_numOplusCalls

Protected Attributes inherited from g2o::BaseVertex< 6, Isometry3 >
HessianBlockType	_hessian

Eigen::Matrix< double, D, 1, Eigen::ColMajor >	_b

EstimateType	_estimate

BackupStackType	_backup

Protected Attributes inherited from g2o::OptimizableGraph::Vertex
OptimizableGraph *	_graph

Data *	_userData

int	_hessianIndex

bool	_fixed

bool	_marginalized

int	_dimension

int	_colInHessian

OpenMPMutex	_quadraticFormMutex

CacheContainer *	_cacheContainer

Protected Attributes inherited from g2o::HyperGraph::Vertex
int	_id

EdgeSet	_edges

Protected Attributes inherited from g2o::HyperGraph::DataContainer
Data *	_userData

Detailed Description

Stereo camera vertex, derived from SE3 class. Note that we use the actual pose of the vertex as its parameterization, rather than the transform from RW to camera coords. Uses static vars for camera params, so there is a single camera setup.

Definition at line 228 of file types_icp.h.

Constructor & Destructor Documentation

◆ VertexSCam()

g2o::VertexSCam::VertexSCam ( )

Definition at line 190 of file types_icp.cpp.

190: VertexSE3() {}

g2o::VertexSE3::VertexSE3

VertexSE3()

Definition vertex_se3.cpp:41

Member Function Documentation

◆ mapPoint()

void g2o::VertexSCam::mapPoint	(	Vector3 &	res,
		const Vector3 &	pt3
	)

inline

Definition at line 314 of file types_icp.h.

                                                  {
    Vector4 pt;
    pt.head<3>() = pt3;
    pt(3) = cst(1.0);
    Vector3 p1 = w2i * pt;
    Vector3 p2 = w2n * pt;
    Vector3 pb(baseline, 0, 0);
 
    double invp1 = cst(1.0) / p1(2);
    res.head<2>() = p1.head<2>() * invp1;
 
    // right camera px
    p2 = Kcam * (p2 - pb);
    res(2) = p2(0) / p2(2);
  }

References g2o::cst().

Referenced by g2o::Edge_XYZ_VSC::computeError().

◆ oplusImpl()

virtual void g2o::VertexSCam::oplusImpl ( const double * update )

inlinevirtual

update the position of this vertex. The update is in the form (x,y,z,qx,qy,qz) whereas (x,y,z) represents the translational update and (qx,qy,qz) corresponds to the respective elements. The missing element qw of the quaternion is recovred by || (qw,qx,qy,qz) || == 1 => qw = sqrt(1 - || (qx,qy,qz) ||

Reimplemented from g2o::VertexSE3.

Definition at line 239 of file types_icp.h.

                                               {
    VertexSE3::oplusImpl(update);
    setAll();
  }

◆ read()

bool g2o::VertexSCam::read ( std::istream & is )

virtual

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

Reimplemented from g2o::VertexSE3.

Definition at line 292 of file types_icp.cpp.

292{ return false; }

◆ setAll()

void g2o::VertexSCam::setAll ( )

inline

Definition at line 307 of file types_icp.h.

                {
    setTransform();
    setProjection();
    setDr();
  }

Referenced by main(), and main().

◆ setDr()

void g2o::VertexSCam::setDr ( )

inline

Definition at line 297 of file types_icp.h.

               {
    // inefficient, just for testing
    // use simple multiplications and additions for production code in
    // calculating dRdx,y,z for dS'*R', with dS the incremental change
    dRdx = dRidx * w2n.block<3, 3>(0, 0);
    dRdy = dRidy * w2n.block<3, 3>(0, 0);
    dRdz = dRidz * w2n.block<3, 3>(0, 0);
  }

◆ setKcam()

static void g2o::VertexSCam::setKcam	(	double	fx,
		double	fy,
		double	cx,
		double	cy,
		double	tx
	)

inlinestatic

Definition at line 276 of file types_icp.h.

                                                                             {
    Kcam.setZero();
    Kcam(0, 0) = fx;
    Kcam(1, 1) = fy;
    Kcam(0, 2) = cx;
    Kcam(1, 2) = cy;
    Kcam(2, 2) = 1.0;
    baseline = tx;
  }

Referenced by main(), and main().

◆ setProjection()

void g2o::VertexSCam::setProjection ( )

inline

Definition at line 294 of file types_icp.h.

294{ w2i = Kcam * w2n; }

◆ setTransform()

void g2o::VertexSCam::setTransform ( )

inline

Definition at line 287 of file types_icp.h.

                      {
    w2n = estimate().inverse().matrix().block<3, 4>(0, 0);
    // transformW2F(w2n,estimate().translation(), estimate().rotation());
  }

◆ transformF2W()

static void g2o::VertexSCam::transformF2W	(	Eigen::Matrix< double, 3, 4, Eigen::ColMajor > &	m,
		const Vector3 &	trans,
		const Quaternion &	qrot
	)

inlinestatic

Definition at line 269 of file types_icp.h.

                                                                         {
    m.block<3, 3>(0, 0) = qrot.toRotationMatrix();
    m.col(3) = trans;
  }

◆ transformW2F()

static void g2o::VertexSCam::transformW2F	(	Eigen::Matrix< double, 3, 4, Eigen::ColMajor > &	m,
		const Vector3 &	trans,
		const Quaternion &	qrot
	)

inlinestatic

Definition at line 259 of file types_icp.h.

                                                                         {
    m.block<3, 3>(0, 0) = qrot.toRotationMatrix().transpose();
    m.col(3).setZero();  // make sure there's no translation
    Vector4 tt;
    tt.head(3) = trans;
    tt[3] = 1.0;
    m.col(3) = -m * tt;
  }

◆ write()

bool g2o::VertexSCam::write ( std::ostream & os ) const

virtual

write the vertex to a stream

Reimplemented from g2o::VertexSE3.

Definition at line 294 of file types_icp.cpp.

294{ return false; }

Member Data Documentation

◆ baseline

double g2o::VertexSCam::baseline

static

Definition at line 246 of file types_icp.h.

◆ dRdx

Matrix3 g2o::VertexSCam::dRdx

Definition at line 256 of file types_icp.h.

◆ dRdy

Matrix3 g2o::VertexSCam::dRdy

Definition at line 256 of file types_icp.h.

◆ dRdz

Matrix3 g2o::VertexSCam::dRdz

Definition at line 256 of file types_icp.h.

◆ dRidx

Matrix3 g2o::VertexSCam::dRidx

static

Definition at line 330 of file types_icp.h.

Referenced by g2o::types_icp::init().

◆ dRidy

Matrix3 g2o::VertexSCam::dRidy

static

Definition at line 331 of file types_icp.h.

Referenced by g2o::types_icp::init().

◆ dRidz

Matrix3 g2o::VertexSCam::dRidz

static

Definition at line 332 of file types_icp.h.

Referenced by g2o::types_icp::init().

◆ Kcam

Matrix3 g2o::VertexSCam::Kcam

static

Definition at line 245 of file types_icp.h.

◆ w2i

Eigen::Matrix<double, 3, 4, Eigen::ColMajor> g2o::VertexSCam::w2i

Definition at line 252 of file types_icp.h.

◆ w2n

Eigen::Matrix<double, 3, 4, Eigen::ColMajor> g2o::VertexSCam::w2n

Definition at line 250 of file types_icp.h.

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

/build/reproducible-path/g2o-0~20230806/g2o/types/icp/types_icp.h
/build/reproducible-path/g2o-0~20230806/g2o/types/icp/types_icp.cpp

Public Member Functions

Static Public Member Functions

Public Attributes

Static Public Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ VertexSCam()

Member Function Documentation

◆ mapPoint()

◆ oplusImpl()

◆ read()

◆ setAll()

◆ setDr()

◆ setKcam()

◆ setProjection()

◆ setTransform()

◆ transformF2W()

◆ transformW2F()

◆ write()

Member Data Documentation

◆ baseline

◆ dRdx

◆ dRdy

◆ dRdz

◆ dRidx

◆ dRidy

◆ dRidz

◆ Kcam

◆ w2i

◆ w2n