g2o::internal Namespace Reference

Classes
struct	BaseEdgeTraits
struct	BaseEdgeTraits<-1 >
struct	QuadraticFormLock
struct	TrivialPair

Functions
template<int K>
std::array< bool, K >	createBoolArray ()
template<>
std::array< bool, 0 >	createBoolArray< 0 > ()
constexpr int	pair_to_index (const int i, const int j)
constexpr TrivialPair	index_to_pair (const int k, const int j=0)
template<typename T >
T	createHessianMapK ()
	helper function to call the c'tor of Eigen::Map
template<typename... Args>
std::tuple< Args... >	createHessianMaps (const std::tuple< Args... > &)
	helper function for creating a tuple of Eigen::Map
template<int I, typename EdgeType , typename... CtorArgs>
std::enable_if< I==-1, OptimizableGraph::Vertex * >::type	createNthVertexType (int, const EdgeType &, CtorArgs...)
template<int I, typename EdgeType , typename... CtorArgs>
std::enable_if< I!=-1, OptimizableGraph::Vertex * >::type	createNthVertexType (int i, const EdgeType &t, CtorArgs... args)
template<typename Derived >
bool	writeVector (std::ostream &os, const Eigen::DenseBase< Derived > &b)
template<typename Derived >
bool	readVector (std::istream &is, Eigen::DenseBase< Derived > &b)
template<typename MatrixType >
void	axpy (const MatrixType &A, const Eigen::Map< const VectorX > &x, int xoff, Eigen::Map< VectorX > &y, int yoff)
template<int t>
void	axpy (const Eigen::Matrix< double, Eigen::Dynamic, t > &A, const Eigen::Map< const VectorX > &x, int xoff, Eigen::Map< VectorX > &y, int yoff)
template<>
void	axpy< MatrixX > (const MatrixX &A, const Eigen::Map< const VectorX > &x, int xoff, Eigen::Map< VectorX > &y, int yoff)
template<typename MatrixType >
void	atxpy (const MatrixType &A, const Eigen::Map< const VectorX > &x, int xoff, Eigen::Map< VectorX > &y, int yoff)
template<int t>
void	atxpy (const Eigen::Matrix< double, Eigen::Dynamic, t > &A, const Eigen::Map< const VectorX > &x, int xoff, Eigen::Map< VectorX > &y, int yoff)
template<>
void	atxpy< MatrixX > (const MatrixX &A, const Eigen::Map< const VectorX > &x, int xoff, Eigen::Map< VectorX > &y, int yoff)
Vector3	invert_depth (const Vector3 &x)
Eigen::Matrix< double, 2, 3, Eigen::ColMajor >	d_proj_d_y (const double &f, const Vector3 &xyz)
Eigen::Matrix< double, 3, 6, Eigen::ColMajor >	d_expy_d_y (const Vector3 &y)
Matrix3	d_Tinvpsi_d_psi (const SE3Quat &T, const Vector3 &psi)
int	_q2m (double &S, double &qw, const double &r00, const double &r10, const double &r20, const double &r01, const double &r11, const double &r21, const double &r02, const double &r12, const double &r22)
void	compute_dq_dR (Eigen::Matrix< double, 3, 9, Eigen::ColMajor > &dq_dR, const double &r11, const double &r21, const double &r31, const double &r12, const double &r22, const double &r32, const double &r13, const double &r23, const double &r33)
void G2O_TYPES_SLAM3D_API	compute_dR_dq (Eigen::Matrix< double, 9, 3, Eigen::ColMajor > &dR_dq, const double &qx, const double &qy, const double &qz, const double &qw)
template<typename Derived , typename DerivedOther , bool transposed = false>
void	skew (Eigen::MatrixBase< Derived > &s, const Eigen::MatrixBase< DerivedOther > &v)
template<typename Derived , typename DerivedOther >
void	skewT (Eigen::MatrixBase< Derived > &s, const Eigen::MatrixBase< DerivedOther > &v)
template<typename Derived , typename DerivedOther , bool transposed = false>
void	skew (Eigen::MatrixBase< Derived > &Sx, Eigen::MatrixBase< Derived > &Sy, Eigen::MatrixBase< Derived > &Sz, const Eigen::MatrixBase< DerivedOther > &R)
template<typename Derived , typename DerivedOther >
void	skewT (Eigen::MatrixBase< Derived > &Sx, Eigen::MatrixBase< Derived > &Sy, Eigen::MatrixBase< Derived > &Sz, const Eigen::MatrixBase< DerivedOther > &R)
template<typename Derived >
void	computeEdgeSE3Gradient (Isometry3 &E, Eigen::MatrixBase< Derived > const &JiConstRef, Eigen::MatrixBase< Derived > const &JjConstRef, const Isometry3 &Z, const Isometry3 &Xi, const Isometry3 &Xj, const Isometry3 &Pi, const Isometry3 &Pj)
template<typename Derived >
void	computeEdgeSE3Gradient (Isometry3 &E, Eigen::MatrixBase< Derived > const &JiConstRef, Eigen::MatrixBase< Derived > const &JjConstRef, const Isometry3 &Z, const Isometry3 &Xi, const Isometry3 &Xj)
template<typename Derived >
void	computeEdgeSE3PriorGradient (Isometry3 &E, const Eigen::MatrixBase< Derived > &JConstRef, const Isometry3 &Z, const Isometry3 &X, const Isometry3 &P=Isometry3::Identity())
Quaternion	normalized (const Quaternion &q)
Quaternion &	normalize (Quaternion &q)
Vector3	toEuler (const Matrix3 &R)
Matrix3	fromEuler (const Vector3 &v)
Vector3	toCompactQuaternion (const Matrix3 &R)
Matrix3	fromCompactQuaternion (const Vector3 &v)
Vector6	toVectorMQT (const Isometry3 &t)
Vector6	toVectorET (const Isometry3 &t)
Vector7	toVectorQT (const Isometry3 &t)
Isometry3	fromVectorMQT (const Vector6 &v)
Isometry3	fromVectorET (const Vector6 &v)
Isometry3	fromVectorQT (const Vector7 &v)
SE3Quat	toSE3Quat (const Isometry3 &t)
Isometry3	fromSE3Quat (const SE3Quat &t)
Isometry3::ConstLinearPart	extractRotation (const Isometry3 &A)
template<typename Derived >
void	nearestOrthogonalMatrix (const Eigen::MatrixBase< Derived > &R)
template<typename Derived >
void	approximateNearestOrthogonalMatrix (const Eigen::MatrixBase< Derived > &R)
Vector6	transformCartesianLine (const Isometry3 &t, const Vector6 &line)
Vector6	normalizeCartesianLine (const Vector6 &line)
static double	mline_elevation (const double v[3])
G2O_TYPES_SLAM3D_ADDONS_API double	getAzimuth (const Vector3 &direction)
G2O_TYPES_SLAM3D_ADDONS_API double	getElevation (const Vector3 &direction)

Detailed Description

internal functions used inside g2o. Those functions may disappear or change their meaning without further notification

Function Documentation

_q2m()

int g2o::internal::_q2m	(	double &	S,
		double &	qw,
		const double &	r00,
		const double &	r10,
		const double &	r20,
		const double &	r01,
		const double &	r11,
		const double &	r21,
		const double &	r02,
		const double &	r12,
		const double &	r22
	)

Definition at line 36 of file dquat2mat.cpp.

                            {
  double tr = r00 + r11 + r22;
  if (tr > 0) {
    S = sqrt(tr + 1.0) * 2;  // S=4*qw
    qw = 0.25 * S;
    // qx = (r21 - r12) / S;
    // qy = (r02 - r20) / S;
    // qz = (r10 - r01) / S;
    return 0;
  } else if ((r00 > r11) & (r00 > r22)) {
    S = sqrt(1.0 + r00 - r11 - r22) * 2;  // S=4*qx
    qw = (r21 - r12) / S;
    // qx = 0.25 * S;
    // qy = (r01 + r10) / S;
    // qz = (r02 + r20) / S;
    return 1;
  } else if (r11 > r22) {
    S = sqrt(1.0 + r11 - r00 - r22) * 2;  // S=4*qy
    qw = (r02 - r20) / S;
    // qx = (r01 + r10) / S;
    // qy = 0.25 * S;
    return 2;
  } else {
    S = sqrt(1.0 + r22 - r00 - r11) * 2;  // S=4*qz
    qw = (r10 - r01) / S;
    // qx = (r02 + r20) / S;
    // qy = (r12 + r21) / S;
    // qz = 0.25 * S;
    return 3;
  }
}

Referenced by compute_dq_dR().

approximateNearestOrthogonalMatrix()

template<typename Derived >

void g2o::internal::approximateNearestOrthogonalMatrix

(

const Eigen::MatrixBase< Derived > &

R

)

compute a fast approximation for the nearest orthogonal rotation matrix. The function computes the residual E = RR^T - I which is then used as follows: R := R - 1/2 R E

Definition at line 81 of file isometry3d_mappings.h.

                                                                           {
  Matrix3 E = R.transpose() * R;
  E.diagonal().array() -= 1;
  const_cast<Eigen::MatrixBase<Derived>&>(R) -= 0.5 * R * E;
}

atxpy() [1/2]

template<int t>

void g2o::internal::atxpy ( const Eigen::Matrix< double, Eigen::Dynamic, t > &  A, const Eigen::Map< const VectorX > &  x, int  xoff, Eigen::Map< VectorX > &  y, int  yoff  )

inline

Definition at line 68 of file matrix_operations.h.

                                                  {
  y.segment<Eigen::Matrix<double, Eigen::Dynamic, t>::ColsAtCompileTime>(
      yoff) += A.transpose() * x.segment(xoff, A.rows());
}

atxpy() [2/2]

template<typename MatrixType >

void g2o::internal::atxpy ( const MatrixType &  A, const Eigen::Map< const VectorX > &  x, int  xoff, Eigen::Map< VectorX > &  y, int  yoff  )

inline

Definition at line 61 of file matrix_operations.h.

                                                            {
  y.segment<MatrixType::ColsAtCompileTime>(yoff) +=
      A.transpose() * x.segment<MatrixType::RowsAtCompileTime>(xoff);
}

atxpy< MatrixX >()

template<>

void g2o::internal::atxpy< MatrixX > ( const MatrixX &  A, const Eigen::Map< const VectorX > &  x, int  xoff, Eigen::Map< VectorX > &  y, int  yoff  )

inline

Definition at line 76 of file matrix_operations.h.

                                                                     {
  y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows());
}

axpy() [1/2]

template<int t>

void g2o::internal::axpy ( const Eigen::Matrix< double, Eigen::Dynamic, t > &  A, const Eigen::Map< const VectorX > &  x, int  xoff, Eigen::Map< VectorX > &  y, int  yoff  )

inline

Definition at line 45 of file matrix_operations.h.

                                                 {
  y.segment(yoff, A.rows()) +=
      A *
      x.segment<Eigen::Matrix<double, Eigen::Dynamic, t>::ColsAtCompileTime>(
          xoff);
}

axpy() [2/2]

template<typename MatrixType >

void g2o::internal::axpy ( const MatrixType &  A, const Eigen::Map< const VectorX > &  x, int  xoff, Eigen::Map< VectorX > &  y, int  yoff  )

inline

Definition at line 38 of file matrix_operations.h.

                                                           {
  y.segment<MatrixType::RowsAtCompileTime>(yoff) +=
      A * x.segment<MatrixType::ColsAtCompileTime>(xoff);
}

axpy< MatrixX >()

template<>

void g2o::internal::axpy< MatrixX > ( const MatrixX &  A, const Eigen::Map< const VectorX > &  x, int  xoff, Eigen::Map< VectorX > &  y, int  yoff  )

inline

Definition at line 55 of file matrix_operations.h.

                                                                    {
  y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols());
}

compute_dq_dR()

void G2O_TYPES_SLAM3D_API g2o::internal::compute_dq_dR	(	Eigen::Matrix< double, 3, 9, Eigen::ColMajor > &	dq_dR,
		const double &	r11,
		const double &	r21,
		const double &	r31,
		const double &	r12,
		const double &	r22,
		const double &	r32,
		const double &	r13,
		const double &	r23,
		const double &	r33
	)

Definition at line 71 of file dquat2mat.cpp.

                                                                            {
  double qw;
  double S;
  int whichCase = _q2m(S, qw, r11, r21, r31, r12, r22, r32, r13, r23, r33);
  S *= .25;
  switch (whichCase) {
    case 0:
      compute_dq_dR_w(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
      break;
    case 1:
      compute_dq_dR_x(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
      break;
    case 2:
      compute_dq_dR_y(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
      break;
    case 3:
      compute_dq_dR_z(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
      break;
  }
  if (qw <= 0) dq_dR *= -1;
}

References _q2m(), compute_dq_dR_w(), compute_dq_dR_x(), compute_dq_dR_y(), and compute_dq_dR_z().

Referenced by computeEdgeSE3Gradient(), computeEdgeSE3Gradient(), and computeEdgeSE3PriorGradient().

compute_dR_dq()

void G2O_TYPES_SLAM3D_API g2o::internal::compute_dR_dq	(	Eigen::Matrix< double, 9, 3, Eigen::ColMajor > &	dR_dq,
		const double &	qx,
		const double &	qy,
		const double &	qz,
		const double &	qw
	)

computeEdgeSE3Gradient() [1/2]

template<typename Derived >

void g2o::internal::computeEdgeSE3Gradient	(	Isometry3 &	E,
		Eigen::MatrixBase< Derived > const &	JiConstRef,
		Eigen::MatrixBase< Derived > const &	JjConstRef,
		const Isometry3 &	Z,
		const Isometry3 &	Xi,
		const Isometry3 &	Xj
	)

Definition at line 206 of file isometry3d_gradients.h.

                                                 {
  Eigen::MatrixBase<Derived>& Ji =
      const_cast<Eigen::MatrixBase<Derived>&>(JiConstRef);
  Eigen::MatrixBase<Derived>& Jj =
      const_cast<Eigen::MatrixBase<Derived>&>(JjConstRef);
  Ji.derived().resize(6, 6);
  Jj.derived().resize(6, 6);
  // compute the error at the linearization point
  const Isometry3 A = Z.inverse();
  const Isometry3 B = Xi.inverse() * Xj;
 
  E = A * B;
 
  Isometry3::ConstLinearPart Re = extractRotation(E);
  Isometry3::ConstLinearPart Ra = extractRotation(A);
  Isometry3::ConstLinearPart Rb = extractRotation(B);
  Isometry3::ConstTranslationPart tb = B.translation();
 
  Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
  compute_dq_dR(dq_dR, Re(0, 0), Re(1, 0), Re(2, 0), Re(0, 1), Re(1, 1),
                Re(2, 1), Re(0, 2), Re(1, 2), Re(2, 2));
 
  Ji.setZero();
  Jj.setZero();
 
  // dte/dti
  Ji.template block<3, 3>(0, 0) = -Ra;
 
  // dte/dtj
  Jj.template block<3, 3>(0, 0) = Re;
 
  // dte/dqi
  {
    Matrix3 S;
    skewT(S, tb);
    Ji.template block<3, 3>(0, 3) = Ra * S;
  }
 
  // dte/dqj: this is zero
 
  double buf[27];
  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
  Matrix3 Sxt, Syt, Szt;
  // dre/dqi
  {
    skewT(Sxt, Syt, Szt, Rb);
    Eigen::Map<Matrix3> Mx(buf);
    Mx.noalias() = Ra * Sxt;
    Eigen::Map<Matrix3> My(buf + 9);
    My.noalias() = Ra * Syt;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz.noalias() = Ra * Szt;
    Ji.template block<3, 3>(3, 3) = dq_dR * M;
  }
 
  // dre/dqj
  {
    Matrix3& Sx = Sxt;
    Matrix3& Sy = Syt;
    Matrix3& Sz = Szt;
    skew(Sx, Sy, Sz, Matrix3::Identity());
    Eigen::Map<Matrix3> Mx(buf);
    Mx.noalias() = Re * Sx;
    Eigen::Map<Matrix3> My(buf + 9);
    My.noalias() = Re * Sy;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz.noalias() = Re * Sz;
    Jj.template block<3, 3>(3, 3) = dq_dR * M;
  }
}

References compute_dq_dR(), extractRotation(), skew(), and skewT().

computeEdgeSE3Gradient() [2/2]

template<typename Derived >

void g2o::internal::computeEdgeSE3Gradient	(	Isometry3 &	E,
		Eigen::MatrixBase< Derived > const &	JiConstRef,
		Eigen::MatrixBase< Derived > const &	JjConstRef,
		const Isometry3 &	Z,
		const Isometry3 &	Xi,
		const Isometry3 &	Xj,
		const Isometry3 &	Pi,
		const Isometry3 &	Pj
	)

Definition at line 89 of file isometry3d_gradients.h.

                                                   {
  Eigen::MatrixBase<Derived>& Ji =
      const_cast<Eigen::MatrixBase<Derived>&>(JiConstRef);
  Eigen::MatrixBase<Derived>& Jj =
      const_cast<Eigen::MatrixBase<Derived>&>(JjConstRef);
  Ji.derived().resize(6, 6);
  Jj.derived().resize(6, 6);
  // compute the error at the linearization point
  const Isometry3 A = Z.inverse() * Pi.inverse();
  const Isometry3 B = Xi.inverse() * Xj;
  const Isometry3& C = Pj;
 
  const Isometry3 AB = A * B;
  const Isometry3 BC = B * C;
  E = AB * C;
 
  Isometry3::ConstLinearPart Re = extractRotation(E);
  Isometry3::ConstLinearPart Ra = extractRotation(A);
  // const Matrix3 Rb = extractRotation(B);
  Isometry3::ConstLinearPart Rc = extractRotation(C);
  Isometry3::ConstTranslationPart tc = C.translation();
  // Isometry3::ConstTranslationParttab=AB.translation();
  Isometry3::ConstLinearPart Rab = extractRotation(AB);
  Isometry3::ConstTranslationPart tbc = BC.translation();
  Isometry3::ConstLinearPart Rbc = extractRotation(BC);
 
  Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
  compute_dq_dR(dq_dR, Re(0, 0), Re(1, 0), Re(2, 0), Re(0, 1), Re(1, 1),
                Re(2, 1), Re(0, 2), Re(1, 2), Re(2, 2));
 
  Ji.setZero();
  Jj.setZero();
 
  // dte/dti
  Ji.template block<3, 3>(0, 0) = -Ra;
 
  // dte/dtj
  Jj.template block<3, 3>(0, 0) = Rab;
 
  // dte/dqi
  {
    Matrix3 S;
    skewT(S, tbc);
    Ji.template block<3, 3>(0, 3) = Ra * S;
  }
 
  // dte/dqj
  {
    Matrix3 S;
    skew(S, tc);
    Jj.template block<3, 3>(0, 3) = Rab * S;
  }
 
  // dre/dqi
  {
    double buf[27];
    Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
    Matrix3 Sxt, Syt, Szt;
    internal::skewT(Sxt, Syt, Szt, Rbc);
#ifdef __clang__
    Matrix3 temp = Rab * Sxt;
    Eigen::Map<Matrix3> M2(temp.data());
    Eigen::Map<Matrix3> Mx(buf);
    Mx = M2;
    temp = Ra * Syt;
    Eigen::Map<Matrix3> My(buf + 9);
    My = M2;
    temp = Ra * Szt;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz = M2;
#else
    Eigen::Map<Matrix3> Mx(buf);
    Mx = Ra * Sxt;
    Eigen::Map<Matrix3> My(buf + 9);
    My = Ra * Syt;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz = Ra * Szt;
#endif
    Ji.template block<3, 3>(3, 3) = dq_dR * M;
  }
 
  // dre/dqj
  {
    double buf[27];
    Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
    Matrix3 Sx, Sy, Sz;
    internal::skew(Sx, Sy, Sz, Rc);
#ifdef __clang__
    Matrix3 temp = Rab * Sx;
    Eigen::Map<Matrix3> M2(temp.data());
    Eigen::Map<Matrix3> Mx(buf);
    Mx = M2;
    temp = Rab * Sy;
    Eigen::Map<Matrix3> My(buf + 9);
    My = M2;
    temp = Rab * Sz;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz = M2;
#else
    Eigen::Map<Matrix3> Mx(buf);
    Mx = Rab * Sx;
    Eigen::Map<Matrix3> My(buf + 9);
    My = Rab * Sy;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz = Rab * Sz;
#endif
    Jj.template block<3, 3>(3, 3) = dq_dR * M;
  }
}

References compute_dq_dR(), extractRotation(), skew(), and skewT().

Referenced by g2o::EdgeSE3::linearizeOplus(), and g2o::EdgeSE3Offset::linearizeOplus().

computeEdgeSE3PriorGradient()

template<typename Derived >

void g2o::internal::computeEdgeSE3PriorGradient	(	Isometry3 &	E,
		const Eigen::MatrixBase< Derived > &	JConstRef,
		const Isometry3 &	Z,
		const Isometry3 &	X,
		const Isometry3 &	P = Isometry3::Identity()
	)

Definition at line 282 of file isometry3d_gradients.h.

                                                                           {
  Eigen::MatrixBase<Derived>& J =
      const_cast<Eigen::MatrixBase<Derived>&>(JConstRef);
  J.derived().resize(6, 6);
  // compute the error at the linearization point
  const Isometry3 A = Z.inverse() * X;
  const Isometry3& B = P;
  Isometry3::ConstLinearPart Ra = extractRotation(A);
  Isometry3::ConstLinearPart Rb = extractRotation(B);
  Isometry3::ConstTranslationPart tb = B.translation();
  E = A * B;
  Isometry3::ConstLinearPart Re = extractRotation(E);
 
  Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
  compute_dq_dR(dq_dR, Re(0, 0), Re(1, 0), Re(2, 0), Re(0, 1), Re(1, 1),
                Re(2, 1), Re(0, 2), Re(1, 2), Re(2, 2));
 
  J.setZero();
 
  // dte/dt
  J.template block<3, 3>(0, 0) = Ra;
 
  // dte/dq =0
  // dte/dqj
  {
    Matrix3 S;
    skew(S, tb);
    J.template block<3, 3>(0, 3) = Ra * S;
  }
 
  // dre/dt =0
 
  // dre/dq
  {
    double buf[27];
    Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
    Matrix3 Sx, Sy, Sz;
    internal::skew(Sx, Sy, Sz, Rb);
#ifdef __clang__
    Matrix3 temp = Ra * Sx;
    Eigen::Map<Matrix3> M2(temp.data());
    Eigen::Map<Matrix3> Mx(buf);
    Mx = M2;
    temp = Ra * Sy;
    Eigen::Map<Matrix3> My(buf + 9);
    My = M2;
    temp = Ra * Sz;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz = M2;
#else
    Eigen::Map<Matrix3> Mx(buf);
    Mx = Ra * Sx;
    Eigen::Map<Matrix3> My(buf + 9);
    My = Ra * Sy;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz = Ra * Sz;
#endif
    J.template block<3, 3>(3, 3) = dq_dR * M;
  }
}

References compute_dq_dR(), extractRotation(), and skew().

Referenced by g2o::EdgeSE3Prior::linearizeOplus().

createBoolArray()

template<int K>

std::array< bool, K > g2o::internal::createBoolArray

(

)

Definition at line 48 of file base_fixed_sized_edge.h.

                                    {
  std::array<bool, K> aux = {false};
  return aux;
}

createBoolArray< 0 >()

template<>

std::array< bool, 0 > g2o::internal::createBoolArray< 0 > ( )

inline

Definition at line 48 of file base_fixed_sized_edge.h.

                                              {
  return std::array<bool, 0>();
}

createHessianMapK()

template<typename T >

T g2o::internal::createHessianMapK

(

)

helper function to call the c'tor of Eigen::Map

Definition at line 92 of file base_fixed_sized_edge.h.

                      {
  // if the size is known at compile time, we have to call the c'tor of
  // Eigen::Map with corresponding values
  constexpr int r =
      T::RowsAtCompileTime == Eigen::Dynamic ? 0 : T::RowsAtCompileTime;
  constexpr int c =
      T::ColsAtCompileTime == Eigen::Dynamic ? 0 : T::ColsAtCompileTime;
  return T(nullptr, r, c);
}

createHessianMaps()

template<typename... Args>

std::tuple< Args... > g2o::internal::createHessianMaps

(

const std::tuple< Args... > &

)

helper function for creating a tuple of Eigen::Map

Definition at line 103 of file base_fixed_sized_edge.h.

                                                              {
  return std::tuple<Args...>{createHessianMapK<Args>()...};
}

createNthVertexType() [1/2]

template<int I, typename EdgeType , typename... CtorArgs>

std::enable_if< I!=-1, OptimizableGraph::Vertex * >::type g2o::internal::createNthVertexType	(	int	i,
		const EdgeType &	t,
		CtorArgs...	args
	)

Definition at line 115 of file base_fixed_sized_edge.h.

                                                                {
  if (i == I) {
    using VertexType = typename EdgeType::template VertexXnType<I>;
    return new VertexType(args...);
  }
  return createNthVertexType<I - 1, EdgeType, CtorArgs...>(i, t, args...);
}

References createNthVertexType().

createNthVertexType() [2/2]

template<int I, typename EdgeType , typename... CtorArgs>

std::enable_if< I==-1, OptimizableGraph::Vertex * >::type g2o::internal::createNthVertexType	(	int	,
		const EdgeType &	,
		CtorArgs...
	)

Definition at line 109 of file base_fixed_sized_edge.h.

                                                             {
  return nullptr;
}

Referenced by createNthVertexType(), and g2o::BaseFixedSizedEdge< D, E, VertexTypes >::createVertex().

d_expy_d_y()

Eigen::Matrix< double, 3, 6, Eigen::ColMajor > g2o::internal::d_expy_d_y ( const Vector3 &  y )

inline

Definition at line 50 of file sba_utils.h.

                      {
  Eigen::Matrix<double, 3, 6, Eigen::ColMajor> J;
  J.topLeftCorner<3, 3>() = -skew(y);
  J.bottomRightCorner<3, 3>().setIdentity();
 
  return J;
}

References skew().

Referenced by g2o::EdgeProjectPSI2UV::linearizeOplus().

d_proj_d_y()

Eigen::Matrix< double, 2, 3, Eigen::ColMajor > g2o::internal::d_proj_d_y ( const double &  f, const Vector3 &  xyz  )

inline

Definition at line 42 of file sba_utils.h.

                                         {
  double z_sq = xyz[2] * xyz[2];
  Eigen::Matrix<double, 2, 3, Eigen::ColMajor> J;
  J << f / xyz[2], 0, -(f * xyz[0]) / z_sq, 0, f / xyz[2], -(f * xyz[1]) / z_sq;
  return J;
}

Referenced by g2o::EdgeProjectPSI2UV::linearizeOplus().

d_Tinvpsi_d_psi()

Matrix3 g2o::internal::d_Tinvpsi_d_psi ( const SE3Quat &  T, const Vector3 &  psi  )

inline

Definition at line 59 of file sba_utils.h.

                                                                     {
  Matrix3 R = T.rotation().toRotationMatrix();
  Vector3 x = invert_depth(psi);
  Vector3 r1 = R.col(0);
  Vector3 r2 = R.col(1);
  Matrix3 J;
  J.col(0) = r1;
  J.col(1) = r2;
  J.col(2) = -R * x;
  J *= 1. / psi.z();
  return J;
}

References invert_depth(), and g2o::SE3Quat::rotation().

Referenced by g2o::EdgeProjectPSI2UV::linearizeOplus().

extractRotation()

Isometry3::ConstLinearPart g2o::internal::extractRotation ( const Isometry3 &  A )

inline

extract the rotation matrix from an Isometry3 matrix. Eigen itself performs an SVD decomposition to recover the nearest orthogonal matrix, since its rotation() function also handles a scaling matrix. An Isometry3 does not have a scaling portion and we assume that the Isometry3 is numerically stable while we compute the error and the Jacobians. Hence, we directly extract the rotation block out of the full matrix.

Note, we could also call .linear() on the Isometry3. However, I dislike the name of that function a bit.

Definition at line 55 of file isometry3d_mappings.h.

                                                                    {
  return A.matrix().topLeftCorner<3, 3>();
}

Referenced by computeEdgeSE3Gradient(), computeEdgeSE3Gradient(), computeEdgeSE3PriorGradient(), g2o::EdgeSE3Prior::initialEstimate(), toVectorET(), toVectorMQT(), and toVectorQT().

fromCompactQuaternion()

G2O_TYPES_SLAM3D_API Matrix3 g2o::internal::fromCompactQuaternion

(

const Vector3 &

v

)

(qx qy, qz) -> Rotation matrix, whereas (qx, qy, qz) are assumed to be part of a quaternion which was normalized with the function above.

Definition at line 87 of file isometry3d_mappings.cpp.

                                                {
  double w = 1 - v.squaredNorm();
  if (w < 0)
    return Matrix3::Identity();
  else
    w = sqrt(w);
  return Quaternion(w, v[0], v[1], v[2]).toRotationMatrix();
}

Referenced by fromVectorMQT().

fromEuler()

G2O_TYPES_SLAM3D_API Matrix3 g2o::internal::fromEuler

(

const Vector3 &

v

)

Euler angles (roll, pitch, yaw) -> Rotation matrix

Definition at line 62 of file isometry3d_mappings.cpp.

                                    {
  // UNOPTIMIZED
  double roll = v[0];
  double pitch = v[1];
  double yaw = v[2];
  double sy = std::sin(yaw * cst(0.5));
  double cy = std::cos(yaw * cst(0.5));
  double sp = std::sin(pitch * cst(0.5));
  double cp = std::cos(pitch * cst(0.5));
  double sr = std::sin(roll * cst(0.5));
  double cr = std::cos(roll * cst(0.5));
  double w = cr * cp * cy + sr * sp * sy;
  double x = sr * cp * cy - cr * sp * sy;
  double y = cr * sp * cy + sr * cp * sy;
  double z = cr * cp * sy - sr * sp * cy;
  return Quaternion(w, x, y, z).toRotationMatrix();
}

References g2o::cst().

Referenced by fromVectorET().

fromSE3Quat()

G2O_TYPES_SLAM3D_API Isometry3 g2o::internal::fromSE3Quat

(

const SE3Quat &

t

)

convert from an old SE3Quat into Isometry3

Definition at line 149 of file isometry3d_mappings.cpp.

                                        {
  Isometry3 result = (Isometry3)t.rotation();
  result.translation() = t.translation();
  return result;
}

References g2o::SE3Quat::rotation(), and g2o::SE3Quat::translation().

fromVectorET()

G2O_TYPES_SLAM3D_API Isometry3 g2o::internal::fromVectorET

(

const Vector6 &

v

)

(x, y, z, roll, pitch, yaw) -> Isometry3

Definition at line 130 of file isometry3d_mappings.cpp.

                                         {
  Isometry3 t;
  t = fromEuler(v.block<3, 1>(3, 0));
  t.translation() = v.block<3, 1>(0, 0);
  return t;
}

References fromEuler().

Referenced by g2o::G2oSlamInterface::addEdge(), g2o::EdgeSE3Euler::read(), and g2o::VertexSE3Euler::read().

fromVectorMQT()

G2O_TYPES_SLAM3D_API Isometry3 g2o::internal::fromVectorMQT

(

const Vector6 &

v

)

(x, y, z, qx, qy, qz) -> Isometry3

Definition at line 123 of file isometry3d_mappings.cpp.

                                          {
  Isometry3 t;
  t = fromCompactQuaternion(v.block<3, 1>(3, 0));
  t.translation() = v.block<3, 1>(0, 0);
  return t;
}

References fromCompactQuaternion().

Referenced by g2o::SensorOdometry3D::addNoise(), g2o::SensorPose3D::addNoise(), g2o::SensorPose3DOffset::addNoise(), and g2o::SensorSE3Prior::addNoise().

fromVectorQT()

G2O_TYPES_SLAM3D_API Isometry3 g2o::internal::fromVectorQT

(

const Vector7 &

v

)

(x, y, z, qx, qy, qz, qw) -> Isometry3

Definition at line 137 of file isometry3d_mappings.cpp.

                                         {
  Isometry3 t;
  t = Quaternion(v[6], v[3], v[4], v[5]).toRotationMatrix();
  t.translation() = v.head<3>();
  return t;
}

Referenced by g2o::G2oSlamInterface::addEdge(), g2o::jac_quat3_euler3(), g2o::EdgeSE3::read(), g2o::EdgeSE3Offset::read(), g2o::EdgeSE3Prior::read(), g2o::ParameterCamera::read(), g2o::ParameterSE3Offset::read(), g2o::VertexSE3::read(), and g2o::EdgeSE3Calib::read().

getAzimuth()

G2O_TYPES_SLAM3D_ADDONS_API double g2o::internal::getAzimuth ( const Vector3 &  direction )

inline

Definition at line 191 of file line3d.h.

                                                                               {
  return std::atan2(direction.y(), direction.x());
}

Referenced by main().

getElevation()

G2O_TYPES_SLAM3D_ADDONS_API double g2o::internal::getElevation ( const Vector3 &  direction )

inline

Definition at line 195 of file line3d.h.

                              {
  return std::atan2(direction.z(), direction.head<2>().norm());
}

Referenced by main().

index_to_pair()

constexpr TrivialPair g2o::internal::index_to_pair ( const int  k, const int  j = 0  )

constexpr

If we would have a constexpr for sqrt (cst_sqrt) it would be sth like. For now we implement as a recursive function at compile time. constexpr TrivialPair index_to_pair(n) { constexpr int j = int(0.5 + cst_sqrt(0.25 + 2 * n)); constexpr int base = pair_to_index(0, j); constexpr int i = n - base; return TrivialPair(i, j); }

Definition at line 86 of file base_fixed_sized_edge.h.

                                                                  {
  return k < j ? TrivialPair{k, j} : index_to_pair(k - j, j + 1);
}

References index_to_pair().

Referenced by index_to_pair().

invert_depth()

Vector3 g2o::internal::invert_depth ( const Vector3 &  x )

inline

Definition at line 37 of file sba_utils.h.

                                              {
  Vector2 aux = x.head<2>();
  return unproject(aux) / x[2];
}

References g2o::unproject().

Referenced by g2o::EdgeProjectPSI2UV::computeError(), d_Tinvpsi_d_psi(), and g2o::EdgeProjectPSI2UV::linearizeOplus().

mline_elevation()

static double g2o::internal::mline_elevation ( const double  v[3] )

inlinestatic

Definition at line 187 of file line3d.h.

                                                        {
  return std::atan2(v[2], sqrt(v[0] * v[0] + v[1] * v[1]));
}

nearestOrthogonalMatrix()

template<typename Derived >

void g2o::internal::nearestOrthogonalMatrix

(

const Eigen::MatrixBase< Derived > &

R

)

computes the nearest orthogonal matrix of a rotation matrix which might be affected by numerical inaccuracies. We periodically call this function after performinag a large number of updates on vertices. This function computes an SVD to reconstruct the nearest orthogonal matrix.

Definition at line 66 of file isometry3d_mappings.h.

                                                                {
  Eigen::JacobiSVD<Matrix3> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV);
  double det = (svd.matrixU() * svd.matrixV().adjoint()).determinant();
  Matrix3 scaledU(svd.matrixU());
  scaledU.col(0) /= det;
  const_cast<Eigen::MatrixBase<Derived>&>(R) =
      scaledU * svd.matrixV().transpose();
}

normalize()

G2O_TYPES_SLAM3D_API Quaternion & g2o::internal::normalize

(

Quaternion &

q

)

as above, but in-place

Definition at line 40 of file isometry3d_mappings.cpp.

                                     {
  q.normalize();
  if (q.w() < 0) {
    q.coeffs() *= -1;
  }
  return q;
}

Referenced by normalized(), and toCompactQuaternion().

normalizeCartesianLine()

G2O_TYPES_SLAM3D_ADDONS_API Vector6 g2o::internal::normalizeCartesianLine

(

const Vector6 &

line

)

Definition at line 68 of file line3d.cpp.

                                                    {
  Vector3 p0 = line.head<3>();
  Vector3 d0 = line.tail<3>();
  d0.normalize();
  p0 -= d0 * (d0.dot(p0));
  Vector6 nl;
  nl.head<3>() = p0;
  nl.tail<3>() = d0;
  return nl;
}

Referenced by transformCartesianLine().

normalized()

G2O_TYPES_SLAM3D_API Quaternion g2o::internal::normalized

(

const Quaternion &

q

)

normalize the quaternion, such that ||q|| == 1 and q.w() > 0

Definition at line 34 of file isometry3d_mappings.cpp.

                                           {
  Quaternion q2(q);
  normalize(q2);
  return q2;
}

References normalize().

pair_to_index()

constexpr int g2o::internal::pair_to_index ( const int  i, const int  j  )

constexpr

Definition at line 60 of file base_fixed_sized_edge.h.

                                                      {
  return j * (j - 1) / 2 + i;
}

readVector()

template<typename Derived >

bool g2o::internal::readVector	(	std::istream &	is,
		Eigen::DenseBase< Derived > &	b
	)

Definition at line 42 of file io_helper.h.

                                                            {
  for (int i = 0; i < b.size() && is.good(); i++) is >> b(i);
  return is.good() || is.eof();
}

Referenced by PolynomialCoefficientVertex::read(), FPolynomialCoefficientVertex::read(), PPolynomialCoefficientVertex::read(), MultipleValueEdge::read(), g2o::EdgeProjectP2MC::read(), g2o::EdgeProjectP2SC::read(), g2o::EdgeProjectPSI2UV::read(), g2o::EdgeStereoSE3ProjectXYZ::read(), g2o::EdgeStereoSE3ProjectXYZOnlyPose::read(), g2o::EdgeSE3ProjectXYZ::read(), g2o::EdgeProjectXYZ2UV::read(), g2o::EdgeProjectXYZ2UVU::read(), g2o::EdgeSE3ProjectXYZOnlyPose::read(), g2o::EdgeSBACam::read(), g2o::EdgeSE3Expmap::read(), g2o::VertexCam::read(), g2o::VertexIntrinsics::read(), g2o::VertexSE3Expmap::read(), g2o::EdgeSE2SensorCalib::read(), g2o::VertexOdomDifferentialParams::read(), g2o::VertexSim3Expmap::read(), g2o::EdgeSim3::read(), g2o::EdgeSim3ProjectXYZ::read(), g2o::EdgeInverseSim3ProjectXYZ::read(), g2o::EdgePointXY::read(), g2o::EdgeSE2::read(), g2o::EdgeSE2Offset::read(), g2o::EdgeSE2PointXY::read(), g2o::EdgeSE2PointXYCalib::read(), g2o::EdgeSE2PointXYOffset::read(), g2o::EdgeSE2Prior::read(), g2o::EdgeSE2XYPrior::read(), g2o::EdgeXYPrior::read(), g2o::ParameterSE2Offset::read(), g2o::VertexPointXY::read(), g2o::VertexSE2::read(), g2o::EdgeLine2D::read(), g2o::EdgeSE2Line2D::read(), g2o::EdgeSE2Segment2D::read(), g2o::EdgeSE2Segment2DLine::read(), g2o::EdgeSE2Segment2DPointLine::read(), g2o::VertexSegment2D::read(), g2o::EdgeSE3::read(), g2o::EdgeSE3Offset::read(), g2o::EdgeSE3PointXYZ::read(), g2o::EdgeSE3PointXYZDepth::read(), g2o::EdgeSE3PointXYZDisparity::read(), g2o::EdgeSE3Prior::read(), g2o::EdgeSE3XYZPrior::read(), g2o::EdgeXYZPrior::read(), g2o::ParameterCamera::read(), g2o::ParameterSE3Offset::read(), g2o::VertexPointXYZ::read(), g2o::VertexSE3::read(), g2o::EdgePlane::read(), g2o::EdgeSE3Calib::read(), g2o::EdgeSE3Line3D::read(), g2o::EdgeSE3PlaneSensorCalib::read(), g2o::VertexLine3D::read(), g2o::VertexPlane::read(), and g2o::VertexSE3Euler::read().

skew() [1/2]

template<typename Derived , typename DerivedOther , bool transposed = false>

void g2o::internal::skew ( Eigen::MatrixBase< Derived > &  s, const Eigen::MatrixBase< DerivedOther > &  v  )

inline

Definition at line 45 of file isometry3d_gradients.h.

                                                         {
  const double x = 2 * v(0);
  const double y = 2 * v(1);
  const double z = 2 * v(2);
  if (transposed)
    s << 0., -z, y, z, 0, -x, -y, x, 0;
  else
    s << 0., z, -y, -z, 0, x, y, -x, 0;
}

Referenced by computeEdgeSE3Gradient(), computeEdgeSE3Gradient(), computeEdgeSE3PriorGradient(), and d_expy_d_y().

skew() [2/2]

template<typename Derived , typename DerivedOther , bool transposed = false>

void g2o::internal::skew	(	Eigen::MatrixBase< Derived > &	Sx,
		Eigen::MatrixBase< Derived > &	Sy,
		Eigen::MatrixBase< Derived > &	Sz,
		const Eigen::MatrixBase< DerivedOther > &	R
	)

Definition at line 63 of file isometry3d_gradients.h.

                                                  {
  const double r11 = 2 * R(0, 0), r12 = 2 * R(0, 1), r13 = 2 * R(0, 2),
               r21 = 2 * R(1, 0), r22 = 2 * R(1, 1), r23 = 2 * R(1, 2),
               r31 = 2 * R(2, 0), r32 = 2 * R(2, 1), r33 = 2 * R(2, 2);
  if (transposed) {
    Sx << 0, 0, 0, r31, r32, r33, -r21, -r22, -r23;
    Sy << -r31, -r32, -r33, 0, 0, 0, r11, r12, r13;
    Sz << r21, r22, r23, -r11, -r12, -r13, 0, 0, 0;
  } else {
    Sx << 0, 0, 0, -r31, -r32, -r33, r21, r22, r23;
    Sy << r31, r32, r33, 0, 0, 0, -r11, -r12, -r13;
    Sz << -r21, -r22, -r23, r11, r12, r13, 0, 0, 0;
  }
}

skewT() [1/2]

template<typename Derived , typename DerivedOther >

void g2o::internal::skewT ( Eigen::MatrixBase< Derived > &  s, const Eigen::MatrixBase< DerivedOther > &  v  )

inline

Definition at line 57 of file isometry3d_gradients.h.

                                                          {
  skew<Derived, DerivedOther, true>(s, v);
}

Referenced by computeEdgeSE3Gradient(), and computeEdgeSE3Gradient().

skewT() [2/2]

template<typename Derived , typename DerivedOther >

void g2o::internal::skewT ( Eigen::MatrixBase< Derived > &  Sx, Eigen::MatrixBase< Derived > &  Sy, Eigen::MatrixBase< Derived > &  Sz, const Eigen::MatrixBase< DerivedOther > &  R  )

inline

Definition at line 81 of file isometry3d_gradients.h.

                                                          {
  skew<Derived, DerivedOther, true>(Sx, Sy, Sz, R);
}

toCompactQuaternion()

G2O_TYPES_SLAM3D_API Vector3 g2o::internal::toCompactQuaternion

(

const Matrix3 &

R

)

Rotation matrix -> (qx qy, qz)

Definition at line 80 of file isometry3d_mappings.cpp.

                                              {
  Quaternion q(R);
  normalize(q);
  // return (x,y,z) of the quaternion
  return q.coeffs().head<3>();
}

References normalize().

Referenced by toVectorMQT().

toEuler()

G2O_TYPES_SLAM3D_API Vector3 g2o::internal::toEuler

(

const Matrix3 &

R

)

Rotation matrix -> Euler angles (roll, pitch, yaw)

Definition at line 49 of file isometry3d_mappings.cpp.

                                  {
  Quaternion q(R);
  const double& q0 = q.w();
  const double& q1 = q.x();
  const double& q2 = q.y();
  const double& q3 = q.z();
  double roll =
      std::atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
  double pitch = std::asin(2 * (q0 * q2 - q3 * q1));
  double yaw = std::atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
  return Vector3(roll, pitch, yaw);
}

Referenced by g2o::G2oSlamInterface::printVertex(), and toVectorET().

toSE3Quat()

G2O_TYPES_SLAM3D_API SE3Quat g2o::internal::toSE3Quat

(

const Isometry3 &

t

)

convert an Isometry3 to the old SE3Quat class

Definition at line 144 of file isometry3d_mappings.cpp.

                                      {
  SE3Quat result(t.matrix().topLeftCorner<3, 3>(), t.translation());
  return result;
}

toVectorET()

G2O_TYPES_SLAM3D_API Vector6 g2o::internal::toVectorET

(

const Isometry3 &

t

)

Isometry3 -> (x, y, z, roll, pitch, yaw)

Definition at line 104 of file isometry3d_mappings.cpp.

                                       {
  Vector6 v;
  v.block<3, 1>(3, 0) = toEuler(extractRotation(t));
  v.block<3, 1>(0, 0) = t.translation();
  return v;
}

References extractRotation(), and toEuler().

Referenced by g2o::jac_quat3_euler3(), g2o::EdgeSE3Euler::write(), and g2o::VertexSE3Euler::write().

toVectorMQT()

G2O_TYPES_SLAM3D_API Vector6 g2o::internal::toVectorMQT

(

const Isometry3 &

t

)

Isometry3 -> (x, y, z, qx, qy, qz)

Definition at line 97 of file isometry3d_mappings.cpp.

                                        {
  Vector6 v;
  v.block<3, 1>(3, 0) = toCompactQuaternion(extractRotation(t));
  v.block<3, 1>(0, 0) = t.translation();
  return v;
}

References extractRotation(), and toCompactQuaternion().

Referenced by g2o::EdgeSE3::computeError(), g2o::EdgeSE3Offset::computeError(), g2o::EdgeSE3Prior::computeError(), g2o::EdgeSE3Calib::computeError(), g2o::EdgeSE3WriteGnuplotAction::operator()(), and g2o::VertexSE3WriteGnuplotAction::operator()().

toVectorQT()

G2O_TYPES_SLAM3D_API Vector7 g2o::internal::toVectorQT

(

const Isometry3 &

t

)

Isometry3 -> (x, y, z, qx, qy, qz, qw)

Definition at line 111 of file isometry3d_mappings.cpp.

                                       {
  Quaternion q(extractRotation(t));
  q.normalize();
  Vector7 v;
  v[3] = q.x();
  v[4] = q.y();
  v[5] = q.z();
  v[6] = q.w();
  v.block<3, 1>(0, 0) = t.translation();
  return v;
}

References extractRotation().

Referenced by g2o::jac_quat3_euler3(), g2o::EdgeSE3::write(), g2o::EdgeSE3Offset::write(), g2o::EdgeSE3Prior::write(), g2o::ParameterCamera::write(), g2o::ParameterSE3Offset::write(), g2o::VertexSE3::write(), and g2o::EdgeSE3Calib::write().

transformCartesianLine()

G2O_TYPES_SLAM3D_ADDONS_API Vector6 g2o::internal::transformCartesianLine	(	const Isometry3 &	t,
		const Vector6 &	line
	)

Definition at line 61 of file line3d.cpp.

                                                                        {
  Vector6 l;
  l.head<3>() = t * line.head<3>();
  l.tail<3>() = t.linear() * line.tail<3>();
  return normalizeCartesianLine(l);
}

References normalizeCartesianLine().

writeVector()

template<typename Derived >

bool g2o::internal::writeVector	(	std::ostream &	os,
		const Eigen::DenseBase< Derived > &	b
	)

Definition at line 36 of file io_helper.h.

                                                                   {
  for (int i = 0; i < b.size(); i++) os << b(i) << " ";
  return os.good();
}

Referenced by PolynomialCoefficientVertex::write(), FPolynomialCoefficientVertex::write(), PPolynomialCoefficientVertex::write(), MultipleValueEdge::write(), g2o::EdgeProjectP2MC::write(), g2o::EdgeProjectP2SC::write(), g2o::EdgeProjectPSI2UV::write(), g2o::EdgeStereoSE3ProjectXYZ::write(), g2o::EdgeStereoSE3ProjectXYZOnlyPose::write(), g2o::EdgeSE3ProjectXYZ::write(), g2o::EdgeProjectXYZ2UV::write(), g2o::EdgeProjectXYZ2UVU::write(), g2o::EdgeSE3ProjectXYZOnlyPose::write(), g2o::EdgeSBACam::write(), g2o::EdgeSE3Expmap::write(), g2o::VertexCam::write(), g2o::VertexIntrinsics::write(), g2o::VertexSE3Expmap::write(), g2o::EdgeSE2SensorCalib::write(), g2o::VertexOdomDifferentialParams::write(), g2o::VertexSim3Expmap::write(), g2o::EdgeSim3::write(), g2o::EdgeSim3ProjectXYZ::write(), g2o::EdgeInverseSim3ProjectXYZ::write(), g2o::EdgePointXY::write(), g2o::EdgeSE2::write(), g2o::EdgeSE2Offset::write(), g2o::EdgeSE2PointXY::write(), g2o::EdgeSE2PointXYCalib::write(), g2o::EdgeSE2PointXYOffset::write(), g2o::EdgeSE2Prior::write(), g2o::EdgeSE2XYPrior::write(), g2o::EdgeXYPrior::write(), g2o::ParameterSE2Offset::write(), g2o::VertexPointXY::write(), g2o::VertexSE2::write(), g2o::EdgeLine2D::write(), g2o::EdgeSE2Line2D::write(), g2o::EdgeSE2Segment2D::write(), g2o::EdgeSE2Segment2DLine::write(), g2o::EdgeSE2Segment2DPointLine::write(), g2o::VertexSegment2D::write(), g2o::EdgeSE3::write(), g2o::EdgeSE3Offset::write(), g2o::EdgeSE3PointXYZ::write(), g2o::EdgeSE3PointXYZDepth::write(), g2o::EdgeSE3PointXYZDisparity::write(), g2o::EdgeSE3Prior::write(), g2o::EdgeSE3XYZPrior::write(), g2o::EdgeXYZPrior::write(), g2o::ParameterCamera::write(), g2o::ParameterSE3Offset::write(), g2o::VertexPointXYZ::write(), g2o::VertexSE3::write(), g2o::EdgePlane::write(), g2o::EdgeSE3Calib::write(), g2o::EdgeSE3Line3D::write(), g2o::EdgeSE3PlaneSensorCalib::write(), g2o::VertexLine3D::write(), g2o::VertexPlane::write(), and g2o::VertexSE3Euler::write().