g2o
Loading...
Searching...
No Matches
Namespaces | Classes | Functions
g2o::internal Namespace Reference

Namespaces

namespace  g2o

Classes

struct  QuadraticFormLock
struct  BaseEdgeTraits
struct  BaseEdgeTraits<-1 >
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)
int computeUpperTriangleIndex (int i, int j)
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)
template<typename MatrixType>
void pcg_axy (const MatrixType &A, const VectorX &x, int xoff, VectorX &y, int yoff)
template<>
void pcg_axy (const MatrixX &A, const VectorX &x, int xoff, VectorX &y, int yoff)
template<typename MatrixType>
void pcg_axpy (const MatrixType &A, const VectorX &x, int xoff, VectorX &y, int yoff)
template<>
void pcg_axpy (const MatrixX &A, const VectorX &x, int xoff, VectorX &y, int yoff)
template<typename MatrixType>
void pcg_atxpy (const MatrixType &A, const VectorX &x, int xoff, VectorX &y, int yoff)
template<>
void pcg_atxpy (const MatrixX &A, const VectorX &x, int xoff, 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)
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, 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.

39 {
40 double tr = r00 + r11 + r22;
41 if (tr > 0) {
42 S = sqrt(tr + 1.0) * 2; // S=4*qw
43 qw = 0.25 * S;
44 // qx = (r21 - r12) / S;
45 // qy = (r02 - r20) / S;
46 // qz = (r10 - r01) / S;
47 return 0;
48 } else if ((r00 > r11) & (r00 > r22)) {
49 S = sqrt(1.0 + r00 - r11 - r22) * 2; // S=4*qx
50 qw = (r21 - r12) / S;
51 // qx = 0.25 * S;
52 // qy = (r01 + r10) / S;
53 // qz = (r02 + r20) / S;
54 return 1;
55 } else if (r11 > r22) {
56 S = sqrt(1.0 + r11 - r00 - r22) * 2; // S=4*qy
57 qw = (r02 - r20) / S;
58 // qx = (r01 + r10) / S;
59 // qy = 0.25 * S;
60 return 2;
61 } else {
62 S = sqrt(1.0 + r22 - r00 - r11) * 2; // S=4*qz
63 qw = (r10 - r01) / S;
64 // qx = (r02 + r20) / S;
65 // qy = (r12 + r21) / S;
66 // qz = 0.25 * S;
67 return 3;
68 }
69}
g2o::ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition jet.h:444

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.

81 {
82 Matrix3 E = R.transpose() * R;
83 E.diagonal().array() -= 1;
84 const_cast<Eigen::MatrixBase<Derived>&>(R) -= 0.5 * R * E;
85}
g2o::Matrix3
MatrixN< 3 > Matrix3
Definition eigen_types.h:72

Referenced by g2o::VertexSE3::oplusImpl().

◆ 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.

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

◆ 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.

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

◆ 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.

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

◆ 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.

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

◆ 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.

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

◆ 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.

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

◆ 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.

74 {
75 double qw;
76 double S;
77 int whichCase = _q2m(S, qw, r11, r21, r31, r12, r22, r32, r13, r23, r33);
78 S *= .25;
79 switch (whichCase) {
80 case 0:
81 compute_dq_dR_w(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
82 break;
83 case 1:
84 compute_dq_dR_x(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
85 break;
86 case 2:
87 compute_dq_dR_y(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
88 break;
89 case 3:
90 compute_dq_dR_z(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
91 break;
92 }
93 if (qw <= 0) dq_dR *= -1;
94}
g2o::internal::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.cpp:70
g2o::internal::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.cpp:160
g2o::internal::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.cpp:28
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:36
g2o::internal::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.cpp:115

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

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

30 {
31namespace internal {
32using namespace std;
33
34#include "dquat2mat_maxima_generated.cpp"
35
36int _q2m(double& S, double& qw, const double& r00, const double& r10,
37 const double& r20, const double& r01, const double& r11,
38 const double& r21, const double& r02, const double& r12,
39 const double& r22) {
40 double tr = r00 + r11 + r22;
41 if (tr > 0) {
42 S = sqrt(tr + 1.0) * 2; // S=4*qw
43 qw = 0.25 * S;
44 // qx = (r21 - r12) / S;
45 // qy = (r02 - r20) / S;
46 // qz = (r10 - r01) / S;
47 return 0;
48 } else if ((r00 > r11) & (r00 > r22)) {
49 S = sqrt(1.0 + r00 - r11 - r22) * 2; // S=4*qx
50 qw = (r21 - r12) / S;
51 // qx = 0.25 * S;
52 // qy = (r01 + r10) / S;
53 // qz = (r02 + r20) / S;
54 return 1;
55 } else if (r11 > r22) {
56 S = sqrt(1.0 + r11 - r00 - r22) * 2; // S=4*qy
57 qw = (r02 - r20) / S;
58 // qx = (r01 + r10) / S;
59 // qy = 0.25 * S;
60 return 2;
61 } else {
62 S = sqrt(1.0 + r22 - r00 - r11) * 2; // S=4*qz
63 qw = (r10 - r01) / S;
64 // qx = (r02 + r20) / S;
65 // qy = (r12 + r21) / S;
66 // qz = 0.25 * S;
67 return 3;
68 }
69}
dquat2mat_maxima_generated.cpp
std
Definition jet.h:876

Referenced by compute_dq_dR().

◆ compute_dq_dR_x()

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

74 {
75 double qw;
76 double S;
77 int whichCase = _q2m(S, qw, r11, r21, r31, r12, r22, r32, r13, r23, r33);
78 S *= .25;
79 switch (whichCase) {
80 case 0:
81 compute_dq_dR_w(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
82 break;
83 case 1:
84 compute_dq_dR_x(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
85 break;
86 case 2:
87 compute_dq_dR_y(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
88 break;
89 case 3:
90 compute_dq_dR_z(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
91 break;
92 }
93 if (qw <= 0) dq_dR *= -1;
94}
95} // namespace internal
96} // namespace g2o

Referenced by compute_dq_dR().

◆ compute_dq_dR_y()

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

144 {
145namespace internal {
146using namespace std;
147
148#include "dquat2mat_maxima_generated.cpp"
149
150int _q2m(double& S, double& qw, const double& r00, const double& r10,
151 const double& r20, const double& r01, const double& r11,
152 const double& r21, const double& r02, const double& r12,
153 const double& r22) {
154 double tr = r00 + r11 + r22;
155 if (tr > 0) {
156 S = sqrt(tr + 1.0) * 2; // S=4*qw
157 qw = 0.25 * S;
158 // qx = (r21 - r12) / S;
159 // qy = (r02 - r20) / S;
160 // qz = (r10 - r01) / S;
161 return 0;
162 } else if ((r00 > r11) & (r00 > r22)) {
163 S = sqrt(1.0 + r00 - r11 - r22) * 2; // S=4*qx
164 qw = (r21 - r12) / S;
165 // qx = 0.25 * S;
166 // qy = (r01 + r10) / S;
167 // qz = (r02 + r20) / S;
168 return 1;
169 } else if (r11 > r22) {
170 S = sqrt(1.0 + r11 - r00 - r22) * 2; // S=4*qy
171 qw = (r02 - r20) / S;
172 // qx = (r01 + r10) / S;
173 // qy = 0.25 * S;
174 return 2;
175 } else {
176 S = sqrt(1.0 + r22 - r00 - r11) * 2; // S=4*qz
177 qw = (r10 - r01) / S;
178 // qx = (r02 + r20) / S;
179 // qy = (r12 + r21) / S;
180 // qz = 0.25 * S;
181 return 3;
182 }
183}
184
185void compute_dq_dR(Eigen::Matrix<double, 3, 9, Eigen::ColMajor>& dq_dR,
186 const double& r11, const double& r21, const double& r31,
187 const double& r12, const double& r22, const double& r32,
188 const double& r13, const double& r23, const double& r33) {
189 double qw;
190 double S;
191 int whichCase = _q2m(S, qw, r11, r21, r31, r12, r22, r32, r13, r23, r33);
192 S *= .25;
193 switch (whichCase) {
194 case 0:
195 compute_dq_dR_w(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
196 break;
197 case 1:
198 compute_dq_dR_x(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
199 break;
200 case 2:
201 compute_dq_dR_y(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
202 break;
203 case 3:
204 compute_dq_dR_z(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
205 break;
206 }
207 if (qw <= 0) dq_dR *= -1;
208}
209} // namespace internal
210} // namespace g2o
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:159
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:69
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:114
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: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:71

Referenced by compute_dq_dR().

◆ compute_dq_dR_z()

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

189 {
190namespace internal {
191using namespace std;
192
193#include "dquat2mat_maxima_generated.cpp"
194
195int _q2m(double& S, double& qw, const double& r00, const double& r10,
196 const double& r20, const double& r01, const double& r11,
197 const double& r21, const double& r02, const double& r12,
198 const double& r22) {
199 double tr = r00 + r11 + r22;
200 if (tr > 0) {
201 S = sqrt(tr + 1.0) * 2; // S=4*qw
202 qw = 0.25 * S;
203 // qx = (r21 - r12) / S;
204 // qy = (r02 - r20) / S;
205 // qz = (r10 - r01) / S;
206 return 0;
207 } else if ((r00 > r11) & (r00 > r22)) {
208 S = sqrt(1.0 + r00 - r11 - r22) * 2; // S=4*qx
209 qw = (r21 - r12) / S;
210 // qx = 0.25 * S;
211 // qy = (r01 + r10) / S;
212 // qz = (r02 + r20) / S;
213 return 1;
214 } else if (r11 > r22) {
215 S = sqrt(1.0 + r11 - r00 - r22) * 2; // S=4*qy
216 qw = (r02 - r20) / S;
217 // qx = (r01 + r10) / S;
218 // qy = 0.25 * S;
219 return 2;
220 } else {
221 S = sqrt(1.0 + r22 - r00 - r11) * 2; // S=4*qz
222 qw = (r10 - r01) / S;
223 // qx = (r02 + r20) / S;
224 // qy = (r12 + r21) / S;
225 // qz = 0.25 * S;
226 return 3;
227 }
228}
229
230void compute_dq_dR(Eigen::Matrix<double, 3, 9, Eigen::ColMajor>& dq_dR,
231 const double& r11, const double& r21, const double& r31,
232 const double& r12, const double& r22, const double& r32,
233 const double& r13, const double& r23, const double& r33) {
234 double qw;
235 double S;
236 int whichCase = _q2m(S, qw, r11, r21, r31, r12, r22, r32, r13, r23, r33);
237 S *= .25;
238 switch (whichCase) {
239 case 0:
240 compute_dq_dR_w(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
241 break;
242 case 1:
243 compute_dq_dR_x(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
244 break;
245 case 2:
246 compute_dq_dR_y(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
247 break;
248 case 3:
249 compute_dq_dR_z(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
250 break;
251 }
252 if (qw <= 0) dq_dR *= -1;
253}
254} // namespace internal
255} // namespace g2o

Referenced by compute_dq_dR().

◆ compute_dR_dq() [1/2]

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

234 {
235namespace internal {
236using namespace std;
237
238#include "dquat2mat_maxima_generated.cpp"
239
240int _q2m(double& S, double& qw, const double& r00, const double& r10,
241 const double& r20, const double& r01, const double& r11,
242 const double& r21, const double& r02, const double& r12,
243 const double& r22) {
244 double tr = r00 + r11 + r22;
245 if (tr > 0) {
246 S = sqrt(tr + 1.0) * 2; // S=4*qw
247 qw = 0.25 * S;
248 // qx = (r21 - r12) / S;
249 // qy = (r02 - r20) / S;
250 // qz = (r10 - r01) / S;
251 return 0;
252 } else if ((r00 > r11) & (r00 > r22)) {
253 S = sqrt(1.0 + r00 - r11 - r22) * 2; // S=4*qx
254 qw = (r21 - r12) / S;
255 // qx = 0.25 * S;
256 // qy = (r01 + r10) / S;
257 // qz = (r02 + r20) / S;
258 return 1;
259 } else if (r11 > r22) {
260 S = sqrt(1.0 + r11 - r00 - r22) * 2; // S=4*qy
261 qw = (r02 - r20) / S;
262 // qx = (r01 + r10) / S;
263 // qy = 0.25 * S;
264 return 2;
265 } else {
266 S = sqrt(1.0 + r22 - r00 - r11) * 2; // S=4*qz
267 qw = (r10 - r01) / S;
268 // qx = (r02 + r20) / S;
269 // qy = (r12 + r21) / S;
270 // qz = 0.25 * S;
271 return 3;
272 }
273}
274
275void compute_dq_dR(Eigen::Matrix<double, 3, 9, Eigen::ColMajor>& dq_dR,
276 const double& r11, const double& r21, const double& r31,
277 const double& r12, const double& r22, const double& r32,
278 const double& r13, const double& r23, const double& r33) {
279 double qw;
280 double S;
281 int whichCase = _q2m(S, qw, r11, r21, r31, r12, r22, r32, r13, r23, r33);
282 S *= .25;
283 switch (whichCase) {
284 case 0:
285 compute_dq_dR_w(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
286 break;
287 case 1:
288 compute_dq_dR_x(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
289 break;
290 case 2:
291 compute_dq_dR_y(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
292 break;
293 case 3:
294 compute_dq_dR_z(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33);
295 break;
296 }
297 if (qw <= 0) dq_dR *= -1;
298}
299} // namespace internal
300} // namespace g2o

◆ compute_dR_dq() [2/2]

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 )

References G2O_TYPES_SLAM3D_API.

◆ 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.

210 {
211 Eigen::MatrixBase<Derived>& Ji =
212 const_cast<Eigen::MatrixBase<Derived>&>(JiConstRef);
213 Eigen::MatrixBase<Derived>& Jj =
214 const_cast<Eigen::MatrixBase<Derived>&>(JjConstRef);
215 Ji.derived().resize(6, 6);
216 Jj.derived().resize(6, 6);
217 // compute the error at the linearization point
218 const Isometry3 A = Z.inverse();
219 const Isometry3 B = Xi.inverse() * Xj;
220
221 E = A * B;
222
223 Isometry3::ConstLinearPart Re = extractRotation(E);
224 Isometry3::ConstLinearPart Ra = extractRotation(A);
225 Isometry3::ConstLinearPart Rb = extractRotation(B);
226 Isometry3::ConstTranslationPart tb = B.translation();
227
228 Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
229 compute_dq_dR(dq_dR, Re(0, 0), Re(1, 0), Re(2, 0), Re(0, 1), Re(1, 1),
230 Re(2, 1), Re(0, 2), Re(1, 2), Re(2, 2));
231
232 Ji.setZero();
233 Jj.setZero();
234
235 // dte/dti
236 Ji.template block<3, 3>(0, 0) = -Ra;
237
238 // dte/dtj
239 Jj.template block<3, 3>(0, 0) = Re;
240
241 // dte/dqi
242 {
243 Matrix3 S;
244 skewT(S, tb);
245 Ji.template block<3, 3>(0, 3) = Ra * S;
246 }
247
248 // dte/dqj: this is zero
249
250 double buf[27];
251 Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
252 Matrix3 Sxt, Syt, Szt;
253 // dre/dqi
254 {
255 skewT(Sxt, Syt, Szt, Rb);
256 Eigen::Map<Matrix3> Mx(buf);
257 Mx.noalias() = Ra * Sxt;
258 Eigen::Map<Matrix3> My(buf + 9);
259 My.noalias() = Ra * Syt;
260 Eigen::Map<Matrix3> Mz(buf + 18);
261 Mz.noalias() = Ra * Szt;
262 Ji.template block<3, 3>(3, 3) = dq_dR * M;
263 }
264
265 // dre/dqj
266 {
267 Matrix3& Sx = Sxt;
268 Matrix3& Sy = Syt;
269 Matrix3& Sz = Szt;
270 skew(Sx, Sy, Sz, Matrix3::Identity());
271 Eigen::Map<Matrix3> Mx(buf);
272 Mx.noalias() = Re * Sx;
273 Eigen::Map<Matrix3> My(buf + 9);
274 My.noalias() = Re * Sy;
275 Eigen::Map<Matrix3> Mz(buf + 18);
276 Mz.noalias() = Re * Sz;
277 Jj.template block<3, 3>(3, 3) = dq_dR * M;
278 }
279}
g2o::internal::skewT
void skewT(Eigen::MatrixBase< Derived > &s, const Eigen::MatrixBase< DerivedOther > &v)
Definition isometry3d_gradients.h:57
g2o::internal::skew
void skew(Eigen::MatrixBase< Derived > &s, const Eigen::MatrixBase< DerivedOther > &v)
Definition isometry3d_gradients.h:45
g2o::internal::extractRotation
Isometry3::ConstLinearPart extractRotation(const Isometry3 &A)
Definition isometry3d_mappings.h:55
g2o::Isometry3
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3
Definition eigen_types.h:77

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.

95 {
96 Eigen::MatrixBase<Derived>& Ji =
97 const_cast<Eigen::MatrixBase<Derived>&>(JiConstRef);
98 Eigen::MatrixBase<Derived>& Jj =
99 const_cast<Eigen::MatrixBase<Derived>&>(JjConstRef);
100 Ji.derived().resize(6, 6);
101 Jj.derived().resize(6, 6);
102 // compute the error at the linearization point
103 const Isometry3 A = Z.inverse() * Pi.inverse();
104 const Isometry3 B = Xi.inverse() * Xj;
105 const Isometry3& C = Pj;
106
107 const Isometry3 AB = A * B;
108 const Isometry3 BC = B * C;
109 E = AB * C;
110
111 Isometry3::ConstLinearPart Re = extractRotation(E);
112 Isometry3::ConstLinearPart Ra = extractRotation(A);
113 // const Matrix3 Rb = extractRotation(B);
114 Isometry3::ConstLinearPart Rc = extractRotation(C);
115 Isometry3::ConstTranslationPart tc = C.translation();
116 // Isometry3::ConstTranslationParttab=AB.translation();
117 Isometry3::ConstLinearPart Rab = extractRotation(AB);
118 Isometry3::ConstTranslationPart tbc = BC.translation();
119 Isometry3::ConstLinearPart Rbc = extractRotation(BC);
120
121 Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
122 compute_dq_dR(dq_dR, Re(0, 0), Re(1, 0), Re(2, 0), Re(0, 1), Re(1, 1),
123 Re(2, 1), 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 Matrix3 S;
137 skewT(S, tbc);
138 Ji.template block<3, 3>(0, 3) = Ra * S;
139 }
140
141 // dte/dqj
142 {
143 Matrix3 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 Matrix3 Sxt, Syt, Szt;
153 internal::skewT(Sxt, Syt, Szt, Rbc);
154#ifdef __clang__
155 Matrix3 temp = Rab * Sxt;
156 Eigen::Map<Matrix3> M2(temp.data());
157 Eigen::Map<Matrix3> Mx(buf);
158 Mx = M2;
159 temp = Ra * Syt;
160 Eigen::Map<Matrix3> My(buf + 9);
161 My = M2;
162 temp = Ra * Szt;
163 Eigen::Map<Matrix3> Mz(buf + 18);
164 Mz = M2;
165#else
166 Eigen::Map<Matrix3> Mx(buf);
167 Mx = Ra * Sxt;
168 Eigen::Map<Matrix3> My(buf + 9);
169 My = Ra * Syt;
170 Eigen::Map<Matrix3> Mz(buf + 18);
171 Mz = Ra * Szt;
172#endif
173 Ji.template block<3, 3>(3, 3) = dq_dR * M;
174 }
175
176 // dre/dqj
177 {
178 double buf[27];
179 Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
180 Matrix3 Sx, Sy, Sz;
181 internal::skew(Sx, Sy, Sz, Rc);
182#ifdef __clang__
183 Matrix3 temp = Rab * Sx;
184 Eigen::Map<Matrix3> M2(temp.data());
185 Eigen::Map<Matrix3> Mx(buf);
186 Mx = M2;
187 temp = Rab * Sy;
188 Eigen::Map<Matrix3> My(buf + 9);
189 My = M2;
190 temp = Rab * Sz;
191 Eigen::Map<Matrix3> Mz(buf + 18);
192 Mz = M2;
193#else
194 Eigen::Map<Matrix3> Mx(buf);
195 Mx = Rab * Sx;
196 Eigen::Map<Matrix3> My(buf + 9);
197 My = Rab * Sy;
198 Eigen::Map<Matrix3> Mz(buf + 18);
199 Mz = Rab * Sz;
200#endif
201 Jj.template block<3, 3>(3, 3) = dq_dR * M;
202 }
203}

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.

285 {
286 Eigen::MatrixBase<Derived>& J =
287 const_cast<Eigen::MatrixBase<Derived>&>(JConstRef);
288 J.derived().resize(6, 6);
289 // compute the error at the linearization point
290 const Isometry3 A = Z.inverse() * X;
291 const Isometry3& B = P;
292 Isometry3::ConstLinearPart Ra = extractRotation(A);
293 Isometry3::ConstLinearPart Rb = extractRotation(B);
294 Isometry3::ConstTranslationPart tb = B.translation();
295 E = A * B;
296 Isometry3::ConstLinearPart Re = extractRotation(E);
297
298 Eigen::Matrix<double, 3, 9, Eigen::ColMajor> dq_dR;
299 compute_dq_dR(dq_dR, Re(0, 0), Re(1, 0), Re(2, 0), Re(0, 1), Re(1, 1),
300 Re(2, 1), Re(0, 2), Re(1, 2), Re(2, 2));
301
302 J.setZero();
303
304 // dte/dt
305 J.template block<3, 3>(0, 0) = Ra;
306
307 // dte/dq =0
308 // dte/dqj
309 {
310 Matrix3 S;
311 skew(S, tb);
312 J.template block<3, 3>(0, 3) = Ra * S;
313 }
314
315 // dre/dt =0
316
317 // dre/dq
318 {
319 double buf[27];
320 Eigen::Map<Eigen::Matrix<double, 9, 3, Eigen::ColMajor> > M(buf);
321 Matrix3 Sx, Sy, Sz;
322 internal::skew(Sx, Sy, Sz, Rb);
323#ifdef __clang__
324 Matrix3 temp = Ra * Sx;
325 Eigen::Map<Matrix3> M2(temp.data());
326 Eigen::Map<Matrix3> Mx(buf);
327 Mx = M2;
328 temp = Ra * Sy;
329 Eigen::Map<Matrix3> My(buf + 9);
330 My = M2;
331 temp = Ra * Sz;
332 Eigen::Map<Matrix3> Mz(buf + 18);
333 Mz = M2;
334#else
335 Eigen::Map<Matrix3> Mx(buf);
336 Mx = Ra * Sx;
337 Eigen::Map<Matrix3> My(buf + 9);
338 My = Ra * Sy;
339 Eigen::Map<Matrix3> Mz(buf + 18);
340 Mz = Ra * Sz;
341#endif
342 J.template block<3, 3>(3, 3) = dq_dR * M;
343 }
344}

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

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

◆ computeUpperTriangleIndex()

int g2o::internal::computeUpperTriangleIndex ( int i,
int j )
inline

Definition at line 31 of file base_variable_sized_edge.h.

◆ createBoolArray()

template<int K>
std::array< bool, K > g2o::internal::createBoolArray ( )

Definition at line 48 of file base_fixed_sized_edge.h.

48 {
49 std::array<bool, K> aux = {false};
50 return aux;
51}

◆ createBoolArray< 0 >()

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

Definition at line 48 of file base_fixed_sized_edge.h.

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

◆ 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.

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

Referenced by createHessianMaps().

◆ 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.

103 {
104 return std::tuple<Args...>{createHessianMapK<Args>()...};
105}
g2o::internal::createHessianMapK
T createHessianMapK()
helper function to call the c'tor of Eigen::Map
Definition base_fixed_sized_edge.h:92

References createHessianMapK().

◆ 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.

115 {
116 if (i == I) {
117 using VertexType = typename EdgeType::template VertexXnType<I>;
118 return new VertexType(args...);
119 }
120 return createNthVertexType<I - 1, EdgeType, CtorArgs...>(i, t, args...);
121}
g2o::internal::createNthVertexType
std::enable_if< I==-1, OptimizableGraph::Vertex * >::type createNthVertexType(int, const EdgeType &, CtorArgs...)
Definition base_fixed_sized_edge.h:109

◆ 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.

109 {
110 return nullptr;
111}

Referenced by g2o::BaseFixedSizedEdge< D, E, VertexTypes >::constructQuadraticFormN(), and g2o::MapHessianMemoryK::operator()().

◆ 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.

51 {
52 Eigen::Matrix<double, 3, 6, Eigen::ColMajor> J;
53 J.topLeftCorner<3, 3>() = -skew(y);
54 J.bottomRightCorner<3, 3>().setIdentity();
55
56 return J;
57}

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.

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

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.

59 {
60 Matrix3 R = T.rotation().toRotationMatrix();
61 Vector3 x = invert_depth(psi);
62 Vector3 r1 = R.col(0);
63 Vector3 r2 = R.col(1);
64 Matrix3 J;
65 J.col(0) = r1;
66 J.col(1) = r2;
67 J.col(2) = -R * x;
68 J *= 1. / psi.z();
69 return J;
70}
g2o::SE3Quat::rotation
const Quaternion & rotation() const
Definition se3quat.h:93
g2o::internal::invert_depth
Vector3 invert_depth(const Vector3 &x)
Definition sba_utils.h:37
g2o::Vector3
VectorN< 3 > Vector3
Definition eigen_types.h:51

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.

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

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

◆ fromCompactQuaternion()

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.

87 {
88 double w = 1 - v.squaredNorm();
89 if (w < 0)
90 return Matrix3::Identity();
91 else
92 w = sqrt(w);
93 return Quaternion(w, v[0], v[1], v[2]).toRotationMatrix();
94}
g2o::Quaternion
Eigen::Quaternion< double > Quaternion
Definition eigen_types.h:81

Referenced by fromVectorMQT().

◆ fromEuler()

Matrix3 g2o::internal::fromEuler ( const Vector3 & v)

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

Definition at line 62 of file isometry3d_mappings.cpp.

62 {
63 // UNOPTIMIZED
64 double roll = v[0];
65 double pitch = v[1];
66 double yaw = v[2];
67 double sy = std::sin(yaw * cst(0.5));
68 double cy = std::cos(yaw * cst(0.5));
69 double sp = std::sin(pitch * cst(0.5));
70 double cp = std::cos(pitch * cst(0.5));
71 double sr = std::sin(roll * cst(0.5));
72 double cr = std::cos(roll * cst(0.5));
73 double w = cr * cp * cy + sr * sp * sy;
74 double x = sr * cp * cy - cr * sp * sy;
75 double y = cr * sp * cy + sr * cp * sy;
76 double z = cr * cp * sy - sr * sp * cy;
77 return Quaternion(w, x, y, z).toRotationMatrix();
78}
g2o::cst
constexpr double cst(long double v)
Definition misc.h:60

References g2o::cst().

Referenced by fromVectorET().

◆ fromSE3Quat()

Isometry3 g2o::internal::fromSE3Quat ( const SE3Quat & t)

convert from an old SE3Quat into Isometry3

Definition at line 149 of file isometry3d_mappings.cpp.

149 {
150 Isometry3 result = (Isometry3)t.rotation();
151 result.translation() = t.translation();
152 return result;
153}
g2o::SE3Quat::translation
const Vector3 & translation() const
Definition se3quat.h:89

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

Referenced by g2o::VertexSE3::G2O_ATTRIBUTE_DEPRECATED().

◆ fromVectorET()

Isometry3 g2o::internal::fromVectorET ( const Vector6 & v)

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

Definition at line 130 of file isometry3d_mappings.cpp.

130 {
131 Isometry3 t;
132 t = fromEuler(v.block<3, 1>(3, 0));
133 t.translation() = v.block<3, 1>(0, 0);
134 return t;
135}
g2o::internal::fromEuler
Matrix3 fromEuler(const Vector3 &v)
Definition isometry3d_mappings.cpp:62

References fromEuler().

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

◆ fromVectorMQT()

Isometry3 g2o::internal::fromVectorMQT ( const Vector6 & v)

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

Definition at line 123 of file isometry3d_mappings.cpp.

123 {
124 Isometry3 t;
125 t = fromCompactQuaternion(v.block<3, 1>(3, 0));
126 t.translation() = v.block<3, 1>(0, 0);
127 return t;
128}
g2o::internal::fromCompactQuaternion
Matrix3 fromCompactQuaternion(const Vector3 &v)
Definition isometry3d_mappings.cpp:87

References fromCompactQuaternion().

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

◆ fromVectorQT()

Isometry3 g2o::internal::fromVectorQT ( const Vector7 & v)

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

Definition at line 137 of file isometry3d_mappings.cpp.

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

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

◆ getAzimuth()

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

Definition at line 191 of file line3d.h.

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

References G2O_TYPES_SLAM3D_ADDONS_API.

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.

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

References G2O_TYPES_SLAM3D_ADDONS_API.

Referenced by main().

◆ index_to_pair()

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.

86 {
87 return k < j ? TrivialPair{k, j} : index_to_pair(k - j, j + 1);
88}
g2o::internal::index_to_pair
constexpr TrivialPair index_to_pair(const int k, const int j=0)
Definition base_fixed_sized_edge.h:86

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.

37 {
38 Vector2 aux = x.head<2>();
39 return unproject(aux) / x[2];
40}
g2o::unproject
G2O_TYPES_SLAM3D_API Vector3 unproject(const Vector2 &)
Definition se3_ops.h:63
g2o::Vector2
VectorN< 2 > Vector2
Definition eigen_types.h:50

References g2o::unproject().

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

◆ mline_elevation()

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

Definition at line 187 of file line3d.h.

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

◆ 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.

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

◆ normalize()

Quaternion & g2o::internal::normalize ( Quaternion & q)

as above, but in-place

Definition at line 40 of file isometry3d_mappings.cpp.

40 {
41 q.normalize();
42 if (q.w() < 0) {
43 q.coeffs() *= -1;
44 }
45 return q;
46}

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.

68 {
69 Vector3 p0 = line.head<3>();
70 Vector3 d0 = line.tail<3>();
71 d0.normalize();
72 p0 -= d0 * (d0.dot(p0));
73 Vector6 nl;
74 nl.head<3>() = p0;
75 nl.tail<3>() = d0;
76 return nl;
77}
g2o::Vector6
VectorN< 6 > Vector6
Definition eigen_types.h:53

Referenced by transformCartesianLine().

◆ normalized()

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.

34 {
35 Quaternion q2(q);
36 normalize(q2);
37 return q2;
38}
g2o::internal::normalize
Quaternion & normalize(Quaternion &q)
Definition isometry3d_mappings.cpp:40

References normalize().

◆ pair_to_index()

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

Definition at line 60 of file base_fixed_sized_edge.h.

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

Referenced by g2o::BaseFixedSizedEdge< D, OdomAndLaserMotion, VertexSE2, VertexBaseline >::constructOffDiagonalQuadraticFormM().

◆ pcg_atxpy() [1/2]

template<typename MatrixType>
void g2o::internal::pcg_atxpy ( const MatrixType & A,
const VectorX & x,
int xoff,
VectorX & y,
int yoff )
inline

Definition at line 73 of file linear_solver_pcg.h.

75 :
76 using MatrixVector = std::vector<MatrixType>;
77 using MatrixPtrVector = std::vector<const MatrixType*>;

◆ pcg_atxpy() [2/2]

template<>
void g2o::internal::pcg_atxpy ( const MatrixX & A,
const VectorX & x,
int xoff,
VectorX & y,
int yoff )
inline

Definition at line 80 of file linear_solver_pcg.h.

◆ pcg_axpy() [1/2]

template<typename MatrixType>
void g2o::internal::pcg_axpy ( const MatrixType & A,
const VectorX & x,
int xoff,
VectorX & y,
int yoff )
inline

Definition at line 60 of file linear_solver_pcg.h.

64 { return _tolerance; }

◆ pcg_axpy() [2/2]

template<>
void g2o::internal::pcg_axpy ( const MatrixX & A,
const VectorX & x,
int xoff,
VectorX & y,
int yoff )
inline

Definition at line 67 of file linear_solver_pcg.h.

67 { return _maxIter; }
68 void setMaxIterations(int maxIter) { _maxIter = maxIter; }
69
70 bool absoluteTolerance() const { return _absoluteTolerance; }

◆ pcg_axy() [1/2]

template<typename MatrixType>
void g2o::internal::pcg_axy ( const MatrixType & A,
const VectorX & x,
int xoff,
VectorX & y,
int yoff )
inline

Definition at line 46 of file linear_solver_pcg.h.

◆ pcg_axy() [2/2]

template<>
void g2o::internal::pcg_axy ( const MatrixX & A,
const VectorX & x,
int xoff,
VectorX & y,
int yoff )
inline

Definition at line 53 of file linear_solver_pcg.h.

54 {
55 _residual = -1.0;
56 _indices.clear();

◆ readVector()

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

Definition at line 42 of file io_helper.h.

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

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

46 {
47 const double x = 2 * v(0);
48 const double y = 2 * v(1);
49 const double z = 2 * v(2);
50 if (transposed)
51 s << 0., -z, y, z, 0, -x, -y, x, 0;
52 else
53 s << 0., z, -y, -z, 0, x, y, -x, 0;
54}

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

◆ 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.

65 {
66 const double r11 = 2 * R(0, 0), r12 = 2 * R(0, 1), r13 = 2 * R(0, 2),
67 r21 = 2 * R(1, 0), r22 = 2 * R(1, 1), r23 = 2 * R(1, 2),
68 r31 = 2 * R(2, 0), r32 = 2 * R(2, 1), r33 = 2 * R(2, 2);
69 if (transposed) {
70 Sx << 0, 0, 0, r31, r32, r33, -r21, -r22, -r23;
71 Sy << -r31, -r32, -r33, 0, 0, 0, r11, r12, r13;
72 Sz << r21, r22, r23, -r11, -r12, -r13, 0, 0, 0;
73 } else {
74 Sx << 0, 0, 0, -r31, -r32, -r33, r21, r22, r23;
75 Sy << r31, r32, r33, 0, 0, 0, -r11, -r12, -r13;
76 Sz << -r21, -r22, -r23, r11, r12, r13, 0, 0, 0;
77 }
78}

◆ 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.

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

References skew().

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.

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

References skew().

◆ toCompactQuaternion()

Vector3 g2o::internal::toCompactQuaternion ( const Matrix3 & R)

Rotation matrix -> (qx qy, qz)

Definition at line 80 of file isometry3d_mappings.cpp.

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

References normalize().

Referenced by toVectorMQT().

◆ toEuler()

Vector3 g2o::internal::toEuler ( const Matrix3 & R)

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

Definition at line 49 of file isometry3d_mappings.cpp.

49 {
50 Quaternion q(R);
51 const double& q0 = q.w();
52 const double& q1 = q.x();
53 const double& q2 = q.y();
54 const double& q3 = q.z();
55 double roll =
56 std::atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
57 double pitch = std::asin(2 * (q0 * q2 - q3 * q1));
58 double yaw = std::atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
59 return Vector3(roll, pitch, yaw);
60}

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

◆ toSE3Quat()

SE3Quat g2o::internal::toSE3Quat ( const Isometry3 & t)

convert an Isometry3 to the old SE3Quat class

Definition at line 144 of file isometry3d_mappings.cpp.

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

Referenced by g2o::VertexSE3::G2O_ATTRIBUTE_DEPRECATED().

◆ toVectorET()

Vector6 g2o::internal::toVectorET ( const Isometry3 & t)

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

Definition at line 104 of file isometry3d_mappings.cpp.

104 {
105 Vector6 v;
106 v.block<3, 1>(3, 0) = toEuler(extractRotation(t));
107 v.block<3, 1>(0, 0) = t.translation();
108 return v;
109}
g2o::internal::toEuler
Vector3 toEuler(const Matrix3 &R)
Definition isometry3d_mappings.cpp:49

References extractRotation(), and toEuler().

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

◆ toVectorMQT()

Vector6 g2o::internal::toVectorMQT ( const Isometry3 & t)

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

Definition at line 97 of file isometry3d_mappings.cpp.

97 {
98 Vector6 v;
99 v.block<3, 1>(3, 0) = toCompactQuaternion(extractRotation(t));
100 v.block<3, 1>(0, 0) = t.translation();
101 return v;
102}
g2o::internal::toCompactQuaternion
Vector3 toCompactQuaternion(const Matrix3 &R)
Definition isometry3d_mappings.cpp:80

References extractRotation(), and toCompactQuaternion().

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

◆ toVectorQT()

Vector7 g2o::internal::toVectorQT ( const Isometry3 & t)

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

Definition at line 111 of file isometry3d_mappings.cpp.

111 {
112 Quaternion q(extractRotation(t));
113 q.normalize();
114 Vector7 v;
115 v[3] = q.x();
116 v[4] = q.y();
117 v[5] = q.z();
118 v[6] = q.w();
119 v.block<3, 1>(0, 0) = t.translation();
120 return v;
121}
g2o::Vector7
VectorN< 7 > Vector7
Definition eigen_types.h:54

References extractRotation().

Referenced by g2o::VertexSE3::getEstimateData(), g2o::EdgeSE3::getMeasurementData(), g2o::EdgeSE3Prior::getMeasurementData(), g2o::jac_quat3_euler3(), g2o::EdgeSE3::write(), g2o::EdgeSE3Calib::write(), g2o::EdgeSE3Offset::write(), g2o::EdgeSE3Prior::write(), g2o::ParameterCamera::write(), g2o::ParameterSE3Offset::write(), and g2o::VertexSE3::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.

61 {
62 Vector6 l;
63 l.head<3>() = t * line.head<3>();
64 l.tail<3>() = t.linear() * line.tail<3>();
65 return normalizeCartesianLine(l);
66}
g2o::internal::normalizeCartesianLine
Vector6 normalizeCartesianLine(const Vector6 &line)
Definition line3d.cpp:68

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.

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

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

Generated on for g2o by doxygen 1.15.0