#include <sbacam.h>

Inheritance diagram for g2o::SBACam:

Collaboration diagram for g2o::SBACam:

Public Member Functions
	SBACam ()

	SBACam (const Quaternion &r_, const Vector3 &t_)

	SBACam (const SE3Quat &p)

void	update (const Vector6 &update)

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

void	setTransform ()

void	setProjection ()

void	setDr ()

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

	SE3Quat (const Matrix3 &R, const Vector3 &t)

	SE3Quat (const Quaternion &q, const Vector3 &t)

template<typename Derived >
	SE3Quat (const Eigen::MatrixBase< Derived > &v)

const Vector3 &	translation () const

void	setTranslation (const Vector3 &t_)

const Quaternion &	rotation () const

void	setRotation (const Quaternion &r_)

SE3Quat	operator* (const SE3Quat &tr2) const

SE3Quat &	operator*= (const SE3Quat &tr2)

Vector3	operator* (const Vector3 &v) const

SE3Quat	inverse () const

double	operator[] (int i) const

Vector7	toVector () const

void	fromVector (const Vector7 &v)

Vector6	toMinimalVector () const

void	fromMinimalVector (const Vector6 &v)

Vector6	log () const

Vector3	map (const Vector3 &xyz) const

Eigen::Matrix< double, 6, 6, Eigen::ColMajor >	adj () const

Eigen::Matrix< double, 4, 4, Eigen::ColMajor >	to_homogeneous_matrix () const

void	normalizeRotation ()

	operator Isometry3 () const

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

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

Static Public Member Functions inherited from g2o::SE3Quat
static SE3Quat	exp (const Vector6 &update)

Public Attributes
Matrix3	Kcam

double	baseline

Eigen::Matrix< double, 3, 4 >	w2n

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

Matrix3	dRdx

Matrix3	dRdy

Matrix3	dRdz

Public Attributes inherited from g2o::SE3Quat
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Additional Inherited Members
Protected Attributes inherited from g2o::SE3Quat
Quaternion	_r

Vector3	_t

Detailed Description

Definition at line 46 of file sbacam.h.

Constructor & Destructor Documentation

◆ SBACam() [1/3]

g2o::SBACam::SBACam ( )

Definition at line 38 of file sbacam.cpp.

               {
  setKcam(1, 1, cst(0.5), cst(0.5), 0);  // unit image projection
}

References g2o::cst(), and setKcam().

◆ SBACam() [2/3]

g2o::SBACam::SBACam	(	const Quaternion &	r_,
		const Vector3 &	t_
	)

Definition at line 43 of file sbacam.cpp.

                                                      : SE3Quat(r_, t_) {
  Kcam.setZero();
  setTransform();
  setProjection();
  setDr();
}

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

◆ SBACam() [3/3]

g2o::SBACam::SBACam ( const SE3Quat & p )

Definition at line 50 of file sbacam.cpp.

                               : SE3Quat(p) {
  Kcam.setZero();
  setTransform();
  setProjection();
  setDr();
}

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

Member Function Documentation

◆ setDr()

void g2o::SBACam::setDr ( )

Definition at line 106 of file sbacam.cpp.

                   {
  // inefficient, just for testing
  // use simple multiplications and additions for production code in calculating
  // dRdx,y,z
  Matrix3 dRidx, dRidy, dRidz;
  dRidx << cst(0.0), cst(0.0), cst(0.0), cst(0.0), cst(0.0), cst(2.0), cst(0.0),
      cst(-2.0), cst(0.0);
  dRidy << cst(0.0), cst(0.0), cst(-2.0), cst(0.0), cst(0.0), cst(0.0),
      cst(2.0), cst(0.0), cst(0.0);
  dRidz << cst(0.0), cst(2.0), cst(0.0), cst(-2.0), cst(0.0), cst(0.0),
      cst(0.0), cst(0.0), cst(0.0);
 
  // 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 g2o::cst(), dRdx, dRdy, dRdz, and w2n.

Referenced by SBACam(), SBACam(), setKcam(), and update().

◆ setKcam()

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

Definition at line 92 of file sbacam.cpp.

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

References baseline, g2o::cst(), Kcam, setDr(), setProjection(), and setTransform().

Referenced by g2o::VertexCam::read(), and SBACam().

◆ setProjection()

void g2o::SBACam::setProjection ( )

inline

Definition at line 89 of file sbacam.h.

89{ w2i = Kcam * w2n; }

g2o::SBACam::w2i

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

Definition sbacam.h:56

Referenced by SBACam(), SBACam(), setKcam(), and update().

◆ setTransform()

void g2o::SBACam::setTransform ( )

inline

Definition at line 85 of file sbacam.h.

85{ transformW2F(w2n, _t, _r); }

g2o::SBACam::transformW2F

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

Definition sbacam.cpp:75

g2o::SE3Quat::_r

Quaternion _r

Definition se3quat.h:44

g2o::SE3Quat::_t

Vector3 _t

Definition se3quat.h:45

Referenced by SBACam(), SBACam(), setKcam(), and update().

◆ transformF2W()

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

static

Definition at line 85 of file sbacam.cpp.

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

◆ transformW2F()

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

static

Definition at line 75 of file sbacam.cpp.

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

◆ update()

void g2o::SBACam::update ( const Vector6 & update )

Definition at line 59 of file sbacam.cpp.

                                         {
  // position update
  _t += update.head(3);
  // small quaternion update
  Quaternion qr;
  qr.vec() = update.segment<3>(3);
  qr.w() =
      sqrt(cst(1.0) - qr.vec().squaredNorm());  // should always be positive
  _r *= qr;                                     // post-multiply
  _r.normalize();
  setTransform();
  setProjection();
  setDr();
}

References g2o::SE3Quat::_r, g2o::SE3Quat::_t, g2o::cst(), setDr(), setProjection(), setTransform(), and update().

Referenced by update().

Member Data Documentation

◆ baseline

double g2o::SBACam::baseline

Definition at line 52 of file sbacam.h.

Referenced by g2o::EdgeProjectP2SC::computeError(), g2o::EdgeProjectP2SC::linearizeOplus(), main(), setKcam(), and g2o::VertexCam::write().

◆ dRdx

Matrix3 g2o::SBACam::dRdx

Definition at line 60 of file sbacam.h.

Referenced by g2o::EdgeProjectP2MC::linearizeOplus(), g2o::EdgeProjectP2SC::linearizeOplus(), and setDr().

◆ dRdy

Matrix3 g2o::SBACam::dRdy

Definition at line 60 of file sbacam.h.

Referenced by g2o::EdgeProjectP2MC::linearizeOplus(), g2o::EdgeProjectP2SC::linearizeOplus(), and setDr().

◆ dRdz

Matrix3 g2o::SBACam::dRdz

Definition at line 60 of file sbacam.h.

Referenced by g2o::EdgeProjectP2MC::linearizeOplus(), g2o::EdgeProjectP2SC::linearizeOplus(), and setDr().

◆ Kcam

Matrix3 g2o::SBACam::Kcam

Definition at line 51 of file sbacam.h.

Referenced by g2o::EdgeProjectP2SC::computeError(), g2o::EdgeProjectP2MC::linearizeOplus(), g2o::EdgeProjectP2SC::linearizeOplus(), main(), g2o::operator<<(), SBACam(), SBACam(), setKcam(), and g2o::VertexCam::write().

◆ w2i

Eigen::Matrix<double, 3, 4> g2o::SBACam::w2i

Definition at line 56 of file sbacam.h.

Referenced by g2o::EdgeProjectP2MC::computeError(), g2o::EdgeProjectP2SC::computeError(), and g2o::operator<<().

◆ w2n

Eigen::Matrix<double, 3, 4> g2o::SBACam::w2n

Definition at line 55 of file sbacam.h.

Referenced by g2o::EdgeProjectP2SC::computeError(), g2o::EdgeProjectP2MC::linearizeOplus(), g2o::EdgeProjectP2SC::linearizeOplus(), g2o::operator<<(), and setDr().

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

/build/reproducible-path/g2o-0~20230806/g2o/types/sba/sbacam.h
/build/reproducible-path/g2o-0~20230806/g2o/types/sba/sbacam.cpp

Public Member Functions

Static Public Member Functions

Public Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ SBACam() [1/3]

◆ SBACam() [2/3]

◆ SBACam() [3/3]

Member Function Documentation

◆ setDr()

◆ setKcam()

◆ setProjection()

◆ setTransform()

◆ transformF2W()

◆ transformW2F()

◆ update()

Member Data Documentation

◆ baseline

◆ dRdx

◆ dRdy

◆ dRdz

◆ Kcam

◆ w2i

◆ w2n