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 &	operator= (const BaseVertex &)=delete
virtual const double &	hessian (int i, int j) const
	get the element from the hessian matrix
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 *	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
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)
virtual double &	hessian (int i, int j)=0
virtual double &	b (int i)=0
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
using	BackupStackType
using	HessianBlockType
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

References g2o::VertexSE3::VertexSE3().

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 baseline, g2o::cst(), Kcam, w2i, and w2n.

Referenced by g2o::Edge_XYZ_VSC::computeError(), main(), and main().

◆ 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();
  }

References g2o::VertexSE3::oplusImpl(), and 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();
  }

References setDr(), setProjection(), and setTransform().

Referenced by main(), main(), and oplusImpl().

◆ 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);
  }

References dRdx, dRdy, dRdz, dRidx, dRidy, dRidz, and w2n.

Referenced by setAll().

◆ setKcam()

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;
  }

References baseline, and Kcam.

Referenced by main(), and main().

◆ setProjection()

void g2o::VertexSCam::setProjection ( )

inline

Definition at line 294 of file types_icp.h.

294{ w2i = Kcam * w2n; }

References Kcam, w2i, and w2n.

Referenced by setAll().

◆ 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());
  }

References g2o::BaseVertex< 6, Isometry3 >::estimate(), and w2n.

Referenced by setAll().

◆ transformF2W()

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()

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.

Referenced by mapPoint(), and setKcam().

◆ dRdx

Matrix3 g2o::VertexSCam::dRdx

Definition at line 256 of file types_icp.h.

Referenced by setDr().

◆ dRdy

Matrix3 g2o::VertexSCam::dRdy

Definition at line 256 of file types_icp.h.

Referenced by setDr().

◆ dRdz

Matrix3 g2o::VertexSCam::dRdz

Definition at line 256 of file types_icp.h.

Referenced by setDr().

◆ dRidx

Matrix3 g2o::VertexSCam::dRidx

static

Definition at line 330 of file types_icp.h.

Referenced by g2o::types_icp::init(), and setDr().

◆ dRidy

Matrix3 g2o::VertexSCam::dRidy

static

Definition at line 331 of file types_icp.h.

Referenced by g2o::types_icp::init(), and setDr().

◆ dRidz

Matrix3 g2o::VertexSCam::dRidz

static

Definition at line 332 of file types_icp.h.

Referenced by g2o::types_icp::init(), and setDr().

◆ Kcam

Matrix3 g2o::VertexSCam::Kcam

static

Definition at line 245 of file types_icp.h.

Referenced by mapPoint(), setKcam(), and setProjection().

◆ w2i

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

Definition at line 252 of file types_icp.h.

Referenced by mapPoint(), and setProjection().

◆ w2n

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

Definition at line 250 of file types_icp.h.

Referenced by mapPoint(), setDr(), setProjection(), and setTransform().

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