g2o
Namespaces | Functions
g2o::internal Namespace Reference

Namespaces

 g2o
 

Functions

int computeUpperTriangleIndex (int i, int j)
 
template<typename MatrixType >
void axpy (const MatrixType &A, const Eigen::Map< const Eigen::VectorXd > &x, int xoff, Eigen::Map< Eigen::VectorXd > &y, int yoff)
 
template<int t>
void axpy (const Eigen::Matrix< double, Eigen::Dynamic, t > &A, const Eigen::Map< const Eigen::VectorXd > &x, int xoff, Eigen::Map< Eigen::VectorXd > &y, int yoff)
 
template<>
void axpy< Eigen::MatrixXd > (const Eigen::MatrixXd &A, const Eigen::Map< const Eigen::VectorXd > &x, int xoff, Eigen::Map< Eigen::VectorXd > &y, int yoff)
 
template<typename MatrixType >
void atxpy (const MatrixType &A, const Eigen::Map< const Eigen::VectorXd > &x, int xoff, Eigen::Map< Eigen::VectorXd > &y, int yoff)
 
template<int t>
void atxpy (const Eigen::Matrix< double, Eigen::Dynamic, t > &A, const Eigen::Map< const Eigen::VectorXd > &x, int xoff, Eigen::Map< Eigen::VectorXd > &y, int yoff)
 
template<>
void atxpy< Eigen::MatrixXd > (const Eigen::MatrixXd &A, const Eigen::Map< const Eigen::VectorXd > &x, int xoff, Eigen::Map< Eigen::VectorXd > &y, int yoff)
 
template<typename MatrixType >
void pcg_axy (const MatrixType &A, const VectorXD &x, int xoff, VectorXD &y, int yoff)
 
template<>
void pcg_axy (const MatrixXD &A, const VectorXD &x, int xoff, VectorXD &y, int yoff)
 
template<typename MatrixType >
void pcg_axpy (const MatrixType &A, const VectorXD &x, int xoff, VectorXD &y, int yoff)
 
template<>
void pcg_axpy (const MatrixXD &A, const VectorXD &x, int xoff, VectorXD &y, int yoff)
 
template<typename MatrixType >
void pcg_atxpy (const MatrixType &A, const VectorXD &x, int xoff, VectorXD &y, int yoff)
 
template<>
void pcg_atxpy (const MatrixXD &A, const VectorXD &x, int xoff, VectorXD &y, int yoff)
 
void compute_dq_dR_w (Eigen::Matrix< double, 3, 9 > &dq_dR_w, const 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_x (Eigen::Matrix< double, 3, 9 > &dq_dR_x, const double &qx, 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_y (Eigen::Matrix< double, 3, 9 > &dq_dR_y, const double &qy, 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_z (Eigen::Matrix< double, 3, 9 > &dq_dR_z, const double &qz, 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_dR_dq (Eigen::Matrix< double, 9, 3 > &dR_dq, const double &qx, const double &qy, const double &qz, const double &qw)
 
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 >
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 >
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 (Isometry3D &E, Eigen::MatrixBase< Derived > const &JiConstRef, Eigen::MatrixBase< Derived > const &JjConstRef, const Isometry3D &Z, const Isometry3D &Xi, const Isometry3D &Xj, const Isometry3D &Pi, const Isometry3D &Pj)
 
template<typename Derived >
void computeEdgeSE3Gradient (Isometry3D &E, Eigen::MatrixBase< Derived > const &JiConstRef, Eigen::MatrixBase< Derived > const &JjConstRef, const Isometry3D &Z, const Isometry3D &Xi, const Isometry3D &Xj)
 
template<typename Derived >
void computeEdgeSE3PriorGradient (Isometry3D &E, const Eigen::MatrixBase< Derived > &JConstRef, const Isometry3D &Z, const Isometry3D &X, const Isometry3D &P=Isometry3D())
 
Eigen::Quaterniond normalized (const Eigen::Quaterniond &q)
 
Eigen::Quaterniond & normalize (Eigen::Quaterniond &q)
 
Vector3D toEuler (const Matrix3D &R)
 
Matrix3D fromEuler (const Vector3D &v)
 
Vector3D toCompactQuaternion (const Matrix3D &R)
 
Matrix3D fromCompactQuaternion (const Vector3D &v)
 
Vector6d toVectorMQT (const Isometry3D &t)
 
Vector6d toVectorET (const Isometry3D &t)
 
Vector7d toVectorQT (const Isometry3D &t)
 
Isometry3D fromVectorMQT (const Vector6d &v)
 
Isometry3D fromVectorET (const Vector6d &v)
 
Isometry3D fromVectorQT (const Vector7d &v)
 
SE3Quat toSE3Quat (const Isometry3D &t)
 
Isometry3D fromSE3Quat (const SE3Quat &t)
 
Isometry3D::ConstLinearPart extractRotation (const Isometry3D &A)
 
template<typename Derived >
void nearestOrthogonalMatrix (const Eigen::MatrixBase< Derived > &R)
 
template<typename Derived >
void approximateNearestOrthogonalMatrix (const Eigen::MatrixBase< Derived > &R)
 
Vector6d transformCartesianLine (const Isometry3D &t, const Vector6d &line)
 
Vector6d normalizeCartesianLine (const Vector6d &line)
 

Detailed Description

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

Function Documentation

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 9 of file dquat2mat.cpp.

Referenced by compute_dq_dR().

9  {
10  double tr=r00 + r11 + r22;
11  if (tr > 0) {
12  S = sqrt(tr + 1.0) * 2; // S=4*qw
13  qw = 0.25 * S;
14  // qx = (r21 - r12) / S;
15  // qy = (r02 - r20) / S;
16  // qz = (r10 - r01) / S;
17  return 0;
18  } else if ((r00 > r11)&(r00 > r22)) {
19  S = sqrt(1.0 + r00 - r11 - r22) * 2; // S=4*qx
20  qw = (r21 - r12) / S;
21  // qx = 0.25 * S;
22  // qy = (r01 + r10) / S;
23  // qz = (r02 + r20) / S;
24  return 1;
25  } else if (r11 > r22) {
26  S = sqrt(1.0 + r11 - r00 - r22) * 2; // S=4*qy
27  qw = (r02 - r20) / S;
28  // qx = (r01 + r10) / S;
29  // qy = 0.25 * S;
30  // qz = (r12 + r21) / S;
31  return 2;
32  } else {
33  S = sqrt(1.0 + r22 - r00 - r11) * 2; // S=4*qz
34  qw = (r10 - r01) / S;
35  // qx = (r02 + r20) / S;
36  // qy = (r12 + r21) / S;
37  // qz = 0.25 * S;
38  return 3;
39  }
40  }
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 85 of file isometry3d_mappings.h.

References fromCompactQuaternion(), fromEuler(), fromSE3Quat(), fromVectorET(), fromVectorMQT(), fromVectorQT(), G2O_TYPES_SLAM3D_API, normalize(), normalized(), toCompactQuaternion(), toEuler(), toSE3Quat(), toVectorET(), toVectorMQT(), and toVectorQT().

Referenced by main(), and g2o::VertexSE3::oplusImpl().

86  {
87  Matrix3D E = R.transpose() * R;
88  E.diagonal().array() -= 1;
89  const_cast<Eigen::MatrixBase<Derived>&>(R) -= 0.5 * R * E;
90  }
g2o::Matrix3D
Eigen::Matrix< double, 3, 3, Eigen::ColMajor > Matrix3D
Definition: eigen_types.h:61
template<typename MatrixType >
void g2o::internal::atxpy ( const MatrixType &  A,
const Eigen::Map< const Eigen::VectorXd > &  x,
int  xoff,
Eigen::Map< Eigen::VectorXd > &  y,
int  yoff 
)
inline

Definition at line 54 of file matrix_operations.h.

55  {
56  y.segment<MatrixType::ColsAtCompileTime>(yoff) += A.transpose() * x.segment<MatrixType::RowsAtCompileTime>(xoff);
57  }
template<int t>
void g2o::internal::atxpy ( const Eigen::Matrix< double, Eigen::Dynamic, t > &  A,
const Eigen::Map< const Eigen::VectorXd > &  x,
int  xoff,
Eigen::Map< Eigen::VectorXd > &  y,
int  yoff 
)
inline

Definition at line 60 of file matrix_operations.h.

61  {
62  y.segment<Eigen::Matrix<double, Eigen::Dynamic, t>::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows());
63  }
template<>
void g2o::internal::atxpy< Eigen::MatrixXd > ( const Eigen::MatrixXd &  A,
const Eigen::Map< const Eigen::VectorXd > &  x,
int  xoff,
Eigen::Map< Eigen::VectorXd > &  y,
int  yoff 
)
inline

Definition at line 66 of file matrix_operations.h.

67  {
68  y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows());
69  }
template<typename MatrixType >
void g2o::internal::axpy ( const MatrixType &  A,
const Eigen::Map< const Eigen::VectorXd > &  x,
int  xoff,
Eigen::Map< Eigen::VectorXd > &  y,
int  yoff 
)
inline

Definition at line 36 of file matrix_operations.h.

37  {
38  y.segment<MatrixType::RowsAtCompileTime>(yoff) += A * x.segment<MatrixType::ColsAtCompileTime>(xoff);
39  }
template<int t>
void g2o::internal::axpy ( const Eigen::Matrix< double, Eigen::Dynamic, t > &  A,
const Eigen::Map< const Eigen::VectorXd > &  x,
int  xoff,
Eigen::Map< Eigen::VectorXd > &  y,
int  yoff 
)
inline

Definition at line 42 of file matrix_operations.h.

43  {
44  y.segment(yoff, A.rows()) += A * x.segment<Eigen::Matrix<double, Eigen::Dynamic, t>::ColsAtCompileTime>(xoff);
45  }
template<>
void g2o::internal::axpy< Eigen::MatrixXd > ( const Eigen::MatrixXd &  A,
const Eigen::Map< const Eigen::VectorXd > &  x,
int  xoff,
Eigen::Map< Eigen::VectorXd > &  y,
int  yoff 
)
inline

Definition at line 48 of file matrix_operations.h.

49  {
50  y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols());
51  }
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 42 of file dquat2mat.cpp.

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

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

42  {
43  double qw;
44  double S;
45  int whichCase=_q2m( S, qw, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 );
46  S*=.25;
47  switch(whichCase){
48  case 0: compute_dq_dR_w(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 );
49  break;
50  case 1: compute_dq_dR_x(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 );
51  break;
52  case 2: compute_dq_dR_y(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 );
53  break;
54  case 3: compute_dq_dR_z(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 );
55  break;
56  }
57  if (qw<=0)
58  dq_dR *= -1;
59  }
g2o::internal::_q2m
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)
Definition: dquat2mat.cpp:9
compute_dq_dR_z
void compute_dq_dR_z(Eigen::Matrix< double, 3, 9 > &dq_dR_z, const double &qz, 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: dquat2mat_maxima_generated.cpp:124
compute_dq_dR_x
void compute_dq_dR_x(Eigen::Matrix< double, 3, 9 > &dq_dR_x, const double &qx, 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: dquat2mat_maxima_generated.cpp:40
compute_dq_dR_y
void compute_dq_dR_y(Eigen::Matrix< double, 3, 9 > &dq_dR_y, const double &qy, 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: dquat2mat_maxima_generated.cpp:82
compute_dq_dR_w
void compute_dq_dR_w(Eigen::Matrix< double, 3, 9 > &dq_dR_w, const 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: dquat2mat_maxima_generated.cpp:1
void g2o::internal::compute_dq_dR_w ( Eigen::Matrix< double, 3, 9 > &  dq_dR_w,
const 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 2 of file dquat2mat.cpp.

Referenced by compute_dq_dR().

3  {
4  namespace internal {
5  using namespace std;
6 
7 #include "dquat2mat_maxima_generated.cpp"
8 
9  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 ){
10  double tr=r00 + r11 + r22;
11  if (tr > 0) {
12  S = sqrt(tr + 1.0) * 2; // S=4*qw
13  qw = 0.25 * S;
14  // qx = (r21 - r12) / S;
15  // qy = (r02 - r20) / S;
16  // qz = (r10 - r01) / S;
17  return 0;
18  } else if ((r00 > r11)&(r00 > r22)) {
19  S = sqrt(1.0 + r00 - r11 - r22) * 2; // S=4*qx
20  qw = (r21 - r12) / S;
21  // qx = 0.25 * S;
22  // qy = (r01 + r10) / S;
23  // qz = (r02 + r20) / S;
24  return 1;
25  } else if (r11 > r22) {
26  S = sqrt(1.0 + r11 - r00 - r22) * 2; // S=4*qy
27  qw = (r02 - r20) / S;
28  // qx = (r01 + r10) / S;
29  // qy = 0.25 * S;
30  // qz = (r12 + r21) / S;
31  return 2;
32  } else {
33  S = sqrt(1.0 + r22 - r00 - r11) * 2; // S=4*qz
34  qw = (r10 - r01) / S;
35  // qx = (r02 + r20) / S;
36  // qy = (r12 + r21) / S;
37  // qz = 0.25 * S;
38  return 3;
39  }
40  }
dquat2mat_maxima_generated.cpp
g2o::internal::_q2m
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)
Definition: dquat2mat.cpp:9
void g2o::internal::compute_dq_dR_x ( Eigen::Matrix< double, 3, 9 > &  dq_dR_x,
const double &  qx,
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 41 of file dquat2mat.cpp.

Referenced by compute_dq_dR().

42  {
43  double qw;
44  double S;
45  int whichCase=_q2m( S, qw, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 );
46  S*=.25;
47  switch(whichCase){
48  case 0: compute_dq_dR_w(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 );
49  break;
50  case 1: compute_dq_dR_x(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 );
51  break;
52  case 2: compute_dq_dR_y(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 );
53  break;
54  case 3: compute_dq_dR_z(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 );
55  break;
56  }
57  if (qw<=0)
58  dq_dR *= -1;
59  }
60  }
61 }
62 
g2o::internal::_q2m
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)
Definition: dquat2mat.cpp:9
compute_dq_dR_z
void compute_dq_dR_z(Eigen::Matrix< double, 3, 9 > &dq_dR_z, const double &qz, 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: dquat2mat_maxima_generated.cpp:124
compute_dq_dR_x
void compute_dq_dR_x(Eigen::Matrix< double, 3, 9 > &dq_dR_x, const double &qx, 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: dquat2mat_maxima_generated.cpp:40
compute_dq_dR_y
void compute_dq_dR_y(Eigen::Matrix< double, 3, 9 > &dq_dR_y, const double &qy, 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: dquat2mat_maxima_generated.cpp:82
compute_dq_dR_w
void compute_dq_dR_w(Eigen::Matrix< double, 3, 9 > &dq_dR_w, const 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: dquat2mat_maxima_generated.cpp:1
void g2o::internal::compute_dq_dR_y ( Eigen::Matrix< double, 3, 9 > &  dq_dR_y,
const double &  qy,
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 83 of file dquat2mat.cpp.

Referenced by compute_dq_dR().

void g2o::internal::compute_dq_dR_z ( Eigen::Matrix< double, 3, 9 > &  dq_dR_z,
const double &  qz,
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 125 of file dquat2mat.cpp.

Referenced by compute_dq_dR().

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 
)
void g2o::internal::compute_dR_dq ( Eigen::Matrix< double, 9, 3 > &  dR_dq,
const double &  qx,
const double &  qy,
const double &  qz,
const double &  qw 
)

Definition at line 167 of file dquat2mat.cpp.

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

Definition at line 87 of file isometry3d_gradients.h.

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

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

95  {
96  Eigen::MatrixBase<Derived>& Ji = const_cast<Eigen::MatrixBase<Derived>&>(JiConstRef);
97  Eigen::MatrixBase<Derived>& Jj = const_cast<Eigen::MatrixBase<Derived>&>(JjConstRef);
98  Ji.derived().resize(6,6);
99  Jj.derived().resize(6,6);
100  // compute the error at the linearization point
101  const Isometry3D A=Z.inverse()*Pi.inverse();
102  const Isometry3D B=Xi.inverse()*Xj;
103  const Isometry3D& C=Pj;
104 
105  const Isometry3D AB=A*B;
106  const Isometry3D BC=B*C;
107  E=AB*C;
108 
109  Isometry3D::ConstLinearPart Re = extractRotation(E);
110  Isometry3D::ConstLinearPart Ra = extractRotation(A);
111  //const Matrix3D Rb = extractRotation(B);
112  Isometry3D::ConstLinearPart Rc = extractRotation(C);
113  Isometry3D::ConstTranslationPart tc = C.translation();
114  //Isometry3D::ConstTranslationParttab=AB.translation();
115  Isometry3D::ConstLinearPart Rab = extractRotation(AB);
116  Isometry3D::ConstTranslationPart tbc = BC.translation();
117  Isometry3D::ConstLinearPart Rbc = extractRotation(BC);
118 
119  Eigen::Matrix<double, 3 , 9, Eigen::ColMajor> dq_dR;
120  compute_dq_dR (dq_dR,
121  Re(0,0),Re(1,0),Re(2,0),
122  Re(0,1),Re(1,1),Re(2,1),
123  Re(0,2),Re(1,2),Re(2,2));
124 
125  Ji.setZero();
126  Jj.setZero();
127 
128  // dte/dti
129  Ji.template block<3,3>(0,0)=-Ra;
130 
131  // dte/dtj
132  Jj.template block<3,3>(0,0)=Rab;
133 
134  // dte/dqi
135  {
136  Matrix3D S;
137  skewT(S,tbc);
138  Ji.template block<3,3>(0,3)=Ra*S;
139  }
140 
141  // dte/dqj
142  {
143  Matrix3D S;
144  skew(S,tc);
145  Jj.template block<3,3>(0,3)=Rab*S;
146  }
147 
148  // dre/dqi
149  {
150  double buf[27];
151  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
152  Matrix3D Sxt,Syt,Szt;
153  internal::skewT(Sxt,Syt,Szt,Rbc);
154 #ifdef __clang__
155  Matrix3D temp = Rab * Sxt;
156  Eigen::Map<Matrix3D> M2(temp.data());
157  Eigen::Map<Matrix3D> Mx(buf); Mx = M2;
158  temp = Ra*Syt;
159  Eigen::Map<Matrix3D> My(buf+9); My = M2;
160  temp = Ra*Szt;
161  Eigen::Map<Matrix3D> Mz(buf+18); Mz = M2;
162 #else
163  Eigen::Map<Matrix3D> Mx(buf); Mx = Ra*Sxt;
164  Eigen::Map<Matrix3D> My(buf+9); My = Ra*Syt;
165  Eigen::Map<Matrix3D> Mz(buf+18); Mz = Ra*Szt;
166 #endif
167  Ji.template block<3,3>(3,3) = dq_dR * M;
168  }
169 
170  // dre/dqj
171  {
172  double buf[27];
173  Eigen::Map <Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
174  Matrix3D Sx,Sy,Sz;
175  internal::skew(Sx,Sy,Sz,Rc);
176 #ifdef __clang__
177  Matrix3D temp = Rab * Sx;
178  Eigen::Map<Matrix3D> M2(temp.data());
179  Eigen::Map<Matrix3D> Mx(buf); Mx = M2;
180  temp = Rab*Sy;
181  Eigen::Map<Matrix3D> My(buf+9); My = M2;
182  temp = Rab*Sz;
183  Eigen::Map<Matrix3D> Mz(buf+18); Mz = M2;
184 #else
185  Eigen::Map<Matrix3D> Mx(buf); Mx = Rab*Sx;
186  Eigen::Map<Matrix3D> My(buf+9); My = Rab*Sy;
187  Eigen::Map<Matrix3D> Mz(buf+18); Mz = Rab*Sz;
188 #endif
189  Jj.template block<3,3>(3,3) = dq_dR * M;
190  }
191  }
skew
Matrix3D skew(const Vector3D &v)
Definition: se3_ops.hpp:27
g2o::internal::compute_dq_dR
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)
Definition: dquat2mat.cpp:42
g2o::Isometry3D
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3D
Definition: eigen_types.h:66
g2o::internal::extractRotation
Isometry3D::ConstLinearPart extractRotation(const Isometry3D &A)
Definition: isometry3d_mappings.h:58
g2o::Matrix3D
Eigen::Matrix< double, 3, 3, Eigen::ColMajor > Matrix3D
Definition: eigen_types.h:61
g2o::internal::skewT
void skewT(Eigen::MatrixBase< Derived > &Sx, Eigen::MatrixBase< Derived > &Sy, Eigen::MatrixBase< Derived > &Sz, const Eigen::MatrixBase< DerivedOther > &R)
Definition: isometry3d_gradients.h:73
template<typename Derived >
void g2o::internal::computeEdgeSE3Gradient ( Isometry3D &  E,
Eigen::MatrixBase< Derived > const &  JiConstRef,
Eigen::MatrixBase< Derived > const &  JjConstRef,
const Isometry3D &  Z,
const Isometry3D &  Xi,
const Isometry3D &  Xj 
)

Definition at line 194 of file isometry3d_gradients.h.

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

200  {
201  Eigen::MatrixBase<Derived>& Ji = const_cast<Eigen::MatrixBase<Derived>&>(JiConstRef);
202  Eigen::MatrixBase<Derived>& Jj = const_cast<Eigen::MatrixBase<Derived>&>(JjConstRef);
203  Ji.derived().resize(6,6);
204  Jj.derived().resize(6,6);
205  // compute the error at the linearization point
206  const Isometry3D A=Z.inverse();
207  const Isometry3D B=Xi.inverse()*Xj;
208 
209  E=A*B;
210 
211  Isometry3D::ConstLinearPart Re = extractRotation(E);
212  Isometry3D::ConstLinearPart Ra = extractRotation(A);
213  Isometry3D::ConstLinearPart Rb = extractRotation(B);
214  Isometry3D::ConstTranslationPart tb = B.translation();
215 
216  Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
217  compute_dq_dR (dq_dR,
218  Re(0,0),Re(1,0),Re(2,0),
219  Re(0,1),Re(1,1),Re(2,1),
220  Re(0,2),Re(1,2),Re(2,2));
221 
222  Ji.setZero();
223  Jj.setZero();
224 
225  // dte/dti
226  Ji.template block<3,3>(0,0)=-Ra;
227 
228  // dte/dtj
229  Jj.template block<3,3>(0,0)=Re;
230 
231  // dte/dqi
232  {
233  Matrix3D S;
234  skewT(S,tb);
235  Ji.template block<3,3>(0,3)=Ra*S;
236  }
237 
238  // dte/dqj: this is zero
239 
240  double buf[27];
241  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
242  Matrix3D Sxt,Syt,Szt;
243  // dre/dqi
244  {
245  skewT(Sxt,Syt,Szt,Rb);
246  Eigen::Map<Matrix3D> Mx(buf); Mx.noalias() = Ra*Sxt;
247  Eigen::Map<Matrix3D> My(buf+9); My.noalias() = Ra*Syt;
248  Eigen::Map<Matrix3D> Mz(buf+18); Mz.noalias() = Ra*Szt;
249  Ji.template block<3,3>(3,3) = dq_dR * M;
250  }
251 
252  // dre/dqj
253  {
254  Matrix3D& Sx = Sxt;
255  Matrix3D& Sy = Syt;
256  Matrix3D& Sz = Szt;
257  skew(Sx,Sy,Sz,Matrix3D::Identity());
258  Eigen::Map<Matrix3D> Mx(buf); Mx.noalias() = Re*Sx;
259  Eigen::Map<Matrix3D> My(buf+9); My.noalias() = Re*Sy;
260  Eigen::Map<Matrix3D> Mz(buf+18); Mz.noalias() = Re*Sz;
261  Jj.template block<3,3>(3,3) = dq_dR * M;
262  }
263  }
skew
Matrix3D skew(const Vector3D &v)
Definition: se3_ops.hpp:27
g2o::internal::compute_dq_dR
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)
Definition: dquat2mat.cpp:42
g2o::Isometry3D
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3D
Definition: eigen_types.h:66
g2o::internal::extractRotation
Isometry3D::ConstLinearPart extractRotation(const Isometry3D &A)
Definition: isometry3d_mappings.h:58
g2o::Matrix3D
Eigen::Matrix< double, 3, 3, Eigen::ColMajor > Matrix3D
Definition: eigen_types.h:61
g2o::internal::skewT
void skewT(Eigen::MatrixBase< Derived > &Sx, Eigen::MatrixBase< Derived > &Sy, Eigen::MatrixBase< Derived > &Sz, const Eigen::MatrixBase< DerivedOther > &R)
Definition: isometry3d_gradients.h:73
template<typename Derived >
void g2o::internal::computeEdgeSE3PriorGradient ( Isometry3D &  E,
const Eigen::MatrixBase< Derived > &  JConstRef,
const Isometry3D &  Z,
const Isometry3D &  X,
const Isometry3D &  P = Isometry3D() 
)

Definition at line 267 of file isometry3d_gradients.h.

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

Referenced by g2o::deprecated::EdgeSE3Prior::linearizeOplus(), and g2o::EdgeSE3Prior::linearizeOplus().

272  {
273  Eigen::MatrixBase<Derived>& J = const_cast<Eigen::MatrixBase<Derived>&>(JConstRef);
274  J.derived().resize(6,6);
275  // compute the error at the linearization point
276  const Isometry3D A = Z.inverse()*X;
277  const Isometry3D& B = P;
278  Isometry3D::ConstLinearPart Ra = extractRotation(A);
279  Isometry3D::ConstLinearPart Rb = extractRotation(B);
280  Isometry3D::ConstTranslationPart tb = B.translation();
281  E = A*B;
282  Isometry3D::ConstLinearPart Re = extractRotation(E);
283 
284  Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
285  compute_dq_dR (dq_dR,
286  Re(0,0),Re(1,0),Re(2,0),
287  Re(0,1),Re(1,1),Re(2,1),
288  Re(0,2),Re(1,2),Re(2,2));
289 
290  J.setZero();
291 
292  // dte/dt
293  J.template block<3,3>(0,0)=Ra;
294 
295  // dte/dq =0
296  // dte/dqj
297  {
298  Matrix3D S;
299  skew(S,tb);
300  J.template block<3,3>(0,3)=Ra*S;
301  }
302 
303  // dre/dt =0
304 
305  // dre/dq
306  {
307  double buf[27];
308  Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
309  Matrix3D Sx,Sy,Sz;
310  internal::skew(Sx,Sy,Sz,Rb);
311 #ifdef __clang__
312  Matrix3D temp = Ra * Sx;
313  Eigen::Map<Matrix3D> M2(temp.data());
314  Eigen::Map<Matrix3D> Mx(buf); Mx = M2;
315  temp = Ra*Sy;
316  Eigen::Map<Matrix3D> My(buf+9); My = M2;
317  temp = Ra*Sz;
318  Eigen::Map<Matrix3D> Mz(buf+18); Mz = M2;
319 #else
320  Eigen::Map<Matrix3D> Mx(buf); Mx = Ra*Sx;
321  Eigen::Map<Matrix3D> My(buf+9); My = Ra*Sy;
322  Eigen::Map<Matrix3D> Mz(buf+18); Mz = Ra*Sz;
323 #endif
324  J.template block<3,3>(3,3) = dq_dR * M;
325  }
326 
327  }
skew
Matrix3D skew(const Vector3D &v)
Definition: se3_ops.hpp:27
g2o::internal::compute_dq_dR
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)
Definition: dquat2mat.cpp:42
g2o::Isometry3D
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3D
Definition: eigen_types.h:66
g2o::internal::extractRotation
Isometry3D::ConstLinearPart extractRotation(const Isometry3D &A)
Definition: isometry3d_mappings.h:58
g2o::Matrix3D
Eigen::Matrix< double, 3, 3, Eigen::ColMajor > Matrix3D
Definition: eigen_types.h:61
int g2o::internal::computeUpperTriangleIndex ( int  i,
int  j 
)
inline

Definition at line 29 of file base_multi_edge.h.

40  {
Isometry3D::ConstLinearPart g2o::internal::extractRotation ( const Isometry3D &  A)
inline

extract the rotation matrix from an Isometry3D matrix. Eigen itself performs an SVD decomposition to recover the nearest orthogonal matrix, since its rotation() function also handles a scaling matrix. An Isometry3D does not have a scaling portion and we assume that the Isometry3D 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 Isometry3D. However, I dislike the name of that function a bit.

Definition at line 58 of file isometry3d_mappings.h.

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

59  {
60  return A.matrix().topLeftCorner<3,3>();
61  }
Matrix3D g2o::internal::fromCompactQuaternion ( const Vector3D &  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 84 of file isometry3d_mappings.cpp.

Referenced by approximateNearestOrthogonalMatrix(), fromVectorMQT(), and main().

84  {
85  double w = 1-v.squaredNorm();
86  if (w<0)
87  return Matrix3D::Identity();
88  else
89  w=sqrt(w);
90  return Eigen::Quaterniond(w, v[0], v[1], v[2]).toRotationMatrix();
91  }
Matrix3D g2o::internal::fromEuler ( const Vector3D &  v)

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

Definition at line 59 of file isometry3d_mappings.cpp.

Referenced by approximateNearestOrthogonalMatrix(), fromVectorET(), and main().

59  {
60  //UNOPTIMIZED
61  double roll = v[0];
62  double pitch = v[1];
63  double yaw = v[2];
64  double sy = sin(yaw*0.5);
65  double cy = cos(yaw*0.5);
66  double sp = sin(pitch*0.5);
67  double cp = cos(pitch*0.5);
68  double sr = sin(roll*0.5);
69  double cr = cos(roll*0.5);
70  double w = cr*cp*cy + sr*sp*sy;
71  double x = sr*cp*cy - cr*sp*sy;
72  double y = cr*sp*cy + sr*cp*sy;
73  double z = cr*cp*sy - sr*sp*cy;
74  return Eigen::Quaterniond(w,x,y,z).toRotationMatrix();
75  }
Isometry3D g2o::internal::fromSE3Quat ( const SE3Quat &  t)

convert from an old SE3Quat into Isometry3D

Definition at line 144 of file isometry3d_mappings.cpp.

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

Referenced by approximateNearestOrthogonalMatrix(), and g2o::VertexSE3::G2O_ATTRIBUTE_DEPRECATED().

145  {
146  Isometry3D result = (Isometry3D) t.rotation();
147  result.translation() = t.translation();
148  return result;
149  }
g2o::Isometry3D
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3D
Definition: eigen_types.h:66
Isometry3D g2o::internal::fromVectorET ( const Vector6d &  v)

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

Definition at line 124 of file isometry3d_mappings.cpp.

References fromEuler().

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

124  {
125  Isometry3D t;
126  t = fromEuler(v.block<3,1>(3,0));
127  t.translation() = v.block<3,1>(0,0);
128  return t;
129  }
g2o::Isometry3D
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3D
Definition: eigen_types.h:66
g2o::internal::fromEuler
Matrix3D fromEuler(const Vector3D &v)
Definition: isometry3d_mappings.cpp:59
Isometry3D g2o::internal::fromVectorMQT ( const Vector6d &  v)

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

Definition at line 117 of file isometry3d_mappings.cpp.

References fromCompactQuaternion().

Referenced by g2o::SensorOdometry3D::addNoise(), g2o::SensorPose3DOffset::addNoise(), g2o::SensorPose3D::addNoise(), g2o::SensorSE3Prior::addNoise(), approximateNearestOrthogonalMatrix(), main(), g2o::VertexSE3::oplusImpl(), g2o::OnlineVertexSE3::oplusUpdatedEstimate(), and g2o::VertexSE3::setMinimalEstimateDataImpl().

117  {
118  Isometry3D t;
119  t = fromCompactQuaternion(v.block<3,1>(3,0));
120  t.translation() = v.block<3,1>(0,0);
121  return t;
122  }
g2o::internal::fromCompactQuaternion
Matrix3D fromCompactQuaternion(const Vector3D &v)
Definition: isometry3d_mappings.cpp:84
g2o::Isometry3D
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3D
Definition: eigen_types.h:66
Isometry3D g2o::internal::fromVectorQT ( const Vector7d &  v)

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

Definition at line 131 of file isometry3d_mappings.cpp.

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

131  {
132  Isometry3D t;
133  t=Eigen::Quaterniond(v[6], v[3], v[4], v[5]).toRotationMatrix();
134  t.translation() = v.head<3>();
135  return t;
136  }
g2o::Isometry3D
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3D
Definition: eigen_types.h:66
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 70 of file isometry3d_mappings.h.

Referenced by main().

71  {
72  Eigen::JacobiSVD<Matrix3D> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV);
73  double det = (svd.matrixU() * svd.matrixV().adjoint()).determinant();
74  Matrix3D scaledU(svd.matrixU());
75  scaledU.col(0) /= det;
76  const_cast<Eigen::MatrixBase<Derived>&>(R) = scaledU * svd.matrixV().transpose();
77  }
g2o::Matrix3D
Eigen::Matrix< double, 3, 3, Eigen::ColMajor > Matrix3D
Definition: eigen_types.h:61
Eigen::Quaterniond& g2o::internal::normalize ( Eigen::Quaterniond &  q)

as above, but in-place

Definition at line 38 of file isometry3d_mappings.cpp.

Referenced by g2o::G2oSlamInterface::addEdge(), approximateNearestOrthogonalMatrix(), g2o::Plane3D::fromVector(), normalized(), g2o::Plane3D::oplus(), g2o::EdgeSE3::read(), g2o::ParameterStereoCamera::read(), g2o::ParameterCamera::read(), g2o::EdgeSE3Calib::read(), g2o::EdgeSE3Offset::read(), g2o::ParameterSE3Offset::read(), and toCompactQuaternion().

38  {
39  q.normalize();
40  if (q.w()<0) {
41  q.coeffs() *= -1;
42  }
43  return q;
44  }
G2O_TYPES_SLAM3D_ADDONS_API Vector6d g2o::internal::normalizeCartesianLine ( const Vector6d &  line)

Definition at line 115 of file line3d.cpp.

Referenced by main(), g2o::Line3D::ominus(), and transformCartesianLine().

115  {
116  Vector3D p0 = line.head<3>();
117  Vector3D d0 = line.tail<3>();
118  d0.normalize();
119  p0-=d0*(d0.dot(p0));
120  Vector6d nl;
121  nl.head<3>()=p0;
122  nl.tail<3>()=d0;
123  return nl;
124  }
g2o::Vector3D
Eigen::Matrix< double, 3, 1, Eigen::ColMajor > Vector3D
Definition: eigen_types.h:46
Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: targetTypes6D.hpp:11
Eigen::Quaterniond g2o::internal::normalized ( const Eigen::Quaterniond &  q)

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

Definition at line 32 of file isometry3d_mappings.cpp.

References normalize().

Referenced by approximateNearestOrthogonalMatrix().

32  {
33  Eigen::Quaterniond q2(q);
34  normalize(q2);
35  return q2;
36  }
g2o::internal::normalize
Eigen::Quaterniond & normalize(Eigen::Quaterniond &q)
Definition: isometry3d_mappings.cpp:38
template<typename MatrixType >
void g2o::internal::pcg_atxpy ( const MatrixType &  A,
const VectorXD &  x,
int  xoff,
VectorXD &  y,
int  yoff 
)
inline

Definition at line 67 of file linear_solver_pcg.h.

75  { return _tolerance;}
template<>
void g2o::internal::pcg_atxpy ( const MatrixXD &  A,
const VectorXD &  x,
int  xoff,
VectorXD &  y,
int  yoff 
)
inline

Definition at line 73 of file linear_solver_pcg.h.

75  { return _tolerance;}
76  void setTolerance(double tolerance) { _tolerance = tolerance;}
template<typename MatrixType >
void g2o::internal::pcg_axpy ( const MatrixType &  A,
const VectorXD &  x,
int  xoff,
VectorXD &  y,
int  yoff 
)
inline

Definition at line 55 of file linear_solver_pcg.h.

61  {
template<>
void g2o::internal::pcg_axpy ( const MatrixXD &  A,
const VectorXD &  x,
int  xoff,
VectorXD &  y,
int  yoff 
)
inline

Definition at line 61 of file linear_solver_pcg.h.

61  {
62  }
63 
64  virtual bool init()
g2o::types_icp::init
void init()
Definition: types_icp.cpp:43
template<typename MatrixType >
void g2o::internal::pcg_axy ( const MatrixType &  A,
const VectorXD &  x,
int  xoff,
VectorXD &  y,
int  yoff 
)
inline

Definition at line 42 of file linear_solver_pcg.h.

47  : public LinearSolver<MatrixType>
template<>
void g2o::internal::pcg_axy ( const MatrixXD &  A,
const VectorXD &  x,
int  xoff,
VectorXD &  y,
int  yoff 
)
inline

Definition at line 48 of file linear_solver_pcg.h.

48  {
49  public:
50  LinearSolverPCG() :
51  LinearSolver<MatrixType>()
template<typename Derived , typename DerivedOther >
void g2o::internal::skew ( Eigen::MatrixBase< Derived > &  s,
const Eigen::MatrixBase< DerivedOther > &  v 
)
inline

Definition at line 43 of file isometry3d_gradients.h.

Referenced by computeEdgeSE3Gradient(), and computeEdgeSE3PriorGradient().

43  {
44  const double x=2*v(0);
45  const double y=2*v(1);
46  const double z=2*v(2);
47  s << 0., z, -y, -z, 0, x, y, -x, 0;
48  }
template<typename Derived , typename DerivedOther >
void g2o::internal::skew ( Eigen::MatrixBase< Derived > &  Sx,
Eigen::MatrixBase< Derived > &  Sy,
Eigen::MatrixBase< Derived > &  Sz,
const Eigen::MatrixBase< DerivedOther > &  R 
)

Definition at line 59 of file isometry3d_gradients.h.

62  {
63  const double
64  r11=2*R(0,0), r12=2*R(0,1), r13=2*R(0,2),
65  r21=2*R(1,0), r22=2*R(1,1), r23=2*R(1,2),
66  r31=2*R(2,0), r32=2*R(2,1), r33=2*R(2,2);
67  Sx << 0, 0, 0, -r31, -r32, -r33, r21, r22, r23;
68  Sy << r31, r32, r33, 0, 0, 0, -r11, -r12, -r13;
69  Sz << -r21, -r22, -r23, r11, r12, r13, 0, 0, 0;
70  }
template<typename Derived , typename DerivedOther >
void g2o::internal::skewT ( Eigen::MatrixBase< Derived > &  s,
const Eigen::MatrixBase< DerivedOther > &  v 
)
inline

Definition at line 51 of file isometry3d_gradients.h.

Referenced by computeEdgeSE3Gradient().

51  {
52  const double x=2*v(0);
53  const double y=2*v(1);
54  const double z=2*v(2);
55  s << 0., -z, y, z, 0, -x, -y, x, 0;
56  }
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 73 of file isometry3d_gradients.h.

76  {
77  const double
78  r11=2*R(0,0), r12=2*R(0,1), r13=2*R(0,2),
79  r21=2*R(1,0), r22=2*R(1,1), r23=2*R(1,2),
80  r31=2*R(2,0), r32=2*R(2,1), r33=2*R(2,2);
81  Sx << 0, 0, 0, r31, r32, r33, -r21, -r22, -r23;
82  Sy << -r31, -r32, -r33, 0, 0, 0, r11, r12, r13;
83  Sz << r21, r22, r23, -r11, -r12, -r13, 0, 0, 0;
84  }
Vector3D g2o::internal::toCompactQuaternion ( const Matrix3D &  R)

Rotation matrix -> (qx qy, qz)

Definition at line 77 of file isometry3d_mappings.cpp.

References normalize().

Referenced by approximateNearestOrthogonalMatrix(), main(), and toVectorMQT().

77  {
78  Eigen::Quaterniond q(R);
79  normalize(q);
80  // return (x,y,z) of the quaternion
81  return q.coeffs().head<3>();
82  }
g2o::internal::normalize
Eigen::Quaterniond & normalize(Eigen::Quaterniond &q)
Definition: isometry3d_mappings.cpp:38
Vector3D g2o::internal::toEuler ( const Matrix3D &  R)

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

Definition at line 47 of file isometry3d_mappings.cpp.

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

47  {
48  Eigen::Quaterniond q(R);
49  const double& q0 = q.w();
50  const double& q1 = q.x();
51  const double& q2 = q.y();
52  const double& q3 = q.z();
53  double roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
54  double pitch = asin(2*(q0*q2-q3*q1));
55  double yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
56  return Vector3D(roll, pitch, yaw);
57  }
g2o::Vector3D
Eigen::Matrix< double, 3, 1, Eigen::ColMajor > Vector3D
Definition: eigen_types.h:46
SE3Quat g2o::internal::toSE3Quat ( const Isometry3D &  t)

convert an Isometry3D to the old SE3Quat class

Definition at line 138 of file isometry3d_mappings.cpp.

Referenced by approximateNearestOrthogonalMatrix(), and g2o::VertexSE3::G2O_ATTRIBUTE_DEPRECATED().

139  {
140  SE3Quat result(t.matrix().topLeftCorner<3,3>(), t.translation());
141  return result;
142  }
Vector6d g2o::internal::toVectorET ( const Isometry3D &  t)

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

Definition at line 101 of file isometry3d_mappings.cpp.

References extractRotation(), and toEuler().

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

101  {
102  Vector6d v;
103  v.block<3,1>(3,0)=toEuler(extractRotation(t));
104  v.block<3,1>(0,0) = t.translation();
105  return v;
106  }
g2o::internal::toEuler
Vector3D toEuler(const Matrix3D &R)
Definition: isometry3d_mappings.cpp:47
g2o::internal::extractRotation
Isometry3D::ConstLinearPart extractRotation(const Isometry3D &A)
Definition: isometry3d_mappings.h:58
Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: targetTypes6D.hpp:11
Vector6d g2o::internal::toVectorMQT ( const Isometry3D &  t)

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

Definition at line 94 of file isometry3d_mappings.cpp.

References extractRotation(), and toCompactQuaternion().

Referenced by approximateNearestOrthogonalMatrix(), g2o::OnlineEdgeSE3::chi2(), g2o::EdgeSE3::computeError(), g2o::EdgeSE3Calib::computeError(), g2o::EdgeSE3Prior::computeError(), g2o::EdgeSE3Offset::computeError(), g2o::VertexSE3::getMinimalEstimateData(), main(), g2o::EdgeSE3WriteGnuplotAction::operator()(), and g2o::VertexSE3WriteGnuplotAction::operator()().

94  {
95  Vector6d v;
96  v.block<3,1>(3,0) = toCompactQuaternion(extractRotation(t));
97  v.block<3,1>(0,0) = t.translation();
98  return v;
99  }
g2o::internal::toCompactQuaternion
Vector3D toCompactQuaternion(const Matrix3D &R)
Definition: isometry3d_mappings.cpp:77
g2o::internal::extractRotation
Isometry3D::ConstLinearPart extractRotation(const Isometry3D &A)
Definition: isometry3d_mappings.h:58
Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: targetTypes6D.hpp:11
Vector7d g2o::internal::toVectorQT ( const Isometry3D &  t)

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

Definition at line 108 of file isometry3d_mappings.cpp.

References extractRotation().

Referenced by approximateNearestOrthogonalMatrix(), g2o::VertexSE3::getEstimateData(), g2o::EdgeSE3::getMeasurementData(), g2o::EdgeSE3Prior::getMeasurementData(), g2o::jac_quat3_euler3(), main(), g2o::EdgeSE3::write(), g2o::ParameterStereoCamera::write(), g2o::ParameterCamera::write(), g2o::EdgeSE3Prior::write(), g2o::EdgeSE3Calib::write(), g2o::EdgeSE3Offset::write(), g2o::ParameterSE3Offset::write(), and g2o::VertexSE3::write().

108  {
109  Eigen::Quaterniond q(extractRotation(t));
110  q.normalize();
111  Vector7d v;
112  v[3] = q.x(); v[4] = q.y(); v[5] = q.z(); v[6] = q.w();
113  v.block<3,1>(0,0) = t.translation();
114  return v;
115  }
g2o::Vector7d
Eigen::Matrix< double, 7, 1 > Vector7d
Definition: sim3.h:35
g2o::internal::extractRotation
Isometry3D::ConstLinearPart extractRotation(const Isometry3D &A)
Definition: isometry3d_mappings.h:58
G2O_TYPES_SLAM3D_ADDONS_API Vector6d g2o::internal::transformCartesianLine ( const Isometry3D &  t,
const Vector6d &  line 
)

Definition at line 108 of file line3d.cpp.

References normalizeCartesianLine().

Referenced by main(), and g2o::Line3D::ominus().

108  {
109  Vector6d l;
110  l.head<3>() = t*line.head<3>();
111  l.tail<3>() = t.linear()*line.tail<3>();
112  return normalizeCartesianLine(l);
113  }
Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: targetTypes6D.hpp:11
g2o::internal::normalizeCartesianLine
Vector6d normalizeCartesianLine(const Vector6d &line)
Definition: line3d.cpp:115
Generated on Mon Mar 18 2019 20:14:54 for g2o by   doxygen 1.8.11