testNavState.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 
19 #include <gtsam/navigation/NavState.h>
20 
21 #include <gtsam/base/lieProxies.h>
22 #include <gtsam/base/numericalDerivative.h>
23 #include <gtsam/base/TestableAssertions.h>
24 #include <gtsam/base/testLie.h>
25 
26 #include <CppUnitLite/TestHarness.h>
27 
28 using namespace std::placeholders;
29 using namespace std;
30 using namespace gtsam;
31 
32 GTSAM_CONCEPT_TESTABLE_INST(NavState)
33 GTSAM_CONCEPT_LIE_INST(NavState)
34 
35 static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3);
36 static const Point3 kPosition(1.0, 2.0, 3.0);
37 static const Pose3 kPose(kAttitude, kPosition);
38 static const Velocity3 kVelocity(0.4, 0.5, 0.6);
39 static const NavState kIdentity;
40 static const NavState kState1(kAttitude, kPosition, kVelocity);
41 static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04);
42 static const Vector3 kGravity(0, 0, 9.81);
43 static const Vector9 kZeroXi = Vector9::Zero();
44 
45 static const Point3 V(3, 0.4, -2.2);
46 static const Point3 P(0.2, 0.7, -2);
47 static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
48 static const Point3 V2(-6.5, 3.5, 6.2);
49 static const Point3 P2(3.5, -8.2, 4.2);
50 static const NavState T(R, P2, V2);
51 static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2);
52 static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7),
53  Point3(1, 2, 3));
54 
55 /* ************************************************************************* */
56 TEST(NavState, Constructor) {
57  std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
58  std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2,
59  std::placeholders::_3, nullptr, nullptr, nullptr);
60  Matrix aH1, aH2, aH3;
61  EXPECT(
62  assert_equal(kState1,
63  NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3)));
64  EXPECT(
65 assert_equal(
66  numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
67  EXPECT(
68 assert_equal(
69  numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
70  EXPECT(
71 assert_equal(
72  numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
73 }
74 
75 /* ************************************************************************* */
76 TEST(NavState, Constructor2) {
77  std::function<NavState(const Pose3&, const Vector3&)> construct =
78  std::bind(&NavState::FromPoseVelocity, std::placeholders::_1,
79  std::placeholders::_2, nullptr, nullptr);
80  Matrix aH1, aH2;
81  EXPECT(
82  assert_equal(kState1,
83  NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
84  EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1));
85  EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2));
86 }
87 
88 /* ************************************************************************* */
89 TEST( NavState, Attitude) {
90  Matrix39 aH, eH;
91  Rot3 actual = kState1.attitude(aH);
92  EXPECT(assert_equal(actual, kAttitude));
93  eH = numericalDerivative11<Rot3, NavState>(
94  std::bind(&NavState::attitude, std::placeholders::_1, nullptr), kState1);
95  EXPECT(assert_equal((Matrix )eH, aH));
96 }
97 
98 /* ************************************************************************* */
99 TEST( NavState, Position) {
100  Matrix39 aH, eH;
101  Point3 actual = kState1.position(aH);
102  EXPECT(assert_equal(actual, kPosition));
103  eH = numericalDerivative11<Point3, NavState>(
104  std::bind(&NavState::position, std::placeholders::_1, nullptr),
105  kState1);
106  EXPECT(assert_equal((Matrix )eH, aH));
107 }
108 
109 /* ************************************************************************* */
110 TEST( NavState, Velocity) {
111  Matrix39 aH, eH;
112  Velocity3 actual = kState1.velocity(aH);
113  EXPECT(assert_equal(actual, kVelocity));
114  eH = numericalDerivative11<Velocity3, NavState>(
115  std::bind(&NavState::velocity, std::placeholders::_1, nullptr),
116  kState1);
117  EXPECT(assert_equal((Matrix )eH, aH));
118 }
119 
120 /* ************************************************************************* */
121 TEST( NavState, BodyVelocity) {
122  Matrix39 aH, eH;
123  Velocity3 actual = kState1.bodyVelocity(aH);
124  EXPECT(assert_equal<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
125  eH = numericalDerivative11<Velocity3, NavState>(
126  std::bind(&NavState::bodyVelocity, std::placeholders::_1, nullptr),
127  kState1);
128  EXPECT(assert_equal((Matrix )eH, aH));
129 }
130 
131 /* ************************************************************************* */
132 TEST( NavState, Manifold ) {
133  // Check zero xi
134  EXPECT(assert_equal(kIdentity, kIdentity.retract(kZeroXi)));
135  EXPECT(assert_equal(kZeroXi, kIdentity.localCoordinates(kIdentity)));
136  EXPECT(assert_equal(kState1, kState1.retract(kZeroXi)));
137  EXPECT(assert_equal(kZeroXi, kState1.localCoordinates(kState1)));
138 
139  // Check definition of retract as operating on components separately
140  Vector9 xi;
141  xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
142  Rot3 drot = Rot3::Expmap(xi.head<3>());
143  Point3 dt = Point3(xi.segment<3>(3));
144  Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
145  NavState state2 = NavState(kState1.attitude() * drot,
146  kState1.position() + kState1.attitude() * dt,
147  kState1.velocity() + kState1.attitude() * dvel);
148  EXPECT(assert_equal(state2, kState1.retract(xi)));
149  EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
150 
151  // roundtrip from state2 to state3 and back
152  NavState state3 = state2.retract(xi);
153  EXPECT(assert_equal(xi, state2.localCoordinates(state3)));
154 
155  // Check retract derivatives
156  Matrix9 aH1, aH2;
157  kState1.retract(xi, aH1, aH2);
158  std::function<NavState(const NavState&, const Vector9&)> retract =
159  std::bind(&NavState::retract, std::placeholders::_1,
160  std::placeholders::_2, nullptr, nullptr);
161  EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
162  EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
163 
164  // Check retract derivatives on state 2
165  const Vector9 xi2 = -3.0*xi;
166  state2.retract(xi2, aH1, aH2);
167  EXPECT(assert_equal(numericalDerivative21(retract, state2, xi2), aH1));
168  EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2));
169 
170  // Check localCoordinates derivatives
171  std::function<Vector9(const NavState&, const NavState&)> local =
172  std::bind(&NavState::localCoordinates, std::placeholders::_1,
173  std::placeholders::_2, nullptr, nullptr);
174  // from state1 to state2
175  kState1.localCoordinates(state2, aH1, aH2);
176  EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
177  EXPECT(assert_equal(numericalDerivative22(local, kState1, state2), aH2));
178  // from identity to state2
179  kIdentity.localCoordinates(state2, aH1, aH2);
180  EXPECT(assert_equal(numericalDerivative21(local, kIdentity, state2), aH1));
181  EXPECT(assert_equal(numericalDerivative22(local, kIdentity, state2), aH2));
182  // from state2 to identity
183  state2.localCoordinates(kIdentity, aH1, aH2);
184  EXPECT(assert_equal(numericalDerivative21(local, state2, kIdentity), aH1));
185  EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
186 }
187 
188 /* ************************************************************************* */
189 TEST(NavState, Equals) {
190  NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3));
191  NavState pose2 = T3;
192  EXPECT(T3.equals(pose2));
193  NavState origin;
194  EXPECT(!T3.equals(origin));
195 }
196 
197 /* ************************************************************************* */
198 TEST(NavState, Compose) {
199  NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0});
200  NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0});
201  NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0},
202  {3.0, -1.0, 1.0});
203 
204  auto ab_c = (nav_state_a * nav_state_b) * nav_state_c;
205  auto a_bc = nav_state_a * (nav_state_b * nav_state_c);
206  CHECK(assert_equal(ab_c, a_bc));
207 
208  Matrix actual = (T2 * T2).matrix();
209 
210  Matrix expected = T2.matrix() * T2.matrix();
211  EXPECT(assert_equal(actual, expected, 1e-8));
212 
213  Matrix actualDcompose1, actualDcompose2;
214  T2.compose(T2, actualDcompose1, actualDcompose2);
215 
216  Matrix numericalH1 =
217  numericalDerivative21(testing::compose<NavState>, T2, T2);
218 
219  EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3));
220  EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3));
221 
222  Matrix numericalH2 =
223  numericalDerivative22(testing::compose<NavState>, T2, T2);
224  EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-4));
225 }
226 
227 /* ************************************************************************* */
228 // Check compose and its push-forward, another case
229 TEST(NavState, Compose2) {
230  const NavState& T1 = T;
231  Matrix actual = (T1 * T2).matrix();
232  Matrix expected = T1.matrix() * T2.matrix();
233  EXPECT(assert_equal(actual, expected, 1e-8));
234 
235  Matrix actualDcompose1, actualDcompose2;
236  T1.compose(T2, actualDcompose1, actualDcompose2);
237 
238  Matrix numericalH1 =
239  numericalDerivative21(testing::compose<NavState>, T1, T2);
240  EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3));
241  EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3));
242 
243  Matrix numericalH2 =
244  numericalDerivative22(testing::compose<NavState>, T1, T2);
245  EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-5));
246 }
247 
248 /* ************************************************************************* */
249 TEST(NavState, Inverse) {
250  NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0});
251  NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0});
252  NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0},
253  {3.0, -1.0, 1.0});
254 
255  auto a_inv = nav_state_a.inverse();
256  auto a_a_inv = nav_state_a * a_inv;
257  CHECK(assert_equal(a_a_inv, NavState()));
258 
259  auto b_inv = nav_state_b.inverse();
260  auto b_b_inv = nav_state_b * b_inv;
261  CHECK(assert_equal(b_b_inv, NavState()));
262 
263  Matrix actualDinverse;
264  Matrix actual = T.inverse(actualDinverse).matrix();
265  Matrix expected = T.matrix().inverse();
266  EXPECT(assert_equal(actual, expected, 1e-8));
267 
268  Matrix numericalH = numericalDerivative11(testing::inverse<NavState>, T);
269  EXPECT(assert_equal(numericalH, actualDinverse, 5e-3));
270  EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3));
271 }
272 
273 /* ************************************************************************* */
274 TEST(NavState, InverseDerivatives) {
275  Rot3 R = Rot3::Rodrigues(0.3, 0.4, -0.5);
276  Vector3 v(3.5, -8.2, 4.2);
277  Point3 p(3.5, -8.2, 4.2);
278  NavState T(R, p, v);
279 
280  Matrix numericalH = numericalDerivative11(testing::inverse<NavState>, T);
281  Matrix actualDinverse;
282  T.inverse(actualDinverse);
283  EXPECT(assert_equal(numericalH, actualDinverse, 5e-3));
284  EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3));
285 }
286 
287 /* ************************************************************************* */
288 TEST(NavState, Compose_Inverse) {
289  NavState actual = T * T.inverse();
290  NavState expected;
291  EXPECT(assert_equal(actual, expected, 1e-8));
292 }
293 
294 /* ************************************************************************* */
295 TEST(NavState, Between) {
296  NavState s1, s2(Rot3(), Point3(1, 2, 3), Velocity3(0, 0, 0));
297 
298  NavState actual = s1.compose(s2);
299  EXPECT(assert_equal(s2, actual));
300 
301  NavState between = s2.between(s1);
302  NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0));
303  EXPECT(assert_equal(expected_between, between));
304 
305  NavState expected = T2.inverse() * T3;
306  Matrix actualDBetween1, actualDBetween2;
307  actual = T2.between(T3, actualDBetween1, actualDBetween2);
308  EXPECT(assert_equal(expected, actual));
309 
310  Matrix numericalH1 =
311  numericalDerivative21(testing::between<NavState>, T2, T3);
312  EXPECT(assert_equal(numericalH1, actualDBetween1, 5e-3));
313 
314  Matrix numericalH2 =
315  numericalDerivative22(testing::between<NavState>, T2, T3);
316  EXPECT(assert_equal(numericalH2, actualDBetween2, 1e-5));
317 }
318 
319 /* ************************************************************************* */
320 TEST(NavState, interpolate) {
321  EXPECT(assert_equal(T2, interpolate(T2, T3, 0.0)));
322  EXPECT(assert_equal(T3, interpolate(T2, T3, 1.0)));
323 }
324 
325 /* ************************************************************************* */
326 static const double dt = 2.0;
327 std::function<Vector9(const NavState&, const bool&)> coriolis =
328  std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis,
329  std::placeholders::_2, nullptr);
330 
331 TEST(NavState, Coriolis) {
332  Matrix9 aH;
333 
334  // first-order
335  kState1.coriolis(dt, kOmegaCoriolis, false, aH);
336  EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), aH));
337  // second-order
338  kState1.coriolis(dt, kOmegaCoriolis, true, aH);
339  EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), aH));
340 }
341 
342 TEST(NavState, Coriolis2) {
343  Matrix9 aH;
344  NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
345  Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
346 
347  // first-order
348  state2.coriolis(dt, kOmegaCoriolis, false, aH);
349  EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), aH));
350  // second-order
351  state2.coriolis(dt, kOmegaCoriolis, true, aH);
352  EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH));
353 }
354 
355 TEST(NavState, Coriolis3) {
363  // Get true first and second order coriolis accelerations
364  double dt = 2.0, dt2 = dt * dt;
365  Vector3 n_omega(0.0, 0.0, 1.0), n_t(1.0, 0.0, 0.0), n_v(0.0, 1.0, 0.0);
366  Vector3 n_aCorr1 = -2.0 * n_omega.cross(n_v),
367  n_aCorr2 = -n_omega.cross(n_omega.cross(n_t));
368  Rot3 nRb = Rot3(-1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0),
369  bRn = nRb.inverse();
370 
371  // Get expected first and second order corrections in the nav frame
372  Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1,
373  n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2),
374  n_dV1e = dt * n_aCorr1,
375  n_dV2e = dt * (n_aCorr1 + n_aCorr2);
376 
377  // Get expected first and second order corrections in the body frame
378  Vector3 dRe = -dt * (bRn * n_omega),
379  b_dP1e = bRn * n_dP1e, b_dP2e = bRn * n_dP2e,
380  b_dV1e = bRn * n_dV1e, b_dV2e = bRn * n_dV2e;
381 
382  // Get actual first and scond order corrections in body frame
383  NavState kState2(nRb, n_t, n_v);
384  Vector9 dXi1a = kState2.coriolis(dt, n_omega, false),
385  dXi2a = kState2.coriolis(dt, n_omega, true);
386  Vector3 dRa = NavState::dR(dXi1a),
387  b_dP1a = NavState::dP(dXi1a), b_dV1a = NavState::dV(dXi1a),
388  b_dP2a = NavState::dP(dXi2a), b_dV2a = NavState::dV(dXi2a);
389 
390  EXPECT(assert_equal(dRe, dRa));
391  EXPECT(assert_equal(b_dP1e, b_dP1a));
392  EXPECT(assert_equal(b_dV1e, b_dV1a));
393  EXPECT(assert_equal(b_dP2e, b_dP2a));
394  EXPECT(assert_equal(b_dV2e, b_dV2a));
395 
396 }
397 
398 /* ************************************************************************* */
399 TEST(NavState, CorrectPIM) {
400  Vector9 xi;
401  xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
402  double dt = 0.5;
403  Matrix9 aH1, aH2;
404  std::function<Vector9(const NavState&, const Vector9&)> correctPIM =
405  std::bind(&NavState::correctPIM, std::placeholders::_1,
406  std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false,
407  nullptr, nullptr);
408  kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2);
409  EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1));
410  EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));
411 }
412 
413 /* ************************************************************************* */
414 TEST(NavState, Stream)
415 {
416  NavState state;
417 
418  std::ostringstream os;
419  os << state;
420 
421  string expected;
422  expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0 0 0\nv: 0 0 0";
423 
424  EXPECT(os.str() == expected);
425 }
426 
427 /* ************************************************************************* */
428 TEST(NavState, Print) {
429  NavState state(Rot3::Identity(), Point3(1, 2, 3), Vector3(1, 2, 3));
430 
431  // Generate the expected output
432  std::string R = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
433  std::string p = "p: 1 2 3\n";
434  std::string v = "v: 1 2 3\n";
435  std::string expected = R + p + v;
436 
437  EXPECT(assert_print_equal(expected, state));
438 }
439 
440 /* ************************************************************************* */
441 #ifndef GTSAM_POSE3_EXPMAP
442 TEST(NavState, Retract_first_order) {
443  NavState id;
444  Vector v = Z_9x1;
445  v(0) = 0.3;
446  EXPECT(assert_equal(NavState(R, Point3(0, 0, 0), Vector3(0, 0, 0)),
447  id.retract(v), 1e-2));
448  v(3) = 0.2;
449  v(4) = 0.7;
450  v(5) = -2;
451  v(6) = 3;
452  v(7) = 0.4;
453  v(8) = -2.2;
454  EXPECT(assert_equal(NavState(R, P, V), id.retract(v), 1e-2));
455 }
456 #endif
457 
458 /* ************************************************************************* */
459 TEST(NavState, RetractExpmap) {
460  Vector xi = Z_9x1;
461  xi(0) = 0.3;
462  NavState pose = NavState::Expmap(xi),
463  expected(R, Point3(0, 0, 0), Point3(0, 0, 0));
464  EXPECT(assert_equal(expected, pose, 1e-2));
465  EXPECT(assert_equal(xi, NavState::Logmap(pose), 1e-2));
466 }
467 
468 /* ************************************************************************* */
469 TEST(NavState, Expmap_A_Full) {
470  NavState id;
471  Vector xi = Z_9x1;
472  xi(0) = 0.3;
473  EXPECT(assert_equal(expmap_default<NavState>(id, xi),
474  NavState(R, Point3(0, 0, 0), Point3(0, 0, 0))));
475  xi(3) = -0.2;
476  xi(4) = -0.394742;
477  xi(5) = 2.08998;
478  xi(6) = 0.2;
479  xi(7) = 0.394742;
480  xi(8) = -2.08998;
481 
482  NavState expected(R, -P, P);
483  EXPECT(assert_equal(expected, expmap_default<NavState>(id, xi), 1e-5));
484 }
485 
486 /* ************************************************************************* */
487 TEST(NavState, Expmap_b) {
488  NavState p1(Rot3(), Point3(-100, 0, 0), Point3(100, 0, 0));
489  NavState p2 = p1.retract(
490  (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished());
491  NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0),
492  Point3(100.0, 0.0, 0.0));
493  EXPECT(assert_equal(expected, p2));
494 }
495 
496 /* ************************************************************************* */
497 // test case for screw motion in the plane
498 namespace screwNavState {
499 double a = 0.3, c = cos(a), s = sin(a), w = 0.3;
500 Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished();
501 Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
502 Point3 expectedV(0.29552, 0.0446635, 1);
503 Point3 expectedP(0.29552, 0.0446635, 1);
504 NavState expected(expectedR, expectedV, expectedP);
505 } // namespace screwNavState
506 
507 /* ************************************************************************* */
508 // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
509 TEST(NavState, Adjoint_full) {
510  NavState expected = T * NavState::Expmap(screwNavState::xi) * T.inverse();
511  Vector xiPrime = T.Adjoint(screwNavState::xi);
512  EXPECT(assert_equal(expected, NavState::Expmap(xiPrime), 1e-6));
513 
514  NavState expected2 = T2 * NavState::Expmap(screwNavState::xi) * T2.inverse();
515  Vector xiPrime2 = T2.Adjoint(screwNavState::xi);
516  EXPECT(assert_equal(expected2, NavState::Expmap(xiPrime2), 1e-6));
517 
518  NavState expected3 = T3 * NavState::Expmap(screwNavState::xi) * T3.inverse();
519  Vector xiPrime3 = T3.Adjoint(screwNavState::xi);
520  EXPECT(assert_equal(expected3, NavState::Expmap(xiPrime3), 1e-6));
521 }
522 
523 /* ************************************************************************* */
524 TEST(NavState, Adjoint_compose_full) {
525  // To debug derivatives of compose, assert that
526  // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
527  const NavState& T1 = T;
528  Vector x =
529  (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished();
530  NavState expected = T1 * NavState::Expmap(x) * T2;
531  Vector y = T2.inverse().Adjoint(x);
532  NavState actual = T1 * T2 * NavState::Expmap(y);
533  EXPECT(assert_equal(expected, actual, 1e-6));
534 }
535 
536 /* ************************************************************************* */
537 TEST(NavState, Retract_LocalCoordinates) {
538  Vector9 d;
539  d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
540  d /= 10;
541  const Rot3 R = Rot3::Retract(d.head<3>());
542  NavState t = NavState::Retract(d);
543  EXPECT(assert_equal(d, NavState::LocalCoordinates(t)));
544 }
545 /* ************************************************************************* */
546 TEST(NavState, retract_localCoordinates) {
547  Vector9 d12;
548  d12 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
549  d12 /= 10;
550  NavState t1 = T, t2 = t1.retract(d12);
551  EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
552 }
553 /* ************************************************************************* */
554 TEST(NavState, expmap_logmap) {
555  Vector d12 = Vector9::Constant(0.1);
556  NavState t1 = T, t2 = t1.expmap(d12);
557  EXPECT(assert_equal(d12, t1.logmap(t2)));
558 }
559 
560 /* ************************************************************************* */
561 TEST(NavState, retract_localCoordinates2) {
562  NavState t1 = T;
563  NavState t2 = T3;
564  NavState origin;
565  Vector d12 = t1.localCoordinates(t2);
566  EXPECT(assert_equal(t2, t1.retract(d12)));
567  Vector d21 = t2.localCoordinates(t1);
568  EXPECT(assert_equal(t1, t2.retract(d21)));
569  // NOTE(FRANK): d12 !== -d21 for arbitrary retractions.
570 }
571 /* ************************************************************************* */
572 TEST(NavState, manifold_expmap) {
573  NavState t1 = T;
574  NavState t2 = T3;
575  NavState origin;
576  Vector d12 = t1.logmap(t2);
577  EXPECT(assert_equal(t2, t1.expmap(d12)));
578  Vector d21 = t2.logmap(t1);
579  EXPECT(assert_equal(t1, t2.expmap(d21)));
580 
581  // Check that log(t1,t2)=-log(t2,t1)
582  EXPECT(assert_equal(d12, -d21));
583 }
584 
585 /* ************************************************************************* */
586 TEST(NavState, subgroups) {
587  // Frank - Below only works for correct "Agrawal06iros style expmap
588  // lines in canonical coordinates correspond to Abelian subgroups in SE(3)
589  Vector d =
590  (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished();
591  // exp(-d)=inverse(exp(d))
592  EXPECT(assert_equal(NavState::Expmap(-d), NavState::Expmap(d).inverse()));
593  // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
594  NavState T2 = NavState::Expmap(2 * d);
595  NavState T3 = NavState::Expmap(3 * d);
596  NavState T5 = NavState::Expmap(5 * d);
597  EXPECT(assert_equal(T5, T2 * T3));
598  EXPECT(assert_equal(T5, T3 * T2));
599 }
600 
601 /* ************************************************************************* */
602 TEST(NavState, adjointMap) {
603  Matrix res = NavState::adjointMap(screwNavState::xi);
604  Matrix wh = skewSymmetric(screwNavState::xi(0), screwNavState::xi(1),
605  screwNavState::xi(2));
606  Matrix vh = skewSymmetric(screwNavState::xi(3), screwNavState::xi(4),
607  screwNavState::xi(5));
608  Matrix rh = skewSymmetric(screwNavState::xi(6), screwNavState::xi(7),
609  screwNavState::xi(8));
610  Matrix9 expected;
611  expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh;
612  EXPECT(assert_equal(expected, res, 1e-5));
613 }
614 
615 /* ************************************************************************* */
616 TEST(NavState, ExpmapDerivative1) {
617  Matrix9 actualH;
618  Vector9 w;
619  w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
620  NavState::Expmap(w, actualH);
621 
622  std::function<NavState(const Vector9&)> f = [](const Vector9& w) {
623  return NavState::Expmap(w);
624  };
625  Matrix expectedH =
626  numericalDerivative21<NavState, Vector9, OptionalJacobian<9, 9> >(
627  &NavState::Expmap, w, {});
628  EXPECT(assert_equal(expectedH, actualH));
629 }
630 
631 /* ************************************************************************* */
632 TEST(NavState, LogmapDerivative) {
633  Matrix9 actualH;
634  Vector9 w;
635  w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
636  NavState p = NavState::Expmap(w);
637  EXPECT(assert_equal(w, NavState::Logmap(p, actualH), 1e-5));
638 
639  std::function<Vector9(const NavState&)> f = [](const NavState& p) {
640  return NavState::Logmap(p);
641  };
642  Matrix expectedH =
643  numericalDerivative21<Vector9, NavState, OptionalJacobian<9, 9> >(
644  &NavState::Logmap, p, {});
645  EXPECT(assert_equal(expectedH, actualH));
646 }
647 
648 //******************************************************************************
649 TEST(NavState, Invariants) {
650  NavState id;
651 
652  EXPECT(check_group_invariants(id, id));
653  EXPECT(check_group_invariants(id, T3));
654  EXPECT(check_group_invariants(T2, id));
655  EXPECT(check_group_invariants(T2, T3));
656 
657  EXPECT(check_manifold_invariants(id, id));
658  EXPECT(check_manifold_invariants(id, T3));
659  EXPECT(check_manifold_invariants(T2, id));
660  EXPECT(check_manifold_invariants(T2, T3));
661 }
662 
663 //******************************************************************************
664 TEST(NavState, LieGroupDerivatives) {
665  NavState id;
666 
667  CHECK_LIE_GROUP_DERIVATIVES(id, id);
668  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
669  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
670  CHECK_LIE_GROUP_DERIVATIVES(T2, T3);
671 }
672 
673 //******************************************************************************
674 TEST(NavState, ChartDerivatives) {
675  NavState id;
676  if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
677  CHECK_CHART_DERIVATIVES(id, id);
678  CHECK_CHART_DERIVATIVES(id,T2);
679  CHECK_CHART_DERIVATIVES(T2,id);
680  CHECK_CHART_DERIVATIVES(T2,T3);
681  }
682 }
683 
684 /* ************************************************************************* */
685 int main() {
686  TestResult tr;
687  return TestRegistry::runAllTests(tr);
688 }
689 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
create
ADT create(const Signature &signature)
Definition: testAlgebraicDecisionTree.cpp:136
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
kPose
static const Pose3 kPose(kAttitude, kPosition)
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
screwNavState::expectedP
Point3 expectedP(0.29552, 0.0446635, 1)
T
static const NavState T(R, P2, V2)
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
kVelocity
static const Velocity3 kVelocity(0.4, 0.5, 0.6)
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition: NavState.h:28
gtsam::assert_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Definition: TestableAssertions.h:352
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
kOmegaCoriolis
static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04)
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(std::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:110
kAttitude
static const Rot3 kAttitude
Definition: testNavState.cpp:35
kPosition
static const Point3 kPosition(1.0, 2.0, 3.0)
screwNavState::xi
Vector xi
Definition: testNavState.cpp:500
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
gtsam::NavState::AdjointMap
Matrix9 AdjointMap() const
Definition: NavState.cpp:168
gtsam::NavState
Definition: NavState.h:38
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
gtsam::NavState::bodyVelocity
Velocity3 bodyVelocity(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:69
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
initial::nRb
const Rot3 nRb
Definition: testScenarioRunner.cpp:149
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
testLie.h
screwNavState::expected
NavState expected(expectedR, expectedV, expectedP)
res
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
Definition: PartialRedux_count.cpp:3
screwNavState::w
double w
Definition: testNavState.cpp:499
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
drot
int BLASFUNC() drot(int *, double *, int *, double *, int *, double *, double *)
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
Definition: Rot3.cpp:136
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
TestableAssertions.h
Provides additional testing facilities for common data structures.
kState1
static const NavState kState1(kAttitude, kPosition, kVelocity)
gtsam::NavState::correctPIM
Vector9 correctPIM(const Vector9 &pim, double dt, const Vector3 &n_gravity, const std::optional< Vector3 > &omegaCoriolis, bool use2ndOrderCoriolis=false, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
Definition: NavState.cpp:417
dt
static const double dt
Definition: testNavState.cpp:326
main
int main()
Definition: testNavState.cpp:685
P2
static const Point3 P2(3.5, -8.2, 4.2)
gtsam::internal::attitude
Rot3 attitude(const NavState &X, OptionalJacobian< 3, 9 > H)
Definition: navigation/expressions.h:22
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
id
static const Similarity3 id
Definition: testSimilarity3.cpp:44
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::NavState::attitude
const Rot3 & attitude(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:48
gtsam::NavState::Adjoint
Vector9 Adjoint(const Vector9 &xi_b, OptionalJacobian< 9, 9 > H_this={}, OptionalJacobian< 9, 9 > H_xib={}) const
Definition: NavState.cpp:179
R
static const Rot3 R
Definition: testNavState.cpp:47
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::numericalDerivative32
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:259
gtsam::Pose3
Definition: Pose3.h:37
T2
static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2)
GTSAM_CONCEPT_LIE_INST
#define GTSAM_CONCEPT_LIE_INST(T)
Definition: Lie.h:372
M_PI_4
#define M_PI_4
Definition: mconf.h:119
NavState.h
Navigation state composing of attitude, position, and velocity.
kGravity
static const Vector3 kGravity(0, 0, 9.81)
gtsam::interpolate
T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx={}, typename MakeOptionalJacobian< T, T >::type Hy={})
Definition: Lie.h:327
gtsam::numericalDerivative31
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:226
gtsam::internal::velocity
Velocity3 velocity(const NavState &X, OptionalJacobian< 3, 9 > H)
Definition: navigation/expressions.h:28
TEST
TEST(NavState, Constructor)
Definition: testNavState.cpp:56
CHECK_LIE_GROUP_DERIVATIVES
#define CHECK_LIE_GROUP_DERIVATIVES(t1, t2)
Definition: testLie.h:82
bodyVelocity
Vector3 bodyVelocity(const Pose3 &w_t_b, const Vector3 &vec_w, OptionalJacobian< 3, 6 > Hpose={}, OptionalJacobian< 3, 3 > Hvel={})
Definition: testFactorTesting.cpp:29
T5
static const Similarity3 T5(R, P, 10)
Eigen::Triplet< double >
kZeroXi
static const Vector9 kZeroXi
Definition: testNavState.cpp:43
common::state2
static const NavState state2(x2, v2)
screwNavState::expectedV
Point3 expectedV(0.29552, 0.0446635, 1)
kIdentity
static const NavState kIdentity
Definition: testNavState.cpp:39
gtsam::LieGroup::logmap
TangentVector logmap(const Class &g) const
Definition: Lie.h:84
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:312
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
TestResult
Definition: TestResult.h:26
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::NavState::matrix
Matrix5 matrix() const
Definition: NavState.cpp:80
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
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
screwNavState::c
double c
Definition: testNavState.cpp:499
V2
static const Point3 V2(-6.5, 3.5, 6.2)
P
static const Point3 P(0.2, 0.7, -2)
gtsam
traits
Definition: SFMdata.h:40
screwNavState::a
double a
Definition: testNavState.cpp:499
construct
void construct(...)
Definition: init.h:107
gtsam::NavState::equals
bool equals(const NavState &other, double tol=1e-8) const
equals
Definition: NavState.cpp:104
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::NavState::position
const Point3 & position(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:55
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
screwNavState::expectedR
Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1)
std
Definition: BFloat16.h:88
origin
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
Definition: gnuplot_common_settings.hh:45
p
float * p
Definition: Tutorial_Map_using.cpp:9
coriolis
std::function< Vector9(const NavState &, const bool &)> coriolis
Definition: testNavState.cpp:327
V
static const Point3 V(3, 0.4, -2.2)
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::internal::position
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
Definition: navigation/expressions.h:25
gtsam::Similarity3::matrix
Matrix4 matrix() const
Calculate 4*4 matrix group equivalent.
Definition: Similarity3.cpp:284
gtsam::NavState::velocity
const Velocity3 & velocity(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:62
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::NavState::inverse
NavState inverse() const
inverse transformation with derivatives
Definition: NavState.cpp:110
gtsam::NavState::retract
NavState retract(const Vector9 &v, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
retract with optional derivatives
Definition: NavState.cpp:264
Inverse
Definition: Inverse.java:13
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
Eigen::Matrix< double, 9, 9 >
screwNavState
Definition: testNavState.cpp:498
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v) const
Definition: Lie.h:78
M_PI
#define M_PI
Definition: mconf.h:117
T1
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1)
gtsam::NavState::localCoordinates
Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
localCoordinates with optional derivatives
Definition: NavState.cpp:291
gtsam::NavState::coriolis
Vector9 coriolis(double dt, const Vector3 &omega, bool secondOrder=false, OptionalJacobian< 9, 9 > H={}) const
Compute tangent space contribution due to Coriolis forces.
Definition: NavState.cpp:372
lieProxies.h
Provides convenient mappings of common member functions for testing.
align_3::t
Point2 t(10, 10)
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
CHECK_CHART_DERIVATIVES
#define CHECK_CHART_DERIVATIVES(t1, t2)
Definition: testLie.h:85
ROT3_DEFAULT_COORDINATES_MODE
#define ROT3_DEFAULT_COORDINATES_MODE
Definition: Rot3.h:43
make_changelog.state
state
Definition: make_changelog.py:29
gtsam::BetweenFactor
Definition: BetweenFactor.h:40
T3
static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3))

gtsam
Author(s):
autogenerated on Thu Dec 19 2024 04:06:48