testRot3.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #include <gtsam/geometry/Point3.h>
21 #include <gtsam/geometry/Rot3.h>
22 #include <gtsam/base/testLie.h>
23 #include <gtsam/base/Testable.h>
25 #include <gtsam/base/lieProxies.h>
26 
27 #include <boost/math/constants/constants.hpp>
28 
30 
31 using namespace std;
32 using namespace gtsam;
33 
36 
37 static Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2);
38 static Point3 P(0.2, 0.7, -2.0);
39 static double error = 1e-9, epsilon = 0.001;
40 
41 //******************************************************************************
42 TEST(Rot3 , Concept) {
44  BOOST_CONCEPT_ASSERT((IsManifold<Rot3 >));
46 }
47 
48 /* ************************************************************************* */
49 TEST( Rot3, chart)
50 {
51  Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished();
52  Rot3 rot3(R);
53 }
54 
55 /* ************************************************************************* */
57 {
58  Rot3 expected((Matrix)I_3x3);
59  Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1);
60  Rot3 actual(r1, r2, r3);
61  CHECK(assert_equal(actual,expected));
62 }
63 
64 /* ************************************************************************* */
65 TEST( Rot3, constructor2)
66 {
67  Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished();
68  Rot3 actual(R);
69  Rot3 expected(0, 1, 0, 1, 0, 0, 0, 0, -1);
70  CHECK(assert_equal(actual,expected));
71 }
72 
73 /* ************************************************************************* */
74 TEST( Rot3, constructor3)
75 {
76  Rot3 expected(0, 1, 0, 1, 0, 0, 0, 0, -1);
77  Point3 r1(0,1,0), r2(1,0,0), r3(0,0,-1);
78  CHECK(assert_equal(expected,Rot3(r1,r2,r3)));
79 }
80 
81 /* ************************************************************************* */
82 TEST( Rot3, transpose)
83 {
84  Point3 r1(0,1,0), r2(1,0,0), r3(0,0,-1);
85  Rot3 R(0, 1, 0, 1, 0, 0, 0, 0, -1);
86  CHECK(assert_equal(R.inverse(),Rot3(r1,r2,r3)));
87 }
88 
89 /* ************************************************************************* */
91 {
92  CHECK(R.equals(R));
93  Rot3 zero;
94  CHECK(!R.equals(zero));
95 }
96 
97 /* ************************************************************************* */
98 // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
100  double t = w.norm();
101  Matrix3 J = skewSymmetric(w / t);
102  if (t < 1e-5) return Rot3();
103  Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
104  return Rot3(R);
105 }
106 
107 /* ************************************************************************* */
108 TEST( Rot3, AxisAngle)
109 {
110  Vector axis = Vector3(0., 1., 0.); // rotation around Y
111  double angle = 3.14 / 4.0;
112  Rot3 expected(0.707388, 0, 0.706825,
113  0, 1, 0,
114  -0.706825, 0, 0.707388);
115  Rot3 actual = Rot3::AxisAngle(axis, angle);
116  CHECK(assert_equal(expected,actual,1e-5));
117  Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI);
118  CHECK(assert_equal(expected,actual2,1e-5));
119 
120  axis = Vector3(0, 50, 0);
121  Rot3 actual3 = Rot3::AxisAngle(axis, angle);
122  CHECK(assert_equal(expected,actual3,1e-5));
123 }
124 
125 /* ************************************************************************* */
126 TEST( Rot3, Rodrigues)
127 {
128  Rot3 R1 = Rot3::Rodrigues(epsilon, 0, 0);
129  Vector w = (Vector(3) << epsilon, 0., 0.).finished();
131  CHECK(assert_equal(R2,R1));
132 }
133 
134 /* ************************************************************************* */
135 TEST( Rot3, Rodrigues2)
136 {
137  Vector axis = Vector3(0., 1., 0.); // rotation around Y
138  double angle = 3.14 / 4.0;
139  Rot3 expected(0.707388, 0, 0.706825,
140  0, 1, 0,
141  -0.706825, 0, 0.707388);
142  Rot3 actual = Rot3::AxisAngle(axis, angle);
143  CHECK(assert_equal(expected,actual,1e-5));
144  Rot3 actual2 = Rot3::Rodrigues(angle*axis);
145  CHECK(assert_equal(expected,actual2,1e-5));
146 }
147 
148 /* ************************************************************************* */
149 TEST( Rot3, Rodrigues3)
150 {
151  Vector w = Vector3(0.1, 0.2, 0.3);
152  Rot3 R1 = Rot3::AxisAngle(w / w.norm(), w.norm());
154  CHECK(assert_equal(R2,R1));
155 }
156 
157 /* ************************************************************************* */
158 TEST( Rot3, Rodrigues4)
159 {
160  Vector axis = Vector3(0., 0., 1.); // rotation around Z
161  double angle = M_PI/2.0;
162  Rot3 actual = Rot3::AxisAngle(axis, angle);
163  double c=cos(angle),s=sin(angle);
164  Rot3 expected(c,-s, 0,
165  s, c, 0,
166  0, 0, 1);
167  CHECK(assert_equal(expected,actual));
168  CHECK(assert_equal(slow_but_correct_Rodrigues(axis*angle),actual));
169 }
170 
171 /* ************************************************************************* */
172 TEST( Rot3, retract)
173 {
174  Vector v = Z_3x1;
175  CHECK(assert_equal(R, R.retract(v)));
176 
177 // // test Canonical coordinates
178 // Canonical<Rot3> chart;
179 // Vector v2 = chart.local(R);
180 // CHECK(assert_equal(R, chart.retract(v2)));
181 }
182 
183 /* ************************************************************************* */
185  static const double PI = boost::math::constants::pi<double>();
186  Vector w;
187  Rot3 R;
188 
189 #define CHECK_OMEGA(X, Y, Z) \
190  w = (Vector(3) << X, Y, Z).finished(); \
191  R = Rot3::Rodrigues(w); \
192  EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
193 
194  // Check zero
195  CHECK_OMEGA(0, 0, 0)
196 
197  // create a random direction:
198  double norm = sqrt(1.0 + 16.0 + 4.0);
199  double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm;
200 
201  // Check very small rotation for Taylor expansion
202  // Note that tolerance above is 1e-12, so Taylor is pretty good !
203  double d = 0.0001;
204  CHECK_OMEGA(d, 0, 0)
205  CHECK_OMEGA(0, d, 0)
206  CHECK_OMEGA(0, 0, d)
207  CHECK_OMEGA(x * d, y * d, z * d)
208 
209  // check normal rotation
210  d = 0.1;
211  CHECK_OMEGA(d, 0, 0)
212  CHECK_OMEGA(0, d, 0)
213  CHECK_OMEGA(0, 0, d)
214  CHECK_OMEGA(x * d, y * d, z * d)
215 
216  // Check 180 degree rotations
217  CHECK_OMEGA(PI, 0, 0)
218  CHECK_OMEGA(0, PI, 0)
219  CHECK_OMEGA(0, 0, PI)
220 
221  // Windows and Linux have flipped sign in quaternion mode
222 #if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
223  w = (Vector(3) << x * PI, y * PI, z * PI).finished();
224  R = Rot3::Rodrigues(w);
225  EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12));
226 #else
227  CHECK_OMEGA(x * PI, y * PI, z * PI)
228 #endif
229 
230  // Check 360 degree rotations
231 #define CHECK_OMEGA_ZERO(X, Y, Z) \
232  w = (Vector(3) << X, Y, Z).finished(); \
233  R = Rot3::Rodrigues(w); \
234  EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
235 
236  CHECK_OMEGA_ZERO(2.0 * PI, 0, 0)
237  CHECK_OMEGA_ZERO(0, 2.0 * PI, 0)
238  CHECK_OMEGA_ZERO(0, 0, 2.0 * PI)
239  CHECK_OMEGA_ZERO(x * 2. * PI, y * 2. * PI, z * 2. * PI)
240 
241  // Check problematic case from Lund dataset vercingetorix.g2o
242  // This is an almost rotation with determinant not *quite* 1.
243  Rot3 Rlund(-0.98582676, -0.03958746, -0.16303092, //
244  -0.03997006, -0.88835923, 0.45740671, //
245  -0.16293753, 0.45743998, 0.87418537);
246 
247  // Rot3's Logmap returns different, but equivalent compacted
248  // axis-angle vectors depending on whether Rot3 is implemented
249  // by Quaternions or SO3.
250  #if defined(GTSAM_USE_QUATERNIONS)
251  // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
252  EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
253  (Vector)Rot3::Logmap(Rlund), 1e-8));
254  #else
255  // SO3 does not bound angle resulting in ~180.1 degrees
256  EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
257  (Vector)Rot3::Logmap(Rlund), 1e-8));
258  #endif
259 }
260 
261 /* ************************************************************************* */
262 TEST(Rot3, retract_localCoordinates)
263 {
264  Vector3 d12 = Vector3::Constant(0.1);
265  Rot3 R2 = R.retract(d12);
267 }
268 /* ************************************************************************* */
269 TEST(Rot3, expmap_logmap)
270 {
271  Vector d12 = Vector3::Constant(0.1);
272  Rot3 R2 = R.expmap(d12);
273  EXPECT(assert_equal(d12, R.logmap(R2)));
274 }
275 
276 /* ************************************************************************* */
277 TEST(Rot3, retract_localCoordinates2)
278 {
279  Rot3 t1 = R, t2 = R*R, origin;
280  Vector d12 = t1.localCoordinates(t2);
281  EXPECT(assert_equal(t2, t1.retract(d12)));
282  Vector d21 = t2.localCoordinates(t1);
283  EXPECT(assert_equal(t1, t2.retract(d21)));
284  EXPECT(assert_equal(d12, -d21));
285 }
286 /* ************************************************************************* */
287 TEST(Rot3, manifold_expmap)
288 {
289  Rot3 gR1 = Rot3::Rodrigues(0.1, 0.4, 0.2);
290  Rot3 gR2 = Rot3::Rodrigues(0.3, 0.1, 0.7);
291  Rot3 origin;
292 
293  // log behaves correctly
294  Vector d12 = Rot3::Logmap(gR1.between(gR2));
295  Vector d21 = Rot3::Logmap(gR2.between(gR1));
296 
297  // Check expmap
298  CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
299  CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
300 
301  // Check that log(t1,t2)=-log(t2,t1)
302  CHECK(assert_equal(d12,-d21));
303 
304  // lines in canonical coordinates correspond to Abelian subgroups in SO(3)
305  Vector d = Vector3(0.1, 0.2, 0.3);
306  // exp(-d)=inverse(exp(d))
308  // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
309  Rot3 R2 = Rot3::Expmap (2 * d);
310  Rot3 R3 = Rot3::Expmap (3 * d);
311  Rot3 R5 = Rot3::Expmap (5 * d);
312  CHECK(assert_equal(R5,R2*R3));
313  CHECK(assert_equal(R5,R3*R2));
314 }
315 
316 /* ************************************************************************* */
317 class AngularVelocity : public Vector3 {
318  public:
319  template <typename Derived>
321  : Vector3(v) {}
322 
323  AngularVelocity(double wx, double wy, double wz) : Vector3(wx, wy, wz) {}
324 };
325 
327  return X.cross(Y);
328 }
329 
330 /* ************************************************************************* */
332 {
333  // Approximate exmap by BCH formula
334  AngularVelocity w1(0.2, -0.1, 0.1);
335  AngularVelocity w2(0.01, 0.02, -0.03);
336  Rot3 R1 = Rot3::Expmap (w1), R2 = Rot3::Expmap (w2);
337  Rot3 R3 = R1 * R2;
338  Vector expected = Rot3::Logmap(R3);
339  Vector actual = BCH(w1, w2);
340  CHECK(assert_equal(expected, actual,1e-5));
341 }
342 
343 /* ************************************************************************* */
344 TEST( Rot3, rotate_derivatives)
345 {
346  Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
347  R.rotate(P, actualDrotate1a, actualDrotate2);
348  R.inverse().rotate(P, actualDrotate1b, boost::none);
349  Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3,Point3>, R, P);
350  Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3,Point3>, R.inverse(), P);
351  Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3,Point3>, R, P);
352  EXPECT(assert_equal(numerical1,actualDrotate1a,error));
353  EXPECT(assert_equal(numerical2,actualDrotate1b,error));
354  EXPECT(assert_equal(numerical3,actualDrotate2, error));
355 }
356 
357 /* ************************************************************************* */
359 {
360  Point3 w = R * P;
361  Matrix H1,H2;
362  Point3 actual = R.unrotate(w,H1,H2);
363  CHECK(assert_equal(P,actual));
364 
365  Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3,Point3>, R, w);
366  CHECK(assert_equal(numerical1,H1,error));
367 
368  Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3,Point3>, R, w);
369  CHECK(assert_equal(numerical2,H2,error));
370 }
371 
372 /* ************************************************************************* */
374 {
375  Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
376  Rot3 R2 = Rot3::Rodrigues(0.2, 0.3, 0.5);
377 
378  Rot3 expected = R1 * R2;
379  Matrix actualH1, actualH2;
380  Rot3 actual = R1.compose(R2, actualH1, actualH2);
381  CHECK(assert_equal(expected,actual));
382 
383  Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3>, R1,
384  R2, 1e-2);
385  CHECK(assert_equal(numericalH1,actualH1));
386 
387  Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3>, R1,
388  R2, 1e-2);
389  CHECK(assert_equal(numericalH2,actualH2));
390 }
391 
392 /* ************************************************************************* */
394 {
395  Rot3 R = Rot3::Rodrigues(0.1, 0.2, 0.3);
396 
397  Rot3 I;
398  Matrix3 actualH;
399  Rot3 actual = R.inverse(actualH);
400  CHECK(assert_equal(I,R*actual));
401  CHECK(assert_equal(I,actual*R));
402  CHECK(assert_equal(actual.matrix(), R.transpose()));
403 
404  Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
405  CHECK(assert_equal(numericalH,actualH));
406 }
407 
408 /* ************************************************************************* */
410 {
411  Rot3 r1 = Rot3::Rz(M_PI/3.0);
412  Rot3 r2 = Rot3::Rz(2.0*M_PI/3.0);
413 
414  Matrix expectedr1 = (Matrix(3, 3) <<
415  0.5, -sqrt(3.0)/2.0, 0.0,
416  sqrt(3.0)/2.0, 0.5, 0.0,
417  0.0, 0.0, 1.0).finished();
418  EXPECT(assert_equal(expectedr1, r1.matrix()));
419 
420  Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2);
421  Rot3 origin;
422  EXPECT(assert_equal(R, origin.between(R)));
423  EXPECT(assert_equal(R.inverse(), R.between(origin)));
424 
425  Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
426  Rot3 R2 = Rot3::Rodrigues(0.2, 0.3, 0.5);
427 
428  Rot3 expected = R1.inverse() * R2;
429  Matrix actualH1, actualH2;
430  Rot3 actual = R1.between(R2, actualH1, actualH2);
431  EXPECT(assert_equal(expected,actual));
432 
433  Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2);
434  CHECK(assert_equal(numericalH1,actualH1));
435 
436  Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2);
437  CHECK(assert_equal(numericalH2,actualH2));
438 }
439 
440 /* ************************************************************************* */
441 TEST( Rot3, xyz )
442 {
443  double t = 0.1, st = sin(t), ct = cos(t);
444 
445  // Make sure all counterclockwise
446  // Diagrams below are all from from unchanging axis
447 
448  // z
449  // | * Y=(ct,st)
450  // x----y
451  Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
452  CHECK(assert_equal(expected1,Rot3::Rx(t)));
453 
454  // x
455  // | * Z=(ct,st)
456  // y----z
457  Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
458  CHECK(assert_equal(expected2,Rot3::Ry(t)));
459 
460  // y
461  // | X=* (ct,st)
462  // z----x
463  Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
464  CHECK(assert_equal(expected3,Rot3::Rz(t)));
465 
466  // Check compound rotation
467  Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1);
468  CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3)));
469 }
470 
471 /* ************************************************************************* */
472 TEST( Rot3, yaw_pitch_roll )
473 {
474  double t = 0.1;
475 
476  // yaw is around z axis
477  CHECK(assert_equal(Rot3::Rz(t),Rot3::Yaw(t)));
478 
479  // pitch is around y axis
480  CHECK(assert_equal(Rot3::Ry(t),Rot3::Pitch(t)));
481 
482  // roll is around x axis
483  CHECK(assert_equal(Rot3::Rx(t),Rot3::Roll(t)));
484 
485  // Check compound rotation
486  Rot3 expected = Rot3::Yaw(0.1) * Rot3::Pitch(0.2) * Rot3::Roll(0.3);
487  CHECK(assert_equal(expected,Rot3::Ypr(0.1,0.2,0.3)));
488 
489  CHECK(assert_equal((Vector)Vector3(0.1, 0.2, 0.3),expected.ypr()));
490 }
491 
492 /* ************************************************************************* */
494 {
495  // Try RQ on a pure rotation
496  Matrix actualK;
497  Vector actual;
498  boost::tie(actualK, actual) = RQ(R.matrix());
499  Vector expected = Vector3(0.14715, 0.385821, 0.231671);
500  CHECK(assert_equal(I_3x3,actualK));
501  CHECK(assert_equal(expected,actual,1e-6));
502 
503  // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
504  CHECK(assert_equal(expected,R.xyz(),1e-6));
505  CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
506 
507  // Try using ypr call, asserting that Rot3::Ypr(y,p,r).ypr()==[y;p;r]
508  CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::Ypr(0.1,0.2,0.3).ypr()));
509  CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::Ypr(0.1,0.2,0.3).rpy()));
510 
511  // Try ypr for pure yaw-pitch-roll matrices
512  CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::Yaw (0.1).ypr()));
513  CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::Pitch(0.1).ypr()));
514  CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::Roll (0.1).ypr()));
515 
516  // Try RQ to recover calibration from 3*3 sub-block of projection matrix
517  Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0).finished();
518  Matrix A = K * R.matrix();
519  boost::tie(actualK, actual) = RQ(A);
520  CHECK(assert_equal(K,actualK));
521  CHECK(assert_equal(expected,actual,1e-6));
522 }
523 
524 /* ************************************************************************* */
525 TEST( Rot3, expmapStability ) {
526  Vector w = Vector3(78e-9, 5e-8, 97e-7);
527  double theta = w.norm();
528  double theta2 = theta*theta;
529  Rot3 actualR = Rot3::Expmap(w);
530  Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1),
531  w(2), 0.0, -w(0),
532  -w(1), w(0), 0.0 ).finished();
533  Matrix W2 = W*W;
534  Matrix Rmat = I_3x3 + (1.0-theta2/6.0 + theta2*theta2/120.0
535  - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
536  Rot3 expectedR( Rmat );
537  CHECK(assert_equal(expectedR, actualR, 1e-10));
538 }
539 
540 /* ************************************************************************* */
541 TEST( Rot3, logmapStability ) {
542  Vector w = Vector3(1e-8, 0.0, 0.0);
543  Rot3 R = Rot3::Expmap(w);
544 // double tr = R.r1().x()+R.r2().y()+R.r3().z();
545 // std::cout.precision(5000);
546 // std::cout << "theta: " << w.norm() << std::endl;
547 // std::cout << "trace: " << tr << std::endl;
548 // R.print("R = ");
549  Vector actualw = Rot3::Logmap(R);
550  CHECK(assert_equal(w, actualw, 1e-15));
551 }
552 
553 /* ************************************************************************* */
555  // NOTE: This is also verifying the ability to convert Vector to Quaternion
556  Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
557  Rot3 R1(0.271018623057411, 0.278786459830371, 0.921318086098018,
558  0.578529366719085, 0.717799701969298, -0.387385285854279,
559  -0.769319620053772, 0.637998195662053, 0.033250932803219);
560 
561  Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335,
562  0.599136268678053);
563  Rot3 R2(-0.207341903877828, 0.250149415542075, 0.945745528564780,
564  0.881304914479026, -0.371869043667957, 0.291573424846290,
565  0.424630407073532, 0.893945571198514, -0.143353873763946);
566 
567  // Check creating Rot3 from quaternion
568  EXPECT(assert_equal(R1, Rot3(q1)));
569  EXPECT(assert_equal(R1, Rot3::Quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
570  EXPECT(assert_equal(R2, Rot3(q2)));
571  EXPECT(assert_equal(R2, Rot3::Quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
572 
573  // Check converting Rot3 to quaterion
576 
577  // Check that quaternion and Rot3 represent the same rotation
578  Point3 p1(1.0, 2.0, 3.0);
579  Point3 p2(8.0, 7.0, 9.0);
580 
581  Point3 expected1 = R1*p1;
582  Point3 expected2 = R2*p2;
583 
584  Point3 actual1 = Point3(q1*p1);
585  Point3 actual2 = Point3(q2*p2);
586 
587  EXPECT(assert_equal(expected1, actual1));
588  EXPECT(assert_equal(expected2, actual2));
589 }
590 
591 /* ************************************************************************* */
593  Matrix::Index n = A.cols();
594  const Matrix I = Matrix::Identity(n,n);
595  return (I-A)*(I+A).inverse();
596 }
597 
599  Matrix A = skewSymmetric(1,2,-3);
600  Matrix Q = Cayley(A);
601  EXPECT(assert_equal((Matrix)I_3x3, trans(Q)*Q));
602  EXPECT(assert_equal(A, Cayley(Q)));
603 }
604 
605 /* ************************************************************************* */
607 {
608  Rot3 R;
609  std::ostringstream os;
610  os << R;
611  string expected = "[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]";
612  EXPECT(os.str() == expected);
613 }
614 
615 /* ************************************************************************* */
616 TEST( Rot3, slerp)
617 {
618  // A first simple test
619  Rot3 R1 = Rot3::Rz(1), R2 = Rot3::Rz(2), R3 = Rot3::Rz(1.5);
620  EXPECT(assert_equal(R1, R1.slerp(0.0,R2)));
621  EXPECT(assert_equal(R2, R1.slerp(1.0,R2)));
622  EXPECT(assert_equal(R3, R1.slerp(0.5,R2)));
623  // Make sure other can be *this
624  EXPECT(assert_equal(R1, R1.slerp(0.5,R1)));
625 }
626 
627 //******************************************************************************
628 Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1));
629 Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2));
630 
631 //******************************************************************************
632 TEST(Rot3 , Invariants) {
633  Rot3 id;
634 
635  EXPECT(check_group_invariants(id,id));
636  EXPECT(check_group_invariants(id,T1));
637  EXPECT(check_group_invariants(T2,id));
638  EXPECT(check_group_invariants(T2,T1));
639  EXPECT(check_group_invariants(T1,T2));
640 
641  EXPECT(check_manifold_invariants(id,id));
642  EXPECT(check_manifold_invariants(id,T1));
643  EXPECT(check_manifold_invariants(T2,id));
644  EXPECT(check_manifold_invariants(T2,T1));
645  EXPECT(check_manifold_invariants(T1,T2));
646 }
647 
648 //******************************************************************************
649 TEST(Rot3 , LieGroupDerivatives) {
650  Rot3 id;
651 
657 }
658 
659 //******************************************************************************
660 TEST(Rot3 , ChartDerivatives) {
661  Rot3 id;
662  if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
668  }
669 }
670 
671 /* ************************************************************************* */
672 TEST(Rot3, ClosestTo) {
673  Matrix3 M;
674  M << 0.79067393, 0.6051136, -0.0930814, //
675  0.4155925, -0.64214347, -0.64324489, //
676  -0.44948549, 0.47046326, -0.75917576;
677 
678  Matrix expected(3, 3);
679  expected << 0.790687, 0.605096, -0.0931312, //
680  0.415746, -0.642355, -0.643844, //
681  -0.449411, 0.47036, -0.759468;
682 
683  auto actual = Rot3::ClosestTo(3*M);
684  EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
685 }
686 
687 /* ************************************************************************* */
688 TEST(Rot3, axisAngle) {
689  Unit3 actualAxis;
690  double actualAngle;
691 
692 // not a lambda as otherwise we can't trace error easily
693 #define CHECK_AXIS_ANGLE(expectedAxis, expectedAngle, rotation) \
694  std::tie(actualAxis, actualAngle) = rotation.axisAngle(); \
695  EXPECT(assert_equal(expectedAxis, actualAxis, 1e-9)); \
696  EXPECT_DOUBLES_EQUAL(expectedAngle, actualAngle, 1e-9); \
697  EXPECT(assert_equal(rotation, Rot3::AxisAngle(expectedAxis, expectedAngle)))
698 
699  // CHECK R defined at top = Rot3::Rodrigues(0.1, 0.4, 0.2)
700  Vector3 omega(0.1, 0.4, 0.2);
701  Unit3 axis(omega), _axis(-omega);
702  CHECK_AXIS_ANGLE(axis, omega.norm(), R);
703 
704  // rotate by 90
705  CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, M_PI_2))
706  CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, M_PI_2, 0))
707  CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(M_PI_2, 0, 0))
708  CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, M_PI_2))
709 
710  // rotate by -90
711  CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, -M_PI_2))
712  CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, -M_PI_2, 0))
713  CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(-M_PI_2, 0, 0))
714  CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, -M_PI_2))
715 
716  // rotate by 270
717  const double theta270 = M_PI + M_PI / 2;
718  CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta270))
719  CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, theta270, 0))
720  CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(theta270, 0, 0))
721  CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, theta270))
722 
723  // rotate by -270
724  const double theta_270 = -(M_PI + M_PI / 2); // 90 (or -270) degrees
725  CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta_270))
726  CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, theta_270, 0))
727  CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(theta_270, 0, 0))
728  CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, theta_270))
729 
730  const double theta195 = 195 * M_PI / 180;
731  const double theta165 = 165 * M_PI / 180;
732 
734  CHECK_AXIS_ANGLE(Unit3(1, 0, 0), theta165, Rot3::Ypr(0, 0, theta165))
735  CHECK_AXIS_ANGLE(Unit3(0, 1, 0), theta165, Rot3::Ypr(0, theta165, 0))
736  CHECK_AXIS_ANGLE(Unit3(0, 0, 1), theta165, Rot3::Ypr(theta165, 0, 0))
737  CHECK_AXIS_ANGLE(axis, theta165, Rot3::AxisAngle(axis, theta165))
738 
739 
740  CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), theta165, Rot3::Ypr(0, 0, theta195))
741  CHECK_AXIS_ANGLE(Unit3(0, -1, 0), theta165, Rot3::Ypr(0, theta195, 0))
742  CHECK_AXIS_ANGLE(Unit3(0, 0, -1), theta165, Rot3::Ypr(theta195, 0, 0))
743  CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195))
744 }
745 
746 /* ************************************************************************* */
747 Rot3 RzRyRx_proxy(double const& a, double const& b, double const& c) {
748  return Rot3::RzRyRx(a, b, c);
749 }
750 
751 TEST(Rot3, RzRyRx_scalars_derivative) {
752  const auto x = 0.1, y = 0.4, z = 0.2;
753  const auto num_x = numericalDerivative31(RzRyRx_proxy, x, y, z);
754  const auto num_y = numericalDerivative32(RzRyRx_proxy, x, y, z);
755  const auto num_z = numericalDerivative33(RzRyRx_proxy, x, y, z);
756 
757  Vector3 act_x, act_y, act_z;
758  Rot3::RzRyRx(x, y, z, act_x, act_y, act_z);
759 
760  CHECK(assert_equal(num_x, act_x));
761  CHECK(assert_equal(num_y, act_y));
762  CHECK(assert_equal(num_z, act_z));
763 }
764 
765 /* ************************************************************************* */
766 Rot3 RzRyRx_proxy(Vector3 const& xyz) { return Rot3::RzRyRx(xyz); }
767 
768 TEST(Rot3, RzRyRx_vector_derivative) {
769  const auto xyz = Vector3{-0.3, 0.1, 0.7};
770  const auto num = numericalDerivative11(RzRyRx_proxy, xyz);
771 
772  Matrix3 act;
773  Rot3::RzRyRx(xyz, act);
774 
775  CHECK(assert_equal(num, act));
776 }
777 
778 /* ************************************************************************* */
779 Rot3 Ypr_proxy(double const& y, double const& p, double const& r) {
780  return Rot3::Ypr(y, p, r);
781 }
782 
783 TEST(Rot3, Ypr_derivative) {
784  const auto y = 0.7, p = -0.3, r = 0.1;
785  const auto num_y = numericalDerivative31(Ypr_proxy, y, p, r);
786  const auto num_p = numericalDerivative32(Ypr_proxy, y, p, r);
787  const auto num_r = numericalDerivative33(Ypr_proxy, y, p, r);
788 
789  Vector3 act_y, act_p, act_r;
790  Rot3::Ypr(y, p, r, act_y, act_p, act_r);
791 
792  CHECK(assert_equal(num_y, act_y));
793  CHECK(assert_equal(num_p, act_p));
794  CHECK(assert_equal(num_r, act_r));
795 }
796 
797 /* ************************************************************************* */
798 Vector3 RQ_proxy(Matrix3 const& R) {
799  const auto RQ_ypr = RQ(R);
800  return RQ_ypr.second;
801 }
802 
803 TEST(Rot3, RQ_derivative) {
804  using VecAndErr = std::pair<Vector3, double>;
805  std::vector<VecAndErr> test_xyz;
806  // Test zeros and a couple of random values
807  test_xyz.push_back(VecAndErr{{0, 0, 0}, error});
808  test_xyz.push_back(VecAndErr{{0, 0.5, -0.5}, error});
809  test_xyz.push_back(VecAndErr{{0.3, 0, 0.2}, error});
810  test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, 1e-8});
811  test_xyz.push_back(VecAndErr{{1.0, 0.7, 0.8}, error});
812  test_xyz.push_back(VecAndErr{{3.0, 0.7, -0.6}, error});
813  test_xyz.push_back(VecAndErr{{M_PI / 2, 0, 0}, error});
814  test_xyz.push_back(VecAndErr{{0, 0, M_PI / 2}, error});
815 
816  // Test close to singularity
817  test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-7});
818  test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-7});
819  test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1.1e-2, 0}, 1e-4});
820  test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1.1e-2, 0}, 1e-4});
821 
822  for (auto const& vec_err : test_xyz) {
823  auto const& xyz = vec_err.first;
824 
825  const auto R = Rot3::RzRyRx(xyz).matrix();
826  const auto num = numericalDerivative11(RQ_proxy, R);
827  Matrix39 calc;
828  RQ(R, calc);
829 
830  const auto err = vec_err.second;
831  CHECK(assert_equal(num, calc, err));
832  }
833 }
834 
835 /* ************************************************************************* */
836 Vector3 xyz_proxy(Rot3 const& R) { return R.xyz(); }
837 
838 TEST(Rot3, xyz_derivative) {
839  const auto aa = Vector3{-0.6, 0.3, 0.2};
840  const auto R = Rot3::Expmap(aa);
841  const auto num = numericalDerivative11(xyz_proxy, R);
842  Matrix3 calc;
843  R.xyz(calc);
844 
845  CHECK(assert_equal(num, calc));
846 }
847 
848 /* ************************************************************************* */
849 Vector3 ypr_proxy(Rot3 const& R) { return R.ypr(); }
850 
851 TEST(Rot3, ypr_derivative) {
852  const auto aa = Vector3{0.1, -0.3, -0.2};
853  const auto R = Rot3::Expmap(aa);
854  const auto num = numericalDerivative11(ypr_proxy, R);
855  Matrix3 calc;
856  R.ypr(calc);
857 
858  CHECK(assert_equal(num, calc));
859 }
860 
861 /* ************************************************************************* */
862 Vector3 rpy_proxy(Rot3 const& R) { return R.rpy(); }
863 
864 TEST(Rot3, rpy_derivative) {
865  const auto aa = Vector3{1.2, 0.3, -0.9};
866  const auto R = Rot3::Expmap(aa);
867  const auto num = numericalDerivative11(rpy_proxy, R);
868  Matrix3 calc;
869  R.rpy(calc);
870 
871  CHECK(assert_equal(num, calc));
872 }
873 
874 /* ************************************************************************* */
875 double roll_proxy(Rot3 const& R) { return R.roll(); }
876 
877 TEST(Rot3, roll_derivative) {
878  const auto aa = Vector3{0.8, -0.8, 0.8};
879  const auto R = Rot3::Expmap(aa);
880  const auto num = numericalDerivative11(roll_proxy, R);
881  Matrix13 calc;
882  R.roll(calc);
883 
884  CHECK(assert_equal(num, calc));
885 }
886 
887 /* ************************************************************************* */
888 double pitch_proxy(Rot3 const& R) { return R.pitch(); }
889 
890 TEST(Rot3, pitch_derivative) {
891  const auto aa = Vector3{0.01, 0.1, 0.0};
892  const auto R = Rot3::Expmap(aa);
893  const auto num = numericalDerivative11(pitch_proxy, R);
894  Matrix13 calc;
895  R.pitch(calc);
896 
897  CHECK(assert_equal(num, calc));
898 }
899 
900 /* ************************************************************************* */
901 double yaw_proxy(Rot3 const& R) { return R.yaw(); }
902 
903 TEST(Rot3, yaw_derivative) {
904  const auto aa = Vector3{0.0, 0.1, 0.6};
905  const auto R = Rot3::Expmap(aa);
906  const auto num = numericalDerivative11(yaw_proxy, R);
907  Matrix13 calc;
908  R.yaw(calc);
909 
910  CHECK(assert_equal(num, calc));
911 }
912 
913 /* ************************************************************************* */
915  size_t degree = 1;
916  Rot3 R_w0; // Zero rotation
917  Rot3 R_w1 = Rot3::Ry(degree * M_PI / 180);
918 
919  Rot3 R_01, R_w2;
920  double actual, expected = 1.0;
921 
922  for (size_t i = 2; i < 360; ++i) {
923  R_01 = R_w0.between(R_w1);
924  R_w2 = R_w1 * R_01;
925  R_w0 = R_w1;
926  R_w1 = R_w2.normalized();
927  actual = R_w2.matrix().determinant();
928 
929  EXPECT_DOUBLES_EQUAL(expected, actual, 1e-7);
930  }
931 }
932 
933 /* ************************************************************************* */
934 int main() {
935  TestResult tr;
936  return TestRegistry::runAllTests(tr);
937 }
938 /* ************************************************************************* */
939 
#define CHECK_OMEGA_ZERO(X, Y, Z)
EIGEN_DEVICE_FUNC Coefficients & coeffs()
Rot3 RzRyRx_proxy(double const &a, double const &b, double const &c)
Definition: testRot3.cpp:747
#define CHECK(condition)
Definition: Test.h:109
Point2 q2
Definition: testPose2.cpp:753
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:38
double yaw(OptionalJacobian< 1, 3 > H=boost::none) const
Definition: Rot3.cpp:219
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
Scalar * y
double pitch(OptionalJacobian< 1, 3 > H=boost::none) const
Definition: Rot3.cpp:207
Point2 q1
Definition: testPose2.cpp:753
EIGEN_DEVICE_FUNC CoeffReturnType x() const
Scalar * b
Definition: benchVecAdd.cpp:17
Concept check for values that can be used in unit tests.
AngularVelocity bracket(const AngularVelocity &X, const AngularVelocity &Y)
Definition: testRot3.cpp:326
Provides convenient mappings of common member functions for testing.
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
T between(const T &t1, const T &t2)
Definition: lieProxies.h:36
Vector3f p1
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
Q id(Eigen::AngleAxisd(0, Q_z_axis))
EIGEN_DEVICE_FUNC CoeffReturnType y() const
Rot3 slerp(double t, const Rot3 &other) const
Spherical Linear intERPolation between *this and other.
Definition: Rot3.cpp:324
Matrix expected
Definition: testMatrix.cpp:974
double roll(OptionalJacobian< 1, 3 > H=boost::none) const
Definition: Rot3.cpp:195
void determinant(const MatrixType &m)
Definition: determinant.cpp:14
static Point3 P(0.2, 0.7,-2.0)
EIGEN_DONT_INLINE Scalar zero()
Definition: svd_common.h:271
Matrix Cayley(const Matrix &A)
Definition: testRot3.cpp:592
ArrayXcf v
Definition: Cwise_arg.cpp:1
Vector3 xyz_proxy(Rot3 const &R)
Definition: testRot3.cpp:836
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept< ListOfOneContainer< int > >))
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
#define M_PI
Definition: main.h:78
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from world to rotated frame
Definition: Rot3.cpp:136
EIGEN_DEVICE_FUNC const LogReturnType log() const
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Pose2_ Expmap(const Vector3_ &xi)
Definition: Half.h:150
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative33(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Key W(std::uint64_t j)
static char trans
Some functions to compute numerical derivatives.
Matrix3 matrix() const
Definition: Rot3M.cpp:219
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
T BCH(const T &X, const T &Y)
AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
Definition: Lie.h:297
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const boost::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Vector3 ypr(OptionalJacobian< 3, 3 > H=boost::none) const
Definition: Rot3.cpp:184
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Definition: Rot3M.cpp:149
Rot2 theta
#define GTSAM_CONCEPT_LIE_INST(T)
Definition: Lie.h:355
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
void quaternion(void)
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
static double epsilon
Definition: testRot3.cpp:39
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:311
Vector3 RQ_proxy(Matrix3 const &R)
Definition: testRot3.cpp:798
Array33i a
EIGEN_DEVICE_FUNC CoeffReturnType w() const
#define CHECK_LIE_GROUP_DERIVATIVES(t1, t2)
Definition: testLie.h:82
EIGEN_DEVICE_FUNC CoeffReturnType z() const
Vector3 xyz(OptionalJacobian< 3, 3 > H=boost::none) const
Definition: Rot3.cpp:161
#define CHECK_OMEGA(X, Y, Z)
const double degree
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
EIGEN_DEVICE_FUNC const CosReturnType cos() const
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(boost::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
pair< Matrix3, Vector3 > RQ(const Matrix3 &A, OptionalJacobian< 3, 9 > H)
Definition: Rot3.cpp:258
Class compose(const Class &g) const
Definition: Lie.h:47
Eigen::VectorXd Vector
Definition: Vector.h:38
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(boost::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
bool equals(const Rot3 &p, double tol=1e-9) const
Definition: Rot3.cpp:100
P unrotate(const T &r, const P &pt)
Definition: lieProxies.h:50
Class expmap(const TangentVector &v) const
Definition: Lie.h:77
double roll_proxy(Rot3 const &R)
Definition: testRot3.cpp:875
AngularVelocity(const Eigen::MatrixBase< Derived > &v)
Definition: testRot3.cpp:320
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:33
#define EXPECT(condition)
Definition: Test.h:151
Vector3 ypr_proxy(Rot3 const &R)
Definition: testRot3.cpp:849
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Rot3 slow_but_correct_Rodrigues(const Vector &w)
Definition: testRot3.cpp:99
RealScalar s
static const double r2
static const Matrix I
Definition: lago.cpp:35
#define ROT3_DEFAULT_COORDINATES_MODE
Definition: Rot3.h:43
JacobiRotation< float > J
Q R1(Eigen::AngleAxisd(1, Q_z_axis))
static const double r3
RowVector3d w
TangentVector logmap(const Class &g) const
Definition: Lie.h:83
traits
Definition: chartTesting.h:28
static Rot3 R3
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:130
double yaw_proxy(Rot3 const &R)
Definition: testRot3.cpp:901
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:404
Rot2 expectedR
Definition: testPose2.cpp:151
static const double r1
ofstream os("timeSchurFactors.csv")
The quaternion class used to represent 3D orientations and rotations.
Rot3 normalized() const
Definition: Rot3M.cpp:112
Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1))
#define CHECK_AXIS_ANGLE(expectedAxis, expectedAngle, rotation)
Rot3 Ypr_proxy(double const &y, double const &p, double const &r)
Definition: testRot3.cpp:779
int main()
Definition: testRot3.cpp:934
#define CHECK_CHART_DERIVATIVES(t1, t2)
Definition: testLie.h:85
3D Point
float * p
Class between(const Class &g) const
Definition: Lie.h:51
Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2))
static Point3 p2
static double error
Definition: testRot3.cpp:39
Vector3 rpy(OptionalJacobian< 3, 3 > H=boost::none) const
Definition: Rot3.cpp:192
EIGEN_DEVICE_FUNC const SinReturnType sin() const
TEST(Rot3, Concept)
Definition: testRot3.cpp:42
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Vector3 Point3
Definition: Point3.h:35
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(boost::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
AngularVelocity(double wx, double wy, double wz)
Definition: testRot3.cpp:323
#define X
Definition: icosphere.cpp:20
double pitch_proxy(Rot3 const &R)
Definition: testRot3.cpp:888
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)))
gtsam::Quaternion toQuaternion() const
Definition: Rot3M.cpp:233
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
static Rot3 R
Definition: testRot3.cpp:37
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:174
Point2 t(10, 10)
3D rotation represented as a rotation matrix or quaternion
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...
Definition: Lie.h:135
Eigen::Quaternion< double, Eigen::DontAlign > Quaternion
Vector3 rpy_proxy(Rot3 const &R)
Definition: testRot3.cpp:862


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:22