testValues.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/nonlinear/Values.h>
20 #include <gtsam/inference/Symbol.h>
21 #include <gtsam/linear/VectorValues.h>
22 #include <gtsam/geometry/Pose2.h>
23 #include <gtsam/geometry/Pose3.h>
24 #include <gtsam/base/Testable.h>
25 #include <gtsam/base/TestableAssertions.h>
26 
27 #include <CppUnitLite/TestHarness.h>
28 #include <stdexcept>
29 #include <limits>
30 #include <type_traits>
31 
32 using namespace std::placeholders;
33 using namespace gtsam;
34 using namespace std;
35 static double inf = std::numeric_limits<double>::infinity();
36 
37 // Convenience for named keys
38 using symbol_shorthand::X;
39 using symbol_shorthand::L;
40 
41 const Symbol key1('v', 1), key2('v', 2), key3('v', 3), key4('v', 4);
42 
43 
44 class TestValueData {
45 public:
46  static int ConstructorCount;
47  static int DestructorCount;
48  TestValueData(const TestValueData& other) { ++ ConstructorCount; }
49  TestValueData() { ++ ConstructorCount; }
50  ~TestValueData() { ++ DestructorCount; }
51 };
52 int TestValueData::ConstructorCount = 0;
53 int TestValueData::DestructorCount = 0;
54 class TestValue {
55  TestValueData data_;
56 public:
57  enum {dimension = 0};
58  void print(const std::string& str = "") const {}
59  bool equals(const TestValue& other, double tol = 1e-9) const { return true; }
60  size_t dim() const { return 0; }
61  TestValue retract(const Vector&,
62  OptionalJacobian<dimension,dimension> H1={},
63  OptionalJacobian<dimension,dimension> H2={}) const {
64  return TestValue();
65  }
66  Vector localCoordinates(const TestValue&,
67  OptionalJacobian<dimension,dimension> H1={},
68  OptionalJacobian<dimension,dimension> H2={}) const {
69  return Vector();
70  }
71 };
72 
73 namespace gtsam {
74 template <> struct traits<TestValue> : public internal::Manifold<TestValue> {};
75 }
76 
77 /* ************************************************************************* */
78 TEST( Values, equals1 )
79 {
80  Values expected;
81  Vector3 v(5.0, 6.0, 7.0);
82 
83  expected.insert(key1, v);
84  Values actual;
85  actual.insert(key1, v);
86  CHECK(assert_equal(expected, actual));
87 }
88 
89 /* ************************************************************************* */
90 TEST( Values, equals2 )
91 {
92  Values cfg1, cfg2;
93  Vector3 v1(5.0, 6.0, 7.0);
94  Vector3 v2(5.0, 6.0, 8.0);
95 
96  cfg1.insert(key1, v1);
97  cfg2.insert(key1, v2);
98  CHECK(!cfg1.equals(cfg2));
99  CHECK(!cfg2.equals(cfg1));
100 }
101 
102 /* ************************************************************************* */
103 TEST( Values, equals_nan )
104 {
105  Values cfg1, cfg2;
106  Vector3 v1(5.0, 6.0, 7.0);
107  Vector3 v2(inf, inf, inf);
108 
109  cfg1.insert(key1, v1);
110  cfg2.insert(key1, v2);
111  CHECK(!cfg1.equals(cfg2));
112  CHECK(!cfg2.equals(cfg1));
113 }
114 
115 /* ************************************************************************* */
116 TEST( Values, insert_good )
117 {
118  Values cfg1, cfg2, expected;
119  Vector3 v1(5.0, 6.0, 7.0);
120  Vector3 v2(8.0, 9.0, 1.0);
121  Vector3 v3(2.0, 4.0, 3.0);
122  Vector3 v4(8.0, 3.0, 7.0);
123 
124  cfg1.insert(key1, v1);
125  cfg1.insert(key2, v2);
126  cfg2.insert(key3, v4);
127 
128  cfg1.insert(cfg2);
129 
130  expected.insert(key1, v1);
131  expected.insert(key2, v2);
132  expected.insert(key3, v4);
133 
134  CHECK(assert_equal(expected, cfg1));
135 }
136 
137 /* ************************************************************************* */
138 TEST( Values, insert_expression )
139 {
140  Point2 p1(0.1, 0.2);
141  Point2 p2(0.3, 0.4);
142  Point2 p3(0.5, 0.6);
143  Point2 p4(p1 + p2 + p3);
144  Point2 p5(-p1);
145  Point2 p6(2.0*p1);
146 
147  Values cfg1, cfg2;
148  cfg1.insert(key1, p1 + p2 + p3);
149  cfg1.insert(key2, -p1);
150  cfg1.insert(key3, 2.0*p1);
151  cfg2.insert(key1, p4);
152  cfg2.insert(key2, p5);
153  cfg2.insert(key3, p6);
154 
155  CHECK(assert_equal(cfg1, cfg2));
156 
157  Point3 p7(0.1, 0.2, 0.3);
158  Point3 p8(0.4, 0.5, 0.6);
159  Point3 p9(0.7, 0.8, 0.9);
160  Point3 p10(p7 + p8 + p9);
161  Point3 p11(-p7);
162  Point3 p12(2.0*p7);
163 
164  Values cfg3, cfg4;
165  cfg3.insert(key1, p7 + p8 + p9);
166  cfg3.insert(key2, -p7);
167  cfg3.insert(key3, 2.0*p7);
168  cfg4.insert(key1, p10);
169  cfg4.insert(key2, p11);
170  cfg4.insert(key3, p12);
171 
172  CHECK(assert_equal(cfg3, cfg4));
173 }
174 
175 /* ************************************************************************* */
176 TEST( Values, insert_bad )
177 {
178  Values cfg1, cfg2;
179  Vector3 v1(5.0, 6.0, 7.0);
180  Vector3 v2(8.0, 9.0, 1.0);
181  Vector3 v3(2.0, 4.0, 3.0);
182  Vector3 v4(8.0, 3.0, 7.0);
183 
184  cfg1.insert(key1, v1);
185  cfg1.insert(key2, v2);
186  cfg2.insert(key2, v3);
187  cfg2.insert(key3, v4);
188 
189  CHECK_EXCEPTION(cfg1.insert(cfg2), ValuesKeyAlreadyExists);
190 }
191 
192 /* ************************************************************************* */
193 TEST( Values, update_element )
194 {
195  Values cfg;
196  Vector3 v1(5.0, 6.0, 7.0);
197  Vector3 v2(8.0, 9.0, 1.0);
198 
199  cfg.insert(key1, v1);
200  CHECK(cfg.size() == 1);
201  CHECK(assert_equal((Vector)v1, cfg.at<Vector3>(key1)));
202 
203  cfg.update(key1, v2);
204  CHECK(cfg.size() == 1);
205  CHECK(assert_equal((Vector)v2, cfg.at<Vector3>(key1)));
206 }
207 
208 /* ************************************************************************* */
209 TEST(Values, update_element_with_expression)
210 {
211  Values cfg;
212  Vector3 v1(5.0, 6.0, 7.0);
213  Vector3 v2(8.0, 9.0, 1.0);
214 
215  cfg.insert(key1, v1);
216  CHECK(cfg.size() == 1);
217  CHECK(assert_equal((Vector)v1, cfg.at<Vector3>(key1)));
218 
219  cfg.update(key1, 2.0*v1 + v2);
220  CHECK(cfg.size() == 1);
221  CHECK(assert_equal((2.0*v1 + v2).eval(), cfg.at<Vector3>(key1)));
222 }
223 
224 /* ************************************************************************* */
225 TEST(Values, InsertOrAssign) {
226  Values values;
227  Key X(0);
228  double x = 1;
229 
230  CHECK(values.size() == 0);
231  // This should perform an insert.
232  values.insert_or_assign(X, x);
233  EXPECT(assert_equal(values.at<double>(X), x));
234 
235  // This should perform an update.
236  double y = 2;
237  values.insert_or_assign(X, y);
238  EXPECT(assert_equal(values.at<double>(X), y));
239 }
240 
241 /* ************************************************************************* */
242 TEST(Values, InsertOrAssignWithExpression) {
243  Values values,expected;
244  Key X(0);
245  Vector3 x{1.0, 2.0, 3.0};
246  Vector3 y{4.0, 5.0, 6.0};
247 
248  CHECK(values.size() == 0);
249  // This should perform an insert.
250  Vector3 z = x + y;
251  values.insert_or_assign(X, x + y);
252  EXPECT(assert_equal(values.at<Vector3>(X), z));
253 
254  // This should perform an update.
255  z = 2.0*x - 3.0*y;
256  values.insert_or_assign(X, 2.0*x - 3.0*y);
257  EXPECT(assert_equal(values.at<Vector3>(X), z));
258 }
259 
260 /* ************************************************************************* */
261 TEST(Values, basic_functions)
262 {
263  Values values;
264  Matrix23 M1 = Matrix23::Zero(), M2 = Matrix23::Zero();
265  values.insert(2, Vector3(0, 0, 0));
266  values.insert(4, Vector3(0, 0, 0));
267  values.insert(6, M1);
268  values.insert(8, M2);
269 
270 
271  EXPECT(!values.exists(1));
272  EXPECT(values.exists(2));
273  EXPECT(values.exists(4));
274  EXPECT(values.exists(6));
275  EXPECT(values.exists(8));
276 
277  size_t count = 0;
278  for (const auto& [key, value] : values) {
279  count += 1;
280  if (key == 2 || key == 4) EXPECT_LONGS_EQUAL(3, value.dim());
281  if (key == 6 || key == 8) EXPECT_LONGS_EQUAL(6, value.dim());
282  }
283  EXPECT_LONGS_EQUAL(4, count);
284 
285  // find
286  EXPECT_LONGS_EQUAL(4, values.find(4)->key);
287 
288  // lower_bound
289  EXPECT_LONGS_EQUAL(4, values.lower_bound(4)->key);
290  EXPECT_LONGS_EQUAL(4, values.lower_bound(3)->key);
291 
292  // upper_bound
293  EXPECT_LONGS_EQUAL(6, values.upper_bound(4)->key);
294  EXPECT_LONGS_EQUAL(4, values.upper_bound(3)->key);
295 }
296 
297 /* ************************************************************************* */
298 TEST(Values, retract_full)
299 {
300  Values config0;
301  config0.insert(key1, Vector3(1.0, 2.0, 3.0));
302  config0.insert(key2, Vector3(5.0, 6.0, 7.0));
303 
304  const VectorValues delta{{key1, Vector3(1.0, 1.1, 1.2)},
305  {key2, Vector3(1.3, 1.4, 1.5)}};
306 
307  Values expected;
308  expected.insert(key1, Vector3(2.0, 3.1, 4.2));
309  expected.insert(key2, Vector3(6.3, 7.4, 8.5));
310 
311  CHECK(assert_equal(expected, config0.retract(delta)));
312  CHECK(assert_equal(expected, Values(config0, delta)));
313 }
314 
315 /* ************************************************************************* */
316 TEST(Values, retract_partial)
317 {
318  Values config0;
319  config0.insert(key1, Vector3(1.0, 2.0, 3.0));
320  config0.insert(key2, Vector3(5.0, 6.0, 7.0));
321 
322  const VectorValues delta{{key2, Vector3(1.3, 1.4, 1.5)}};
323 
324  Values expected;
325  expected.insert(key1, Vector3(1.0, 2.0, 3.0));
326  expected.insert(key2, Vector3(6.3, 7.4, 8.5));
327 
328  CHECK(assert_equal(expected, config0.retract(delta)));
329  CHECK(assert_equal(expected, Values(config0, delta)));
330 }
331 
332 /* ************************************************************************* */
333 TEST(Values, retract_masked)
334 {
335  Values config0;
336  config0.insert(key1, Vector3(1.0, 2.0, 3.0));
337  config0.insert(key2, Vector3(5.0, 6.0, 7.0));
338 
339  const VectorValues delta{{key1, Vector3(1.0, 1.1, 1.2)},
340  {key2, Vector3(1.3, 1.4, 1.5)}};
341 
342  Values expected;
343  expected.insert(key1, Vector3(1.0, 2.0, 3.0));
344  expected.insert(key2, Vector3(6.3, 7.4, 8.5));
345 
346  config0.retractMasked(delta, {key2});
347  CHECK(assert_equal(expected, config0));
348 }
349 
350 /* ************************************************************************* */
351 TEST(Values, equals)
352 {
353  Values config0;
354  config0.insert(key1, Vector3(1.0, 2.0, 3.0));
355  config0.insert(key2, Vector3(5.0, 6.0, 7.0));
356 
357  CHECK(equal(config0, config0));
358  CHECK(config0.equals(config0));
359 
360  Values poseconfig;
361  poseconfig.insert(key1, Pose2(1, 2, 3));
362  poseconfig.insert(key2, Pose2(0.3, 0.4, 0.5));
363 
364  CHECK(equal(poseconfig, poseconfig));
365  CHECK(poseconfig.equals(poseconfig));
366 }
367 
368 /* ************************************************************************* */
369 TEST(Values, localCoordinates)
370 {
371  Values valuesA;
372  valuesA.insert(key1, Vector3(1.0, 2.0, 3.0));
373  valuesA.insert(key2, Vector3(5.0, 6.0, 7.0));
374 
375  VectorValues expDelta{{key1, Vector3(0.1, 0.2, 0.3)},
376  {key2, Vector3(0.4, 0.5, 0.6)}};
377 
378  Values valuesB = valuesA.retract(expDelta);
379 
380  EXPECT(assert_equal(expDelta, valuesA.localCoordinates(valuesB)));
381 }
382 
383 /* ************************************************************************* */
384 TEST(Values, extract_keys)
385 {
386  Values config;
387 
388  config.insert(key1, Pose2());
389  config.insert(key2, Pose2());
390  config.insert(key3, Pose2());
391  config.insert(key4, Pose2());
392 
393  KeyVector expected {key1, key2, key3, key4};
394  KeyVector actual = config.keys();
395 
396  CHECK(actual.size() == expected.size());
397  KeyVector::const_iterator itAct = actual.begin(), itExp = expected.begin();
398  for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
399  EXPECT(*itExp == *itAct);
400  }
401 }
402 
403 /* ************************************************************************* */
404 TEST(Values, exists_)
405 {
406  Values config0;
407  config0.insert(key1, 1.0);
408  config0.insert(key2, 2.0);
409 
410  const double* v = config0.exists<double>(key1);
411  DOUBLES_EQUAL(1.0,*v,1e-9);
412 }
413 
414 /* ************************************************************************* */
415 TEST(Values, update)
416 {
417  Values config0;
418  config0.insert(key1, 1.0);
419  config0.insert(key2, 2.0);
420 
421  Values superset;
422  superset.insert(key1, -1.0);
423  superset.insert(key2, -2.0);
424  config0.update(superset);
425 
426  Values expected;
427  expected.insert(key1, -1.0);
428  expected.insert(key2, -2.0);
429  CHECK(assert_equal(expected, config0));
430 }
431 
432 /* ************************************************************************* */
433 TEST(Values, filter) {
434  Pose2 pose0(1.0, 2.0, 0.3);
435  Pose3 pose1(Pose2(0.1, 0.2, 0.3));
436  Pose2 pose2(4.0, 5.0, 0.6);
437  Pose3 pose3(Pose2(0.3, 0.7, 0.9));
438 
439  Values values;
440  values.insert(0, pose0);
441  values.insert(1, pose1);
442  values.insert(2, pose2);
443  values.insert(3, pose3);
444 
445  // Test counting by type.
446  EXPECT_LONGS_EQUAL(2, (long)values.count<Pose3>());
447  EXPECT_LONGS_EQUAL(2, (long)values.count<Pose2>());
448 
449  // Filter by type using extract.
450  auto extracted_pose3s = values.extract<Pose3>();
451  EXPECT_LONGS_EQUAL(2, (long)extracted_pose3s.size());
452 }
453 
454 /* ************************************************************************* */
455 TEST(Values, Symbol_filter) {
456  Pose2 pose0(1.0, 2.0, 0.3);
457  Pose3 pose1(Pose2(0.1, 0.2, 0.3));
458  Pose2 pose2(4.0, 5.0, 0.6);
459  Pose3 pose3(Pose2(0.3, 0.7, 0.9));
460 
461  Values values;
462  values.insert(X(0), pose0);
463  values.insert(Symbol('y', 1), pose1);
464  values.insert(X(2), pose2);
465  values.insert(Symbol('y', 3), pose3);
466 
467  // Test extract with filter on symbol:
468  auto extracted_pose3s = values.extract<Pose3>(Symbol::ChrTest('y'));
469  EXPECT_LONGS_EQUAL(2, (long)extracted_pose3s.size());
470 }
471 
472 /* ************************************************************************* */
473 // Check that Value destructors are called when Values container is deleted
474 TEST(Values, Destructors) {
475  {
476  Values values;
477  {
478  TestValue value1;
479  TestValue value2;
480  LONGS_EQUAL(2, (long)TestValueData::ConstructorCount);
481  LONGS_EQUAL(0, (long)TestValueData::DestructorCount);
482  values.insert(0, value1);
483  values.insert(1, value2);
484  }
485  // additional 2 con/destructor counts for the temporary
486  // GenericValue<TestValue> in insert()
487  // but I'm sure some advanced programmer can figure out
488  // a way to avoid the temporary, or optimize it out
489  LONGS_EQUAL(4 + 2, (long)TestValueData::ConstructorCount);
490  LONGS_EQUAL(2 + 2, (long)TestValueData::DestructorCount);
491  }
492  LONGS_EQUAL(4 + 2, (long)TestValueData::ConstructorCount);
493  LONGS_EQUAL(4 + 2, (long)TestValueData::DestructorCount);
494 }
495 
496 /* ************************************************************************* */
497 TEST(Values, copy_constructor) {
498  {
499  Values values;
500  TestValueData::ConstructorCount = 0;
501  TestValueData::DestructorCount = 0;
502  {
503  TestValue value1;
504  TestValue value2;
505  EXPECT_LONGS_EQUAL(2, (long)TestValueData::ConstructorCount);
506  EXPECT_LONGS_EQUAL(0, (long)TestValueData::DestructorCount);
507  values.insert(0, value1);
508  values.insert(1, value2);
509  }
510  EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount);
511  EXPECT_LONGS_EQUAL(4, (long)TestValueData::DestructorCount);
512 
513  // Copy constructor
514  {
515  Values copied(values); // makes 2 extra copies
516  EXPECT_LONGS_EQUAL(8, (long)TestValueData::ConstructorCount);
517  EXPECT_LONGS_EQUAL(4, (long)TestValueData::DestructorCount);
518  }
519  EXPECT_LONGS_EQUAL(8, (long)TestValueData::ConstructorCount);
520  EXPECT_LONGS_EQUAL(6, (long)TestValueData::DestructorCount); // copied destructed !
521  }
522  EXPECT_LONGS_EQUAL(8, (long)TestValueData::ConstructorCount);
523  EXPECT_LONGS_EQUAL(8, (long)TestValueData::DestructorCount); // values destructed !
524 }
525 
526 /* ************************************************************************* */
527 // small class with a constructor to create an rvalue
528 struct TestValues : Values {
529  using Values::Values; // inherits move constructor
530 
531  TestValues(const TestValue& value1, const TestValue& value2) {
532  insert(0, value1);
533  insert(1, value2);
534  }
535 };
536 static_assert(std::is_move_constructible<Values>::value, "");
537 static_assert(std::is_move_constructible<TestValues>::value, "");
538 
539 // test move semantics
540 TEST(Values, move_constructor) {
541  {
542  TestValueData::ConstructorCount = 0;
543  TestValueData::DestructorCount = 0;
544  TestValue value1;
545  TestValue value2;
546  EXPECT_LONGS_EQUAL(2, (long)TestValueData::ConstructorCount);
547  EXPECT_LONGS_EQUAL(0, (long)TestValueData::DestructorCount);
548  TestValues values(TestValues(value1, value2)); // Move happens here ! (could be optimization?)
549  EXPECT_LONGS_EQUAL(2, values.size());
550  EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount); // yay ! We don't copy
551  EXPECT_LONGS_EQUAL(2, (long)TestValueData::DestructorCount); // extra insert copies
552  }
553  EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount);
554  EXPECT_LONGS_EQUAL(6, (long)TestValueData::DestructorCount);
555 }
556 
557 // test use of std::move
558 TEST(Values, std_move) {
559  {
560  TestValueData::ConstructorCount = 0;
561  TestValueData::DestructorCount = 0;
562  {
563  TestValue value1;
564  TestValue value2;
565  TestValues values(value1, value2);
566  EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount);
567  EXPECT_LONGS_EQUAL(2, (long)TestValueData::DestructorCount);
568  EXPECT_LONGS_EQUAL(2, values.size());
569  TestValues moved(std::move(values)); // Move happens here !
570  EXPECT_LONGS_EQUAL(0, values.size()); // Should be 0 !
571  EXPECT_LONGS_EQUAL(2, moved.size());
572  EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount); // Should be 6 :-)
573  EXPECT_LONGS_EQUAL(2, (long)TestValueData::DestructorCount); // extra insert copies
574  }
575  EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount);
576  EXPECT_LONGS_EQUAL(6, (long)TestValueData::DestructorCount);
577  }
578 }
579 
580 /* ************************************************************************* */
581 TEST(Values, VectorDynamicInsertFixedRead) {
582  Values values;
583  Vector v(3); v << 5.0, 6.0, 7.0;
584  values.insert<Vector3>(key1, v);
585  Vector3 expected(5.0, 6.0, 7.0);
586  Vector3 actual = values.at<Vector3>(key1);
587  CHECK(assert_equal(expected, actual));
588  CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
589 }
590 
591 /* ************************************************************************* */
592 TEST(Values, VectorDynamicInsertDynamicRead) {
593  Values values;
594  Vector v(3); v << 5.0, 6.0, 7.0;
595  values.insert(key1, v);
596  Vector expected(3); expected << 5.0, 6.0, 7.0;
597  Vector actual = values.at<Vector>(key1);
598  LONGS_EQUAL(3, actual.rows());
599  LONGS_EQUAL(1, actual.cols());
600  CHECK(assert_equal(expected, actual));
601 }
602 
603 /* ************************************************************************* */
604 TEST(Values, VectorFixedInsertFixedRead) {
605  Values values;
606  Vector3 v; v << 5.0, 6.0, 7.0;
607  values.insert(key1, v);
608  Vector3 expected; expected << 5.0, 6.0, 7.0;
609  Vector3 actual = values.at<Vector3>(key1);
610  CHECK(assert_equal(expected, actual));
611  CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
612 }
613 
614 /* ************************************************************************* */
615 // NOTE(frank): test is broken, because the scheme it tested was *very* slow.
616 // TODO(frank): find long-term solution. that works w matlab/python.
617 //TEST(Values, VectorFixedInsertDynamicRead) {
618 // Values values;
619 // Vector3 v; v << 5.0, 6.0, 7.0;
620 // values.insert(key1, v);
621 // Vector expected(3); expected << 5.0, 6.0, 7.0;
622 // Vector actual = values.at<Vector>(key1);
623 // LONGS_EQUAL(3, actual.rows());
624 // LONGS_EQUAL(1, actual.cols());
625 // CHECK(assert_equal(expected, actual));
626 //}
627 
628 /* ************************************************************************* */
629 TEST(Values, MatrixDynamicInsertFixedRead) {
630  Values values;
631  Matrix v(1,3); v << 5.0, 6.0, 7.0;
632  values.insert<Matrix13>(key1, v);
633  Vector3 expected(5.0, 6.0, 7.0);
634  CHECK(assert_equal((Vector)expected, values.at<Matrix13>(key1)));
635  CHECK_EXCEPTION(values.at<Matrix23>(key1), exception);
636 }
637 
638 TEST(Values, Demangle) {
639  Values values;
640  Matrix13 v; v << 5.0, 6.0, 7.0;
641  values.insert(key1, v);
642 #ifdef __GNUG__
643  string expected =
644  "Values with 1 values:\nValue v1: (Eigen::Matrix<double, 1, 3, 1, 1, "
645  "3>)\n[\n 5, 6, 7\n]\n\n";
646 #elif _WIN32
647  string expected =
648  "Values with 1 values:\nValue v1: "
649  "(class Eigen::Matrix<double,1,3,1,1,3>)\n[\n 5, 6, 7\n]\n\n";
650 #endif
651 
652  EXPECT(assert_print_equal(expected, values));
653 }
654 
655 /* ************************************************************************* */
656 TEST(Values, brace_initializer) {
657  const Pose2 poseA(1.0, 2.0, 0.3), poseC(.0, .0, .0);
658  const Pose3 poseB(Pose2(0.1, 0.2, 0.3));
659 
660  {
661  Values values;
662  EXPECT_LONGS_EQUAL(0, values.size());
663  values = { {key1, genericValue(1.0)} };
664  EXPECT_LONGS_EQUAL(1, values.size());
665  CHECK(values.at<double>(key1) == 1.0);
666  }
667  {
668  Values values = { {key1, genericValue(poseA)}, {key2, genericValue(poseB)} };
669  EXPECT_LONGS_EQUAL(2, values.size());
670  EXPECT(assert_equal(values.at<Pose2>(key1), poseA));
671  EXPECT(assert_equal(values.at<Pose3>(key2), poseB));
672  }
673  // Test exception: duplicated key:
674  {
675  Values values;
676  CHECK_EXCEPTION((values = {
677  {key1, genericValue(poseA)},
678  {key2, genericValue(poseB)},
679  {key1, genericValue(poseC)}
680  }), std::exception);
681  }
682 }
683 
684 
685 /* ************************************************************************* */
686 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
687 /* ************************************************************************* */
key1
const Symbol key1('v', 1)
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
relicense.update
def update(text)
Definition: relicense.py:46
CHECK_EXCEPTION
#define CHECK_EXCEPTION(condition, exception_name)
Definition: Test.h:118
Pose2.h
2D Pose
gtsam::Values::exists
bool exists(Key j) const
Definition: Values.cpp:93
TestValueData::DestructorCount
static int DestructorCount
Definition: testValues.cpp:47
M2
M1<< 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;Map< MatrixXf > M2(M1.data(), 6, 2)
gtsam::Values::keys
KeyVector keys() const
Definition: Values.cpp:218
gtsam::Values::size
size_t size() const
Definition: Values.h:178
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
p5
KeyInt p5(x5, 5)
TestHarness.h
TestValues
Definition: testValues.cpp:528
TestValueData::~TestValueData
~TestValueData()
Definition: testValues.cpp:50
gtsam::Values::retractMasked
void retractMasked(const VectorValues &delta, const KeySet &mask)
Definition: Values.cpp:103
gtsam::assert_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Definition: TestableAssertions.h:352
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
pose3
static Pose3 pose3(rt3, pt3)
simple::pose0
static Pose3 pose0
Definition: testInitializePose3.cpp:56
gtsam::Values::lower_bound
deref_iterator lower_bound(Key j) const
Definition: Values.h:213
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
TestValue::retract
TestValue retract(const Vector &, OptionalJacobian< dimension, dimension > H1={}, OptionalJacobian< dimension, dimension > H2={}) const
Definition: testValues.cpp:61
TEST
TEST(Values, equals1)
Definition: testValues.cpp:78
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
X
#define X
Definition: icosphere.cpp:20
TestValueData
Definition: testValues.cpp:44
gtsam::genericValue
GenericValue< T > genericValue(const T &v)
Definition: GenericValue.h:211
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
gtsam::Values::update
void update(Key j, const Value &val)
Definition: Values.cpp:169
TestValues::TestValues
TestValues(const TestValue &value1, const TestValue &value2)
Definition: testValues.cpp:531
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::Values::localCoordinates
VectorValues localCoordinates(const Values &cp) const
Definition: Values.cpp:129
gtsam::Values::upper_bound
deref_iterator upper_bound(Key j) const
Definition: Values.h:216
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
TestValue
Definition: testValues.cpp:54
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
key2
const Symbol key2('v', 2)
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::Values::extract
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Definition: Values-inl.h:105
TestValue::localCoordinates
Vector localCoordinates(const TestValue &, OptionalJacobian< dimension, dimension > H1={}, OptionalJacobian< dimension, dimension > H2={}) const
Definition: testValues.cpp:66
gtsam::Pose3
Definition: Pose3.h:37
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::Values::count
size_t count() const
Definition: Values-inl.h:94
inf
static double inf
Definition: testValues.cpp:35
TestValue::equals
bool equals(const TestValue &other, double tol=1e-9) const
Definition: testValues.cpp:59
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
key3
const Symbol key3('v', 3)
gtsam::Values::equals
bool equals(const Values &other, double tol=1e-9) const
Definition: Values.cpp:77
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
simple::p3
static Point3 p3
Definition: testInitializePose3.cpp:53
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
VectorValues.h
Factor Graph Values.
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
main
int main()
Definition: testValues.cpp:686
M1
MatrixXf M1
Definition: Tutorial_SlicingCol.cpp:1
gtsam::ValuesKeyAlreadyExists
Definition: Values.h:408
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
p4
KeyInt p4(x4, 4)
gtsam::equals
Definition: Testable.h:112
TestResult
Definition: TestResult.h:26
y
Scalar * y
Definition: level1_cplx_impl.h:124
str
Definition: pytypes.h:1524
key
const gtsam::Symbol key('X', 0)
Values
std::vector< float > Values
Definition: sparse_setter.cpp:45
gtsam
traits
Definition: chartTesting.h:28
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
leaf::values
leaf::MyValues values
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
v2
Vector v2
Definition: testSerializationBase.cpp:39
TestValueData::ConstructorCount
static int ConstructorCount
Definition: testValues.cpp:46
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
TestValue::data_
TestValueData data_
Definition: testValues.cpp:55
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
v3
Vector v3
Definition: testSerializationBase.cpp:40
key4
const Symbol key4('v', 4)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
TestValue::dim
size_t dim() const
Definition: testValues.cpp:60
insert
A insert(1, 2)=0
TestValueData::TestValueData
TestValueData()
Definition: testValues.cpp:49
TestValue::print
void print(const std::string &str="") const
Definition: testValues.cpp:58
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
test_callbacks.value
value
Definition: test_callbacks.py:158
eval
internal::nested_eval< T, 1 >::type eval(const T &xpr)
Definition: sparse_permutations.cpp:38
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
Values.h
A non-templated config holding any types of Manifold-group elements.
Pose3.h
3D Pose
v1
Vector v1
Definition: testSerializationBase.cpp:38
gtsam::Pose2
Definition: Pose2.h:39
gtsam::Symbol
Definition: inference/Symbol.h:37
gtsam::Values::insert_or_assign
void insert_or_assign(Key j, const Value &val)
If key j exists, update value, else perform an insert.
Definition: Values.cpp:192
gtsam::Values::find
deref_iterator find(Key j) const
Definition: Values.h:210
TestValueData::TestValueData
TestValueData(const TestValueData &other)
Definition: testValues.cpp:48

gtsam
Author(s):
autogenerated on Tue Jun 25 2024 03:07:12