testPose2.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 <CppUnitLite/TestHarness.h>
18 #include <gtsam/base/Testable.h>
19 #include <gtsam/base/TestableAssertions.h>
20 #include <gtsam/base/lieProxies.h>
21 #include <gtsam/base/testLie.h>
22 #include <gtsam/geometry/Point2.h>
23 #include <gtsam/geometry/Pose2.h>
24 #include <gtsam/geometry/Rot2.h>
25 
26 #include <optional>
27 #include <cmath>
28 #include <iostream>
29 
30 using namespace gtsam;
31 using namespace std;
32 
33 GTSAM_CONCEPT_TESTABLE_INST(Pose2)
34 GTSAM_CONCEPT_LIE_INST(Pose2)
35 
36 //******************************************************************************
37 TEST(Pose2 , Concept) {
38  GTSAM_CONCEPT_ASSERT(IsGroup<Pose2 >);
39  GTSAM_CONCEPT_ASSERT(IsManifold<Pose2 >);
40  GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Pose2 >);
41 }
42 
43 /* ************************************************************************* */
44 TEST(Pose2, constructors) {
45  Point2 p(0,0);
46  Pose2 pose(0,p);
47  Pose2 origin;
48  assert_equal(pose,origin);
49  Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01));
50  EXPECT(assert_equal(t,Pose2((Matrix)t.matrix())));
51 }
52 
53 /* ************************************************************************* */
54 TEST(Pose2, manifold) {
55  Pose2 t1(M_PI/2.0, Point2(1, 2));
56  Pose2 t2(M_PI/2.0+0.018, Point2(1.015, 2.01));
57  Pose2 origin;
58  Vector d12 = t1.localCoordinates(t2);
59  EXPECT(assert_equal(t2, t1.retract(d12)));
60  EXPECT(assert_equal(t2, t1*origin.retract(d12)));
61  Vector d21 = t2.localCoordinates(t1);
62  EXPECT(assert_equal(t1, t2.retract(d21)));
63  EXPECT(assert_equal(t1, t2*origin.retract(d21)));
64 }
65 
66 /* ************************************************************************* */
67 TEST(Pose2, retract) {
68  Pose2 pose(M_PI/2.0, Point2(1, 2));
69 #ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
70  Pose2 expected(1.00811, 2.01528, 2.5608);
71 #else
72  Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01));
73 #endif
74  Pose2 actual = pose.retract(Vector3(0.01, -0.015, 0.99));
75  EXPECT(assert_equal(expected, actual, 1e-5));
76 }
77 
78 /* ************************************************************************* */
79 TEST(Pose2, expmap) {
80  Pose2 pose(M_PI/2.0, Point2(1, 2));
81  Pose2 expected(1.00811, 2.01528, 2.5608);
82  Pose2 actual = expmap_default<Pose2>(pose, Vector3(0.01, -0.015, 0.99));
83  EXPECT(assert_equal(expected, actual, 1e-5));
84 }
85 
86 /* ************************************************************************* */
87 TEST(Pose2, expmap2) {
88  Pose2 pose(M_PI/2.0, Point2(1, 2));
89  Pose2 expected(1.00811, 2.01528, 2.5608);
90  Pose2 actual = expmap_default<Pose2>(pose, Vector3(0.01, -0.015, 0.99));
91  EXPECT(assert_equal(expected, actual, 1e-5));
92 }
93 
94 /* ************************************************************************* */
95 TEST(Pose2, expmap3) {
96  // do an actual series exponential map
97  // see e.g. http://www.cis.upenn.edu/~cis610/cis610lie1.ps
98  Matrix A = (Matrix(3,3) <<
99  0.0, -0.99, 0.01,
100  0.99, 0.0, -0.015,
101  0.0, 0.0, 0.0).finished();
102  Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0;
103  Matrix expected = I_3x3 + A + A2 + A3 + A4;
104 
105  Vector v = Vector3(0.01, -0.015, 0.99);
106  Pose2 pose = Pose2::Expmap(v);
107  Pose2 pose2(v);
108  EXPECT(assert_equal(pose, pose2));
109  Matrix actual = pose.matrix();
110  //EXPECT(assert_equal(expected, actual));
111 }
112 
113 /* ************************************************************************* */
114 TEST(Pose2, expmap0a) {
115  Pose2 expected(0.0101345, -0.0149092, 0.018);
116  Pose2 actual = Pose2::Expmap(Vector3(0.01, -0.015, 0.018));
117  EXPECT(assert_equal(expected, actual, 1e-5));
118 }
119 
120 /* ************************************************************************* */
121 TEST(Pose2, expmap0b) {
122  // a quarter turn
123  Pose2 expected(1.0, 1.0, M_PI/2);
124  Pose2 actual = Pose2::Expmap((Vector(3) << M_PI/2, 0.0, M_PI/2).finished());
125  EXPECT(assert_equal(expected, actual, 1e-5));
126 }
127 
128 /* ************************************************************************* */
129 TEST(Pose2, expmap0c) {
130  // a half turn
131  Pose2 expected(0.0, 2.0, M_PI);
132  Pose2 actual = Pose2::Expmap((Vector(3) << M_PI, 0.0, M_PI).finished());
133  EXPECT(assert_equal(expected, actual, 1e-5));
134 }
135 
136 /* ************************************************************************* */
137 TEST(Pose2, expmap0d) {
138  // a full turn
139  Pose2 expected(0, 0, 0);
140  Pose2 actual = Pose2::Expmap((Vector(3) << 2*M_PI, 0.0, 2*M_PI).finished());
141  EXPECT(assert_equal(expected, actual, 1e-5));
142 }
143 
144 /* ************************************************************************* */
145 TEST(Pose2, HatAndVee) {
146  // Create a few test vectors
147  Vector3 v1(1, 2, 3);
148  Vector3 v2(0.1, -0.5, 1.0);
149  Vector3 v3(0.0, 0.0, 0.0);
150 
151  // Test that Vee(Hat(v)) == v for various inputs
152  EXPECT(assert_equal(v1, Pose2::Vee(Pose2::Hat(v1))));
153  EXPECT(assert_equal(v2, Pose2::Vee(Pose2::Hat(v2))));
154  EXPECT(assert_equal(v3, Pose2::Vee(Pose2::Hat(v3))));
155 
156  // Check the structure of the Lie Algebra element
157  Matrix3 expected;
158  expected << 0, -3, 1,
159  3, 0, 2,
160  0, 0, 0;
161 
162  EXPECT(assert_equal(expected, Pose2::Hat(v1)));
163 }
164 
165 /* ************************************************************************* */
166 // test case for screw motion in the plane
167 namespace screwPose2 {
168  double w=0.3;
169  Vector xi = (Vector(3) << 0.0, w, w).finished();
170  Rot2 expectedR = Rot2::fromAngle(w);
171  Point2 expectedT(-0.0446635, 0.29552);
172  Pose2 expected(expectedR, expectedT);
173 }
174 
175 TEST(Pose2, expmap_c)
176 {
177  EXPECT(assert_equal(screwPose2::expected, expm<Pose2>(screwPose2::xi),1e-6));
178  EXPECT(assert_equal(screwPose2::expected, Pose2::Expmap(screwPose2::xi),1e-6));
179  EXPECT(assert_equal(screwPose2::xi, Pose2::Logmap(screwPose2::expected),1e-6));
180 }
181 
182 /* ************************************************************************* */
183 TEST(Pose2, expmap_c_full)
184 {
185  double w=0.3;
186  Vector xi = (Vector(3) << 0.0, w, w).finished();
187  Rot2 expectedR = Rot2::fromAngle(w);
188  Point2 expectedT(-0.0446635, 0.29552);
189  Pose2 expected(expectedR, expectedT);
190  EXPECT(assert_equal(expected, expm<Pose2>(xi),1e-6));
191  EXPECT(assert_equal(expected, Pose2::Expmap(xi),1e-6));
192  EXPECT(assert_equal(xi, Pose2::Logmap(expected),1e-6));
193 }
194 
195 /* ************************************************************************* */
196 // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
197 TEST(Pose2, Adjoint_full) {
198  Pose2 T(1, 2, 3);
199  Pose2 expected = T * Pose2::Expmap(screwPose2::xi) * T.inverse();
200  Vector xiprime = T.Adjoint(screwPose2::xi);
201  EXPECT(assert_equal(expected, Pose2::Expmap(xiprime), 1e-6));
202 
203  Vector3 xi2(4, 5, 6);
204  Pose2 expected2 = T * Pose2::Expmap(xi2) * T.inverse();
205  Vector xiprime2 = T.Adjoint(xi2);
206  EXPECT(assert_equal(expected2, Pose2::Expmap(xiprime2), 1e-6));
207 }
208 
209 /* ************************************************************************* */
210 // assert that T*Hat(xi)*T^-1 is equal to Hat(Ad_T(xi))
211 TEST(Pose2, Adjoint_hat) {
212  Pose2 T(1, 2, 3);
213  Matrix3 expected = T.matrix() * Pose2::Hat(screwPose2::xi) * T.matrix().inverse();
214  Matrix3 xiprime = Pose2::Hat(T.Adjoint(screwPose2::xi));
215  EXPECT(assert_equal(expected, xiprime, 1e-6));
216 
217  Vector3 xi2(4, 5, 6);
218  Matrix3 expected2 = T.matrix() * Pose2::Hat(xi2) * T.matrix().inverse();
219  Matrix3 xiprime2 = Pose2::Hat(T.Adjoint(xi2));
220  EXPECT(assert_equal(expected2, xiprime2, 1e-6));
221 }
222 
223 /* ************************************************************************* */
224 TEST(Pose2, logmap) {
225  Pose2 pose0(M_PI/2.0, Point2(1, 2));
226  Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
227 #ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
228  Vector3 expected(0.00986473, -0.0150896, 0.018);
229 #else
230  Vector3 expected(0.01, -0.015, 0.018);
231 #endif
232  Vector actual = pose0.localCoordinates(pose);
233  EXPECT(assert_equal(expected, actual, 1e-5));
234 }
235 
236 /* ************************************************************************* */
237 TEST(Pose2, logmap_full) {
238  Pose2 pose0(M_PI/2.0, Point2(1, 2));
239  Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
240  Vector expected = Vector3(0.00986473, -0.0150896, 0.018);
241  Vector actual = logmap_default<Pose2>(pose0, pose);
242  EXPECT(assert_equal(expected, actual, 1e-5));
243 }
244 
245 /* ************************************************************************* */
246 TEST( Pose2, ExpmapDerivative1) {
247  Matrix3 actualH;
248  Vector3 w(0.1, 0.27, -0.3);
249  Pose2::Expmap(w,actualH);
250  Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
251  OptionalJacobian<3, 3> >(&Pose2::Expmap, w, {}, 1e-2);
252  EXPECT(assert_equal(expectedH, actualH, 1e-5));
253 }
254 
255 /* ************************************************************************* */
256 TEST( Pose2, ExpmapDerivative2) {
257  Matrix3 actualH;
258  Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
259  Pose2::Expmap(w0,actualH);
260  Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
261  OptionalJacobian<3, 3> >(&Pose2::Expmap, w0, {}, 1e-2);
262  EXPECT(assert_equal(expectedH, actualH, 1e-5));
263 }
264 
265 /* ************************************************************************* */
266 TEST( Pose2, LogmapDerivative1) {
267  Matrix3 actualH;
268  Vector3 w(0.1, 0.27, -0.3);
269  Pose2 p = Pose2::Expmap(w);
270  EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5));
271  Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
272  OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2);
273  EXPECT(assert_equal(expectedH, actualH, 1e-5));
274 }
275 
276 /* ************************************************************************* */
277 TEST( Pose2, LogmapDerivative2) {
278  Matrix3 actualH;
279  Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
280  Pose2 p = Pose2::Expmap(w0);
281  EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5));
282  Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
283  OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2);
284  EXPECT(assert_equal(expectedH, actualH, 1e-5));
285 }
286 
287 /* ************************************************************************* */
288 static Point2 transformTo_(const Pose2& pose, const Point2& point) {
289  return pose.transformTo(point);
290 }
291 
292 TEST(Pose2, transformTo) {
293  Pose2 pose(M_PI / 2.0, Point2(1, 2)); // robot at (1,2) looking towards y
294  Point2 point(-1, 4); // landmark at (-1,4)
295 
296  // expected
297  Point2 expected(2, 2);
298  Matrix expectedH1 =
299  (Matrix(2, 3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0).finished();
300  Matrix expectedH2 = (Matrix(2, 2) << 0.0, 1.0, -1.0, 0.0).finished();
301 
302  // actual
303  Matrix actualH1, actualH2;
304  Point2 actual = pose.transformTo(point, actualH1, actualH2);
305  EXPECT(assert_equal(expected, actual));
306 
307  EXPECT(assert_equal(expectedH1, actualH1));
308  Matrix numericalH1 = numericalDerivative21(transformTo_, pose, point);
309  EXPECT(assert_equal(numericalH1, actualH1));
310 
311  EXPECT(assert_equal(expectedH2, actualH2));
312  Matrix numericalH2 = numericalDerivative22(transformTo_, pose, point);
313  EXPECT(assert_equal(numericalH2, actualH2));
314 }
315 
316 /* ************************************************************************* */
317 static Point2 transformFrom_(const Pose2& pose, const Point2& point) {
318  return pose.transformFrom(point);
319 }
320 
321 TEST(Pose2, transformFrom) {
322  Pose2 pose(1., 0., M_PI / 2.0);
323  Point2 pt(2., 1.);
324  Matrix H1, H2;
325  Point2 actual = pose.transformFrom(pt, H1, H2);
326 
327  Point2 expected(0., 2.);
328  EXPECT(assert_equal(expected, actual));
329 
330  Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.).finished();
331  Matrix H2_expected = (Matrix(2, 2) << 0., -1., 1., 0.).finished();
332 
333  Matrix numericalH1 = numericalDerivative21(transformFrom_, pose, pt);
334  EXPECT(assert_equal(H1_expected, H1));
335  EXPECT(assert_equal(H1_expected, numericalH1));
336 
337  Matrix numericalH2 = numericalDerivative22(transformFrom_, pose, pt);
338  EXPECT(assert_equal(H2_expected, H2));
339  EXPECT(assert_equal(H2_expected, numericalH2));
340 }
341 
342 /* ************************************************************************* */
343 TEST(Pose2, compose_a)
344 {
345  Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)));
346  Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0));
347 
348  Matrix actualDcompose1;
349  Matrix actualDcompose2;
350  Pose2 actual = pose1.compose(pose2, actualDcompose1, actualDcompose2);
351 
352  Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
353  EXPECT(assert_equal(expected, actual));
354 
355  Matrix expectedH1 = (Matrix(3,3) <<
356  0.0, 1.0, 0.0,
357  -1.0, 0.0, 2.0,
358  0.0, 0.0, 1.0
359  ).finished();
360  Matrix expectedH2 = I_3x3;
361  Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
362  Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
363  EXPECT(assert_equal(expectedH1,actualDcompose1));
364  EXPECT(assert_equal(numericalH1,actualDcompose1));
365  EXPECT(assert_equal(expectedH2,actualDcompose2));
366  EXPECT(assert_equal(numericalH2,actualDcompose2));
367 
368  Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
369  Point2 expected_point(-1.0, -1.0);
370  Point2 actual_point1 = (pose1 * pose2).transformTo(point);
371  Point2 actual_point2 = pose2.transformTo(pose1.transformTo(point));
372  EXPECT(assert_equal(expected_point, actual_point1));
373  EXPECT(assert_equal(expected_point, actual_point2));
374 }
375 
376 /* ************************************************************************* */
377 TEST(Pose2, compose_b)
378 {
379  Pose2 pose1(Rot2::fromAngle(M_PI/10.0), Point2(.75, .5));
380  Pose2 pose2(Rot2::fromAngle(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585));
381 
382  Pose2 pose_expected(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 2.0));
383 
384  Pose2 pose_actual_op = pose1 * pose2;
385  Matrix actualDcompose1, actualDcompose2;
386  Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2);
387 
388  Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
389  Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
390  EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5));
391  EXPECT(assert_equal(numericalH2,actualDcompose2));
392 
393  EXPECT(assert_equal(pose_expected, pose_actual_op));
394  EXPECT(assert_equal(pose_expected, pose_actual_fcn));
395 }
396 
397 /* ************************************************************************* */
398 TEST(Pose2, compose_c)
399 {
400  Pose2 pose1(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 1.0));
401  Pose2 pose2(Rot2::fromAngle(M_PI/4.0), Point2(sqrt(.5), sqrt(.5)));
402 
403  Pose2 pose_expected(Rot2::fromAngle(M_PI/2.0), Point2(1.0, 2.0));
404 
405  Pose2 pose_actual_op = pose1 * pose2;
406  Matrix actualDcompose1, actualDcompose2;
407  Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2);
408 
409  Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
410  Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
411  EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5));
412  EXPECT(assert_equal(numericalH2,actualDcompose2));
413 
414  EXPECT(assert_equal(pose_expected, pose_actual_op));
415  EXPECT(assert_equal(pose_expected, pose_actual_fcn));
416 }
417 
418 /* ************************************************************************* */
419 TEST(Pose2, inverse )
420 {
421  Point2 origin(0,0), t(1,2);
422  Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
423 
424  Pose2 identity, lTg = gTl.inverse();
425  EXPECT(assert_equal(identity,lTg.compose(gTl)));
426  EXPECT(assert_equal(identity,gTl.compose(lTg)));
427 
428  Point2 l(4,5), g(-4,6);
429  EXPECT(assert_equal(g,gTl*l));
430  EXPECT(assert_equal(l,lTg*g));
431 
432  // Check derivative
433  Matrix numericalH = numericalDerivative11<Pose2,Pose2>(testing::inverse, lTg);
434  Matrix actualDinverse;
435  lTg.inverse(actualDinverse);
436  EXPECT(assert_equal(numericalH,actualDinverse));
437 }
438 
439 namespace {
440  /* ************************************************************************* */
441  Vector homogeneous(const Point2& p) {
442  return (Vector(3) << p.x(), p.y(), 1.0).finished();
443  }
444 
445  /* ************************************************************************* */
446  Matrix matrix(const Pose2& gTl) {
447  Matrix gRl = gTl.r().matrix();
448  Point2 gt = gTl.t();
449  return (Matrix(3, 3) <<
450  gRl(0, 0), gRl(0, 1), gt.x(),
451  gRl(1, 0), gRl(1, 1), gt.y(),
452  0.0, 0.0, 1.0).finished();
453  }
454 }
455 
456 /* ************************************************************************* */
457 TEST( Pose2, matrix )
458 {
459  Point2 origin(0,0), t(1,2);
460  Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
461  Matrix gMl = matrix(gTl);
462  EXPECT(assert_equal((Matrix(3,3) <<
463  0.0, -1.0, 1.0,
464  1.0, 0.0, 2.0,
465  0.0, 0.0, 1.0).finished(),
466  gMl));
467  Rot2 gR1 = gTl.r();
468  EXPECT(assert_equal(homogeneous(t),gMl*homogeneous(origin)));
469  Point2 x_axis(1,0), y_axis(0,1);
470  EXPECT(assert_equal((Matrix(2,2) <<
471  0.0, -1.0,
472  1.0, 0.0).finished(),
473  gR1.matrix()));
474  EXPECT(assert_equal(Point2(0,1),gR1*x_axis));
475  EXPECT(assert_equal(Point2(-1,0),gR1*y_axis));
476  EXPECT(assert_equal(homogeneous(Point2(1+0,2+1)),gMl*homogeneous(x_axis)));
477  EXPECT(assert_equal(homogeneous(Point2(1-1,2+0)),gMl*homogeneous(y_axis)));
478 
479  // check inverse pose
480  Matrix lMg = matrix(gTl.inverse());
481  EXPECT(assert_equal((Matrix(3,3) <<
482  0.0, 1.0,-2.0,
483  -1.0, 0.0, 1.0,
484  0.0, 0.0, 1.0).finished(),
485  lMg));
486 }
487 
488 /* ************************************************************************* */
489 TEST( Pose2, compose_matrix )
490 {
491  Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
492  Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) looking at negative x
493  Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
494  EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
495 }
496 
497 
498 
499 /* ************************************************************************* */
500 TEST( Pose2, translation ) {
501  Pose2 pose(3.5, -8.2, 4.2);
502 
503  Matrix actualH;
504  EXPECT(assert_equal((Vector2() << 3.5, -8.2).finished(), pose.translation(actualH), 1e-8));
505 
506  std::function<Point2(const Pose2&)> f = [](const Pose2& T) { return T.translation(); };
507  Matrix numericalH = numericalDerivative11<Point2, Pose2>(f, pose);
508  EXPECT(assert_equal(numericalH, actualH, 1e-6));
509 }
510 
511 /* ************************************************************************* */
512 TEST( Pose2, rotation ) {
513  Pose2 pose(3.5, -8.2, 4.2);
514 
515  Matrix actualH(4, 3);
516  EXPECT(assert_equal(Rot2(4.2), pose.rotation(actualH), 1e-8));
517 
518  std::function<Rot2(const Pose2&)> f = [](const Pose2& T) { return T.rotation(); };
519  Matrix numericalH = numericalDerivative11<Rot2, Pose2>(f, pose);
520  EXPECT(assert_equal(numericalH, actualH, 1e-6));
521 }
522 
523 
524 /* ************************************************************************* */
525 TEST( Pose2, between )
526 {
527  // <
528  //
529  // ^
530  //
531  // *--0--*--*
532  Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
533  Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) looking at negative x
534 
535  Matrix actualH1,actualH2;
536  Pose2 expected(M_PI/2.0, Point2(2,2));
537  Pose2 actual1 = gT1.between(gT2);
538  Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
539  EXPECT(assert_equal(expected,actual1));
540  EXPECT(assert_equal(expected,actual2));
541 
542  Matrix expectedH1 = (Matrix(3,3) <<
543  0.0,-1.0,-2.0,
544  1.0, 0.0,-2.0,
545  0.0, 0.0,-1.0
546  ).finished();
547  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
548  EXPECT(assert_equal(expectedH1,actualH1));
549  EXPECT(assert_equal(numericalH1,actualH1));
550  // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
551  EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
552 
553  Matrix expectedH2 = (Matrix(3,3) <<
554  1.0, 0.0, 0.0,
555  0.0, 1.0, 0.0,
556  0.0, 0.0, 1.0
557  ).finished();
558  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
559  EXPECT(assert_equal(expectedH2,actualH2));
560  EXPECT(assert_equal(numericalH2,actualH2));
561 
562 }
563 
564 /* ************************************************************************* */
565 // reverse situation for extra test
566 TEST( Pose2, between2 )
567 {
568  Pose2 p2(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
569  Pose2 p1(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
570 
571  Matrix actualH1,actualH2;
572  p1.between(p2,actualH1,actualH2);
573  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, p1, p2);
574  EXPECT(assert_equal(numericalH1,actualH1));
575  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, p1, p2);
576  EXPECT(assert_equal(numericalH2,actualH2));
577 }
578 
579 /* ************************************************************************* */
580 // arbitrary, non perpendicular angles to be extra safe
581 TEST( Pose2, between3 )
582 {
583  Pose2 p2(M_PI/3.0, Point2(1,2));
584  Pose2 p1(M_PI/6.0, Point2(-1,4));
585 
586  Matrix actualH1,actualH2;
587  p1.between(p2,actualH1,actualH2);
588  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, p1, p2);
589  EXPECT(assert_equal(numericalH1,actualH1));
590  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, p1, p2);
591  EXPECT(assert_equal(numericalH2,actualH2));
592 }
593 
594 /* ************************************************************************* */
595 TEST( Pose2, round_trip )
596 {
597  Pose2 p1(1.23, 2.30, 0.2);
598  Pose2 odo(0.53, 0.39, 0.15);
599  Pose2 p2 = p1.compose(odo);
600  EXPECT(assert_equal(odo, p1.between(p2)));
601 }
602 
603 namespace {
604  /* ************************************************************************* */
605  // some shared test values
606  Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI/4.0);
607  Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
608 
609  /* ************************************************************************* */
610  Rot2 bearing_proxy(const Pose2& pose, const Point2& pt) {
611  return pose.bearing(pt);
612  }
613 }
614 
615 TEST( Pose2, bearing )
616 {
617  Matrix expectedH1, actualH1, expectedH2, actualH2;
618 
619  // establish bearing is indeed zero
620  EXPECT(assert_equal(Rot2(),x1.bearing(l1)));
621 
622  // establish bearing is indeed 45 degrees
623  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),x1.bearing(l2)));
624 
625  // establish bearing is indeed 45 degrees even if shifted
626  Rot2 actual23 = x2.bearing(l3, actualH1, actualH2);
627  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual23));
628 
629  // Check numerical derivatives
630  expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
631  EXPECT(assert_equal(expectedH1,actualH1));
632  expectedH2 = numericalDerivative22(bearing_proxy, x2, l3);
633  EXPECT(assert_equal(expectedH2,actualH2));
634 
635  // establish bearing is indeed 45 degrees even if rotated
636  Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
637  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual34));
638 
639  // Check numerical derivatives
640  expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
641  expectedH2 = numericalDerivative22(bearing_proxy, x3, l4);
642  EXPECT(assert_equal(expectedH1,actualH1));
643  EXPECT(assert_equal(expectedH2,actualH2));
644 }
645 
646 /* ************************************************************************* */
647 namespace {
648  Rot2 bearing_pose_proxy(const Pose2& pose, const Pose2& pt) {
649  return pose.bearing(pt);
650  }
651 }
652 
653 TEST( Pose2, bearing_pose )
654 {
655  Pose2 xl1(1, 0, M_PI/2.0), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI/2.0), xl4(1, 3, 0);
656 
657  Matrix expectedH1, actualH1, expectedH2, actualH2;
658 
659  // establish bearing is indeed zero
660  EXPECT(assert_equal(Rot2(),x1.bearing(xl1)));
661 
662  // establish bearing is indeed 45 degrees
663  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),x1.bearing(xl2)));
664 
665  // establish bearing is indeed 45 degrees even if shifted
666  Rot2 actual23 = x2.bearing(xl3, actualH1, actualH2);
667  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual23));
668 
669  // Check numerical derivatives
670  expectedH1 = numericalDerivative21(bearing_pose_proxy, x2, xl3);
671  expectedH2 = numericalDerivative22(bearing_pose_proxy, x2, xl3);
672  EXPECT(assert_equal(expectedH1,actualH1));
673  EXPECT(assert_equal(expectedH2,actualH2));
674 
675  // establish bearing is indeed 45 degrees even if rotated
676  Rot2 actual34 = x3.bearing(xl4, actualH1, actualH2);
677  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual34));
678 
679  // Check numerical derivatives
680  expectedH1 = numericalDerivative21(bearing_pose_proxy, x3, xl4);
681  expectedH2 = numericalDerivative22(bearing_pose_proxy, x3, xl4);
682  EXPECT(assert_equal(expectedH1,actualH1));
683  EXPECT(assert_equal(expectedH2,actualH2));
684 }
685 
686 /* ************************************************************************* */
687 namespace {
688  double range_proxy(const Pose2& pose, const Point2& point) {
689  return pose.range(point);
690  }
691 }
692 TEST( Pose2, range )
693 {
694  Matrix expectedH1, actualH1, expectedH2, actualH2;
695 
696  // establish range is indeed zero
697  EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
698 
699  // establish range is indeed 45 degrees
700  EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(l2),1e-9);
701 
702  // Another pair
703  double actual23 = x2.range(l3, actualH1, actualH2);
704  EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
705 
706  // Check numerical derivatives
707  expectedH1 = numericalDerivative21(range_proxy, x2, l3);
708  expectedH2 = numericalDerivative22(range_proxy, x2, l3);
709  EXPECT(assert_equal(expectedH1,actualH1));
710  EXPECT(assert_equal(expectedH2,actualH2));
711 
712  // Another test
713  double actual34 = x3.range(l4, actualH1, actualH2);
714  EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);
715 
716  // Check numerical derivatives
717  expectedH1 = numericalDerivative21(range_proxy, x3, l4);
718  expectedH2 = numericalDerivative22(range_proxy, x3, l4);
719  EXPECT(assert_equal(expectedH1,actualH1));
720  EXPECT(assert_equal(expectedH2,actualH2));
721 }
722 
723 /* ************************************************************************* */
724 namespace {
725  double range_pose_proxy(const Pose2& pose, const Pose2& point) {
726  return pose.range(point);
727  }
728 }
729 TEST( Pose2, range_pose )
730 {
731  Pose2 xl1(1, 0, M_PI/2.0), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI/2.0), xl4(1, 3, 0);
732 
733  Matrix expectedH1, actualH1, expectedH2, actualH2;
734 
735  // establish range is indeed zero
736  EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
737 
738  // establish range is indeed 45 degrees
739  EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(xl2),1e-9);
740 
741  // Another pair
742  double actual23 = x2.range(xl3, actualH1, actualH2);
743  EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
744 
745  // Check numerical derivatives
746  expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
747  expectedH2 = numericalDerivative22(range_pose_proxy, x2, xl3);
748  EXPECT(assert_equal(expectedH1,actualH1));
749  EXPECT(assert_equal(expectedH2,actualH2));
750 
751  // Another test
752  double actual34 = x3.range(xl4, actualH1, actualH2);
753  EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);
754 
755  // Check numerical derivatives
756  expectedH1 = numericalDerivative21(range_pose_proxy, x3, xl4);
757  expectedH2 = numericalDerivative22(range_pose_proxy, x3, xl4);
758  EXPECT(assert_equal(expectedH1,actualH1));
759  EXPECT(assert_equal(expectedH2,actualH2));
760 }
761 
762 /* ************************************************************************* */
763 
764 TEST(Pose2, align_1) {
765  Pose2 expected(Rot2::fromAngle(0), Point2(10, 10));
766  Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)},
767  {Point2(30, 20), Point2(20, 10)}};
768  std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
769  EXPECT(assert_equal(expected, *aTb));
770 }
771 
772 TEST(Pose2, align_2) {
773  Point2 t(20, 10);
774  Rot2 R = Rot2::fromAngle(M_PI/2.0);
775  Pose2 expected(R, t);
776 
777  Point2 b1(0, 0), b2(10, 0);
778  Point2Pairs ab_pairs {{expected.transformFrom(b1), b1},
779  {expected.transformFrom(b2), b2}};
780 
781  std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
782  EXPECT(assert_equal(expected, *aTb));
783 }
784 
785 namespace align_3 {
786  Point2 t(10, 10);
787  Pose2 expected(Rot2::fromAngle(2*M_PI/3), t);
788  Point2 b1(0, 0), b2(10, 0), b3(10, 10);
789  Point2 a1 = expected.transformFrom(b1),
790  a2 = expected.transformFrom(b2),
791  a3 = expected.transformFrom(b3);
792 }
793 
794 TEST(Pose2, align_3) {
795  using namespace align_3;
796 
797  Point2Pair ab1(make_pair(a1, b1));
798  Point2Pair ab2(make_pair(a2, b2));
799  Point2Pair ab3(make_pair(a3, b3));
800  const Point2Pairs ab_pairs{ab1, ab2, ab3};
801 
802  std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
803  EXPECT(assert_equal(expected, *aTb));
804 }
805 
806 namespace {
807  /* ************************************************************************* */
808  // Prototype code to align two triangles using a rigid transform
809  /* ************************************************************************* */
810  struct Triangle { size_t i_, j_, k_;};
811 
812  std::optional<Pose2> align2(const Point2Vector& as, const Point2Vector& bs,
813  const pair<Triangle, Triangle>& trianglePair) {
814  const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
815  Point2Pairs ab_pairs = {{as[t1.i_], bs[t2.i_]},
816  {as[t1.j_], bs[t2.j_]},
817  {as[t1.k_], bs[t2.k_]}};
818  return Pose2::Align(ab_pairs);
819  }
820 }
821 
822 TEST(Pose2, align_4) {
823  using namespace align_3;
824 
825  Point2Vector as{a1, a2, a3}, bs{b3, b1, b2}; // note in 3,1,2 order !
826 
827  Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2;
828  Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0;
829 
830  std::optional<Pose2> actual = align2(as, bs, {t1, t2});
831  EXPECT(assert_equal(expected, *actual));
832 }
833 
834 //******************************************************************************
835 namespace {
836 Pose2 id;
837 Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5)));
838 Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
839 } // namespace
840 
841 //******************************************************************************
842 TEST(Pose2, Invariants) {
843  EXPECT(check_group_invariants(id, id));
844  EXPECT(check_group_invariants(id, T1));
845  EXPECT(check_group_invariants(T2, id));
846  EXPECT(check_group_invariants(T2, T1));
847 
848  EXPECT(check_manifold_invariants(id, id));
849  EXPECT(check_manifold_invariants(id, T1));
850  EXPECT(check_manifold_invariants(T2, id));
851  EXPECT(check_manifold_invariants(T2, T1));
852 }
853 
854 //******************************************************************************
855 TEST(Pose2, LieGroupDerivatives) {
856  CHECK_LIE_GROUP_DERIVATIVES(id, id);
857  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
858  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
859  CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
860 }
861 
862 //******************************************************************************
863 TEST(Pose2, ChartDerivatives) {
864  CHECK_CHART_DERIVATIVES(id, id);
865  CHECK_CHART_DERIVATIVES(id, T2);
866  CHECK_CHART_DERIVATIVES(T2, id);
867  CHECK_CHART_DERIVATIVES(T2, T1);
868 }
869 
870 //******************************************************************************
871 #include "testPoseAdjointMap.h"
872 
873 TEST(Pose2 , TransformCovariance3) {
874  // Use simple covariance matrices and transforms to create tests that can be
875  // validated with simple computations.
876  using namespace test_pose_adjoint_map;
877 
878  // rotate
879  {
880  auto cov = FullCovarianceFromSigmas<Pose2>({0.1, 0.3, 0.7});
881  auto transformed = TransformCovariance<Pose2>{{0., 0., 90 * degree}}(cov);
882  // interchange x and y axes
883  EXPECT(assert_equal(
884  Vector3{cov(1, 1), cov(0, 0), cov(2, 2)},
885  Vector3{transformed.diagonal()}));
886  EXPECT(assert_equal(
887  Vector3{-cov(1, 0), -cov(2, 1), cov(2, 0)},
888  Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
889  }
890 
891  // translate along x with uncertainty in x
892  {
893  auto cov = SingleVariableCovarianceFromSigma<Pose2>(0, 0.1);
894  auto transformed = TransformCovariance<Pose2>{{20., 0., 0.}}(cov);
895  // No change
896  EXPECT(assert_equal(cov, transformed));
897  }
898 
899  // translate along x with uncertainty in y
900  {
901  auto cov = SingleVariableCovarianceFromSigma<Pose2>(1, 0.1);
902  auto transformed = TransformCovariance<Pose2>{{20., 0., 0.}}(cov);
903  // No change
904  EXPECT(assert_equal(cov, transformed));
905  }
906 
907  // translate along x with uncertainty in theta
908  {
909  auto cov = SingleVariableCovarianceFromSigma<Pose2>(2, 0.1);
910  auto transformed = TransformCovariance<Pose2>{{20., 0., 0.}}(cov);
911  EXPECT(assert_equal(
912  Vector3{0., 0.1 * 0.1 * 20. * 20., 0.1 * 0.1},
913  Vector3{transformed.diagonal()}));
914  EXPECT(assert_equal(
915  Vector3{0., 0., -0.1 * 0.1 * 20.},
916  Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
917  }
918 
919  // rotate and translate along x with uncertainty in x
920  {
921  auto cov = SingleVariableCovarianceFromSigma<Pose2>(0, 0.1);
922  auto transformed = TransformCovariance<Pose2>{{20., 0., 90 * degree}}(cov);
923  // interchange x and y axes
924  EXPECT(assert_equal(
925  Vector3{cov(1, 1), cov(0, 0), cov(2, 2)},
926  Vector3{transformed.diagonal()}));
927  EXPECT(assert_equal(
928  Vector3{Z_3x1},
929  Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
930  }
931 
932  // rotate and translate along x with uncertainty in theta
933  {
934  auto cov = SingleVariableCovarianceFromSigma<Pose2>(2, 0.1);
935  auto transformed = TransformCovariance<Pose2>{{20., 0., 90 * degree}}(cov);
936  EXPECT(assert_equal(
937  Vector3{0., 0.1 * 0.1 * 20. * 20., 0.1 * 0.1},
938  Vector3{transformed.diagonal()}));
939  EXPECT(assert_equal(
940  Vector3{0., 0., -0.1 * 0.1 * 20.},
941  Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
942  }
943 }
944 
945 /* ************************************************************************* */
946 TEST(Pose2, Print) {
947  Pose2 pose(Rot2::Identity(), Point2(1, 2));
948 
949  // Generate the expected output
950  string s = "Planar Pose";
951  string expected_stdout = "(1, 2, 0)";
952  string expected1 = expected_stdout + "\n";
953  string expected2 = s + " " + expected1;
954 
955  EXPECT(assert_stdout_equal(expected_stdout, pose));
956 
957  EXPECT(assert_print_equal(expected1, pose));
958  EXPECT(assert_print_equal(expected2, pose, s));
959 }
960 
961 /* ************************************************************************* */
962 TEST(Pose2, vec) {
963  // Test a simple pose
964  Pose2 pose(Rot2::fromAngle(M_PI / 4), Point2(1, 2));
965 
966  // Test the 'vec' method
967  Vector9 expected_vec = Eigen::Map<Vector9>(pose.matrix().data());
968  Matrix93 actualH;
969  Vector9 actual_vec = pose.vec(actualH);
970  EXPECT(assert_equal(expected_vec, actual_vec));
971 
972  // Verify Jacobian with numerical derivatives
973  std::function<Vector9(const Pose2&)> f = [](const Pose2& p) { return p.vec(); };
974  Matrix93 numericalH = numericalDerivative11<Vector9, Pose2>(f, pose);
975  EXPECT(assert_equal(numericalH, actualH, 1e-9));
976 }
977 
978 /* ************************************************************************* */
979 int main() {
980  TestResult tr;
981  return TestRegistry::runAllTests(tr);
982 }
983 /* ************************************************************************* */
984 
gtsam::Pose2::inverse
Pose2 inverse() const
inverse
Definition: Pose2.cpp:202
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
gtsam::logmap
gtsam::Expression< typename gtsam::traits< T >::TangentVector > logmap(const gtsam::Expression< T > &x1, const gtsam::Expression< T > &x2)
logmap
Definition: slam/expressions.h:196
Pose2.h
2D Pose
gtsam::TransformCovariance
Definition: Lie.h:390
l3
Point3 l3(2, 2, 0)
gtsam::translation
Point3_ translation(const Pose3_ &pose)
Definition: slam/expressions.h:97
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
A3
static const double A3[]
Definition: expn.h:8
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::between
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:17
gtsam::Pose3::transformTo
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:413
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Pose3::vec
Vector vec(OptionalJacobian< 16, 6 > H={}) const
Return vectorized SE(3) matrix in column order.
Definition: Pose3.cpp:550
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
gtsam::IsMatrixLieGroup
Definition: Lie.h:311
screwPose2::xi
Vector xi
Definition: testPose2.cpp:169
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:43
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:47
l2
gtsam::Key l2
Definition: testLinearContainerFactor.cpp:24
TestHarness.h
align_3::expected
Pose2 expected(Rot2::fromAngle(2 *M_PI/3), t)
gtsam::assert_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Definition: TestableAssertions.h:353
gtsam::Pose2::t
const Point2 & t() const
translation
Definition: Pose2.h:256
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
simple::pose0
static Pose3 pose0
Definition: testInitializePose3.cpp:56
align_3::b3
Point2 b3(10, 10)
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
bearing_proxy
Unit3 bearing_proxy(const Pose3 &pose, const Point3 &point)
Definition: testPose3.cpp:770
pt
static const Point3 pt(1.0, 2.0, 3.0)
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
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
transformFrom_
static Point2 transformFrom_(const Pose2 &pose, const Point2 &point)
Definition: testPose2.cpp:317
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
gtsam::Rot2::Identity
static Rot2 Identity()
Definition: Rot2.h:113
gtsam::Rot2::matrix
Matrix2 matrix() const
Definition: Rot2.cpp:100
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
testLie.h
gtsam::Pose2::Expmap
static Pose2 Expmap(const Vector3 &xi, ChartJacobian H={})
Exponential map at identity - create a rotation from canonical coordinates .
Definition: Pose2.cpp:67
test_pose_adjoint_map
Definition: testPoseAdjointMap.h:24
gtsam::IsGroup
Definition: Group.h:42
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::Pose3::matrix
Matrix4 matrix() const
Definition: Pose3.cpp:365
Rot2.h
2D rotation
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
screwPose2::expectedR
Rot2 expectedR
Definition: testPose2.cpp:170
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::Pose3::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself={}, OptionalJacobian< 1, 3 > Hpoint={}) const
Definition: Pose3.cpp:441
Point2.h
2D Point
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::Point2Pair
std::pair< Point2, Point2 > Point2Pair
Definition: Point2.h:35
screwPose2::expected
Pose2 expected(expectedR, expectedT)
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
gtsam::Pose2::Logmap
static Vector3 Logmap(const Pose2 &p, ChartJacobian H={})
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose2.cpp:83
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:64
align_3::a1
Point2 a1
Definition: testPose2.cpp:789
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
id
static const Similarity3 id
Definition: testSimilarity3.cpp:44
align_3::a3
Point2 a3
Definition: testPose2.cpp:791
gtsam::transformFrom
Point3_ transformFrom(const Pose3_ &x, const Point3_ &p)
Definition: slam/expressions.h:47
testPoseAdjointMap.h
Support utilities for using AdjointMap for transforming Pose2 and Pose3 covariance matrices.
gtsam::assert_stdout_equal
bool assert_stdout_equal(const std::string &expected, const V &actual)
Definition: TestableAssertions.h:331
xl4
Pose3 xl4(Rot3::Ypr(0.0, 0.0, 1.0), Point3(1, 4,-4))
gtsam::testing::inverse
T inverse(const T &t)
Definition: lieProxies.h:43
align_3::b2
Point2 b2(10, 0)
xl2
Pose3 xl2(Rot3::Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
screwPose2
Definition: testPose2.cpp:167
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
x1
Pose3 x1
Definition: testPose3.cpp:692
GTSAM_CONCEPT_LIE_INST
#define GTSAM_CONCEPT_LIE_INST(T)
Definition: Lie.h:410
l
static const Line3 l(Rot3(), 1, 1)
main
int main()
Definition: testPose2.cpp:979
gtsam::Pose2::r
const Rot2 & r() const
rotation
Definition: Pose2.h:259
A2
static const double A2[]
Definition: expn.h:7
degree
const double degree
Definition: SimpleRotation.cpp:59
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:351
l4
Point3 l4(1, 4,-4)
CHECK_LIE_GROUP_DERIVATIVES
#define CHECK_LIE_GROUP_DERIVATIVES(t1, t2)
Definition: testLie.h:82
example2::aTb
Point3 aTb
Definition: testEssentialMatrixFactor.cpp:585
gtsam::Pose2::Align
static std::optional< Pose2 > Align(const Point2Pairs &abPointPairs)
Definition: Pose2.cpp:371
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
T2
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
Eigen::Triplet< double >
homogeneous
void homogeneous(void)
Definition: geo_homogeneous.cpp:13
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
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
range_proxy
double range_proxy(const Pose3 &pose, const Point3 &point)
Definition: testPose3.cpp:700
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
TestResult
Definition: TestResult.h:26
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
align_3
Definition: testPose2.cpp:785
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::Rot2
Definition: Rot2.h:35
gtsam::Pose3::bearing
Unit3 bearing(const Point3 &point, OptionalJacobian< 2, 6 > Hself={}, OptionalJacobian< 2, 3 > Hpoint={}) const
Definition: Pose3.cpp:469
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:357
pose2
Definition: testFrobeniusFactor.cpp:193
xl3
Pose3 xl3(Rot3::Ypr(1.0, 0.0, 0.0), Point3(2, 2, 0))
gtsam
traits
Definition: SFMdata.h:40
gtsam::rotation
Rot3_ rotation(const Pose3_ &pose)
Definition: slam/expressions.h:93
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::testing::compose
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
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
align_3::a2
Point2 a2
Definition: testPose2.cpp:790
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:24
std
Definition: BFloat16.h:88
gtsam::Pose3::transformFrom
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:389
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
xl1
Pose3 xl1(Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0))
p
float * p
Definition: Tutorial_Map_using.cpp:9
v2
Vector v2
Definition: testSerializationBase.cpp:39
simulated2D::odo
Point2 odo(const Point2 &x1, const Point2 &x2)
odometry between two poses
Definition: simulated2D.h:99
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::testing::between
T between(const T &t1, const T &t2)
Definition: lieProxies.h:36
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
gtsam::Point2Vector
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
Eigen::Matrix< double, 9, 3 >
gtsam::Point2Pairs
std::vector< Point2Pair > Point2Pairs
Definition: Point2.h:38
gtsam::Pose2::AdjointMap
Matrix3 AdjointMap() const
Definition: Pose2.cpp:127
v3
Vector v3
Definition: testSerializationBase.cpp:40
gtsam::Pose2::transformFrom
Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
Definition: Pose2.cpp:241
M_PI
#define M_PI
Definition: mconf.h:117
T1
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1)
screwPose2::expectedT
Point2 expectedT(-0.0446635, 0.29552)
A4
static const double A4[]
Definition: expn.h:9
lieProxies.h
Provides convenient mappings of common member functions for testing.
align_3::t
Point2 t(10, 10)
screwPose2::w
double w
Definition: testPose2.cpp:168
GTSAM_CONCEPT_ASSERT
#define GTSAM_CONCEPT_ASSERT(concept)
Definition: base/concepts.h:25
align_3::b1
Point2 b1(0, 0)
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
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
R
Rot2 R(Rot2::fromAngle(0.1))
v1
Vector v1
Definition: testSerializationBase.cpp:38
gtsam::Pose2::Vee
static Vector3 Vee(const Matrix3 &X)
Vee maps from Lie algebra to tangent vector.
Definition: Pose2.cpp:216
gtsam::Pose2
Definition: Pose2.h:39
gtsam::Pose2::Hat
static Matrix3 Hat(const Vector3 &xi)
Hat maps from tangent vector to Lie algebra.
Definition: Pose2.cpp:207
range_pose_proxy
double range_pose_proxy(const Pose3 &pose, const Pose3 &point)
Definition: testPose3.cpp:735
transformTo_
static Point2 transformTo_(const Pose2 &pose, const Point2 &point)
Definition: testPose2.cpp:288

gtsam
Author(s):
autogenerated on Wed Mar 19 2025 03:07:20