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


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:24