g2o
Classes | Functions
g2o::deprecated Namespace Reference

Classes

class  CacheCamera
 
class  CacheSE3Offset
 caching the offset related to a vertex More...
 
class  EdgeSE3
 3D edge between two VertexSE3 More...
 
class  EdgeSE3Offset
 Offset edge. More...
 
class  EdgeSE3PointXYZ
 
class  EdgeSE3PointXYZDepth
 
class  EdgeSE3PointXYZDisparity
 
class  EdgeSE3Prior
 
class  EdgeSE3WriteGnuplotAction
 
class  ParameterCamera
 parameters for a camera More...
 
class  ParameterSE3Offset
 offset for an SE3 More...
 
class  VertexPointXYZ
 
class  VertexPointXYZWriteGnuplotAction
 
class  VertexSE3
 3D pose Vertex, (x,y,z,qw,qx,qy,qz) the parameterization for the increments constructed is a 6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. More...
 
class  VertexSE3WriteGnuplotAction
 

Functions

Eigen::Quaterniond euler_to_quat (double yaw, double pitch, double roll)
 
void quat_to_euler (const Eigen::Quaterniond &q, double &yaw, double &pitch, double &roll)
 
void jac_quat3_euler3 (Eigen::Matrix< double, 6, 6 > &J, const SE3Quat &t)
 
template<typename Derived >
void jacobian_3d_qman (Eigen::MatrixBase< Derived > &Ji, Eigen::MatrixBase< Derived > &Jj, const double &z11, const double &z12, const double &z13, const double &z14, const double &z21, const double &z22, const double &z23, const double &z24, const double &z31, const double &z32, const double &z33, const double &z34, const double &xab11, const double &xab12, const double &xab13, const double &xab14, const double &xab21, const double &xab22, const double &xab23, const double &xab24, const double &xab31, const double &xab32, const double &xab33, const double &xab34)
 
 G2O_REGISTER_TYPE_GROUP (deprecated_slam3d)
 

Function Documentation

Eigen::Quaterniond g2o::deprecated::euler_to_quat ( double  yaw,
double  pitch,
double  roll 
)

conversion code from Euler angles

Definition at line 33 of file se3quat_gradients.cpp.

34 {
35  double sy = sin(yaw*0.5);
36  double cy = cos(yaw*0.5);
37  double sp = sin(pitch*0.5);
38  double cp = cos(pitch*0.5);
39  double sr = sin(roll*0.5);
40  double cr = cos(roll*0.5);
41  double w = cr*cp*cy + sr*sp*sy;
42  double x = sr*cp*cy - cr*sp*sy;
43  double y = cr*sp*cy + sr*cp*sy;
44  double z = cr*cp*sy - sr*sp*cy;
45  return Eigen::Quaterniond(w,x,y,z);
46 }
g2o::deprecated::G2O_REGISTER_TYPE_GROUP ( deprecated_slam3d  )
void g2o::deprecated::jac_quat3_euler3 ( Eigen::Matrix< double, 6, 6 > &  J,
const SE3Quat &  t 
)

Definition at line 59 of file se3quat_gradients.cpp.

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

60 {
61  const Eigen::Vector3d& tr0 = t.translation();
62  const Eigen::Quaterniond& q0 = t.rotation();
63 
64  double delta=1e-6;
65  double idelta= 1. / (2. * delta);
66 
67  for (int i=0; i<6; i++){
68  SE3Quat ta, tb;
69  if (i<3){
70  Eigen::Vector3d tra=tr0;
71  Eigen::Vector3d trb=tr0;
72  tra[i] -= delta;
73  trb[i] += delta;
74  ta = SE3Quat(q0, tra);
75  tb = SE3Quat(q0, trb);
76  } else {
77  Eigen::Quaterniond qa=q0;
78  Eigen::Quaterniond qb=q0;
79  if (i == 3) {
80  qa.x() -= delta;
81  qb.x() += delta;
82  }
83  else if (i == 4) {
84  qa.y() -= delta;
85  qb.y() += delta;
86  }
87  else if (i == 5) {
88  qa.z() -= delta;
89  qb.z() += delta;
90  }
91  qa.normalize();
92  qb.normalize();
93  ta = SE3Quat(qa, tr0);
94  tb = SE3Quat(qb, tr0);
95  }
96 
97  Eigen::Vector3d dtr = (tb.translation() - ta.translation())*idelta;
98  Eigen::Vector3d taAngles, tbAngles;
99  quat_to_euler(ta.rotation(), taAngles(2), taAngles(1), taAngles(0));
100  quat_to_euler(tb.rotation(), tbAngles(2), tbAngles(1), tbAngles(0));
101  Eigen::Vector3d da = (tbAngles - taAngles) * idelta; //TODO wraparounds not handled
102 
103  for (int j=0; j<6; j++){
104  if (j<3){
105  J(j, i) = dtr(j);
106  } else {
107  J(j, i) = da(j-3);
108  }
109  }
110  }
111 }
g2o::deprecated::quat_to_euler
void quat_to_euler(const Eigen::Quaterniond &q, double &yaw, double &pitch, double &roll)
Definition: se3quat_gradients.cpp:48
template<typename Derived >
void g2o::deprecated::jacobian_3d_qman ( Eigen::MatrixBase< Derived > &  Ji,
Eigen::MatrixBase< Derived > &  Jj,
const double &  z11,
const double &  z12,
const double &  z13,
const double &  z14,
const double &  z21,
const double &  z22,
const double &  z23,
const double &  z24,
const double &  z31,
const double &  z32,
const double &  z33,
const double &  z34,
const double &  xab11,
const double &  xab12,
const double &  xab13,
const double &  xab14,
const double &  xab21,
const double &  xab22,
const double &  xab23,
const double &  xab24,
const double &  xab31,
const double &  xab32,
const double &  xab33,
const double &  xab34 
)

Definition at line 36 of file se3quat_gradients.h.

Referenced by g2o::deprecated::EdgeSE3::linearizeOplus().

43 {
44  Ji.derived().resize(6,6);
45  Jj.derived().resize(6,6);
46  (void) z14;
47  (void) z24;
48  (void) z34;
49  double _aux1 = xab11*z11 ;
50  double _aux2 = xab21*z12 ;
51  double _aux3 = xab31*z13 ;
52  double _aux4 = xab13*z11 ;
53  double _aux5 = xab23*z12 ;
54  double _aux6 = xab33*z13 ;
55  double _aux7 = xab11*z21 ;
56  double _aux8 = xab21*z22 ;
57  double _aux9 = xab31*z23 ;
58  double _aux10 = xab12*z21 ;
59  double _aux11 = xab22*z22 ;
60  double _aux12 = xab32*z23 ;
61  double _aux13 = xab12*z31 ;
62  double _aux14 = xab22*z32 ;
63  double _aux15 = xab32*z33 ;
64  double _aux16 = xab13*z31 ;
65  double _aux17 = xab23*z32 ;
66  double _aux18 = xab33*z33 ;
67  double _aux19 = _aux15+_aux14+_aux13-xab33*z23-xab23*z22-xab13*z21 ;
68  double _aux20 = -2*xab21*z23+2*xab31*z22+2*xab22*z13-2*xab32*z12 ;
69  double _aux21 = _aux9+_aux8+_aux7-xab32*z13-xab22*z12-xab12*z11 ;
70  double _aux22 = 2*xab21*z33-2*xab31*z32-2*xab23*z13+2*xab33*z12 ;
71  double _aux23 = _aux6+_aux5+_aux4-xab31*z33-xab21*z32-xab11*z31 ;
72  double _aux24 = -2*xab22*z33+2*xab32*z32+2*xab23*z23-2*xab33*z22 ;
73  double _aux25 = _aux3+_aux2+_aux18+_aux17+_aux16+_aux12+_aux11+_aux10+_aux1+1 ;
74  double _aux26 = 2*(-2*xab23*z33+2*xab33*z32-2*xab22*z23+2*xab32*z22-2*xab21*z13+2*xab31*z12)*_aux25+2*_aux19*_aux24+2*_aux22*_aux23+2*_aux20*_aux21 ;
75  double _aux27 = sqrt(pow(_aux25,2)+pow(_aux23,2)+pow(_aux21,2)+pow(_aux19,2)) ;
76  double _aux28 = 1/pow(_aux27,3) ;
77  double _aux29 = 1/_aux27 ;
78  double _aux30 = 2*xab11*z23-2*xab31*z21-2*xab12*z13+2*xab32*z11 ;
79  double _aux31 = -2*xab11*z33+2*xab31*z31+2*xab13*z13-2*xab33*z11 ;
80  double _aux32 = 2*xab12*z33-2*xab32*z31-2*xab13*z23+2*xab33*z21 ;
81  double _aux33 = 2*_aux19*_aux32+2*_aux23*_aux31+2*_aux21*_aux30+2*(2*xab13*z33-2*xab33*z31+2*xab12*z23-2*xab32*z21+2*xab11*z13-2*xab31*z11)*_aux25 ;
82  double _aux34 = -2*xab11*z22+2*xab21*z21+2*xab12*z12-2*xab22*z11 ;
83  double _aux35 = 2*xab11*z32-2*xab21*z31-2*xab13*z12+2*xab23*z11 ;
84  double _aux36 = -2*xab12*z32+2*xab22*z31+2*xab13*z22-2*xab23*z21 ;
85  double _aux37 = 2*_aux19*_aux36+2*_aux23*_aux35+2*_aux21*_aux34+2*(-2*xab13*z32+2*xab23*z31-2*xab12*z22+2*xab22*z21-2*xab11*z12+2*xab21*z11)*_aux25 ;
86  double _aux38 = 2*xab12*z21 ;
87  double _aux39 = 2*xab22*z22 ;
88  double _aux40 = 2*xab32*z23 ;
89  double _aux41 = 2*xab13*z31 ;
90  double _aux42 = 2*xab23*z32 ;
91  double _aux43 = 2*xab33*z33 ;
92  double _aux44 = _aux43+_aux42+_aux41+_aux40+_aux39+_aux38 ;
93  double _aux45 = -2*xab13*z11 ;
94  double _aux46 = -2*xab23*z12 ;
95  double _aux47 = -2*xab33*z13 ;
96  double _aux48 = _aux47+_aux46+_aux45 ;
97  double _aux49 = -2*xab32*z13-2*xab22*z12-2*xab12*z11 ;
98  double _aux50 = -2*xab12*z31 ;
99  double _aux51 = -2*xab22*z32 ;
100  double _aux52 = -2*xab32*z33 ;
101  double _aux53 = 2*_aux25*(_aux52+_aux51+_aux50+2*xab33*z23+2*xab23*z22+2*xab13*z21)+2*_aux23*_aux49+2*_aux21*_aux48+2*_aux19*_aux44 ;
102  double _aux54 = -2*xab11*z21 ;
103  double _aux55 = -2*xab21*z22 ;
104  double _aux56 = -2*xab31*z23 ;
105  double _aux57 = _aux56+_aux55+_aux54 ;
106  double _aux58 = -2*xab33*z23-2*xab23*z22-2*xab13*z21 ;
107  double _aux59 = 2*xab11*z11 ;
108  double _aux60 = 2*xab21*z12 ;
109  double _aux61 = 2*xab31*z13 ;
110  double _aux62 = _aux61+_aux60+_aux59+_aux43+_aux42+_aux41 ;
111  double _aux63 = 2*_aux23*_aux62+2*_aux21*_aux58+2*_aux19*_aux57+2*_aux25*(_aux47+_aux46+_aux45+2*xab31*z33+2*xab21*z32+2*xab11*z31) ;
112  double _aux64 = _aux61+_aux60+_aux59+_aux40+_aux39+_aux38 ;
113  double _aux65 = _aux52+_aux51+_aux50 ;
114  double _aux66 = -2*xab31*z33-2*xab21*z32-2*xab11*z31 ;
115  double _aux67 = 2*_aux19*_aux66+2*_aux23*_aux65+2*_aux21*_aux64+2*_aux25*(_aux56+_aux55+_aux54+2*xab32*z13+2*xab22*z12+2*xab12*z11) ;
116 
117  Ji ( 0 , 0 ) = -z11 ;
118  Ji ( 0 , 1 ) = -z12 ;
119  Ji ( 0 , 2 ) = -z13 ;
120  Ji ( 0 , 3 ) = 2*xab34*z12-2*xab24*z13 ;
121  Ji ( 0 , 4 ) = 2*xab14*z13-2*xab34*z11 ;
122  Ji ( 0 , 5 ) = 2*xab24*z11-2*xab14*z12 ;
123  Ji ( 1 , 0 ) = -z21 ;
124  Ji ( 1 , 1 ) = -z22 ;
125  Ji ( 1 , 2 ) = -z23 ;
126  Ji ( 1 , 3 ) = 2*xab34*z22-2*xab24*z23 ;
127  Ji ( 1 , 4 ) = 2*xab14*z23-2*xab34*z21 ;
128  Ji ( 1 , 5 ) = 2*xab24*z21-2*xab14*z22 ;
129  Ji ( 2 , 0 ) = -z31 ;
130  Ji ( 2 , 1 ) = -z32 ;
131  Ji ( 2 , 2 ) = -z33 ;
132  Ji ( 2 , 3 ) = 2*xab34*z32-2*xab24*z33 ;
133  Ji ( 2 , 4 ) = 2*xab14*z33-2*xab34*z31 ;
134  Ji ( 2 , 5 ) = 2*xab24*z31-2*xab14*z32 ;
135  Ji ( 3 , 0 ) = 0 ;
136  Ji ( 3 , 1 ) = 0 ;
137  Ji ( 3 , 2 ) = 0 ;
138  Ji ( 3 , 3 ) = _aux24*_aux29-0.5*_aux19*_aux26*_aux28 ;
139  Ji ( 3 , 4 ) = _aux29*_aux32-0.5*_aux19*_aux28*_aux33 ;
140  Ji ( 3 , 5 ) = _aux29*_aux36-0.5*_aux19*_aux28*_aux37 ;
141  Ji ( 4 , 0 ) = 0 ;
142  Ji ( 4 , 1 ) = 0 ;
143  Ji ( 4 , 2 ) = 0 ;
144  Ji ( 4 , 3 ) = _aux22*_aux29-0.5*_aux23*_aux26*_aux28 ;
145  Ji ( 4 , 4 ) = _aux29*_aux31-0.5*_aux23*_aux28*_aux33 ;
146  Ji ( 4 , 5 ) = _aux29*_aux35-0.5*_aux23*_aux28*_aux37 ;
147  Ji ( 5 , 0 ) = 0 ;
148  Ji ( 5 , 1 ) = 0 ;
149  Ji ( 5 , 2 ) = 0 ;
150  Ji ( 5 , 3 ) = _aux20*_aux29-0.5*_aux21*_aux26*_aux28 ;
151  Ji ( 5 , 4 ) = _aux29*_aux30-0.5*_aux21*_aux28*_aux33 ;
152  Ji ( 5 , 5 ) = _aux29*_aux34-0.5*_aux21*_aux28*_aux37 ;
153 
154  Jj( 0 , 0 ) = _aux3+_aux2+_aux1 ;
155  Jj( 0 , 1 ) = xab32*z13+xab22*z12+xab12*z11 ;
156  Jj( 0 , 2 ) = _aux6+_aux5+_aux4 ;
157  Jj( 0 , 3 ) = 0 ;
158  Jj( 0 , 4 ) = 0 ;
159  Jj( 0 , 5 ) = 0 ;
160  Jj( 1 , 0 ) = _aux9+_aux8+_aux7 ;
161  Jj( 1 , 1 ) = _aux12+_aux11+_aux10 ;
162  Jj( 1 , 2 ) = xab33*z23+xab23*z22+xab13*z21 ;
163  Jj( 1 , 3 ) = 0 ;
164  Jj( 1 , 4 ) = 0 ;
165  Jj( 1 , 5 ) = 0 ;
166  Jj( 2 , 0 ) = xab31*z33+xab21*z32+xab11*z31 ;
167  Jj( 2 , 1 ) = _aux15+_aux14+_aux13 ;
168  Jj( 2 , 2 ) = _aux18+_aux17+_aux16 ;
169  Jj( 2 , 3 ) = 0 ;
170  Jj( 2 , 4 ) = 0 ;
171  Jj( 2 , 5 ) = 0 ;
172  Jj( 3 , 0 ) = 0 ;
173  Jj( 3 , 1 ) = 0 ;
174  Jj( 3 , 2 ) = 0 ;
175  Jj( 3 , 3 ) = _aux29*_aux44-0.5*_aux19*_aux28*_aux53 ;
176  Jj( 3 , 4 ) = _aux29*_aux57-0.5*_aux19*_aux28*_aux63 ;
177  Jj( 3 , 5 ) = _aux29*_aux66-0.5*_aux19*_aux28*_aux67 ;
178  Jj( 4 , 0 ) = 0 ;
179  Jj( 4 , 1 ) = 0 ;
180  Jj( 4 , 2 ) = 0 ;
181  Jj( 4 , 3 ) = _aux29*_aux49-0.5*_aux23*_aux28*_aux53 ;
182  Jj( 4 , 4 ) = _aux29*_aux62-0.5*_aux23*_aux28*_aux63 ;
183  Jj( 4 , 5 ) = _aux29*_aux65-0.5*_aux23*_aux28*_aux67 ;
184  Jj( 5 , 0 ) = 0 ;
185  Jj( 5 , 1 ) = 0 ;
186  Jj( 5 , 2 ) = 0 ;
187  Jj( 5 , 3 ) = _aux29*_aux48-0.5*_aux21*_aux28*_aux53 ;
188  Jj( 5 , 4 ) = _aux29*_aux58-0.5*_aux21*_aux28*_aux63 ;
189  Jj( 5 , 5 ) = _aux29*_aux64-0.5*_aux21*_aux28*_aux67 ;
190 }
void g2o::deprecated::quat_to_euler ( const Eigen::Quaterniond &  q,
double &  yaw,
double &  pitch,
double &  roll 
)

Definition at line 48 of file se3quat_gradients.cpp.

Referenced by jac_quat3_euler3().

49 {
50  const double& q0 = q.w();
51  const double& q1 = q.x();
52  const double& q2 = q.y();
53  const double& q3 = q.z();
54  roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
55  pitch = asin(2*(q0*q2-q3*q1));
56  yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
57 }
Generated on Mon Mar 18 2019 20:14:53 for g2o by   doxygen 1.8.11