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
12/*
13 * @file testNonlinearEquality.cpp
14 * @author Alex Cunningham
15 */
16
17#include <tests/simulated2DConstraints.h>
18
19#include <gtsam/nonlinear/PriorFactor.h>
20#include <gtsam/slam/ProjectionFactor.h>
21#include <gtsam/nonlinear/NonlinearEquality.h>
22#include <gtsam/nonlinear/NonlinearFactorGraph.h>
23#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
24#include <gtsam/linear/GaussianBayesNet.h>
25#include <gtsam/inference/Symbol.h>
26#include <gtsam/geometry/Point2.h>
27#include <gtsam/geometry/Pose2.h>
28#include <gtsam/geometry/Point3.h>
29#include <gtsam/geometry/Pose3.h>
30#include <gtsam/geometry/Cal3_S2.h>
31#include <gtsam/geometry/PinholeCamera.h>
32
33#include <CppUnitLite/TestHarness.h>
34
35using namespace std;
36using namespace gtsam;
37
38namespace eq2D = simulated2D::equality_constraints;
39
40static const double tol = 1e-5;
41
42typedef PriorFactor<Pose2> PosePrior;
43typedef NonlinearEquality<Pose2> PoseNLE;
44typedef std::shared_ptr<PoseNLE> shared_poseNLE;
45
46static Symbol key('x', 1);
47
48//******************************************************************************
49TEST ( NonlinearEquality, linearization ) {
50 Pose2 value = Pose2(2.1, 1.0, 2.0);
51 Values linearize;
52 linearize.insert(j: key, val: value);
53
54 // create a nonlinear equality constraint
55 shared_poseNLE nle(new PoseNLE(key, value));
56
57 // check linearize
58 SharedDiagonal constraintModel = noiseModel::Constrained::All(dim: 3);
59 JacobianFactor expLF(key, I_3x3, Z_3x1, constraintModel);
60 GaussianFactor::shared_ptr actualLF = nle->linearize(x: linearize);
61 EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF));
62}
63
64//******************************************************************************
65TEST ( NonlinearEquality, linearization_pose ) {
66
67 Symbol key('x', 1);
68 Pose2 value;
69 Values config;
70 config.insert(j: key, val: value);
71
72 // create a nonlinear equality constraint
73 shared_poseNLE nle(new PoseNLE(key, value));
74
75 GaussianFactor::shared_ptr actualLF = nle->linearize(x: config);
76 EXPECT(true);
77}
78
79//******************************************************************************
80TEST ( NonlinearEquality, linearization_fail ) {
81 Pose2 value = Pose2(2.1, 1.0, 2.0);
82 Pose2 wrong = Pose2(2.1, 3.0, 4.0);
83 Values bad_linearize;
84 bad_linearize.insert(j: key, val: wrong);
85
86 // create a nonlinear equality constraint
87 shared_poseNLE nle(new PoseNLE(key, value));
88
89 // check linearize to ensure that it fails for bad linearization points
90 CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
91}
92
93//******************************************************************************
94TEST ( NonlinearEquality, linearization_fail_pose ) {
95
96 Symbol key('x', 1);
97 Pose2 value(2.0, 1.0, 2.0), wrong(2.0, 3.0, 4.0);
98 Values bad_linearize;
99 bad_linearize.insert(j: key, val: wrong);
100
101 // create a nonlinear equality constraint
102 shared_poseNLE nle(new PoseNLE(key, value));
103
104 // check linearize to ensure that it fails for bad linearization points
105 CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
106}
107
108//******************************************************************************
109TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
110
111 Symbol key('x', 1);
112 Pose2 value, wrong(2.0, 3.0, 4.0);
113 Values bad_linearize;
114 bad_linearize.insert(j: key, val: wrong);
115
116 // create a nonlinear equality constraint
117 shared_poseNLE nle(new PoseNLE(key, value));
118
119 // check linearize to ensure that it fails for bad linearization points
120 CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
121}
122
123//******************************************************************************
124TEST ( NonlinearEquality, error ) {
125 Pose2 value = Pose2(2.1, 1.0, 2.0);
126 Pose2 wrong = Pose2(2.1, 3.0, 4.0);
127 Values feasible, bad_linearize;
128 feasible.insert(j: key, val: value);
129 bad_linearize.insert(j: key, val: wrong);
130
131 // create a nonlinear equality constraint
132 shared_poseNLE nle(new PoseNLE(key, value));
133
134 // check error function outputs
135 Vector actual = nle->unwhitenedError(x: feasible);
136 EXPECT(assert_equal(actual, Z_3x1));
137
138 actual = nle->unwhitenedError(x: bad_linearize);
139 EXPECT(
140 assert_equal(actual, Vector::Constant(3, std::numeric_limits<double>::infinity())));
141}
142
143//******************************************************************************
144TEST ( NonlinearEquality, equals ) {
145 Pose2 value1 = Pose2(2.1, 1.0, 2.0);
146 Pose2 value2 = Pose2(2.1, 3.0, 4.0);
147
148 // create some constraints to compare
149 shared_poseNLE nle1(new PoseNLE(key, value1));
150 shared_poseNLE nle2(new PoseNLE(key, value1));
151 shared_poseNLE nle3(new PoseNLE(key, value2));
152
153 // verify
154 EXPECT(nle1->equals(*nle2));
155 // basic equality = true
156 EXPECT(nle2->equals(*nle1));
157 // test symmetry of equals()
158 EXPECT(!nle1->equals(*nle3));
159 // test config
160}
161
162//******************************************************************************
163TEST ( NonlinearEquality, allow_error_pose ) {
164 Symbol key1('x', 1);
165 Pose2 feasible1(1.0, 2.0, 3.0);
166 double error_gain = 500.0;
167 PoseNLE nle(key1, feasible1, error_gain);
168
169 // the unwhitened error should provide logmap to the feasible state
170 Pose2 badPoint1(0.0, 2.0, 3.0);
171 Vector actVec = nle.evaluateError(xj: badPoint1);
172 Vector expVec = Vector3(-0.989992, -0.14112, 0.0);
173 EXPECT(assert_equal(expVec, actVec, 1e-5));
174
175 // the actual error should have a gain on it
176 Values config;
177 config.insert(j: key1, val: badPoint1);
178 double actError = nle.error(c: config);
179 DOUBLES_EQUAL(500.0, actError, 1e-9);
180
181 // check linearization
182 GaussianFactor::shared_ptr actLinFactor = nle.linearize(x: config);
183 Matrix A1 = I_3x3;
184 Vector b = expVec;
185 SharedDiagonal model = noiseModel::Constrained::All(dim: 3);
186 GaussianFactor::shared_ptr expLinFactor(
187 new JacobianFactor(key1, A1, b, model));
188 EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5));
189}
190
191//******************************************************************************
192TEST ( NonlinearEquality, allow_error_optimize ) {
193 Symbol key1('x', 1);
194 Pose2 feasible1(1.0, 2.0, 3.0);
195 double error_gain = 500.0;
196
197 // add to a graph
198 NonlinearFactorGraph graph;
199 graph.emplace_shared<PoseNLE>(args&: key1, args&: feasible1, args&: error_gain);
200
201 // initialize away from the ideal
202 Pose2 initPose(0.0, 2.0, 3.0);
203 Values init;
204 init.insert(j: key1, val: initPose);
205
206 // optimize
207 Ordering ordering;
208 ordering.push_back(x: key1);
209 Values result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
210
211 // verify
212 Values expected;
213 expected.insert(j: key1, val: feasible1);
214 EXPECT(assert_equal(expected, result));
215}
216
217//******************************************************************************
218TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
219
220 // create a hard constraint
221 Symbol key1('x', 1);
222 Pose2 feasible1(1.0, 2.0, 3.0);
223
224 // initialize away from the ideal
225 Values init;
226 Pose2 initPose(0.0, 2.0, 3.0);
227 init.insert(j: key1, val: initPose);
228
229 // add nle to a graph
230 double error_gain = 500.0;
231 NonlinearFactorGraph graph;
232 graph.emplace_shared<PoseNLE>(args&: key1, args&: feasible1, args&: error_gain);
233
234 // add a soft prior that conflicts
235 graph.emplace_shared<PosePrior>(args&: key1, args&: initPose, args: noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1));
236
237 // optimize
238 Ordering ordering;
239 ordering.push_back(x: key1);
240 Values actual = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
241
242 // verify
243 Values expected;
244 expected.insert(j: key1, val: feasible1);
245 EXPECT(assert_equal(expected, actual));
246}
247
248//******************************************************************************
249static SharedDiagonal hard_model = noiseModel::Constrained::All(dim: 2);
250static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(dim: 2, sigma: 1.0);
251
252//******************************************************************************
253TEST( testNonlinearEqualityConstraint, unary_basics ) {
254 Point2 pt(1.0, 2.0);
255 Symbol key1('x', 1);
256 double mu = 1000.0;
257 eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
258
259 Values config1;
260 config1.insert(j: key, val: pt);
261 EXPECT(constraint.active(config1));
262 EXPECT(assert_equal(Z_2x1, constraint.evaluateError(pt), tol));
263 EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol));
264 EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
265
266 Values config2;
267 Point2 ptBad1(2.0, 2.0);
268 config2.insert(j: key, val: ptBad1);
269 EXPECT(constraint.active(config2));
270 EXPECT(
271 assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol));
272 EXPECT(
273 assert_equal(Vector2(1.0, 0.0), constraint.unwhitenedError(config2), tol));
274 EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol);
275}
276
277//******************************************************************************
278TEST( testNonlinearEqualityConstraint, unary_linearization ) {
279 Point2 pt(1.0, 2.0);
280 Symbol key1('x', 1);
281 double mu = 1000.0;
282 eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
283
284 Values config1;
285 config1.insert(j: key, val: pt);
286 GaussianFactor::shared_ptr actual1 = constraint.linearize(x: config1);
287 GaussianFactor::shared_ptr expected1(
288 new JacobianFactor(key, I_2x2, Z_2x1, hard_model));
289 EXPECT(assert_equal(*expected1, *actual1, tol));
290
291 Values config2;
292 Point2 ptBad(2.0, 2.0);
293 config2.insert(j: key, val: ptBad);
294 GaussianFactor::shared_ptr actual2 = constraint.linearize(x: config2);
295 GaussianFactor::shared_ptr expected2(
296 new JacobianFactor(key, I_2x2, Vector2(-1.0, 0.0), hard_model));
297 EXPECT(assert_equal(*expected2, *actual2, tol));
298}
299
300//******************************************************************************
301TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
302 // create a single-node graph with a soft and hard constraint to
303 // ensure that the hard constraint overrides the soft constraint
304 Point2 truth_pt(1.0, 2.0);
305 Symbol key('x', 1);
306 double mu = 10.0;
307 eq2D::UnaryEqualityConstraint::shared_ptr constraint(
308 new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));
309
310 Point2 badPt(100.0, -200.0);
311 simulated2D::Prior::shared_ptr factor(
312 new simulated2D::Prior(badPt, soft_model, key));
313
314 NonlinearFactorGraph graph;
315 graph.push_back(factor: constraint);
316 graph.push_back(factor);
317
318 Values initValues;
319 initValues.insert(j: key, val: badPt);
320
321 // verify error values
322 EXPECT(constraint->active(initValues));
323
324 Values expected;
325 expected.insert(j: key, val: truth_pt);
326 EXPECT(constraint->active(expected));
327 EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);
328
329 Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
330 EXPECT(assert_equal(expected, actual, tol));
331}
332
333//******************************************************************************
334TEST( testNonlinearEqualityConstraint, odo_basics ) {
335 Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
336 Symbol key1('x', 1), key2('x', 2);
337 double mu = 1000.0;
338 eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
339
340 Values config1;
341 config1.insert(j: key1, val: x1);
342 config1.insert(j: key2, val: x2);
343 EXPECT(constraint.active(config1));
344 EXPECT(assert_equal(Z_2x1, constraint.evaluateError(x1, x2), tol));
345 EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol));
346 EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
347
348 Values config2;
349 Point2 x1bad(2.0, 2.0);
350 Point2 x2bad(2.0, 2.0);
351 config2.insert(j: key1, val: x1bad);
352 config2.insert(j: key2, val: x2bad);
353 EXPECT(constraint.active(config2));
354 EXPECT(
355 assert_equal(Vector2(-1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol));
356 EXPECT(
357 assert_equal(Vector2(-1.0, -1.0), constraint.unwhitenedError(config2), tol));
358 EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol);
359}
360
361//******************************************************************************
362TEST( testNonlinearEqualityConstraint, odo_linearization ) {
363 Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
364 Symbol key1('x', 1), key2('x', 2);
365 double mu = 1000.0;
366 eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
367
368 Values config1;
369 config1.insert(j: key1, val: x1);
370 config1.insert(j: key2, val: x2);
371 GaussianFactor::shared_ptr actual1 = constraint.linearize(x: config1);
372 GaussianFactor::shared_ptr expected1(
373 new JacobianFactor(key1, -I_2x2, key2, I_2x2, Z_2x1,
374 hard_model));
375 EXPECT(assert_equal(*expected1, *actual1, tol));
376
377 Values config2;
378 Point2 x1bad(2.0, 2.0);
379 Point2 x2bad(2.0, 2.0);
380 config2.insert(j: key1, val: x1bad);
381 config2.insert(j: key2, val: x2bad);
382 GaussianFactor::shared_ptr actual2 = constraint.linearize(x: config2);
383 GaussianFactor::shared_ptr expected2(
384 new JacobianFactor(key1, -I_2x2, key2, I_2x2, Vector2(1.0, 1.0),
385 hard_model));
386 EXPECT(assert_equal(*expected2, *actual2, tol));
387}
388
389//******************************************************************************
390TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
391 // create a two-node graph, connected by an odometry constraint, with
392 // a hard prior on one variable, and a conflicting soft prior
393 // on the other variable - the constraints should override the soft constraint
394 Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0);
395 Symbol key1('x', 1), key2('x', 2);
396
397 // hard prior on x1
398 eq2D::UnaryEqualityConstraint::shared_ptr constraint1(
399 new eq2D::UnaryEqualityConstraint(truth_pt1, key1));
400
401 // soft prior on x2
402 Point2 badPt(100.0, -200.0);
403 simulated2D::Prior::shared_ptr factor(
404 new simulated2D::Prior(badPt, soft_model, key2));
405
406 // odometry constraint
407 eq2D::OdoEqualityConstraint::shared_ptr constraint2(
408 new eq2D::OdoEqualityConstraint(truth_pt2-truth_pt1, key1, key2));
409
410 NonlinearFactorGraph graph;
411 graph.push_back(factor: constraint1);
412 graph.push_back(factor: constraint2);
413 graph.push_back(factor);
414
415 Values initValues;
416 initValues.insert(j: key1, val: Point2(0,0));
417 initValues.insert(j: key2, val: badPt);
418
419 Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
420 Values expected;
421 expected.insert(j: key1, val: truth_pt1);
422 expected.insert(j: key2, val: truth_pt2);
423 CHECK(assert_equal(expected, actual, tol));
424}
425
426//******************************************************************************
427TEST (testNonlinearEqualityConstraint, two_pose ) {
428 /*
429 * Determining a ground truth linear system
430 * with two poses seeing one landmark, with each pose
431 * constrained to a particular value
432 */
433
434 NonlinearFactorGraph graph;
435
436 Symbol x1('x', 1), x2('x', 2);
437 Symbol l1('l', 1), l2('l', 2);
438 Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0);
439 graph.emplace_shared<eq2D::UnaryEqualityConstraint>(args&: pt_x1, args&: x1);
440 graph.emplace_shared<eq2D::UnaryEqualityConstraint>(args&: pt_x2, args&: x2);
441
442 Point2 z1(0.0, 5.0);
443 SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(dim: 2, sigma: 0.1));
444 graph.emplace_shared<simulated2D::Measurement>(args&: z1, args&: sigma, args&: x1, args&: l1);
445
446 Point2 z2(-4.0, 0.0);
447 graph.emplace_shared<simulated2D::Measurement>(args&: z2, args&: sigma, args&: x2, args&: l2);
448
449 graph.emplace_shared<eq2D::PointEqualityConstraint>(args&: l1, args&: l2);
450
451 Values initialEstimate;
452 initialEstimate.insert(j: x1, val: pt_x1);
453 initialEstimate.insert(j: x2, val: Point2(0,0));
454 initialEstimate.insert(j: l1, val: Point2(1.0, 6.0)); // ground truth
455 initialEstimate.insert(j: l2, val: Point2(-4.0, 0.0)); // starting with a separate reference frame
456
457 Values actual =
458 LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
459
460 Values expected;
461 expected.insert(j: x1, val: pt_x1);
462 expected.insert(j: l1, val: Point2(1.0, 6.0));
463 expected.insert(j: l2, val: Point2(1.0, 6.0));
464 expected.insert(j: x2, val: Point2(5.0, 6.0));
465 CHECK(assert_equal(expected, actual, 1e-5));
466}
467
468//******************************************************************************
469TEST (testNonlinearEqualityConstraint, map_warp ) {
470 // get a graph
471 NonlinearFactorGraph graph;
472
473 // keys
474 Symbol x1('x', 1), x2('x', 2);
475 Symbol l1('l', 1), l2('l', 2);
476
477 // constant constraint on x1
478 Point2 pose1(1.0, 1.0);
479 graph.emplace_shared<eq2D::UnaryEqualityConstraint>(args&: pose1, args&: x1);
480
481 SharedDiagonal sigma = noiseModel::Isotropic::Sigma(dim: 2, sigma: 0.1);
482
483 // measurement from x1 to l1
484 Point2 z1(0.0, 5.0);
485 graph.emplace_shared<simulated2D::Measurement>(args&: z1, args&: sigma, args&: x1, args&: l1);
486
487 // measurement from x2 to l2
488 Point2 z2(-4.0, 0.0);
489 graph.emplace_shared<simulated2D::Measurement>(args&: z2, args&: sigma, args&: x2, args&: l2);
490
491 // equality constraint between l1 and l2
492 graph.emplace_shared<eq2D::PointEqualityConstraint>(args&: l1, args&: l2);
493
494 // create an initial estimate
495 Values initialEstimate;
496 initialEstimate.insert(j: x1, val: Point2(1.0, 1.0));
497 initialEstimate.insert(j: l1, val: Point2(1.0, 6.0));
498 initialEstimate.insert(j: l2, val: Point2(-4.0, 0.0)); // starting with a separate reference frame
499 initialEstimate.insert(j: x2, val: Point2(0.0, 0.0)); // other pose starts at origin
500
501 // optimize
502 Values actual =
503 LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
504
505 Values expected;
506 expected.insert(j: x1, val: Point2(1.0, 1.0));
507 expected.insert(j: l1, val: Point2(1.0, 6.0));
508 expected.insert(j: l2, val: Point2(1.0, 6.0));
509 expected.insert(j: x2, val: Point2(5.0, 6.0));
510 CHECK(assert_equal(expected, actual, tol));
511}
512
513//******************************************************************************
514TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
515
516 // make a realistic calibration matrix
517 static double fov = 60; // degrees
518 static int w = 640, h = 480;
519 static Cal3_S2 K(fov, w, h);
520 static std::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
521
522 // create initial estimates
523 Rot3 faceTowardsY(Point3(1, 0, 0), Point3(0, 0, -1), Point3(0, 1, 0));
524
525 Pose3 poseLeft(faceTowardsY, Point3(0, 0, 0)); // origin, left camera
526 PinholeCamera<Cal3_S2> leftCamera(poseLeft, K);
527
528 Pose3 poseRight(faceTowardsY, Point3(2, 0, 0)); // 2 units to the right
529 PinholeCamera<Cal3_S2> rightCamera(poseRight, K);
530
531 Point3 landmark(1, 5, 0); //centered between the cameras, 5 units away
532
533 // keys
534 Symbol key_x1('x', 1), key_x2('x', 2);
535 Symbol key_l1('l', 1), key_l2('l', 2);
536
537 // create graph
538 NonlinearFactorGraph graph;
539
540 // create equality constraints for poses
541 graph.emplace_shared<NonlinearEquality<Pose3>>(args&: key_x1, args: leftCamera.pose());
542 graph.emplace_shared<NonlinearEquality<Pose3>>(args&: key_x2, args: rightCamera.pose());
543
544 // create factors
545 SharedDiagonal vmodel = noiseModel::Unit::Create(dim: 2);
546 graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
547 args: leftCamera.project(pw: landmark), args&: vmodel, args&: key_x1, args&: key_l1, args&: shK);
548 graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
549 args: rightCamera.project(pw: landmark), args&: vmodel, args&: key_x2, args&: key_l2, args&: shK);
550
551 // add equality constraint saying there is only one point
552 graph.emplace_shared<NonlinearEquality2<Point3>>(args&: key_l1, args&: key_l2);
553
554 // create initial data
555 Point3 landmark1(0.5, 5, 0);
556 Point3 landmark2(1.5, 5, 0);
557
558 Values initValues;
559 initValues.insert(j: key_x1, val: poseLeft);
560 initValues.insert(j: key_x2, val: poseRight);
561 initValues.insert(j: key_l1, val: landmark1);
562 initValues.insert(j: key_l2, val: landmark2);
563
564 // optimize
565 Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
566
567 // create config
568 Values truthValues;
569 truthValues.insert(j: key_x1, val: leftCamera.pose());
570 truthValues.insert(j: key_x2, val: rightCamera.pose());
571 truthValues.insert(j: key_l1, val: landmark);
572 truthValues.insert(j: key_l2, val: landmark);
573
574 // check if correct
575 CHECK(assert_equal(truthValues, actual, 1e-5));
576}
577
578//******************************************************************************
579int main() {
580 TestResult tr;
581 return TestRegistry::runAllTests(result&: tr);
582}
583//******************************************************************************
584