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(IsLieGroup<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 case for screw motion in the plane
146 namespace screwPose2 {
147  double w=0.3;
148  Vector xi = (Vector(3) << 0.0, w, w).finished();
149  Rot2 expectedR = Rot2::fromAngle(w);
150  Point2 expectedT(-0.0446635, 0.29552);
151  Pose2 expected(expectedR, expectedT);
152 }
153 
154 TEST(Pose2, expmap_c)
155 {
156  EXPECT(assert_equal(screwPose2::expected, expm<Pose2>(screwPose2::xi),1e-6));
157  EXPECT(assert_equal(screwPose2::expected, Pose2::Expmap(screwPose2::xi),1e-6));
158  EXPECT(assert_equal(screwPose2::xi, Pose2::Logmap(screwPose2::expected),1e-6));
159 }
160 
161 /* ************************************************************************* */
162 TEST(Pose2, expmap_c_full)
163 {
164  double w=0.3;
165  Vector xi = (Vector(3) << 0.0, w, w).finished();
166  Rot2 expectedR = Rot2::fromAngle(w);
167  Point2 expectedT(-0.0446635, 0.29552);
168  Pose2 expected(expectedR, expectedT);
169  EXPECT(assert_equal(expected, expm<Pose2>(xi),1e-6));
170  EXPECT(assert_equal(expected, Pose2::Expmap(xi),1e-6));
171  EXPECT(assert_equal(xi, Pose2::Logmap(expected),1e-6));
172 }
173 
174 /* ************************************************************************* */
175 // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
176 TEST(Pose2, Adjoint_full) {
177  Pose2 T(1, 2, 3);
178  Pose2 expected = T * Pose2::Expmap(screwPose2::xi) * T.inverse();
179  Vector xiprime = T.Adjoint(screwPose2::xi);
180  EXPECT(assert_equal(expected, Pose2::Expmap(xiprime), 1e-6));
181 
182  Vector3 xi2(4, 5, 6);
183  Pose2 expected2 = T * Pose2::Expmap(xi2) * T.inverse();
184  Vector xiprime2 = T.Adjoint(xi2);
185  EXPECT(assert_equal(expected2, Pose2::Expmap(xiprime2), 1e-6));
186 }
187 
188 /* ************************************************************************* */
189 // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
190 TEST(Pose2, Adjoint_hat) {
191  Pose2 T(1, 2, 3);
192  auto hat = [](const Vector& xi) { return ::wedge<Pose2>(xi); };
193  Matrix3 expected = T.matrix() * hat(screwPose2::xi) * T.matrix().inverse();
194  Matrix3 xiprime = hat(T.Adjoint(screwPose2::xi));
195  EXPECT(assert_equal(expected, xiprime, 1e-6));
196 
197  Vector3 xi2(4, 5, 6);
198  Matrix3 expected2 = T.matrix() * hat(xi2) * T.matrix().inverse();
199  Matrix3 xiprime2 = hat(T.Adjoint(xi2));
200  EXPECT(assert_equal(expected2, xiprime2, 1e-6));
201 }
202 
203 /* ************************************************************************* */
204 TEST(Pose2, logmap) {
205  Pose2 pose0(M_PI/2.0, Point2(1, 2));
206  Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
207 #ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
208  Vector3 expected(0.00986473, -0.0150896, 0.018);
209 #else
210  Vector3 expected(0.01, -0.015, 0.018);
211 #endif
212  Vector actual = pose0.localCoordinates(pose);
213  EXPECT(assert_equal(expected, actual, 1e-5));
214 }
215 
216 /* ************************************************************************* */
217 TEST(Pose2, logmap_full) {
218  Pose2 pose0(M_PI/2.0, Point2(1, 2));
219  Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
220  Vector expected = Vector3(0.00986473, -0.0150896, 0.018);
221  Vector actual = logmap_default<Pose2>(pose0, pose);
222  EXPECT(assert_equal(expected, actual, 1e-5));
223 }
224 
225 /* ************************************************************************* */
226 TEST( Pose2, ExpmapDerivative1) {
227  Matrix3 actualH;
228  Vector3 w(0.1, 0.27, -0.3);
229  Pose2::Expmap(w,actualH);
230  Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
231  OptionalJacobian<3, 3> >(&Pose2::Expmap, w, {}, 1e-2);
232  EXPECT(assert_equal(expectedH, actualH, 1e-5));
233 }
234 
235 /* ************************************************************************* */
236 TEST( Pose2, ExpmapDerivative2) {
237  Matrix3 actualH;
238  Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
239  Pose2::Expmap(w0,actualH);
240  Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
241  OptionalJacobian<3, 3> >(&Pose2::Expmap, w0, {}, 1e-2);
242  EXPECT(assert_equal(expectedH, actualH, 1e-5));
243 }
244 
245 /* ************************************************************************* */
246 TEST( Pose2, LogmapDerivative1) {
247  Matrix3 actualH;
248  Vector3 w(0.1, 0.27, -0.3);
249  Pose2 p = Pose2::Expmap(w);
250  EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5));
251  Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
252  OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2);
253  EXPECT(assert_equal(expectedH, actualH, 1e-5));
254 }
255 
256 /* ************************************************************************* */
257 TEST( Pose2, LogmapDerivative2) {
258  Matrix3 actualH;
259  Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
260  Pose2 p = Pose2::Expmap(w0);
261  EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5));
262  Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
263  OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2);
264  EXPECT(assert_equal(expectedH, actualH, 1e-5));
265 }
266 
267 /* ************************************************************************* */
268 static Point2 transformTo_(const Pose2& pose, const Point2& point) {
269  return pose.transformTo(point);
270 }
271 
272 TEST(Pose2, transformTo) {
273  Pose2 pose(M_PI / 2.0, Point2(1, 2)); // robot at (1,2) looking towards y
274  Point2 point(-1, 4); // landmark at (-1,4)
275 
276  // expected
277  Point2 expected(2, 2);
278  Matrix expectedH1 =
279  (Matrix(2, 3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0).finished();
280  Matrix expectedH2 = (Matrix(2, 2) << 0.0, 1.0, -1.0, 0.0).finished();
281 
282  // actual
283  Matrix actualH1, actualH2;
284  Point2 actual = pose.transformTo(point, actualH1, actualH2);
285  EXPECT(assert_equal(expected, actual));
286 
287  EXPECT(assert_equal(expectedH1, actualH1));
288  Matrix numericalH1 = numericalDerivative21(transformTo_, pose, point);
289  EXPECT(assert_equal(numericalH1, actualH1));
290 
291  EXPECT(assert_equal(expectedH2, actualH2));
292  Matrix numericalH2 = numericalDerivative22(transformTo_, pose, point);
293  EXPECT(assert_equal(numericalH2, actualH2));
294 }
295 
296 /* ************************************************************************* */
297 static Point2 transformFrom_(const Pose2& pose, const Point2& point) {
298  return pose.transformFrom(point);
299 }
300 
301 TEST(Pose2, transformFrom) {
302  Pose2 pose(1., 0., M_PI / 2.0);
303  Point2 pt(2., 1.);
304  Matrix H1, H2;
305  Point2 actual = pose.transformFrom(pt, H1, H2);
306 
307  Point2 expected(0., 2.);
308  EXPECT(assert_equal(expected, actual));
309 
310  Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.).finished();
311  Matrix H2_expected = (Matrix(2, 2) << 0., -1., 1., 0.).finished();
312 
313  Matrix numericalH1 = numericalDerivative21(transformFrom_, pose, pt);
314  EXPECT(assert_equal(H1_expected, H1));
315  EXPECT(assert_equal(H1_expected, numericalH1));
316 
317  Matrix numericalH2 = numericalDerivative22(transformFrom_, pose, pt);
318  EXPECT(assert_equal(H2_expected, H2));
319  EXPECT(assert_equal(H2_expected, numericalH2));
320 }
321 
322 /* ************************************************************************* */
323 TEST(Pose2, compose_a)
324 {
325  Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)));
326  Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0));
327 
328  Matrix actualDcompose1;
329  Matrix actualDcompose2;
330  Pose2 actual = pose1.compose(pose2, actualDcompose1, actualDcompose2);
331 
332  Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
333  EXPECT(assert_equal(expected, actual));
334 
335  Matrix expectedH1 = (Matrix(3,3) <<
336  0.0, 1.0, 0.0,
337  -1.0, 0.0, 2.0,
338  0.0, 0.0, 1.0
339  ).finished();
340  Matrix expectedH2 = I_3x3;
341  Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
342  Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
343  EXPECT(assert_equal(expectedH1,actualDcompose1));
344  EXPECT(assert_equal(numericalH1,actualDcompose1));
345  EXPECT(assert_equal(expectedH2,actualDcompose2));
346  EXPECT(assert_equal(numericalH2,actualDcompose2));
347 
348  Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
349  Point2 expected_point(-1.0, -1.0);
350  Point2 actual_point1 = (pose1 * pose2).transformTo(point);
351  Point2 actual_point2 = pose2.transformTo(pose1.transformTo(point));
352  EXPECT(assert_equal(expected_point, actual_point1));
353  EXPECT(assert_equal(expected_point, actual_point2));
354 }
355 
356 /* ************************************************************************* */
357 TEST(Pose2, compose_b)
358 {
359  Pose2 pose1(Rot2::fromAngle(M_PI/10.0), Point2(.75, .5));
360  Pose2 pose2(Rot2::fromAngle(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585));
361 
362  Pose2 pose_expected(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 2.0));
363 
364  Pose2 pose_actual_op = pose1 * pose2;
365  Matrix actualDcompose1, actualDcompose2;
366  Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2);
367 
368  Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
369  Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
370  EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5));
371  EXPECT(assert_equal(numericalH2,actualDcompose2));
372 
373  EXPECT(assert_equal(pose_expected, pose_actual_op));
374  EXPECT(assert_equal(pose_expected, pose_actual_fcn));
375 }
376 
377 /* ************************************************************************* */
378 TEST(Pose2, compose_c)
379 {
380  Pose2 pose1(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 1.0));
381  Pose2 pose2(Rot2::fromAngle(M_PI/4.0), Point2(sqrt(.5), sqrt(.5)));
382 
383  Pose2 pose_expected(Rot2::fromAngle(M_PI/2.0), Point2(1.0, 2.0));
384 
385  Pose2 pose_actual_op = pose1 * pose2;
386  Matrix actualDcompose1, actualDcompose2;
387  Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2);
388 
389  Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
390  Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
391  EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5));
392  EXPECT(assert_equal(numericalH2,actualDcompose2));
393 
394  EXPECT(assert_equal(pose_expected, pose_actual_op));
395  EXPECT(assert_equal(pose_expected, pose_actual_fcn));
396 }
397 
398 /* ************************************************************************* */
399 TEST(Pose2, inverse )
400 {
401  Point2 origin(0,0), t(1,2);
402  Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
403 
404  Pose2 identity, lTg = gTl.inverse();
405  EXPECT(assert_equal(identity,lTg.compose(gTl)));
406  EXPECT(assert_equal(identity,gTl.compose(lTg)));
407 
408  Point2 l(4,5), g(-4,6);
409  EXPECT(assert_equal(g,gTl*l));
410  EXPECT(assert_equal(l,lTg*g));
411 
412  // Check derivative
413  Matrix numericalH = numericalDerivative11<Pose2,Pose2>(testing::inverse, lTg);
414  Matrix actualDinverse;
415  lTg.inverse(actualDinverse);
416  EXPECT(assert_equal(numericalH,actualDinverse));
417 }
418 
419 namespace {
420  /* ************************************************************************* */
421  Vector homogeneous(const Point2& p) {
422  return (Vector(3) << p.x(), p.y(), 1.0).finished();
423  }
424 
425  /* ************************************************************************* */
426  Matrix matrix(const Pose2& gTl) {
427  Matrix gRl = gTl.r().matrix();
428  Point2 gt = gTl.t();
429  return (Matrix(3, 3) <<
430  gRl(0, 0), gRl(0, 1), gt.x(),
431  gRl(1, 0), gRl(1, 1), gt.y(),
432  0.0, 0.0, 1.0).finished();
433  }
434 }
435 
436 /* ************************************************************************* */
437 TEST( Pose2, matrix )
438 {
439  Point2 origin(0,0), t(1,2);
440  Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
441  Matrix gMl = matrix(gTl);
442  EXPECT(assert_equal((Matrix(3,3) <<
443  0.0, -1.0, 1.0,
444  1.0, 0.0, 2.0,
445  0.0, 0.0, 1.0).finished(),
446  gMl));
447  Rot2 gR1 = gTl.r();
448  EXPECT(assert_equal(homogeneous(t),gMl*homogeneous(origin)));
449  Point2 x_axis(1,0), y_axis(0,1);
450  EXPECT(assert_equal((Matrix(2,2) <<
451  0.0, -1.0,
452  1.0, 0.0).finished(),
453  gR1.matrix()));
454  EXPECT(assert_equal(Point2(0,1),gR1*x_axis));
455  EXPECT(assert_equal(Point2(-1,0),gR1*y_axis));
456  EXPECT(assert_equal(homogeneous(Point2(1+0,2+1)),gMl*homogeneous(x_axis)));
457  EXPECT(assert_equal(homogeneous(Point2(1-1,2+0)),gMl*homogeneous(y_axis)));
458 
459  // check inverse pose
460  Matrix lMg = matrix(gTl.inverse());
461  EXPECT(assert_equal((Matrix(3,3) <<
462  0.0, 1.0,-2.0,
463  -1.0, 0.0, 1.0,
464  0.0, 0.0, 1.0).finished(),
465  lMg));
466 }
467 
468 /* ************************************************************************* */
469 TEST( Pose2, compose_matrix )
470 {
471  Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
472  Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) looking at negative x
473  Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
474  EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
475 }
476 
477 
478 
479 /* ************************************************************************* */
480 TEST( Pose2, translation ) {
481  Pose2 pose(3.5, -8.2, 4.2);
482 
483  Matrix actualH;
484  EXPECT(assert_equal((Vector2() << 3.5, -8.2).finished(), pose.translation(actualH), 1e-8));
485 
486  std::function<Point2(const Pose2&)> f = [](const Pose2& T) { return T.translation(); };
487  Matrix numericalH = numericalDerivative11<Point2, Pose2>(f, pose);
488  EXPECT(assert_equal(numericalH, actualH, 1e-6));
489 }
490 
491 /* ************************************************************************* */
492 TEST( Pose2, rotation ) {
493  Pose2 pose(3.5, -8.2, 4.2);
494 
495  Matrix actualH(4, 3);
496  EXPECT(assert_equal(Rot2(4.2), pose.rotation(actualH), 1e-8));
497 
498  std::function<Rot2(const Pose2&)> f = [](const Pose2& T) { return T.rotation(); };
499  Matrix numericalH = numericalDerivative11<Rot2, Pose2>(f, pose);
500  EXPECT(assert_equal(numericalH, actualH, 1e-6));
501 }
502 
503 
504 /* ************************************************************************* */
505 TEST( Pose2, between )
506 {
507  // <
508  //
509  // ^
510  //
511  // *--0--*--*
512  Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
513  Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) looking at negative x
514 
515  Matrix actualH1,actualH2;
516  Pose2 expected(M_PI/2.0, Point2(2,2));
517  Pose2 actual1 = gT1.between(gT2);
518  Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
519  EXPECT(assert_equal(expected,actual1));
520  EXPECT(assert_equal(expected,actual2));
521 
522  Matrix expectedH1 = (Matrix(3,3) <<
523  0.0,-1.0,-2.0,
524  1.0, 0.0,-2.0,
525  0.0, 0.0,-1.0
526  ).finished();
527  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
528  EXPECT(assert_equal(expectedH1,actualH1));
529  EXPECT(assert_equal(numericalH1,actualH1));
530  // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
531  EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
532 
533  Matrix expectedH2 = (Matrix(3,3) <<
534  1.0, 0.0, 0.0,
535  0.0, 1.0, 0.0,
536  0.0, 0.0, 1.0
537  ).finished();
538  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2);
539  EXPECT(assert_equal(expectedH2,actualH2));
540  EXPECT(assert_equal(numericalH2,actualH2));
541 
542 }
543 
544 /* ************************************************************************* */
545 // reverse situation for extra test
546 TEST( Pose2, between2 )
547 {
548  Pose2 p2(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
549  Pose2 p1(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
550 
551  Matrix actualH1,actualH2;
552  p1.between(p2,actualH1,actualH2);
553  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, p1, p2);
554  EXPECT(assert_equal(numericalH1,actualH1));
555  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, p1, p2);
556  EXPECT(assert_equal(numericalH2,actualH2));
557 }
558 
559 /* ************************************************************************* */
560 // arbitrary, non perpendicular angles to be extra safe
561 TEST( Pose2, between3 )
562 {
563  Pose2 p2(M_PI/3.0, Point2(1,2));
564  Pose2 p1(M_PI/6.0, Point2(-1,4));
565 
566  Matrix actualH1,actualH2;
567  p1.between(p2,actualH1,actualH2);
568  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, p1, p2);
569  EXPECT(assert_equal(numericalH1,actualH1));
570  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, p1, p2);
571  EXPECT(assert_equal(numericalH2,actualH2));
572 }
573 
574 /* ************************************************************************* */
575 TEST( Pose2, round_trip )
576 {
577  Pose2 p1(1.23, 2.30, 0.2);
578  Pose2 odo(0.53, 0.39, 0.15);
579  Pose2 p2 = p1.compose(odo);
580  EXPECT(assert_equal(odo, p1.between(p2)));
581 }
582 
583 namespace {
584  /* ************************************************************************* */
585  // some shared test values
586  Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI/4.0);
587  Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
588 
589  /* ************************************************************************* */
590  Rot2 bearing_proxy(const Pose2& pose, const Point2& pt) {
591  return pose.bearing(pt);
592  }
593 }
594 
595 TEST( Pose2, bearing )
596 {
597  Matrix expectedH1, actualH1, expectedH2, actualH2;
598 
599  // establish bearing is indeed zero
600  EXPECT(assert_equal(Rot2(),x1.bearing(l1)));
601 
602  // establish bearing is indeed 45 degrees
603  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),x1.bearing(l2)));
604 
605  // establish bearing is indeed 45 degrees even if shifted
606  Rot2 actual23 = x2.bearing(l3, actualH1, actualH2);
607  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual23));
608 
609  // Check numerical derivatives
610  expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
611  EXPECT(assert_equal(expectedH1,actualH1));
612  expectedH2 = numericalDerivative22(bearing_proxy, x2, l3);
613  EXPECT(assert_equal(expectedH2,actualH2));
614 
615  // establish bearing is indeed 45 degrees even if rotated
616  Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
617  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual34));
618 
619  // Check numerical derivatives
620  expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
621  expectedH2 = numericalDerivative22(bearing_proxy, x3, l4);
622  EXPECT(assert_equal(expectedH1,actualH1));
623  EXPECT(assert_equal(expectedH2,actualH2));
624 }
625 
626 /* ************************************************************************* */
627 namespace {
628  Rot2 bearing_pose_proxy(const Pose2& pose, const Pose2& pt) {
629  return pose.bearing(pt);
630  }
631 }
632 
633 TEST( Pose2, bearing_pose )
634 {
635  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);
636 
637  Matrix expectedH1, actualH1, expectedH2, actualH2;
638 
639  // establish bearing is indeed zero
640  EXPECT(assert_equal(Rot2(),x1.bearing(xl1)));
641 
642  // establish bearing is indeed 45 degrees
643  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),x1.bearing(xl2)));
644 
645  // establish bearing is indeed 45 degrees even if shifted
646  Rot2 actual23 = x2.bearing(xl3, actualH1, actualH2);
647  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual23));
648 
649  // Check numerical derivatives
650  expectedH1 = numericalDerivative21(bearing_pose_proxy, x2, xl3);
651  expectedH2 = numericalDerivative22(bearing_pose_proxy, x2, xl3);
652  EXPECT(assert_equal(expectedH1,actualH1));
653  EXPECT(assert_equal(expectedH2,actualH2));
654 
655  // establish bearing is indeed 45 degrees even if rotated
656  Rot2 actual34 = x3.bearing(xl4, actualH1, actualH2);
657  EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual34));
658 
659  // Check numerical derivatives
660  expectedH1 = numericalDerivative21(bearing_pose_proxy, x3, xl4);
661  expectedH2 = numericalDerivative22(bearing_pose_proxy, x3, xl4);
662  EXPECT(assert_equal(expectedH1,actualH1));
663  EXPECT(assert_equal(expectedH2,actualH2));
664 }
665 
666 /* ************************************************************************* */
667 namespace {
668  double range_proxy(const Pose2& pose, const Point2& point) {
669  return pose.range(point);
670  }
671 }
672 TEST( Pose2, range )
673 {
674  Matrix expectedH1, actualH1, expectedH2, actualH2;
675 
676  // establish range is indeed zero
677  EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
678 
679  // establish range is indeed 45 degrees
680  EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(l2),1e-9);
681 
682  // Another pair
683  double actual23 = x2.range(l3, actualH1, actualH2);
684  EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
685 
686  // Check numerical derivatives
687  expectedH1 = numericalDerivative21(range_proxy, x2, l3);
688  expectedH2 = numericalDerivative22(range_proxy, x2, l3);
689  EXPECT(assert_equal(expectedH1,actualH1));
690  EXPECT(assert_equal(expectedH2,actualH2));
691 
692  // Another test
693  double actual34 = x3.range(l4, actualH1, actualH2);
694  EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);
695 
696  // Check numerical derivatives
697  expectedH1 = numericalDerivative21(range_proxy, x3, l4);
698  expectedH2 = numericalDerivative22(range_proxy, x3, l4);
699  EXPECT(assert_equal(expectedH1,actualH1));
700  EXPECT(assert_equal(expectedH2,actualH2));
701 }
702 
703 /* ************************************************************************* */
704 namespace {
705  double range_pose_proxy(const Pose2& pose, const Pose2& point) {
706  return pose.range(point);
707  }
708 }
709 TEST( Pose2, range_pose )
710 {
711  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);
712 
713  Matrix expectedH1, actualH1, expectedH2, actualH2;
714 
715  // establish range is indeed zero
716  EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
717 
718  // establish range is indeed 45 degrees
719  EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(xl2),1e-9);
720 
721  // Another pair
722  double actual23 = x2.range(xl3, actualH1, actualH2);
723  EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
724 
725  // Check numerical derivatives
726  expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
727  expectedH2 = numericalDerivative22(range_pose_proxy, x2, xl3);
728  EXPECT(assert_equal(expectedH1,actualH1));
729  EXPECT(assert_equal(expectedH2,actualH2));
730 
731  // Another test
732  double actual34 = x3.range(xl4, actualH1, actualH2);
733  EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);
734 
735  // Check numerical derivatives
736  expectedH1 = numericalDerivative21(range_pose_proxy, x3, xl4);
737  expectedH2 = numericalDerivative22(range_pose_proxy, x3, xl4);
738  EXPECT(assert_equal(expectedH1,actualH1));
739  EXPECT(assert_equal(expectedH2,actualH2));
740 }
741 
742 /* ************************************************************************* */
743 
744 TEST(Pose2, align_1) {
745  Pose2 expected(Rot2::fromAngle(0), Point2(10, 10));
746  Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)},
747  {Point2(30, 20), Point2(20, 10)}};
748  std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
749  EXPECT(assert_equal(expected, *aTb));
750 }
751 
752 TEST(Pose2, align_2) {
753  Point2 t(20, 10);
754  Rot2 R = Rot2::fromAngle(M_PI/2.0);
755  Pose2 expected(R, t);
756 
757  Point2 b1(0, 0), b2(10, 0);
758  Point2Pairs ab_pairs {{expected.transformFrom(b1), b1},
759  {expected.transformFrom(b2), b2}};
760 
761  std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
762  EXPECT(assert_equal(expected, *aTb));
763 }
764 
765 namespace align_3 {
766  Point2 t(10, 10);
767  Pose2 expected(Rot2::fromAngle(2*M_PI/3), t);
768  Point2 b1(0, 0), b2(10, 0), b3(10, 10);
769  Point2 a1 = expected.transformFrom(b1),
770  a2 = expected.transformFrom(b2),
771  a3 = expected.transformFrom(b3);
772 }
773 
774 TEST(Pose2, align_3) {
775  using namespace align_3;
776 
777  Point2Pair ab1(make_pair(a1, b1));
778  Point2Pair ab2(make_pair(a2, b2));
779  Point2Pair ab3(make_pair(a3, b3));
780  const Point2Pairs ab_pairs{ab1, ab2, ab3};
781 
782  std::optional<Pose2> aTb = Pose2::Align(ab_pairs);
783  EXPECT(assert_equal(expected, *aTb));
784 }
785 
786 namespace {
787  /* ************************************************************************* */
788  // Prototype code to align two triangles using a rigid transform
789  /* ************************************************************************* */
790  struct Triangle { size_t i_, j_, k_;};
791 
792  std::optional<Pose2> align2(const Point2Vector& as, const Point2Vector& bs,
793  const pair<Triangle, Triangle>& trianglePair) {
794  const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
795  Point2Pairs ab_pairs = {{as[t1.i_], bs[t2.i_]},
796  {as[t1.j_], bs[t2.j_]},
797  {as[t1.k_], bs[t2.k_]}};
798  return Pose2::Align(ab_pairs);
799  }
800 }
801 
802 TEST(Pose2, align_4) {
803  using namespace align_3;
804 
805  Point2Vector as{a1, a2, a3}, bs{b3, b1, b2}; // note in 3,1,2 order !
806 
807  Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2;
808  Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0;
809 
810  std::optional<Pose2> actual = align2(as, bs, {t1, t2});
811  EXPECT(assert_equal(expected, *actual));
812 }
813 
814 //******************************************************************************
815 namespace {
816 Pose2 id;
817 Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5)));
818 Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
819 } // namespace
820 
821 //******************************************************************************
822 TEST(Pose2, Invariants) {
823  EXPECT(check_group_invariants(id, id));
824  EXPECT(check_group_invariants(id, T1));
825  EXPECT(check_group_invariants(T2, id));
826  EXPECT(check_group_invariants(T2, T1));
827 
828  EXPECT(check_manifold_invariants(id, id));
829  EXPECT(check_manifold_invariants(id, T1));
830  EXPECT(check_manifold_invariants(T2, id));
831  EXPECT(check_manifold_invariants(T2, T1));
832 }
833 
834 //******************************************************************************
835 TEST(Pose2, LieGroupDerivatives) {
836  CHECK_LIE_GROUP_DERIVATIVES(id, id);
837  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
838  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
839  CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
840 }
841 
842 //******************************************************************************
843 TEST(Pose2, ChartDerivatives) {
844  CHECK_CHART_DERIVATIVES(id, id);
845  CHECK_CHART_DERIVATIVES(id, T2);
846  CHECK_CHART_DERIVATIVES(T2, id);
847  CHECK_CHART_DERIVATIVES(T2, T1);
848 }
849 
850 //******************************************************************************
851 #include "testPoseAdjointMap.h"
852 
853 TEST(Pose2 , TransformCovariance3) {
854  // Use simple covariance matrices and transforms to create tests that can be
855  // validated with simple computations.
856  using namespace test_pose_adjoint_map;
857 
858  // rotate
859  {
860  auto cov = FullCovarianceFromSigmas<Pose2>({0.1, 0.3, 0.7});
861  auto transformed = TransformCovariance<Pose2>{{0., 0., 90 * degree}}(cov);
862  // interchange x and y axes
863  EXPECT(assert_equal(
864  Vector3{cov(1, 1), cov(0, 0), cov(2, 2)},
865  Vector3{transformed.diagonal()}));
866  EXPECT(assert_equal(
867  Vector3{-cov(1, 0), -cov(2, 1), cov(2, 0)},
868  Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
869  }
870 
871  // translate along x with uncertainty in x
872  {
873  auto cov = SingleVariableCovarianceFromSigma<Pose2>(0, 0.1);
874  auto transformed = TransformCovariance<Pose2>{{20., 0., 0.}}(cov);
875  // No change
876  EXPECT(assert_equal(cov, transformed));
877  }
878 
879  // translate along x with uncertainty in y
880  {
881  auto cov = SingleVariableCovarianceFromSigma<Pose2>(1, 0.1);
882  auto transformed = TransformCovariance<Pose2>{{20., 0., 0.}}(cov);
883  // No change
884  EXPECT(assert_equal(cov, transformed));
885  }
886 
887  // translate along x with uncertainty in theta
888  {
889  auto cov = SingleVariableCovarianceFromSigma<Pose2>(2, 0.1);
890  auto transformed = TransformCovariance<Pose2>{{20., 0., 0.}}(cov);
891  EXPECT(assert_equal(
892  Vector3{0., 0.1 * 0.1 * 20. * 20., 0.1 * 0.1},
893  Vector3{transformed.diagonal()}));
894  EXPECT(assert_equal(
895  Vector3{0., 0., -0.1 * 0.1 * 20.},
896  Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
897  }
898 
899  // rotate and translate along x with uncertainty in x
900  {
901  auto cov = SingleVariableCovarianceFromSigma<Pose2>(0, 0.1);
902  auto transformed = TransformCovariance<Pose2>{{20., 0., 90 * degree}}(cov);
903  // interchange x and y axes
904  EXPECT(assert_equal(
905  Vector3{cov(1, 1), cov(0, 0), cov(2, 2)},
906  Vector3{transformed.diagonal()}));
907  EXPECT(assert_equal(
908  Vector3{Z_3x1},
909  Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
910  }
911 
912  // rotate and translate along x with uncertainty in theta
913  {
914  auto cov = SingleVariableCovarianceFromSigma<Pose2>(2, 0.1);
915  auto transformed = TransformCovariance<Pose2>{{20., 0., 90 * degree}}(cov);
916  EXPECT(assert_equal(
917  Vector3{0., 0.1 * 0.1 * 20. * 20., 0.1 * 0.1},
918  Vector3{transformed.diagonal()}));
919  EXPECT(assert_equal(
920  Vector3{0., 0., -0.1 * 0.1 * 20.},
921  Vector3{transformed(1, 0), transformed(2, 0), transformed(2, 1)}));
922  }
923 }
924 
925 /* ************************************************************************* */
926 TEST(Pose2, Print) {
927  Pose2 pose(Rot2::Identity(), Point2(1, 2));
928 
929  // Generate the expected output
930  string s = "Planar Pose";
931  string expected_stdout = "(1, 2, 0)";
932  string expected1 = expected_stdout + "\n";
933  string expected2 = s + " " + expected1;
934 
935  EXPECT(assert_stdout_equal(expected_stdout, pose));
936 
937  EXPECT(assert_print_equal(expected1, pose));
938  EXPECT(assert_print_equal(expected2, pose, s));
939 }
940 
941 /* ************************************************************************* */
942 int main() {
943  TestResult tr;
944  return TestRegistry::runAllTests(tr);
945 }
946 /* ************************************************************************* */
947 
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:192
Pose2.h
2D Pose
gtsam::TransformCovariance
Definition: Lie.h:352
l3
Point3 l3(2, 2, 0)
gtsam::translation
Point3_ translation(const Pose3_ &pose)
Definition: slam/expressions.h:93
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:372
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
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:352
gtsam::Pose2::t
const Point2 & t() const
translation
Definition: Pose2.h:259
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:741
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:297
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
gtsam::Rot2::Identity
static Rot2 Identity()
Definition: Rot2.h:110
gtsam::Rot2::matrix
Matrix2 matrix() const
Definition: Rot2.cpp:85
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:324
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:149
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:400
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:61
align_3::a1
Point2 a1
Definition: testPose2.cpp:769
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:771
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:330
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:146
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:663
GTSAM_CONCEPT_LIE_INST
#define GTSAM_CONCEPT_LIE_INST(T)
Definition: Lie.h:372
gtsam::IsLieGroup
Definition: Lie.h:260
l
static const Line3 l(Rot3(), 1, 1)
main
int main()
Definition: testPose2.cpp:942
gtsam::Pose2::r
const Rot2 & r() const
rotation
Definition: Pose2.h:262
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:310
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:331
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
range_proxy
double range_proxy(const Pose3 &pose, const Point3 &point)
Definition: testPose3.cpp:671
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:765
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:426
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:316
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:89
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:770
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:348
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
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
gtsam::Point2Pairs
std::vector< Point2Pair > Point2Pairs
Definition: Point2.h:38
gtsam::Pose2::AdjointMap
Matrix3 AdjointMap() const
Definition: Pose2.cpp:127
gtsam::Pose2::transformFrom
Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
Definition: Pose2.cpp:227
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:147
GTSAM_CONCEPT_ASSERT
#define GTSAM_CONCEPT_ASSERT(concept)
Definition: base/concepts.h:22
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
gtsam::wedge< Pose2 >
Matrix wedge< Pose2 >(const Vector &xi)
Definition: Pose2.h:359
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::Pose2
Definition: Pose2.h:39
range_pose_proxy
double range_pose_proxy(const Pose3 &pose, const Pose3 &point)
Definition: testPose3.cpp:706
transformTo_
static Point2 transformTo_(const Pose2 &pose, const Point2 &point)
Definition: testPose2.cpp:268

gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:16:55