testPose3.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 
17 #include <gtsam/geometry/Pose3.h>
18 #include <gtsam/geometry/Pose2.h>
19 #include <gtsam/base/testLie.h>
20 #include <gtsam/base/lieProxies.h>
22 
23 
25 #include <cmath>
26 #include <functional>
27 
28 using namespace std;
29 using namespace gtsam;
30 using namespace std::placeholders;
31 
34 
35 static const Point3 P(0.2,0.7,-2);
36 static const Rot3 R = Rot3::Rodrigues(0.3,0,0);
37 static const Point3 P2(3.5,-8.2,4.2);
38 static const Pose3 T(R,P2);
39 static const Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),P2);
40 static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3));
41 static const double tol=1e-5;
42 
43 /* ************************************************************************* */
45 {
46  Pose3 pose2 = T3;
47  EXPECT(T3.equals(pose2));
48  Pose3 origin;
49  EXPECT(!T3.equals(origin));
50 }
51 
52 /* ************************************************************************* */
53 TEST( Pose3, constructors)
54 {
55  Pose3 expected(Rot3::Rodrigues(0,0,3),Point3(1,2,0));
56  Pose2 pose2(1,2,3);
57  EXPECT(assert_equal(expected,Pose3(pose2)));
58 }
59 
60 /* ************************************************************************* */
61 #ifndef GTSAM_POSE3_EXPMAP
62 TEST( Pose3, retract_first_order)
63 {
64  Pose3 id;
65  Vector v = Z_6x1;
66  v(0) = 0.3;
67  EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2));
68  v(3)=0.2;v(4)=0.7;v(5)=-2;
69  EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2));
70 }
71 #endif
72 /* ************************************************************************* */
73 TEST( Pose3, retract_expmap)
74 {
75  Vector v = Z_6x1; v(0) = 0.3;
77  EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2));
78  EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2));
79 }
80 
81 /* ************************************************************************* */
82 TEST( Pose3, expmap_a_full)
83 {
84  Pose3 id;
85  Vector v = Z_6x1;
86  v(0) = 0.3;
87  EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
88  v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
89  EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
90 }
91 
92 /* ************************************************************************* */
93 TEST( Pose3, expmap_a_full2)
94 {
95  Pose3 id;
96  Vector v = Z_6x1;
97  v(0) = 0.3;
98  EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
99  v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
100  EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
101 }
102 
103 /* ************************************************************************* */
104 TEST(Pose3, expmap_b)
105 {
106  Pose3 p1(Rot3(), Point3(100, 0, 0));
107  Pose3 p2 = p1.retract((Vector(6) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0).finished());
108  Pose3 expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
109  EXPECT(assert_equal(expected, p2,1e-2));
110 }
111 
112 /* ************************************************************************* */
113 // test case for screw motion in the plane
114 namespace screwPose3 {
115  double a=0.3, c=cos(a), s=sin(a), w=0.3;
116  Vector xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0).finished();
117  Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
118  Point3 expectedT(0.29552, 0.0446635, 1);
120 }
121 
122 /* ************************************************************************* */
123 // Checks correct exponential map (Expmap) with brute force matrix exponential
124 TEST(Pose3, expmap_c_full)
125 {
128 }
129 
130 /* ************************************************************************* */
131 // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
132 TEST(Pose3, Adjoint_full)
133 {
134  Pose3 expected = T * Pose3::Expmap(screwPose3::xi) * T.inverse();
135  Vector xiprime = T.Adjoint(screwPose3::xi);
136  EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));
137 
138  Pose3 expected2 = T2 * Pose3::Expmap(screwPose3::xi) * T2.inverse();
139  Vector xiprime2 = T2.Adjoint(screwPose3::xi);
140  EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));
141 
142  Pose3 expected3 = T3 * Pose3::Expmap(screwPose3::xi) * T3.inverse();
143  Vector xiprime3 = T3.Adjoint(screwPose3::xi);
144  EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
145 }
146 
147 /* ************************************************************************* */
148 // Check Adjoint numerical derivatives
149 TEST(Pose3, Adjoint_jacobians)
150 {
151  Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
152 
153  // Check evaluation sanity check
154  EQUALITY(static_cast<gtsam::Vector>(T.AdjointMap() * xi), T.Adjoint(xi));
155  EQUALITY(static_cast<gtsam::Vector>(T2.AdjointMap() * xi), T2.Adjoint(xi));
156  EQUALITY(static_cast<gtsam::Vector>(T3.AdjointMap() * xi), T3.Adjoint(xi));
157 
158  // Check jacobians
159  Matrix6 actualH1, actualH2, expectedH1, expectedH2;
160  std::function<Vector6(const Pose3&, const Vector6&)> Adjoint_proxy =
161  [&](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); };
162 
163  T.Adjoint(xi, actualH1, actualH2);
164  expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi);
165  expectedH2 = numericalDerivative22(Adjoint_proxy, T, xi);
166  EXPECT(assert_equal(expectedH1, actualH1));
167  EXPECT(assert_equal(expectedH2, actualH2));
168 
169  T2.Adjoint(xi, actualH1, actualH2);
170  expectedH1 = numericalDerivative21(Adjoint_proxy, T2, xi);
171  expectedH2 = numericalDerivative22(Adjoint_proxy, T2, xi);
172  EXPECT(assert_equal(expectedH1, actualH1));
173  EXPECT(assert_equal(expectedH2, actualH2));
174 
175  T3.Adjoint(xi, actualH1, actualH2);
176  expectedH1 = numericalDerivative21(Adjoint_proxy, T3, xi);
177  expectedH2 = numericalDerivative22(Adjoint_proxy, T3, xi);
178  EXPECT(assert_equal(expectedH1, actualH1));
179  EXPECT(assert_equal(expectedH2, actualH2));
180 }
181 
182 /* ************************************************************************* */
183 // Check AdjointTranspose and jacobians
184 TEST(Pose3, AdjointTranspose)
185 {
186  Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
187 
188  // Check evaluation
189  EQUALITY(static_cast<Vector>(T.AdjointMap().transpose() * xi),
190  T.AdjointTranspose(xi));
191  EQUALITY(static_cast<Vector>(T2.AdjointMap().transpose() * xi),
192  T2.AdjointTranspose(xi));
193  EQUALITY(static_cast<Vector>(T3.AdjointMap().transpose() * xi),
194  T3.AdjointTranspose(xi));
195 
196  // Check jacobians
197  Matrix6 actualH1, actualH2, expectedH1, expectedH2;
198  std::function<Vector6(const Pose3&, const Vector6&)> AdjointTranspose_proxy =
199  [&](const Pose3& T, const Vector6& xi) {
200  return T.AdjointTranspose(xi);
201  };
202 
203  T.AdjointTranspose(xi, actualH1, actualH2);
204  expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi);
205  expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi);
206  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
207  EXPECT(assert_equal(expectedH2, actualH2));
208 
209  T2.AdjointTranspose(xi, actualH1, actualH2);
210  expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi);
211  expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi);
212  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
213  EXPECT(assert_equal(expectedH2, actualH2));
214 
215  T3.AdjointTranspose(xi, actualH1, actualH2);
216  expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi);
217  expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi);
218  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
219  EXPECT(assert_equal(expectedH2, actualH2));
220 }
221 
222 /* ************************************************************************* */
223 // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
224 TEST(Pose3, Adjoint_hat)
225 {
226  auto hat = [](const Vector& xi) { return ::wedge<Pose3>(xi); };
227  Matrix4 expected = T.matrix() * hat(screwPose3::xi) * T.matrix().inverse();
228  Matrix4 xiprime = hat(T.Adjoint(screwPose3::xi));
229  EXPECT(assert_equal(expected, xiprime, 1e-6));
230 
231  Matrix4 expected2 = T2.matrix() * hat(screwPose3::xi) * T2.matrix().inverse();
232  Matrix4 xiprime2 = hat(T2.Adjoint(screwPose3::xi));
233  EXPECT(assert_equal(expected2, xiprime2, 1e-6));
234 
235  Matrix4 expected3 = T3.matrix() * hat(screwPose3::xi) * T3.matrix().inverse();
236  Matrix4 xiprime3 = hat(T3.Adjoint(screwPose3::xi));
237  EXPECT(assert_equal(expected3, xiprime3, 1e-6));
238 }
239 
240 /* ************************************************************************* */
243  Vector w = xi.head(3);
244  Vector v = xi.tail(3);
245  double t = w.norm();
246  if (t < 1e-5)
247  return Pose3(Rot3(), Point3(v));
248  else {
249  Matrix W = skewSymmetric(w/t);
250  Matrix A = I_3x3 + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
251  return Pose3(Rot3::Expmap (w), Point3(A * v));
252  }
253 }
254 
255 /* ************************************************************************* */
256 TEST(Pose3, expmaps_galore_full)
257 {
258  Vector xi; Pose3 actual;
259  xi = (Vector(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6).finished();
260  actual = Pose3::Expmap(xi);
261  EXPECT(assert_equal(expm<Pose3>(xi), actual,1e-6));
262  EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
263  EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6));
264 
265  xi = (Vector(6) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6).finished();
266  for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
267  Vector txi = xi*theta;
268  actual = Pose3::Expmap(txi);
269  EXPECT(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
270  EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6));
271  Vector log = Pose3::Logmap(actual);
272  EXPECT(assert_equal(actual, Pose3::Expmap(log),1e-6));
273  EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps
274  }
275 
276  // Works with large v as well, but expm needs 10 iterations!
277  xi = (Vector(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0).finished();
278  actual = Pose3::Expmap(xi);
279  EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
280  EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-9));
281  EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-9));
282 }
283 
284 /* ************************************************************************* */
285 // Check translation and its pushforward
287  Matrix actualH;
288  EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8));
289 
290  std::function<Point3(const Pose3&)> f = [](const Pose3& T) { return T.translation(); };
291  Matrix numericalH = numericalDerivative11<Point3, Pose3>(f, T);
292  EXPECT(assert_equal(numericalH, actualH, 1e-6));
293 }
294 
295 /* ************************************************************************* */
296 // Check rotation and its pushforward
298  Matrix actualH;
299  EXPECT(assert_equal(R, T.rotation(actualH), 1e-8));
300 
301  std::function<Rot3(const Pose3&)> f = [](const Pose3& T) { return T.rotation(); };
302  Matrix numericalH = numericalDerivative11<Rot3, Pose3>(f, T);
303  EXPECT(assert_equal(numericalH, actualH, 1e-6));
304 }
305 
306 /* ************************************************************************* */
307 TEST(Pose3, Adjoint_compose_full)
308 {
309  // To debug derivatives of compose, assert that
310  // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
311  const Pose3& T1 = T;
312  Vector x = (Vector(6) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8).finished();
313  Pose3 expected = T1 * Pose3::Expmap(x) * T2;
314  Vector y = T2.inverse().Adjoint(x);
315  Pose3 actual = T1 * T2 * Pose3::Expmap(y);
316  EXPECT(assert_equal(expected, actual, 1e-6));
317 }
318 
319 /* ************************************************************************* */
320 // Check compose and its pushforward
321 // NOTE: testing::compose<Pose3>(t1,t2) = t1.compose(t2) (see lieProxies.h)
323 {
324  Matrix actual = (T2*T2).matrix();
326  EXPECT(assert_equal(actual,expected,1e-8));
327 
328  Matrix actualDcompose1, actualDcompose2;
329  T2.compose(T2, actualDcompose1, actualDcompose2);
330 
331  Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2);
332  EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
333  EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3));
334 
335  Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2);
336  EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4));
337 }
338 
339 /* ************************************************************************* */
340 // Check compose and its pushforward, another case
341 TEST( Pose3, compose2 )
342 {
343  const Pose3& T1 = T;
344  Matrix actual = (T1*T2).matrix();
345  Matrix expected = T1.matrix()*T2.matrix();
346  EXPECT(assert_equal(actual,expected,1e-8));
347 
348  Matrix actualDcompose1, actualDcompose2;
349  T1.compose(T2, actualDcompose1, actualDcompose2);
350 
351  Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2);
352  EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
353  EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3));
354 
355  Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2);
356  EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5));
357 }
358 
359 /* ************************************************************************* */
361 {
362  Matrix actualDinverse;
363  Matrix actual = T.inverse(actualDinverse).matrix();
364  Matrix expected = T.matrix().inverse();
365  EXPECT(assert_equal(actual,expected,1e-8));
366 
367  Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
368  EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
369  EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3));
370 }
371 
372 /* ************************************************************************* */
373 TEST( Pose3, inverseDerivatives2)
374 {
375  Rot3 R = Rot3::Rodrigues(0.3,0.4,-0.5);
376  Point3 t(3.5,-8.2,4.2);
377  Pose3 T(R,t);
378 
379  Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
380  Matrix actualDinverse;
381  T.inverse(actualDinverse);
382  EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
383  EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3));
384 }
385 
386 /* ************************************************************************* */
387 TEST( Pose3, compose_inverse)
388 {
389  Matrix actual = (T*T.inverse()).matrix();
390  Matrix expected = I_4x4;
391  EXPECT(assert_equal(actual,expected,1e-8));
392 }
393 
394 /* ************************************************************************* */
396  return pose.transformFrom(point);
397 }
398 TEST(Pose3, Dtransform_from1_a) {
399  Matrix actualDtransform_from1;
400  T.transformFrom(P, actualDtransform_from1, {});
402  EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
403 }
404 
405 TEST(Pose3, Dtransform_from1_b) {
406  Pose3 origin;
407  Matrix actualDtransform_from1;
408  origin.transformFrom(P, actualDtransform_from1, {});
409  Matrix numerical = numericalDerivative21(transformFrom_, origin, P);
410  EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
411 }
412 
413 TEST(Pose3, Dtransform_from1_c) {
414  Point3 origin(0, 0, 0);
415  Pose3 T0(R, origin);
416  Matrix actualDtransform_from1;
417  T0.transformFrom(P, actualDtransform_from1, {});
418  Matrix numerical = numericalDerivative21(transformFrom_, T0, P);
419  EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
420 }
421 
422 TEST(Pose3, Dtransform_from1_d) {
423  Rot3 I;
424  Point3 t0(100, 0, 0);
425  Pose3 T0(I, t0);
426  Matrix actualDtransform_from1;
427  T0.transformFrom(P, actualDtransform_from1, {});
428  // print(computed, "Dtransform_from1_d computed:");
429  Matrix numerical = numericalDerivative21(transformFrom_, T0, P);
430  // print(numerical, "Dtransform_from1_d numerical:");
431  EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8));
432 }
433 
434 /* ************************************************************************* */
435 TEST(Pose3, Dtransform_from2) {
436  Matrix actualDtransform_from2;
437  T.transformFrom(P, {}, actualDtransform_from2);
439  EXPECT(assert_equal(numerical, actualDtransform_from2, 1e-8));
440 }
441 
442 /* ************************************************************************* */
444  return pose.transformTo(point);
445 }
446 TEST(Pose3, Dtransform_to1) {
447  Matrix computed;
448  T.transformTo(P, computed, {});
450  EXPECT(assert_equal(numerical, computed, 1e-8));
451 }
452 
453 /* ************************************************************************* */
454 TEST(Pose3, Dtransform_to2) {
455  Matrix computed;
456  T.transformTo(P, {}, computed);
458  EXPECT(assert_equal(numerical, computed, 1e-8));
459 }
460 
461 /* ************************************************************************* */
462 TEST(Pose3, transform_to_with_derivatives) {
463  Matrix actH1, actH2;
464  T.transformTo(P, actH1, actH2);
467  EXPECT(assert_equal(expH1, actH1, 1e-8));
468  EXPECT(assert_equal(expH2, actH2, 1e-8));
469 }
470 
471 /* ************************************************************************* */
472 TEST(Pose3, transform_from_with_derivatives) {
473  Matrix actH1, actH2;
474  T.transformFrom(P, actH1, actH2);
477  EXPECT(assert_equal(expH1, actH1, 1e-8));
478  EXPECT(assert_equal(expH2, actH2, 1e-8));
479 }
480 
481 /* ************************************************************************* */
482 TEST(Pose3, transform_to_translate) {
483  Point3 actual =
484  Pose3(Rot3(), Point3(1, 2, 3)).transformTo(Point3(10., 20., 30.));
485  Point3 expected(9., 18., 27.);
486  EXPECT(assert_equal(expected, actual));
487 }
488 
489 /* ************************************************************************* */
490 TEST(Pose3, transform_to_rotate) {
491  Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(0, 0, 0));
492  Point3 actual = transform.transformTo(Point3(2, 1, 10));
493  Point3 expected(-1, 2, 10);
494  EXPECT(assert_equal(expected, actual, 0.001));
495 }
496 
497 /* ************************************************************************* */
498 // Check transformPoseFrom and its pushforward
499 Pose3 transformPoseFrom_(const Pose3& wTa, const Pose3& aTb) {
500  return wTa.transformPoseFrom(aTb);
501 }
502 
503 TEST(Pose3, transformPoseFrom)
504 {
505  Matrix actual = (T2*T2).matrix();
507  EXPECT(assert_equal(actual, expected, 1e-8));
508 
509  Matrix H1, H2;
510  T2.transformPoseFrom(T2, H1, H2);
511 
513  EXPECT(assert_equal(numericalH1, H1, 5e-3));
514  EXPECT(assert_equal(T2.inverse().AdjointMap(), H1, 5e-3));
515 
517  EXPECT(assert_equal(numericalH2, H2, 1e-4));
518 }
519 
520 /* ************************************************************************* */
522  Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(2, 4, 0));
523  Point3 actual = transform.transformTo(Point3(3, 2, 10));
524  Point3 expected(2, 1, 10);
525  EXPECT(assert_equal(expected, actual, 0.001));
526 }
527 
529  return pose.transformPoseTo(pose2);
530 }
531 
532 /* ************************************************************************* */
534  Pose3 origin = T.transformPoseTo(T);
536 }
537 
538 /* ************************************************************************* */
539 TEST(Pose3, transformPoseTo_with_derivatives) {
540  Matrix actH1, actH2;
541  Pose3 res = T.transformPoseTo(T2, actH1, actH2);
542  EXPECT(assert_equal(res, T.inverse().compose(T2)));
543 
546  EXPECT(assert_equal(expH1, actH1, 1e-8));
547  EXPECT(assert_equal(expH2, actH2, 1e-8));
548 }
549 
550 /* ************************************************************************* */
551 TEST(Pose3, transformPoseTo_with_derivatives2) {
552  Matrix actH1, actH2;
553  Pose3 res = T.transformPoseTo(T3, actH1, actH2);
554  EXPECT(assert_equal(res, T.inverse().compose(T3)));
555 
558  EXPECT(assert_equal(expH1, actH1, 1e-8));
559  EXPECT(assert_equal(expH2, actH2, 1e-8));
560 }
561 
562 /* ************************************************************************* */
564  Point3 actual = T3.transformFrom(Point3(0, 0, 0));
565  Point3 expected = Point3(1., 2., 3.);
566  EXPECT(assert_equal(expected, actual));
567 }
568 
569 /* ************************************************************************* */
570 TEST(Pose3, transform_roundtrip) {
571  Point3 actual = T3.transformFrom(T3.transformTo(Point3(12., -0.11, 7.0)));
572  Point3 expected(12., -0.11, 7.0);
573  EXPECT(assert_equal(expected, actual));
574 }
575 
576 /* ************************************************************************* */
577 TEST(Pose3, Retract_LocalCoordinates)
578 {
579  Vector6 d;
580  d << 1,2,3,4,5,6; d/=10;
581  const Rot3 R = Rot3::Retract(d.head<3>());
582  Pose3 t = Pose3::Retract(d);
583  EXPECT(assert_equal(d, Pose3::LocalCoordinates(t)));
584 }
585 /* ************************************************************************* */
586 TEST(Pose3, retract_localCoordinates)
587 {
588  Vector6 d12;
589  d12 << 1,2,3,4,5,6; d12/=10;
590  Pose3 t1 = T, t2 = t1.retract(d12);
591  EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
592 }
593 /* ************************************************************************* */
594 TEST(Pose3, expmap_logmap)
595 {
596  Vector d12 = Vector6::Constant(0.1);
597  Pose3 t1 = T, t2 = t1.expmap(d12);
598  EXPECT(assert_equal(d12, t1.logmap(t2)));
599 }
600 
601 /* ************************************************************************* */
602 TEST(Pose3, retract_localCoordinates2)
603 {
604  Pose3 t1 = T;
605  Pose3 t2 = T3;
606  Pose3 origin;
607  Vector d12 = t1.localCoordinates(t2);
608  EXPECT(assert_equal(t2, t1.retract(d12)));
609  Vector d21 = t2.localCoordinates(t1);
610  EXPECT(assert_equal(t1, t2.retract(d21)));
611  // TODO(hayk): This currently fails!
612  // EXPECT(assert_equal(d12, -d21));
613 }
614 /* ************************************************************************* */
615 TEST(Pose3, manifold_expmap)
616 {
617  Pose3 t1 = T;
618  Pose3 t2 = T3;
619  Pose3 origin;
620  Vector d12 = t1.logmap(t2);
621  EXPECT(assert_equal(t2, t1.expmap(d12)));
622  Vector d21 = t2.logmap(t1);
623  EXPECT(assert_equal(t1, t2.expmap(d21)));
624 
625  // Check that log(t1,t2)=-log(t2,t1)
626  EXPECT(assert_equal(d12,-d21));
627 }
628 
629 /* ************************************************************************* */
630 TEST(Pose3, subgroups)
631 {
632  // Frank - Below only works for correct "Agrawal06iros style expmap
633  // lines in canonical coordinates correspond to Abelian subgroups in SE(3)
634  Vector d = (Vector(6) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6).finished();
635  // exp(-d)=inverse(exp(d))
637  // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
638  Pose3 T2 = Pose3::Expmap(2*d);
639  Pose3 T3 = Pose3::Expmap(3*d);
640  Pose3 T5 = Pose3::Expmap(5*d);
641  EXPECT(assert_equal(T5,T2*T3));
642  EXPECT(assert_equal(T5,T3*T2));
643 }
644 
645 /* ************************************************************************* */
647 {
648  Pose3 expected = T2.inverse() * T3;
649  Matrix actualDBetween1,actualDBetween2;
650  Pose3 actual = T2.between(T3, actualDBetween1,actualDBetween2);
651  EXPECT(assert_equal(expected,actual));
652 
653  Matrix numericalH1 = numericalDerivative21(testing::between<Pose3> , T2, T3);
654  EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3));
655 
656  Matrix numericalH2 = numericalDerivative22(testing::between<Pose3> , T2, T3);
657  EXPECT(assert_equal(numericalH2,actualDBetween2,1e-5));
658 }
659 
660 /* ************************************************************************* */
661 // some shared test values - pulled from equivalent test in Pose2
662 Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4);
663 Pose3 x1, x2(Rot3::Ypr(0.0, 0.0, 0.0), l2), x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2);
664 Pose3
665  xl1(Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)),
666  xl2(Rot3::Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)),
667  xl3(Rot3::Ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)),
668  xl4(Rot3::Ypr(0.0, 0.0, 1.0), Point3(1, 4,-4));
669 
670 /* ************************************************************************* */
671 double range_proxy(const Pose3& pose, const Point3& point) {
672  return pose.range(point);
673 }
675 {
676  Matrix expectedH1, actualH1, expectedH2, actualH2;
677 
678  // establish range is indeed zero
679  EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
680 
681  // establish range is indeed sqrt2
682  EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(l2),1e-9);
683 
684  // Another pair
685  double actual23 = x2.range(l3, actualH1, actualH2);
686  EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
687 
688  // Check numerical derivatives
689  expectedH1 = numericalDerivative21(range_proxy, x2, l3);
690  expectedH2 = numericalDerivative22(range_proxy, x2, l3);
691  EXPECT(assert_equal(expectedH1,actualH1));
692  EXPECT(assert_equal(expectedH2,actualH2));
693 
694  // Another test
695  double actual34 = x3.range(l4, actualH1, actualH2);
696  EXPECT_DOUBLES_EQUAL(5,actual34,1e-9);
697 
698  // Check numerical derivatives
699  expectedH1 = numericalDerivative21(range_proxy, x3, l4);
700  expectedH2 = numericalDerivative22(range_proxy, x3, l4);
701  EXPECT(assert_equal(expectedH1,actualH1));
702  EXPECT(assert_equal(expectedH2,actualH2));
703 }
704 
705 /* ************************************************************************* */
706 double range_pose_proxy(const Pose3& pose, const Pose3& point) {
707  return pose.range(point);
708 }
709 TEST( Pose3, range_pose )
710 {
711  Matrix expectedH1, actualH1, expectedH2, actualH2;
712 
713  // establish range is indeed zero
714  EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
715 
716  // establish range is indeed sqrt2
717  EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(xl2),1e-9);
718 
719  // Another pair
720  double actual23 = x2.range(xl3, actualH1, actualH2);
721  EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
722 
723  // Check numerical derivatives
726  EXPECT(assert_equal(expectedH1,actualH1));
727  EXPECT(assert_equal(expectedH2,actualH2));
728 
729  // Another test
730  double actual34 = x3.range(xl4, actualH1, actualH2);
731  EXPECT_DOUBLES_EQUAL(5,actual34,1e-9);
732 
733  // Check numerical derivatives
736  EXPECT(assert_equal(expectedH1,actualH1));
737  EXPECT(assert_equal(expectedH2,actualH2));
738 }
739 
740 /* ************************************************************************* */
742  return pose.bearing(point);
743 }
745  Matrix expectedH1, actualH1, expectedH2, actualH2;
746  EXPECT(assert_equal(Unit3(1, 0, 0), x1.bearing(l1, actualH1, actualH2), 1e-9));
747 
748  // Check numerical derivatives
749  expectedH1 = numericalDerivative21(bearing_proxy, x1, l1);
750  expectedH2 = numericalDerivative22(bearing_proxy, x1, l1);
751  EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
752  EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
753 }
754 
755 TEST(Pose3, Bearing2) {
756  Matrix expectedH1, actualH1, expectedH2, actualH2;
757  EXPECT(assert_equal(Unit3(0,0.6,-0.8), x2.bearing(l4, actualH1, actualH2), 1e-9));
758 
759  // Check numerical derivatives
760  expectedH1 = numericalDerivative21(bearing_proxy, x2, l4);
761  expectedH2 = numericalDerivative22(bearing_proxy, x2, l4);
762  EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
763  EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
764 }
765 
766 TEST(Pose3, PoseToPoseBearing) {
767  Matrix expectedH1, actualH1, expectedH2, actualH2, H2block;
768  EXPECT(assert_equal(Unit3(0,1,0), xl1.bearing(xl2, actualH1, actualH2), 1e-9));
769 
770  // Check numerical derivatives
771  expectedH1 = numericalDerivative21(bearing_proxy, xl1, l2);
772 
773  // Since the second pose is treated as a point, the value calculated by
774  // numericalDerivative22 only depends on the position of the pose. Here, we
775  // calculate the Jacobian w.r.t. the second pose's position, and then augment
776  // that with zeroes in the block that is w.r.t. the second pose's
777  // orientation.
779  expectedH2 = Matrix(2, 6);
780  expectedH2.setZero();
781  expectedH2.block<2, 3>(0, 3) = H2block;
782 
783  EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
784  EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
785 }
786 
787 /* ************************************************************************* */
788 TEST( Pose3, unicycle )
789 {
790  // velocity in X should be X in inertial frame, rather than global frame
791  Vector x_step = Vector::Unit(6,3)*1.0;
792  EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
793  EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
794  EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2.0) * x_step), tol));
795 }
796 
797 /* ************************************************************************* */
798 TEST( Pose3, adjointMap) {
799  Matrix res = Pose3::adjointMap(screwPose3::xi);
802  Matrix6 expected;
803  expected << wh, Z_3x3, vh, wh;
804  EXPECT(assert_equal(expected,res,1e-5));
805 }
806 
807 /* ************************************************************************* */
808 TEST(Pose3, Align1) {
809  Pose3 expected(Rot3(), Point3(10,10,0));
810 
811  Point3Pair ab1(Point3(10,10,0), Point3(0,0,0));
812  Point3Pair ab2(Point3(30,20,0), Point3(20,10,0));
813  Point3Pair ab3(Point3(20,30,0), Point3(10,20,0));
814  const vector<Point3Pair> correspondences{ab1, ab2, ab3};
815 
816  std::optional<Pose3> actual = Pose3::Align(correspondences);
817  EXPECT(assert_equal(expected, *actual));
818 }
819 
820 /* ************************************************************************* */
821 TEST(Pose3, Align2) {
822  Point3 t(20,10,5);
823  Rot3 R = Rot3::RzRyRx(0.3, 0.2, 0.1);
824  Pose3 expected(R, t);
825 
826  Point3 p1(0,0,1), p2(10,0,2), p3(20,-10,30);
827  Point3 q1 = expected.transformFrom(p1),
828  q2 = expected.transformFrom(p2),
829  q3 = expected.transformFrom(p3);
830  const Point3Pair ab1{q1, p1}, ab2{q2, p2}, ab3{q3, p3};
831  const vector<Point3Pair> correspondences{ab1, ab2, ab3};
832 
833  std::optional<Pose3> actual = Pose3::Align(correspondences);
834  EXPECT(assert_equal(expected, *actual, 1e-5));
835 }
836 
837 /* ************************************************************************* */
838 TEST( Pose3, ExpmapDerivative1) {
839  Matrix6 actualH;
840  Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
841  Pose3::Expmap(w,actualH);
842  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
844  EXPECT(assert_equal(expectedH, actualH));
845 }
846 
847 /* ************************************************************************* */
848 TEST(Pose3, ExpmapDerivative2) {
849  // Iserles05an (Lie-group Methods) says:
850  // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
851  // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
852  // where A(t): T -> se(3) is a trajectory in the tangent space of SE(3)
853  // and dexp[A] is a linear map from 4*4 to 4*4 derivatives of se(3)
854  // Hence, the above matrix equation is typed: 4*4 = SE(3) * linear_map(4*4)
855 
856  // In GTSAM, we don't work with the Lie-algebra elements A directly, but with 6-vectors.
857  // xi is easy: d Expmap(xi(t)) / dt = ExmapDerivative[xi(t)] * xi'(t)
858 
859  // Let's verify the above formula.
860 
861  auto xi = [](double t) {
862  Vector6 v;
863  v << 2 * t, sin(t), 4 * t * t, 2 * t, sin(t), 4 * t * t;
864  return v;
865  };
866  auto xi_dot = [](double t) {
867  Vector6 v;
868  v << 2, cos(t), 8 * t, 2, cos(t), 8 * t;
869  return v;
870  };
871 
872  // We define a function T
873  auto T = [xi](double t) { return Pose3::Expmap(xi(t)); };
874 
875  for (double t = -2.0; t < 2.0; t += 0.3) {
876  const Matrix expected = numericalDerivative11<Pose3, double>(T, t);
877  const Matrix actual = Pose3::ExpmapDerivative(xi(t)) * xi_dot(t);
878  CHECK(assert_equal(expected, actual, 1e-7));
879  }
880 }
881 
882 TEST( Pose3, ExpmapDerivativeQr) {
883  Vector6 w = Vector6::Random();
884  w.head<3>().normalize();
885  w.head<3>() = w.head<3>() * 0.9e-2;
886  Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
887  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
889  Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
890  EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
891 }
892 
893 /* ************************************************************************* */
894 TEST( Pose3, LogmapDerivative) {
895  Matrix6 actualH;
896  Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
897  Pose3 p = Pose3::Expmap(w);
898  EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
899  Matrix expectedH = numericalDerivative21<Vector6, Pose3,
900  OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {});
901  EXPECT(assert_equal(expectedH, actualH));
902 }
903 
904 /* ************************************************************************* */
905 Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
906  return Pose3::adjointMap(xi) * v;
907 }
908 
910  Vector6 v = (Vector6() << 1, 2, 3, 4, 5, 6).finished();
912 
913  Matrix actualH1, actualH2;
914  Vector actual = Pose3::adjoint(screwPose3::xi, v, actualH1, actualH2);
915 
916  Matrix numericalH1 = numericalDerivative21<Vector6, Vector6, Vector6>(
918  Matrix numericalH2 = numericalDerivative22<Vector6, Vector6, Vector6>(
920 
921  EXPECT(assert_equal(expected,actual,1e-5));
922  EXPECT(assert_equal(numericalH1,actualH1,1e-5));
923  EXPECT(assert_equal(numericalH2,actualH2,1e-5));
924 }
925 
926 /* ************************************************************************* */
927 Vector6 testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) {
928  return Pose3::adjointMap(xi).transpose() * v;
929 }
930 
931 TEST( Pose3, adjointTranspose) {
932  Vector xi = (Vector(6) << 0.01, 0.02, 0.03, 1.0, 2.0, 3.0).finished();
933  Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished();
935 
936  Matrix actualH1, actualH2;
937  Vector actual = Pose3::adjointTranspose(xi, v, actualH1, actualH2);
938 
939  Matrix numericalH1 = numericalDerivative21<Vector6, Vector6, Vector6>(
941  Matrix numericalH2 = numericalDerivative22<Vector6, Vector6, Vector6>(
943 
944  EXPECT(assert_equal(expected,actual,1e-15));
945  EXPECT(assert_equal(numericalH1,actualH1,1e-5));
946  EXPECT(assert_equal(numericalH2,actualH2,1e-5));
947 }
948 
949 /* ************************************************************************* */
951  std::ostringstream os;
952  os << Pose3();
953 
954  string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 0 0 0";
955  EXPECT(os.str() == expected);
956 }
957 
958 //******************************************************************************
959 TEST(Pose3 , Invariants) {
960  Pose3 id;
961 
962  EXPECT(check_group_invariants(id,id));
963  EXPECT(check_group_invariants(id,T3));
964  EXPECT(check_group_invariants(T2,id));
965  EXPECT(check_group_invariants(T2,T3));
966 
967  EXPECT(check_manifold_invariants(id,id));
968  EXPECT(check_manifold_invariants(id,T3));
969  EXPECT(check_manifold_invariants(T2,id));
970  EXPECT(check_manifold_invariants(T2,T3));
971 }
972 
973 //******************************************************************************
974 TEST(Pose3 , LieGroupDerivatives) {
975  Pose3 id;
976 
981 }
982 
983 //******************************************************************************
984 TEST(Pose3 , ChartDerivatives) {
985  Pose3 id;
986  if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
991  }
992 }
993 
994 //******************************************************************************
995 #include "testPoseAdjointMap.h"
996 
997 TEST(Pose3, TransformCovariance6MapTo2d) {
998  // Create 3d scenarios that map to 2d configurations and compare with Pose2 results.
999  using namespace test_pose_adjoint_map;
1000 
1001  Vector3 s2{0.1, 0.3, 0.7};
1002  Pose2 p2{1.1, 1.5, 31. * degree};
1003  auto cov2 = FullCovarianceFromSigmas<Pose2>(s2);
1004  auto transformed2 = TransformCovariance<Pose2>{p2}(cov2);
1005 
1006  auto match_cov3_to_cov2 = [&](int spatial_axis0, int spatial_axis1, int r_axis,
1007  const Pose2::Jacobian &cov2, const Pose3::Jacobian &cov3) -> void
1008  {
1010  Vector3{cov2.diagonal()},
1011  Vector3{cov3(spatial_axis0, spatial_axis0), cov3(spatial_axis1, spatial_axis1), cov3(r_axis, r_axis)}));
1013  Vector3{cov2(1, 0), cov2(2, 0), cov2(2, 1)},
1014  Vector3{cov3(spatial_axis1, spatial_axis0), cov3(r_axis, spatial_axis0), cov3(r_axis, spatial_axis1)}));
1015  };
1016 
1017  // rotate around x axis
1018  {
1019  auto cov3 = FullCovarianceFromSigmas<Pose3>((Vector6{} << s2(2), 0., 0., 0., s2(0), s2(1)).finished());
1020  auto transformed3 = TransformCovariance<Pose3>{{Rot3::RzRyRx(p2.theta(), 0., 0.), {0., p2.x(), p2.y()}}}(cov3);
1021  match_cov3_to_cov2(4, 5, 0, transformed2, transformed3);
1022  }
1023 
1024  // rotate around y axis
1025  {
1026  auto cov3 = FullCovarianceFromSigmas<Pose3>((Vector6{} << 0., s2(2), 0., s2(1), 0., s2(0)).finished());
1027  auto transformed3 = TransformCovariance<Pose3>{{Rot3::RzRyRx(0., p2.theta(), 0.), {p2.y(), 0., p2.x()}}}(cov3);
1028  match_cov3_to_cov2(5, 3, 1, transformed2, transformed3);
1029  }
1030 
1031  // rotate around z axis
1032  {
1033  auto cov3 = FullCovarianceFromSigmas<Pose3>((Vector6{} << 0., 0., s2(2), s2(0), s2(1), 0.).finished());
1034  auto transformed3 = TransformCovariance<Pose3>{{Rot3::RzRyRx(0., 0., p2.theta()), {p2.x(), p2.y(), 0.}}}(cov3);
1035  match_cov3_to_cov2(3, 4, 2, transformed2, transformed3);
1036  }
1037 }
1038 
1039 /* ************************************************************************* */
1040 TEST(Pose3, TransformCovariance6) {
1041  // Use simple covariance matrices and transforms to create tests that can be
1042  // validated with simple computations.
1043  using namespace test_pose_adjoint_map;
1044 
1045  // rotate 90 around z axis and then 90 around y axis
1046  {
1047  auto cov = FullCovarianceFromSigmas<Pose3>((Vector6{} << 0.1, 0.2, 0.3, 0.5, 0.7, 1.1).finished());
1048  auto transformed = TransformCovariance<Pose3>{{Rot3::RzRyRx(0., 90 * degree, 90 * degree), {0., 0., 0.}}}(cov);
1049  // x from y, y from z, z from x
1051  (Vector6{} << cov(1, 1), cov(2, 2), cov(0, 0), cov(4, 4), cov(5, 5), cov(3, 3)).finished(),
1052  Vector6{transformed.diagonal()}));
1053  // Both the x and z axes are pointing in the negative direction.
1055  (Vector5{} << -cov(2, 1), cov(0, 1), cov(4, 1), -cov(5, 1), cov(3, 1)).finished(),
1056  (Vector5{} << transformed(1, 0), transformed(2, 0), transformed(3, 0),
1057  transformed(4, 0), transformed(5, 0)).finished()));
1058  }
1059 
1060  // translate along the x axis with uncertainty in roty and rotz
1061  {
1062  auto cov = TwoVariableCovarianceFromSigmas<Pose3>(1, 2, 0.7, 0.3);
1063  auto transformed = TransformCovariance<Pose3>{{Rot3::RzRyRx(0., 0., 0.), {20., 0., 0.}}}(cov);
1064  // The uncertainty in roty and rotz causes off-diagonal covariances
1065  EXPECT(assert_equal(0.7 * 0.7 * 20., transformed(5, 1)));
1066  EXPECT(assert_equal(0.7 * 0.7 * 20. * 20., transformed(5, 5)));
1067  EXPECT(assert_equal(-0.3 * 0.3 * 20., transformed(4, 2)));
1068  EXPECT(assert_equal(0.3 * 0.3 * 20. * 20., transformed(4, 4)));
1069  EXPECT(assert_equal(-0.3 * 0.7 * 20., transformed(4, 1)));
1070  EXPECT(assert_equal(0.3 * 0.7 * 20., transformed(5, 2)));
1071  EXPECT(assert_equal(-0.3 * 0.7 * 20. * 20., transformed(5, 4)));
1072  }
1073 
1074  // rotate around x axis and translate along the x axis with uncertainty in rotx
1075  {
1076  auto cov = SingleVariableCovarianceFromSigma<Pose3>(0, 0.1);
1077  auto transformed = TransformCovariance<Pose3>{{Rot3::RzRyRx(90 * degree, 0., 0.), {20., 0., 0.}}}(cov);
1078  // No change
1079  EXPECT(assert_equal(cov, transformed));
1080  }
1081 
1082  // rotate around x axis and translate along the x axis with uncertainty in roty
1083  {
1084  auto cov = SingleVariableCovarianceFromSigma<Pose3>(1, 0.1);
1085  auto transformed = TransformCovariance<Pose3>{{Rot3::RzRyRx(90 * degree, 0., 0.), {20., 0., 0.}}}(cov);
1086  // Uncertainty is spread to other dimensions.
1088  (Vector6{} << 0., 0., 0.1 * 0.1, 0., 0.1 * 0.1 * 20. * 20., 0.).finished(),
1089  Vector6{transformed.diagonal()}));
1090  }
1091 }
1092 
1093 /* ************************************************************************* */
1097 
1098  // Trivial example: start at origin and move to (1, 0, 0) while rotating pi/2
1099  // about z-axis.
1100  Pose3 start;
1101  Pose3 end(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
1102  // This interpolation is easy to calculate by hand.
1103  double t = 0.5;
1104  Pose3 expected0(Rot3::Rz(M_PI_4), Point3(0.5, 0, 0));
1105  EXPECT(assert_equal(expected0, start.interpolateRt(end, t)));
1106 
1107  // Example from Peter Corke
1108  // https://robotacademy.net.au/lesson/interpolating-pose-in-3d/
1109  t = 0.0759; // corresponds to the 10th element when calling `ctraj` in
1110  // the video
1111  Pose3 O;
1112  Pose3 F(Rot3::Roll(0.6).compose(Rot3::Pitch(0.8)).compose(Rot3::Yaw(1.4)),
1113  Point3(1, 2, 3));
1114 
1115  // The expected answer matches the result presented in the video.
1116  Pose3 expected1(interpolate(O.rotation(), F.rotation(), t),
1117  interpolate(O.translation(), F.translation(), t));
1118  EXPECT(assert_equal(expected1, O.interpolateRt(F, t)));
1119 
1120  // Non-trivial interpolation, translation value taken from output.
1121  Pose3 expected2(interpolate(T2.rotation(), T3.rotation(), t),
1123  EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t)));
1124 }
1125 
1126 /* ************************************************************************* */
1127 Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { return interpolate(t1,t2,gamma); }
1128 
1129 TEST(Pose3, interpolateJacobians) {
1130  {
1131  Pose3 X = Pose3::Identity();
1132  Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
1133  double t = 0.5;
1134  Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation
1135  Matrix actualJacobianX, actualJacobianY;
1136  EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
1137 
1138  Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
1139  EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
1140 
1141  Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
1142  EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
1143  }
1144  {
1145  Pose3 X = Pose3::Identity();
1146  Pose3 Y(Rot3::Identity(), Point3(1, 0, 0));
1147  double t = 0.3;
1148  Pose3 expectedPoseInterp(Rot3::Identity(), Point3(0.3, 0, 0));
1149  Matrix actualJacobianX, actualJacobianY;
1150  EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
1151 
1152  Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
1153  EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
1154 
1155  Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
1156  EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
1157  }
1158  {
1159  Pose3 X = Pose3::Identity();
1160  Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
1161  double t = 0.5;
1162  Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
1163  Matrix actualJacobianX, actualJacobianY;
1164  EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
1165 
1166  Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
1167  EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
1168 
1169  Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
1170  EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
1171  }
1172  {
1173  Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2));
1174  Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1));
1175  double t = 0.3;
1176  Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
1177  Matrix actualJacobianX, actualJacobianY;
1178  interpolate(X, Y, t, actualJacobianX, actualJacobianY);
1179 
1180  Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
1181  EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
1182 
1183  Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
1184  EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
1185  }
1186 }
1187 
1188 /* ************************************************************************* */
1189 TEST(Pose3, Create) {
1190  Matrix63 actualH1, actualH2;
1191  Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2);
1192  EXPECT(assert_equal(T, actual));
1193  std::function<Pose3(Rot3, Point3)> create = [](Rot3 R, Point3 t) {
1194  return Pose3::Create(R, t);
1195  };
1196  EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
1197  EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
1198 }
1199 
1200 /* ************************************************************************* */
1202  Pose3 pose(Rot3::Identity(), Point3(1, 2, 3));
1203 
1204  // Generate the expected output
1205  std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n";
1206 
1207  EXPECT(assert_print_equal(expected, pose));
1208 }
1209 
1210 /* ************************************************************************* */
1211 int main() {
1212  TestResult tr;
1213  return TestRegistry::runAllTests(tr);
1214 }
1215 /* ************************************************************************* */
Provides additional testing facilities for common data structures.
Point3 l2(1, 1, 0)
#define CHECK(condition)
Definition: Test.h:108
#define EQUALITY(expected, actual)
Definition: Test.h:127
const char Y
#define I
Definition: main.h:112
Point3 translation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Point3 transformFrom_(const Pose3 &pose, const Point3 &point)
Definition: testPose3.cpp:395
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Scalar * y
Key F(std::uint64_t j)
void adjoint(const MatrixType &m)
Definition: adjoint.cpp:67
static Point3 p3
static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3))
Provides convenient mappings of common member functions for testing.
double c
Definition: testPose3.cpp:115
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
T between(const T &t1, const T &t2)
Definition: lieProxies.h:36
Pose3 xl4(Rot3::Ypr(0.0, 0.0, 1.0), Point3(1, 4,-4))
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
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
#define M_PI
Definition: main.h:106
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Pose3 transformPoseFrom(const Pose3 &aTb, OptionalJacobian< 6, 6 > Hself={}, OptionalJacobian< 6, 6 > HaTb={}) const
Definition: Pose3.cpp:331
Pose2_ Expmap(const Vector3_ &xi)
Vector6 testDerivAdjoint(const Vector6 &xi, const Vector6 &v)
Definition: testPose3.cpp:905
Definition: BFloat16.h:88
Key W(std::uint64_t j)
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
Pose3 expected(expectedR, expectedT)
Pose3_ transformPoseTo(const Pose3_ &p, const Pose3_ &q)
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition: Pose3.cpp:157
EIGEN_DEVICE_FUNC const LogReturnType log() const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Pose3 xl2(Rot3::Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
#define GTSAM_CONCEPT_LIE_INST(T)
Definition: Lie.h:372
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
static const Similarity3 id
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1)
Unit3 bearing_proxy(const Pose3 &pose, const Point3 &point)
Definition: testPose3.cpp:741
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.
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:49
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
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)
Pose3 Agrawal06iros(const Vector &xi)
Definition: testPose3.cpp:242
static void normalize(Signature::Row &row)
Definition: Signature.cpp:88
double w
Definition: testPose3.cpp:115
#define CHECK_LIE_GROUP_DERIVATIVES(t1, t2)
Definition: testLie.h:82
Point3 l3(2, 2, 0)
Pose3 xl3(Rot3::Ypr(1.0, 0.0, 0.0), Point3(2, 2, 0))
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...
Definition: Lie.h:136
const double degree
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
static const double tol
Definition: testPose3.cpp:41
Point3_ transformFrom(const Pose3_ &x, const Point3_ &p)
Eigen::VectorXd Vector
Definition: Vector.h:38
Key O(std::uint64_t j)
int main()
Definition: testPose3.cpp:1211
double range_proxy(const Pose3 &pose, const Point3 &point)
Definition: testPose3.cpp:671
#define EXPECT(condition)
Definition: Test.h:150
Unit3 bearing(const Point3 &point, OptionalJacobian< 2, 6 > Hself={}, OptionalJacobian< 2, 3 > Hpoint={}) const
Definition: Pose3.cpp:425
Vector6 AdjointTranspose(const Vector6 &x, OptionalJacobian< 6, 6 > H_this={}, OptionalJacobian< 6, 6 > H_x={}) const
The dual version of Adjoint.
Definition: Pose3.cpp:85
Array< int, Dynamic, 1 > v
Point3 expectedT(0.29552, 0.0446635, 1)
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in Pose coordinates and transforms it to world coordinates
Definition: Pose3.cpp:347
Point3 l1(1, 0, 0)
Vector6 testDerivAdjointTranspose(const Vector6 &xi, const Vector6 &v)
Definition: testPose3.cpp:927
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Matrix wedge< Pose3 >(const Vector &xi)
Definition: Pose3.h:419
Support utilities for using AdjointMap for transforming Pose2 and Pose3 covariance matrices...
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
#define ROT3_DEFAULT_COORDINATES_MODE
Definition: Rot3.h:43
TEST(Pose3, equals)
Definition: testPose3.cpp:44
Class compose(const Class &g) const
Definition: Lie.h:48
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
Pose3 xl1(Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0))
Point3 l4(1, 4,-4)
static const Pose3 T(R, P2)
Vector xi
Definition: testPose3.cpp:116
static const Point3 P2(3.5,-8.2, 4.2)
traits
Definition: chartTesting.h:28
double a
Definition: testPose3.cpp:115
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Pose3 transformPoseFrom_(const Pose3 &wTa, const Pose3 &aTb)
Definition: testPose3.cpp:499
ADT create(const Signature &signature)
ofstream os("timeSchurFactors.csv")
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1)
Double_ range(const Point2_ &p, const Point2_ &q)
Class between(const Class &g) const
Definition: Lie.h:52
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:371
Pose3 x1
Definition: testPose3.cpp:663
static EIGEN_DEPRECATED const end_t end
#define CHECK_CHART_DERIVATIVES(t1, t2)
Definition: testLie.h:85
float * p
static Pose3 pose2
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
static const Similarity3 T5(R, P, 10)
Class expmap(const TangentVector &v) const
Definition: Lie.h:78
static Point3 p2
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
Pose3 interpolateRt(const Pose3 &T, double t) const
Definition: Pose3.cpp:162
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Matrix6 AdjointMap() const
Definition: Pose3.cpp:57
Pose3 testing_interpolate(const Pose3 &t1, const Pose3 &t2, double gamma)
Definition: testPose3.cpp:1127
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Vector3 Point3
Definition: Point3.h:38
TangentVector logmap(const Class &g) const
Definition: Lie.h:84
double range_pose_proxy(const Pose3 &pose, const Pose3 &point)
Definition: testPose3.cpp:706
2D Pose
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Rot3 rotation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
static const Rot3 R
Definition: testPose3.cpp:36
#define X
Definition: icosphere.cpp:20
Pose3 transformPoseTo(const Pose3 &wTb, OptionalJacobian< 6, 6 > Hself={}, OptionalJacobian< 6, 6 > HwTb={}) const
Definition: Pose3.cpp:338
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
3D Pose
T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx={}, typename MakeOptionalJacobian< T, T >::type Hy={})
Definition: Lie.h:327
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Pose3 transformPoseTo_(const Pose3 &pose, const Pose3 &pose2)
Definition: testPose3.cpp:528
Point3 transform_to_(const Pose3 &pose, const Point3 &point)
Definition: testPose3.cpp:443
Vector6 Adjoint(const Vector6 &xi_b, OptionalJacobian< 6, 6 > H_this={}, OptionalJacobian< 6, 6 > H_xib={}) const
Definition: Pose3.cpp:67
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:62
Matrix4 matrix() const
Definition: Pose3.cpp:323
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
static const Point3 P(0.2, 0.7,-2)
Point2 t(10, 10)
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself={}, OptionalJacobian< 1, 3 > Hpoint={}) const
Definition: Pose3.cpp:399
static Similarity2 Align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb, const Point2Pair &centroids)
This method estimates the similarity transform from differences point pairs, given a known or estimat...
Definition: Similarity2.cpp:74
std::pair< Point3, Point3 > Point3Pair
Definition: Point3.h:42


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:55