testConcurrentIncrementalFilter.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 
22 #include <gtsam/nonlinear/ISAM2.h>
26 #include <gtsam/nonlinear/Values.h>
27 #include <gtsam/inference/Symbol.h>
28 #include <gtsam/inference/Key.h>
30 #include <gtsam/geometry/Pose3.h>
33 
34 using namespace std;
35 using namespace gtsam;
36 
37 namespace {
38 
39 // Set up initial pose, odometry difference, loop closure difference, and initialization errors
40 const Pose3 poseInitial;
41 const Pose3 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
42 //const Pose3 poseError( Rot3::RzRyRx(Vector3(0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
43 const Pose3 poseError( Rot3::RzRyRx(Vector3(0.1, 0.02, -0.1)), Point3(0.5, -0.05, 0.2) );
44 
45 // Set up noise models for the factors
46 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
47 const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished());
48 const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished());
49 
50 /* ************************************************************************* */
51 Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) {
52 
53  // Create an ISAM2-based optimizer
56 // parameters.maxIterations = maxIter;
57 // parameters.lambdaUpperBound = 1;
58 // parameters.lambdaInitial = 1;
59 // parameters.verbosity = NonlinearOptimizerParams::ERROR;
60 // parameters.verbosityLM = ISAM2Params::DAMPED;
61 // parameters.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_QR;
62 
63  // it is the same as the input graph, but we removed the empty factors that may be present in the input graph
64  NonlinearFactorGraph graphForISAM2;
65  for(NonlinearFactor::shared_ptr factor: graph){
66  if(factor)
67  graphForISAM2.push_back(factor);
68  }
69 
70  ISAM2 optimizer(parameters);
71  optimizer.update( graphForISAM2, theta );
72  Values result = optimizer.calculateEstimate();
73  return result;
74 
75 }
76 
77 
78 /* ************************************************************************* */
79 NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, const Values& linPoint, const FastList<Key>& keysToMarginalize){
80 
81 
82  std::set<Key> KeysToKeep;
83  for(const auto key: linPoint.keys()) { // we cycle over all the keys of factorGraph
84  KeysToKeep.insert(key);
85  } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize
86  for(Key key: keysToMarginalize) {
87  KeysToKeep.erase(key);
88  } // we removed the keys that we have to marginalize
89 
91  for(Key key: keysToMarginalize) {
92  ordering.push_back(key);
93  } // the keys that we marginalize should be at the beginning in the ordering
94  for(Key key: KeysToKeep) {
95  ordering.push_back(key);
96  }
97 
98 
99  GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
100 
101  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
102 
103  NonlinearFactorGraph LinearContainerForGaussianMarginals;
104  for(const GaussianFactor::shared_ptr& factor: marginal) {
105  LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint));
106  }
107 
108  return LinearContainerForGaussianMarginals;
109 }
110 
111 
112 } // end namespace
113 
114 
115 
116 /* ************************************************************************* */
118 {
119  // TODO: Test 'equals' more vigorously
120 
121  // Create a Concurrent incremental Filter
122  ISAM2Params parameters1;
123  ConcurrentIncrementalFilter filter1(parameters1);
124 
125  // Create an identical Concurrent incremental Filter
126  ISAM2Params parameters2;
127  ConcurrentIncrementalFilter filter2(parameters2);
128 
129  // Create a different Concurrent incremental Filter
130  ISAM2Params parameters3;
131  // ISAM2 always performs a single iteration
132  // parameters3.maxIterations = 1;
133  ConcurrentIncrementalFilter filter3(parameters3);
134 
135  CHECK(assert_equal(filter1, filter1));
136  CHECK(assert_equal(filter1, filter2));
137 // CHECK(assert_inequal(filter1, filter3));
138 }
139 
140 /* ************************************************************************* */
142 {
143  // Create a Concurrent Incremental Filter
145  ConcurrentIncrementalFilter filter(parameters);
146 
147  // Expected graph is empty
148  NonlinearFactorGraph expected1;
149  // Get actual graph
150  NonlinearFactorGraph actual1 = filter.getFactors();
151  // Check
152  CHECK(assert_equal(expected1, actual1));
153 
154  // Add some factors to the filter
155  NonlinearFactorGraph newFactors1;
156  newFactors1.addPrior(1, poseInitial, noisePrior);
157  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
158  Values newValues1;
159  newValues1.insert(1, Pose3());
160  newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
161  filter.update(newFactors1, newValues1);
162 
163  // Expected graph
164  NonlinearFactorGraph expected2;
165  expected2.push_back(newFactors1);
166  // Get actual graph
167  NonlinearFactorGraph actual2 = filter.getFactors();
168  // Check
169  CHECK(assert_equal(expected2, actual2));
170 
171  // Add some more factors to the filter
172  NonlinearFactorGraph newFactors2;
173  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
174  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
175  Values newValues2;
176  newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
177  newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
178  filter.update(newFactors2, newValues2);
179 
180  // Expected graph
181  NonlinearFactorGraph expected3;
182  expected3.push_back(newFactors1);
183  expected3.push_back(newFactors2);
184  // Get actual graph
185  NonlinearFactorGraph actual3 = filter.getFactors();
186  // Check
187  CHECK(assert_equal(expected3, actual3));
188 }
189 
190 /* ************************************************************************* */
191 TEST( ConcurrentIncrementalFilter, getLinearizationPoint )
192 {
193  // Create a Concurrent Incremental Filter
195  ConcurrentIncrementalFilter filter(parameters);
196 
197  // Expected values is empty
198  Values expected1;
199  // Get Linearization Point
200  Values actual1 = filter.getLinearizationPoint();
201  // Check
202  CHECK(assert_equal(expected1, actual1));
203 
204  // Add some factors to the filter
205  NonlinearFactorGraph newFactors1;
206  newFactors1.addPrior(1, poseInitial, noisePrior);
207  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
208  Values newValues1;
209  newValues1.insert(1, Pose3());
210  newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
211  filter.update(newFactors1, newValues1);
212 
213  // Expected values is equivalent to the provided values only because the provided linearization points were optimal
214  Values expected2;
215  expected2.insert(newValues1);
216  // Get actual linearization point
217  Values actual2 = filter.getLinearizationPoint();
218  // Check
219  CHECK(assert_equal(expected2, actual2));
220 
221  // Add some more factors to the filter
222  NonlinearFactorGraph newFactors2;
223  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
224  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
225  Values newValues2;
226  newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
227  newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
228  filter.update(newFactors2, newValues2);
229 
230  // Expected values is equivalent to the provided values only because the provided linearization points were optimal
231  Values expected3;
232  expected3.insert(newValues1);
233  expected3.insert(newValues2);
234  // Get actual linearization point
235  Values actual3 = filter.getLinearizationPoint();
236  // Check
237  CHECK(assert_equal(expected3, actual3));
238 }
239 
240 /* ************************************************************************* */
242 {
243  // TODO: Think about how to check delta...
244 }
245 
246 /* ************************************************************************* */
247 TEST( ConcurrentIncrementalFilter, calculateEstimate )
248 {
249  // Create a Concurrent Incremental Filter
251  ConcurrentIncrementalFilter filter(parameters);
252 
253  // Expected values is empty
254  Values expected1;
255  // Get Linearization Point
256  Values actual1 = filter.calculateEstimate();
257  // Check
258  CHECK(assert_equal(expected1, actual1));
259 
260  // Add some factors to the filter
261  NonlinearFactorGraph newFactors2;
262  newFactors2.addPrior(1, poseInitial, noisePrior);
263  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
264  Values newValues2;
265  newValues2.insert(1, Pose3().compose(poseError));
266  newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
267  filter.update(newFactors2, newValues2);
268 
269  // Expected values from full Incremental optimization
270  NonlinearFactorGraph allFactors2;
271  allFactors2.push_back(newFactors2);
272  Values allValues2;
273  allValues2.insert(newValues2);
274  Values expected2 = BatchOptimize(allFactors2, allValues2);
275  // Get actual linearization point
276  Values actual2 = filter.calculateEstimate();
277  // Check
278  CHECK(assert_equal(expected2, actual2, 1e-6));
279 
280  // Add some more factors to the filter
281  NonlinearFactorGraph newFactors3;
282  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
283  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
284  Values newValues3;
285  newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
286  newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
287  filter.update(newFactors3, newValues3);
288 
289  // Expected values from full Incrementaloptimization
290  NonlinearFactorGraph allFactors3;
291  allFactors3.push_back(newFactors2);
292  allFactors3.push_back(newFactors3);
293  Values allValues3;
294  allValues3.insert(newValues2);
295  allValues3.insert(newValues3);
296  Values expected3 = BatchOptimize(allFactors3, allValues3);
297  // Get actual linearization point
298  Values actual3 = filter.calculateEstimate();
299  // Check
300  CHECK(assert_equal(expected3, actual3, 1e-6));
301 
302  // Also check the single-variable version
303  Pose3 expectedPose1 = expected3.at<Pose3>(1);
304  Pose3 expectedPose2 = expected3.at<Pose3>(2);
305  Pose3 expectedPose3 = expected3.at<Pose3>(3);
306  Pose3 expectedPose4 = expected3.at<Pose3>(4);
307  // Also check the single-variable version
308  Pose3 actualPose1 = filter.calculateEstimate<Pose3>(1);
309  Pose3 actualPose2 = filter.calculateEstimate<Pose3>(2);
310  Pose3 actualPose3 = filter.calculateEstimate<Pose3>(3);
311  Pose3 actualPose4 = filter.calculateEstimate<Pose3>(4);
312  // Check
313  CHECK(assert_equal(expectedPose1, actualPose1, 1e-6));
314  CHECK(assert_equal(expectedPose2, actualPose2, 1e-6));
315  CHECK(assert_equal(expectedPose3, actualPose3, 1e-6));
316  CHECK(assert_equal(expectedPose4, actualPose4, 1e-6));
317 }
318 
319 /* ************************************************************************* */
321 {
322  // Create a set of optimizer parameters
324  ConcurrentIncrementalFilter filter(parameters);
325 
326  // Call update
327  filter.update();
328 }
329 
330 /* ************************************************************************* */
332 {
333  // Create a Concurrent IncrementalFilter
335  ConcurrentIncrementalFilter filter(parameters);
336 
337  // Expected values is empty
338  Values expected1;
339  // Get Linearization Point
340  Values actual1 = filter.calculateEstimate();
341  // Check
342  CHECK(assert_equal(expected1, actual1));
343 
344  // Add some factors to the filter
345  NonlinearFactorGraph newFactors2;
346  newFactors2.addPrior(1, poseInitial, noisePrior);
347  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
348  Values newValues2;
349  newValues2.insert(1, Pose3().compose(poseError));
350  newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
351  filter.update(newFactors2, newValues2);
352 
353  // Expected values from full Incrementaloptimization
354  NonlinearFactorGraph allFactors2;
355  allFactors2.push_back(newFactors2);
356  Values allValues2;
357  allValues2.insert(newValues2);
358  Values expected2 = BatchOptimize(allFactors2, allValues2);
359  // Get actual linearization point
360  Values actual2 = filter.calculateEstimate();
361  // Check
362  CHECK(assert_equal(expected2, actual2, 1e-6));
363 
364  // Add some more factors to the filter
365  NonlinearFactorGraph newFactors3;
366  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
367  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
368  Values newValues3;
369  newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
370  newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
371  filter.update(newFactors3, newValues3);
372 
373  // Expected values from full Incrementaloptimization
374  NonlinearFactorGraph allFactors3;
375  allFactors3.push_back(newFactors2);
376  allFactors3.push_back(newFactors3);
377  Values allValues3;
378  allValues3.insert(newValues2);
379  allValues3.insert(newValues3);
380  Values expected3 = BatchOptimize(allFactors3, allValues3);
381  // Get actual linearization point
382  Values actual3 = filter.calculateEstimate();
383  // Check
384  CHECK(assert_equal(expected3, actual3, 1e-6));
385 }
386 
387 /* ************************************************************************* */
388 TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
389 {
390  // Create a set of optimizer parameters
392  ConcurrentIncrementalFilter filter(parameters);
393 
394  // Add some factors to the filter
395  NonlinearFactorGraph newFactors;
396  newFactors.addPrior(1, poseInitial, noisePrior);
397  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
398  newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
399  newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
400  Values newValues;
401  newValues.insert(1, Pose3().compose(poseError));
402  newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
403  newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
404  newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
405 
406  // Specify a subset of variables to marginalize/move to the smoother
407  FastList<Key> keysToMove;
408  keysToMove.push_back(1);
409  keysToMove.push_back(2);
410 
411  // Update the filter
412  filter.update(newFactors, newValues, keysToMove);
413 
414  // Calculate expected factor graph and values
415  Values optimalValues = BatchOptimize(newFactors, newValues);
416 
417  Values expectedValues = optimalValues;
418 
419  // Check
420  for(Key key: keysToMove) {
421  expectedValues.erase(key);
422  }
423 
424  // ----------------------------------------------------------------------------------------------
425  NonlinearFactorGraph partialGraph;
426  partialGraph.addPrior(1, poseInitial, noisePrior);
427  partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
428  partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
429 
430  GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues);
431 
432  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
433 
435 
436  // These three lines create three empty factors in expectedGraph:
437  // this is done since the equal function in NonlinearFactorGraph also cares about the ordering of the factors
438  // and in the actualGraph produced by the HMF we first insert 5 nonlinear factors, then we delete 3 of them, by
439  // substituting them with a linear marginal
440  expectedGraph.push_back(NonlinearFactor::shared_ptr());
441  expectedGraph.push_back(NonlinearFactor::shared_ptr());
442  expectedGraph.push_back(NonlinearFactor::shared_ptr());
443  // ==========================================================
444  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
445 
446  for(const GaussianFactor::shared_ptr& factor: marginal) {
447  // the linearization point for the linear container is optional, but it is not used in the filter,
448  // therefore if we add it here it will not pass the test
449  // expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues));
450  expectedGraph.push_back(LinearContainerFactor(factor));
451  }
452 
453  // ----------------------------------------------------------------------------------------------
454 
455  // Get the actual factor graph and optimal point
456  NonlinearFactorGraph actualGraph = filter.getFactors();
457  Values actualValues = filter.calculateEstimate();
458 
459 // expectedGraph.print("expectedGraph ---------------------------------------------- \n");
460 // actualGraph.print("actualGraph ---------------------------------------------- \n");
461 
462  CHECK(assert_equal(expectedValues, actualValues, 1e-12));
463  CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
464 }
465 
466 /* ************************************************************************* */
467 TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
468 {
469  // Create a set of optimizer parameters
471  parameters.relinearizeThreshold = 0.;
472  // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the
473  // default value for that is 10 (if you set that to zero the code will crash)
474  parameters.relinearizeSkip = 1;
475  ConcurrentIncrementalFilter filter(parameters);
476 
477  // Add some factors to the filter
478  NonlinearFactorGraph newFactors;
479  newFactors.addPrior(1, poseInitial, noisePrior);
480  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
481  newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
482  newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
483  Values newValues;
484  newValues.insert(1, Pose3().compose(poseError));
485  newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
486  newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
487  newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
488 
489  // Specify a subset of variables to marginalize/move to the smoother
490  FastList<Key> keysToMove;
491  keysToMove.push_back(1);
492  keysToMove.push_back(2);
493 
494  // Update the filter
495  filter.update(newFactors, newValues);
496  filter.update(NonlinearFactorGraph(), Values(), keysToMove);
497 
498  // Calculate expected factor graph and values
499  Values optimalValues = BatchOptimize(newFactors, newValues);
500 
501  Values expectedValues = optimalValues;
502 
503  // Check
504  for(Key key: keysToMove) {
505  expectedValues.erase(key);
506  }
507 
508  // ----------------------------------------------------------------------------------------------
509  NonlinearFactorGraph partialGraph;
510  partialGraph.addPrior(1, poseInitial, noisePrior);
511  partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
512  partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
513 
514  GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues);
515 
516  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
517 
519 
520  // These three lines create three empty factors in expectedGraph:
521  // this is done since the equal function in NonlinearFactorGraph also cares about the ordering of the factors
522  // and in the actualGraph produced by the HMF we first insert 5 nonlinear factors, then we delete 3 of them, by
523  // substituting them with a linear marginal
524  expectedGraph.push_back(NonlinearFactor::shared_ptr());
525  expectedGraph.push_back(NonlinearFactor::shared_ptr());
526  expectedGraph.push_back(NonlinearFactor::shared_ptr());
527  // ==========================================================
528  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
529 
530  for(const GaussianFactor::shared_ptr& factor: marginal) {
531  // the linearization point for the linear container is optional, but it is not used in the filter,
532  // therefore if we add it here it will not pass the test
533  // expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues));
534  expectedGraph.push_back(LinearContainerFactor(factor));
535  }
536 
537  // ----------------------------------------------------------------------------------------------
538 
539  // Get the actual factor graph and optimal point
540  NonlinearFactorGraph actualGraph = filter.getFactors();
541  Values actualValues = filter.getLinearizationPoint();
542 
543  Values optimalValues2 = BatchOptimize(newFactors, optimalValues);
544  Values expectedValues2 = optimalValues2;
545  // Check
546  for(Key key: keysToMove) {
547  expectedValues2.erase(key);
548  }
549  Values actualValues2 = filter.calculateEstimate();
550 
551 // expectedGraph.print("expectedGraph ---------------------------------------------- \n");
552 // actualGraph.print("actualGraph ---------------------------------------------- \n");
553 
554  CHECK(assert_equal(expectedValues, actualValues, 1e-12));
555  CHECK(assert_equal(expectedValues2, actualValues2, 1e-12));
556  CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
557 }
558 
559 /* ************************************************************************* */
561 {
562  // Create a set of optimizer parameters
564 
565  // Create a Concurrent IncrementalFilter
566  ConcurrentIncrementalFilter filter(parameters);
567 
568  // Create empty containers *from* the smoother
569  NonlinearFactorGraph smootherSummarization;
570  Values smootherSeparatorValues;
571 
572  // Create expected values from the filter. For the case where the filter is empty, the expected values are also empty
573  NonlinearFactorGraph expectedSmootherFactors, expectedFilterSummarization;
574  Values expectedSmootherValues, expectedFilterSeparatorValues;
575 
576  // Synchronize
577  NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
578  Values actualSmootherValues, actualFilterSeparatorValues;
579  filter.presync();
580  filter.synchronize(smootherSummarization, smootherSeparatorValues);
581  filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
582  filter.getSummarizedFactors(actualFilterSummarization, actualFilterSeparatorValues);
583  filter.postsync();
584 
585  // Check
586  CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
587  CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
588  CHECK(assert_equal(expectedFilterSummarization, actualFilterSummarization, 1e-6));
589  CHECK(assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1e-6));
590 }
591 
594 {
595  // Create a set of optimizer parameters
597  parameters.relinearizeThreshold = 0.;
598  // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the
599  // default value for that is 10 (if you set that to zero the code will crash)
600  parameters.relinearizeSkip = 1;
601 
602  // Create a Concurrent IncrementalFilter
603  ConcurrentIncrementalFilter filter(parameters);
604 
605  // Insert factors into the filter, but do not marginalize out any variables.
606  // The synchronization should still be empty
607  NonlinearFactorGraph newFactors;
608  newFactors.addPrior(1, poseInitial, noisePrior);
609  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
610  Values newValues;
611  newValues.insert(1, Pose3().compose(poseError));
612  newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
613  filter.update(newFactors, newValues);
614 
615  // Create empty containers *from* the smoother
616  NonlinearFactorGraph smootherSummarization;
617  Values smootherSeparatorValues;
618 
619  // Create expected values from the filter. For the case when nothing has been marginalized from the filter, the expected values are empty
620  NonlinearFactorGraph expectedSmootherFactors, expectedFilterSummarization;
621  Values expectedSmootherValues, expectedFilterSeparatorValues;
622 
623  // Synchronize
624  NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
625  Values actualSmootherValues, actualFilterSeparatorValues;
626  filter.presync();
627  filter.synchronize(smootherSummarization, smootherSeparatorValues);
628  filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
629  filter.getSummarizedFactors(actualFilterSummarization, actualFilterSeparatorValues);
630  filter.postsync();
631 
632  // Check
633  CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
634  CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
635  CHECK(assert_equal(expectedFilterSummarization, actualFilterSummarization, 1e-6));
636  CHECK(assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1e-6));
637 }
638 
639 /* ************************************************************************* */
641 {
642  // Create a set of optimizer parameters
644  parameters.relinearizeThreshold = 0.;
645  // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the
646  // default value for that is 10 (if you set that to zero the code will crash)
647  parameters.relinearizeSkip = 1;
648 
649  // Create a Concurrent IncrementalFilter
650  ConcurrentIncrementalFilter filter(parameters);
651 
652  // Insert factors into the filter, and marginalize out one variable.
653  // There should not be information transmitted to the smoother from the filter
654  NonlinearFactorGraph newFactors;
655  NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
656  NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
657  newFactors.push_back(factor1);
658  newFactors.push_back(factor2);
659  Values newValues;
660  Pose3 value1 = Pose3().compose(poseError);
661  Pose3 value2 = value1.compose(poseOdometry).compose(poseError);
662  newValues.insert(1, value1);
663  newValues.insert(2, value2);
664  FastList<Key> keysToMove;
665  keysToMove.push_back(1);
666  filter.update(newFactors, newValues, keysToMove);
667  // this will not work, as in the filter only remains node 2, while 1 was marginalized out
668  // Values optimalValues = filter.calculateEstimate();
669 
670  Values optimalValues = BatchOptimize(newFactors, newValues);
671 
672  // Create empty containers *from* the smoother
673  NonlinearFactorGraph smootherSummarization;
674  Values smootherSeparatorValues;
675 
676 
677  // Create expected values from the filter.
678  // The smoother factors include any factor adjacent to a marginalized variable
679  NonlinearFactorGraph expectedSmootherFactors;
680  expectedSmootherFactors.push_back(factor1);
681  expectedSmootherFactors.push_back(factor2);
682  Values expectedSmootherValues;
683  // We only pass linearization points for the marginalized variables
684  expectedSmootherValues.insert(1, newValues.at(1));
685 
686  // The filter summarization is the remaining factors from marginalizing out the requested variable
687  // In the current example, after marginalizing out 1, the filter only contains the separator (2), with
688  // no nonlinear factor attached to it, therefore no filter summarization needs to be passed to the smoother
689  NonlinearFactorGraph expectedFilterSummarization;
690  Values expectedFilterSeparatorValues;
691  expectedFilterSeparatorValues.insert(2, newValues.at(2));
692 
693  // Synchronize
694  NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
695  Values actualSmootherValues, actualFilterSeparatorValues;
696  filter.presync();
697  filter.synchronize(smootherSummarization, smootherSeparatorValues);
698  filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
699  filter.getSummarizedFactors(actualFilterSummarization, actualFilterSeparatorValues);
700  filter.postsync();
701 
702  // Check
703  CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
704  CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
705  CHECK(assert_equal(expectedFilterSummarization, actualFilterSummarization, 1e-6));
706  CHECK(assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1e-6));
707 }
708 
709 /* ************************************************************************* */
711 {
712  // Create a set of optimizer parameters
714  parameters.relinearizeThreshold = 0.;
715  // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the
716  // default value for that is 10 (if you set that to zero the code will crash)
717  parameters.relinearizeSkip = 1;
718 
719  // Create a Concurrent IncrementalFilter
720  ConcurrentIncrementalFilter filter(parameters);
721 
722  // Insert factors into the filter, and marginalize out one variable.
723  // There should not be information transmitted to the smoother from the filter
724  NonlinearFactorGraph newFactors;
725  NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
726  NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
727  NonlinearFactor::shared_ptr factor3(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
728  newFactors.push_back(factor1);
729  newFactors.push_back(factor2);
730  newFactors.push_back(factor3);
731 
732  Values newValues;
733  Pose3 value1 = Pose3().compose(poseError);
734  Pose3 value2 = value1.compose(poseOdometry).compose(poseError);
735  Pose3 value3 = value2.compose(poseOdometry).compose(poseError);
736  newValues.insert(1, value1);
737  newValues.insert(2, value2);
738  newValues.insert(3, value3);
739 
740  FastList<Key> keysToMove;
741  keysToMove.push_back(1);
742  // we add factors to the filter while marginalizing node 1
743  filter.update(newFactors, newValues, keysToMove);
744 
745  Values optimalValues = BatchOptimize(newFactors, newValues);
746 
747  // In this example the smoother is empty
748  // Create empty containers *from* the smoother
749  NonlinearFactorGraph smootherSummarization;
750  Values smootherSeparatorValues;
751 
752  // Create expected values from the filter.
753  // The smoother factors include any factor adjacent to a marginalized variable
754  NonlinearFactorGraph expectedSmootherFactors;
755  expectedSmootherFactors.push_back(factor1);
756  expectedSmootherFactors.push_back(factor2);
757  Values expectedSmootherValues;
758  // We only pass linearization points for the marginalized variables
759  expectedSmootherValues.insert(1, newValues.at<Pose3>(1));
760 
761  // In the current example, after marginalizing out 1, the filter contains the separator 2 and node 3, with
762  // a nonlinear factor attached to them
763  // Why there is no summarization from filter ????
764  NonlinearFactorGraph expectedFilterSummarization;
765  Values expectedFilterSeparatorValues;
766  expectedFilterSeparatorValues.insert(2, newValues.at(2));
767  // ------------------------------------------------------------------------------
768  NonlinearFactorGraph partialGraph;
769  partialGraph.push_back(factor3);
770 
771  Values partialValues;
772  partialValues.insert(2, newValues.at<Pose3>(2));
773  partialValues.insert(3, newValues.at<Pose3>(3));
774 
775  FastList<Key> keysToMarginalize;
776  keysToMarginalize.push_back(3);
777 
778  expectedFilterSummarization = CalculateMarginals(partialGraph, partialValues, keysToMarginalize);
779  // ------------------------------------------------------------------------------
780  // Synchronize
781  NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
782  Values actualSmootherValues, actualFilterSeparatorValues;
783  filter.presync();
784  filter.synchronize(smootherSummarization, smootherSeparatorValues);
785  filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
786  filter.getSummarizedFactors(actualFilterSummarization, actualFilterSeparatorValues);
787  filter.postsync();
788 
789  // Check
790  CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
791  CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
792  CHECK(assert_equal(expectedFilterSummarization, actualFilterSummarization, 1e-6));
793  CHECK(assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1e-6));
794 }
795 
796 /* ************************************************************************* */
798 {
799  // Create a set of optimizer parameters
801  parameters.relinearizeThreshold = 0.;
802  // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the
803  // default value for that is 10 (if you set that to zero the code will crash)
804  parameters.relinearizeSkip = 1;
805 
806  // Create a Concurrent IncrementalFilter
807  ConcurrentIncrementalFilter filter(parameters);
808 
809  // Insert factors into the filter, and marginalize out one variable.
810  // There should not be information transmitted to the smoother from the filter
811  NonlinearFactorGraph newFactors;
812  NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
813  NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
814  NonlinearFactor::shared_ptr factor3(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
815  newFactors.push_back(factor1);
816  newFactors.push_back(factor2);
817  newFactors.push_back(factor3);
818 
819  Values newValues;
820  Pose3 value1 = Pose3().compose(poseError);
821  Pose3 value2 = value1.compose(poseOdometry).compose(poseError);
822  Pose3 value3 = value2.compose(poseOdometry).compose(poseError);
823  newValues.insert(1, value1);
824  newValues.insert(2, value2);
825  newValues.insert(3, value3);
826 
827  FastList<Key> keysToMove;
828  keysToMove.push_back(1);
829  // we add factors to the filter while marginalizing node 1
830  filter.update(newFactors, newValues, keysToMove);
831 
832  Values optimalValuesFilter = BatchOptimize(newFactors, newValues,1);
833 
834  // In this example the smoother contains a between factor and a prior factor
835  // COMPUTE SUMMARIZATION ON THE SMOOTHER SIDE
836  NonlinearFactorGraph smootherSummarization;
837  Values smootherSeparatorValues;
838 
839  // Create expected values from the filter.
840  // The smoother factors include any factor adjacent to a marginalized variable
841  NonlinearFactorGraph expectedSmootherFactors;
842  expectedSmootherFactors.push_back(factor1);
843  expectedSmootherFactors.push_back(factor2);
844  Values expectedSmootherValues;
845  // We only pass linearization points for the marginalized variables
846  expectedSmootherValues.insert(1, newValues.at<Pose3>(1));
847 
848  // COMPUTE SUMMARIZATION ON THE FILTER SIDE
849  // In the current example, after marginalizing out 1, the filter contains the separator 2 and node 3, with
850  // a nonlinear factor attached to them
851  // Why there is no summarization from filter ????
852  NonlinearFactorGraph expectedFilterSummarization;
853  Values expectedFilterSeparatorValues;
854  expectedFilterSeparatorValues.insert(2, newValues.at<Pose3>(2));
855  // ------------------------------------------------------------------------------
856  NonlinearFactorGraph partialGraphFilter;
857  partialGraphFilter.push_back(factor3);
858 
859  Values partialValuesFilter;
860  partialValuesFilter.insert(2, newValues.at<Pose3>(2));
861  partialValuesFilter.insert(3, newValues.at<Pose3>(3));
862 
863  // Create an ordering
864  Ordering orderingFilter;
865  orderingFilter.push_back(3);
866  orderingFilter.push_back(2);
867 
868  FastList<Key> keysToMarginalize;
869  keysToMarginalize.push_back(3);
870 
871  expectedFilterSummarization = CalculateMarginals(partialGraphFilter, partialValuesFilter, keysToMarginalize);
872  // ------------------------------------------------------------------------------
873  // Synchronize
874  // This is only an information compression/exchange: to actually incorporate the info we should call update
875  NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
876  Values actualSmootherValues, actualFilterSeparatorValues;
877  filter.presync();
878  filter.synchronize(smootherSummarization, smootherSeparatorValues);
879  filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
880  filter.getSummarizedFactors(actualFilterSummarization, actualFilterSeparatorValues);
881  filter.postsync();
882 
883  // Check
884  CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
885  CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
886  CHECK(assert_equal(expectedFilterSummarization, actualFilterSummarization, 1e-6));
887  CHECK(assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1e-6));
888 }
889 
890 
891 /* ************************************************************************* */
893 {
894  // Create a set of optimizer parameters
896  parameters.relinearizeThreshold = 0.;
897  // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the
898  // default value for that is 10 (if you set that to zero the code will crash)
899  parameters.relinearizeSkip = 1;
900 
901  // Create a Concurrent IncrementalFilter
902  ConcurrentIncrementalFilter filter(parameters);
903 
904  // Insert factors into the filter, and marginalize out one variable.
905  // There should not be information transmitted to the smoother from the filter
906  NonlinearFactorGraph newFactors;
907  NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
908  NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
909  NonlinearFactor::shared_ptr factor3(new PriorFactor<Pose3>(2, poseInitial, noisePrior));
910  NonlinearFactor::shared_ptr factor4(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
911  NonlinearFactor::shared_ptr factor5(new BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
912  newFactors.push_back(factor1);
913  newFactors.push_back(factor2);
914  newFactors.push_back(factor3);
915  newFactors.push_back(factor4);
916  newFactors.push_back(factor5);
917 
918  Values newValues;
919  Pose3 value1 = Pose3().compose(poseError);
920  Pose3 value2 = value1.compose(poseOdometry).compose(poseError);
921  Pose3 value3 = value2.compose(poseOdometry).compose(poseError);
922  Pose3 value4 = value3.compose(poseOdometry).compose(poseError);
923 
924  newValues.insert(1, value1);
925  newValues.insert(2, value2);
926  newValues.insert(3, value3);
927  newValues.insert(4, value4);
928 
929  FastList<Key> keysToMove;
930  keysToMove.push_back(1);
931  // we add factors to the filter while marginalizing node 1
932  filter.update(newFactors, newValues, keysToMove);
933 
934  // At the beginning the smoother is empty
935  NonlinearFactorGraph smootherSummarization;
936  Values smootherSeparatorValues;
937 
938  // Synchronize
939  // This is only an information compression/exchange: to actually incorporate the info we should call update
940  NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
941  Values actualSmootherValues, actualFilterSeparatorValues;
942  filter.presync();
943  filter.synchronize(smootherSummarization, smootherSeparatorValues);
944  filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues);
945  filter.getSummarizedFactors(actualFilterSummarization, actualFilterSeparatorValues);
946  filter.postsync();
947 
948  NonlinearFactorGraph expectedSmootherFactors;
949  expectedSmootherFactors.push_back(factor1);
950  expectedSmootherFactors.push_back(factor2);
951 
952  Values optimalValues = BatchOptimize(newFactors, newValues, 1);
953  Values expectedSmootherValues;
954  // Pose3 cast is useless in this case (but we still put it as an example): values and graphs can handle generic
955  // geometric objects. You really need the <Pose3> when you need to fill in a Pose3 object with the .at()
956  expectedSmootherValues.insert(1,newValues.at(1));
957 
958  // Check
959  CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
960  CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
961 
962  // at this point the filter contains: nodes 2 3 4 and factors 3 4 5 + marginal on 2
963  Values optimalValues2 = BatchOptimize(filter.getFactors(),filter.getLinearizationPoint(),1);
964 
965  FastList<Key> keysToMove2;
966  keysToMove2.push_back(2);
967 
968  // we add factors to the filter while marginalizing node 1
969  filter.update(NonlinearFactorGraph(), Values(), keysToMove2);
970 
971  // At the beginning the smoother is empty
972  NonlinearFactorGraph smootherSummarization2;
973  Values smootherSeparatorValues2;
974 
975  // ------------------------------------------------------------------------------
976  // We fake the computation of the smoother separator
977  // currently the smoother contains factor 1 and 2 and node 1 and 2
978 
979  NonlinearFactorGraph partialGraph;
980  partialGraph.push_back(factor1);
981  partialGraph.push_back(factor2);
982 
983  // we also assume that the smoother received an extra factor (e.g., a prior on 1)
984  partialGraph.push_back(factor1);
985 
986  Values partialValues;
987  // Incrementaloptimization updates all linearization points but the ones of the separator
988  // In this case, we start with no separator (everything is in the filter), therefore,
989  // we update all linearization point
990  partialValues.insert(2, newValues.at(2)); //<-- does not actually exist
991  //The linearization point of 1 is controlled by the smoother and
992  // we are artificially setting that to something different to what was in the filter
993  partialValues.insert(1, Pose3().compose(poseError.inverse()));
994 
995  FastList<Key> keysToMarginalize;
996  keysToMarginalize.push_back(1);
997 
998  smootherSummarization2 = CalculateMarginals(partialGraph, partialValues, keysToMarginalize);
999  smootherSeparatorValues2.insert(2, partialValues.at(2));
1000 
1001  // ------------------------------------------------------------------------------
1002  // Synchronize
1003  // This is only an information compression/exchange: to actually incorporate the info we should call update
1004  NonlinearFactorGraph actualSmootherFactors2, actualFilterSummarization2;
1005  Values actualSmootherValues2, actualFilterSeparatorValues2;
1006  filter.presync();
1007  filter.synchronize(smootherSummarization2, smootherSeparatorValues2);
1008  filter.getSmootherFactors(actualSmootherFactors2, actualSmootherValues2);
1009  filter.getSummarizedFactors(actualFilterSummarization2, actualFilterSeparatorValues2);
1010  filter.postsync();
1011 
1012  NonlinearFactorGraph expectedSmootherFactors2;
1013  expectedSmootherFactors2.push_back(factor3);
1014  expectedSmootherFactors2.push_back(factor4);
1015 
1016  Values expectedSmootherValues2;
1017  expectedSmootherValues2.insert(2, newValues.at(2));
1018 
1019  // Check
1020  CHECK(assert_equal(expectedSmootherFactors2, actualSmootherFactors2, 1e-6));
1021  CHECK(assert_equal(expectedSmootherValues2, actualSmootherValues2, 1e-6));
1022 
1023 
1024  // In this example the smoother contains a between factor and a prior factor
1025  // COMPUTE SUMMARIZATION ON THE FILTER SIDE
1026  // ------------------------------------------------------------------------------
1027  // This cannot be nonempty for the first call to synchronize
1028  NonlinearFactorGraph partialGraphFilter;
1029  partialGraphFilter.push_back(factor5);
1030 
1031 
1032  Values partialValuesFilter;
1033  partialValuesFilter.insert(3, optimalValues.at(3));
1034  partialValuesFilter.insert(4, optimalValues.at(4));
1035 
1036  FastList<Key> keysToMarginalize2;
1037  keysToMarginalize2.push_back(4);
1038 
1039  NonlinearFactorGraph expectedFilterSummarization2 = CalculateMarginals(partialGraphFilter, partialValuesFilter, keysToMarginalize2);
1040  Values expectedFilterSeparatorValues2;
1041  expectedFilterSeparatorValues2.insert(3, optimalValues.at(3));
1042 
1043  CHECK(assert_equal(expectedFilterSeparatorValues2, actualFilterSeparatorValues2, 1e-6));
1044  CHECK(assert_equal(expectedFilterSummarization2, actualFilterSummarization2, 1e-6));
1045 
1046 
1047  // Now we should check that the smooother summarization on the old separator is correctly propagated
1048  // on the new separator by the filter
1049  NonlinearFactorGraph partialGraphTransition;
1050  partialGraphTransition.push_back(factor3);
1051  partialGraphTransition.push_back(factor4);
1052  partialGraphTransition.push_back(smootherSummarization2);
1053 
1054  Values partialValuesTransition;
1055  partialValuesTransition.insert(2,newValues.at(2));
1056  partialValuesTransition.insert(3,optimalValues.at(3));
1057 
1058  FastList<Key> keysToMarginalize3;
1059  keysToMarginalize3.push_back(2);
1060 
1061  NonlinearFactorGraph expectedFilterGraph;
1062 
1063  // The assert equal will check if the expected and the actual graphs are the same, taking into account
1064  // orders of the factors, and empty factors:
1065  // in the filter we originally had 5 factors, and by marginalizing 1 and 2 we got rid of factors 1 2 3 4,
1066  // therefore in the expected Factor we should include 4 empty factors.
1067  // Note that the unit test will fail also if we change the order of the factors, due to the definition of assert_equal
1068  NonlinearFactor::shared_ptr factorEmpty;
1069  expectedFilterGraph.push_back(factorEmpty);
1070  expectedFilterGraph.push_back(factorEmpty);
1071  expectedFilterGraph.push_back(factorEmpty);
1072  expectedFilterGraph.push_back(factorEmpty);
1073  expectedFilterGraph.push_back(factor5);
1074  expectedFilterGraph.push_back(factorEmpty);
1075  expectedFilterGraph.push_back(factorEmpty);
1076  expectedFilterGraph.push_back(factorEmpty);
1077  expectedFilterGraph.push_back(CalculateMarginals(partialGraphTransition, partialValuesTransition, keysToMarginalize3));
1078 
1079  NonlinearFactorGraph actualFilterGraph;
1080  actualFilterGraph = filter.getFactors();
1081 
1082  CHECK(assert_equal(expectedFilterGraph, actualFilterGraph, 1e-6));
1083 }
1084 
1085 
1087 TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
1088 {
1089  // We compare the manual computation of the linear marginals from a factor graph, with the function CalculateMarginals
1090  NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
1091  NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1092  NonlinearFactor::shared_ptr factor3(new PriorFactor<Pose3>(2, poseInitial, noisePrior));
1093  NonlinearFactor::shared_ptr factor4(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
1094 
1095  NonlinearFactorGraph factorGraph;
1096  factorGraph.push_back(factor1);
1097  factorGraph.push_back(factor2);
1098  factorGraph.push_back(factor3);
1099  factorGraph.push_back(factor4);
1100 
1101  Values newValues;
1102  Pose3 value1 = Pose3().compose(poseError);
1103  Pose3 value2 = value1.compose(poseOdometry).compose(poseError);
1104  Pose3 value3 = value2.compose(poseOdometry).compose(poseError);
1105 
1106  newValues.insert(1, value1);
1107  newValues.insert(2, value2);
1108  newValues.insert(3, value3);
1109 
1110  // Create the set of marginalizable variables
1111  GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
1112 
1113  KeyVector linearIndices {1};
1114  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
1115 
1116  NonlinearFactorGraph expectedMarginals;
1117  for(const GaussianFactor::shared_ptr& factor: marginal) {
1118  expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
1119  }
1120 
1121  FastList<Key> keysToMarginalize;
1122  keysToMarginalize.push_back(1);
1123  NonlinearFactorGraph actualMarginals;
1124  actualMarginals = CalculateMarginals(factorGraph, newValues, keysToMarginalize);
1125 
1126  // Check
1127  CHECK(assert_equal(expectedMarginals, actualMarginals, 1e-6));
1128 // actualMarginals.print("actualMarginals \n");
1129 // expectedMarginals.print("expectedMarginals \n");
1130 }
1131 
1133 TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
1134 {
1135  // We compare the manual computation of the linear marginals from a factor graph, with the function CalculateMarginals
1136  NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
1137  NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1138  NonlinearFactor::shared_ptr factor3(new PriorFactor<Pose3>(2, poseInitial, noisePrior));
1139  NonlinearFactor::shared_ptr factor4(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
1140 
1141  NonlinearFactorGraph factorGraph;
1142  factorGraph.push_back(factor1);
1143  factorGraph.push_back(factor2);
1144  factorGraph.push_back(factor3);
1145  factorGraph.push_back(factor4);
1146 
1147  Values newValues;
1148  Pose3 value1 = Pose3().compose(poseError);
1149  Pose3 value2 = value1.compose(poseOdometry).compose(poseError);
1150  Pose3 value3 = value2.compose(poseOdometry).compose(poseError);
1151 
1152  newValues.insert(1, value1);
1153  newValues.insert(2, value2);
1154  newValues.insert(3, value3);
1155 
1156 
1157  // Create the set of marginalizable variables
1158  KeyVector linearIndices {1, 2};
1159  GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
1160  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
1161 
1162  NonlinearFactorGraph expectedMarginals;
1163  for(const GaussianFactor::shared_ptr& factor: marginal) {
1164  expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
1165  }
1166 
1167 
1168  FastList<Key> keysToMarginalize;
1169  keysToMarginalize.push_back(1);
1170  keysToMarginalize.push_back(2);
1171  NonlinearFactorGraph actualMarginals;
1172  actualMarginals = CalculateMarginals(factorGraph, newValues, keysToMarginalize);
1173 
1174  // Check
1175  CHECK(assert_equal(expectedMarginals, actualMarginals, 1e-6));
1176 }
1177 
1178 
1179 
1181 TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
1182 {
1183  // Create a set of optimizer parameters
1185  parameters.relinearizeThreshold = 0.;
1186  // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the
1187  // default value for that is 10 (if you set that to zero the code will crash)
1188  parameters.relinearizeSkip = 1;
1189 
1190  // Create a Concurrent IncrementalFilter
1191  ConcurrentIncrementalFilter filter(parameters);
1192 
1193  // Add some factors to the filter
1194  NonlinearFactorGraph newFactors;
1195  newFactors.addPrior(1, poseInitial, noisePrior);
1196  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1197  newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
1198  newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
1199  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1200 
1201  Values newValues;
1202  newValues.insert(1, Pose3().compose(poseError));
1203  newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
1204  newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
1205  newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
1206 
1207  // Specify a subset of variables to marginalize/move to the smoother
1208  FastList<Key> keysToMove;
1209 
1210  // Update the filter: add all factors
1211  filter.update(newFactors, newValues, keysToMove);
1212 
1213  // factor we want to remove
1214  // NOTE: we can remove factors, paying attention that the remaining graph remains connected
1215  // we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery)
1216  FactorIndices removeFactorIndices;
1217  removeFactorIndices.push_back(1);
1218 
1219  // Add no factors to the filter (we only want to test the removal)
1220  NonlinearFactorGraph noFactors;
1221  Values noValues;
1222  filter.update(noFactors, noValues, keysToMove, removeFactorIndices);
1223 
1224  NonlinearFactorGraph actualGraph = filter.getFactors();
1225 
1227  expectedGraph.addPrior(1, poseInitial, noisePrior);
1228  // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery))
1229  // we should add an empty one, so that the ordering and labeling of the factors is preserved
1230  expectedGraph.push_back(NonlinearFactor::shared_ptr());
1231  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
1232  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
1233  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
1234 
1235  CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
1236 }
1237 
1239 TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 )
1240 {
1241  // we try removing the last factor
1242 
1244  parameters.relinearizeThreshold = 0.;
1245  // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the
1246  // default value for that is 10 (if you set that to zero the code will crash)
1247  parameters.relinearizeSkip = 1;
1248 
1249  // Create a Concurrent IncrementalFilter
1250  ConcurrentIncrementalFilter filter(parameters);
1251 
1252  // Add some factors to the filter
1253  NonlinearFactorGraph newFactors;
1254  newFactors.addPrior(1, poseInitial, noisePrior);
1255  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1256  newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
1257  newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
1258  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1259 
1260  Values newValues;
1261  newValues.insert(1, Pose3().compose(poseError));
1262  newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
1263  newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
1264  newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
1265 
1266  // Specify a subset of variables to marginalize/move to the smoother
1267  FastList<Key> keysToMove;
1268 
1269  // Update the filter: add all factors
1270  filter.update(newFactors, newValues, keysToMove);
1271 
1272  // factor we want to remove
1273  // NOTE: we can remove factors, paying attention that the remaining graph remains connected
1274  // we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
1275  FactorIndices removeFactorIndices(1,4);
1276 
1277  // Add no factors to the filter (we only want to test the removal)
1278  NonlinearFactorGraph noFactors;
1279  Values noValues;
1280  filter.update(noFactors, noValues, keysToMove, removeFactorIndices);
1281 
1282  NonlinearFactorGraph actualGraph = filter.getFactors();
1283 
1285  expectedGraph.addPrior(1, poseInitial, noisePrior);
1286  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
1287  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
1288  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
1289  // we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
1290  // we should add an empty one, so that the ordering and labeling of the factors is preserved
1291  expectedGraph.push_back(NonlinearFactor::shared_ptr());
1292 
1293  CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
1294 }
1295 
1296 
1298 TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 )
1299 {
1300  // we try removing the first factor
1301 
1303  parameters.relinearizeThreshold = 0.;
1304  // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the
1305  // default value for that is 10 (if you set that to zero the code will crash)
1306  parameters.relinearizeSkip = 1;
1307 
1308  // Create a Concurrent IncrementalFilter
1309  ConcurrentIncrementalFilter filter(parameters);
1310 
1311  // Add some factors to the filter
1312  NonlinearFactorGraph newFactors;
1313  newFactors.addPrior(1, poseInitial, noisePrior);
1314  newFactors.addPrior(1, poseInitial, noisePrior);
1315  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1316  newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
1317  newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
1318 
1319  Values newValues;
1320  newValues.insert(1, Pose3().compose(poseError));
1321  newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
1322  newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
1323  newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
1324 
1325  // Specify a subset of variables to marginalize/move to the smoother
1326  FastList<Key> keysToMove;
1327 
1328  // Update the filter: add all factors
1329  filter.update(newFactors, newValues, keysToMove);
1330 
1331  // factor we want to remove
1332  // NOTE: we can remove factors, paying attention that the remaining graph remains connected
1333  // we remove a single factor, the number 0, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
1334  FactorIndices removeFactorIndices(1,0);
1335 
1336  // Add no factors to the filter (we only want to test the removal)
1337  NonlinearFactorGraph noFactors;
1338  Values noValues;
1339  filter.update(noFactors, noValues, keysToMove, removeFactorIndices);
1340 
1341  NonlinearFactorGraph actualGraph = filter.getFactors();
1342 
1344  // we should add an empty one, so that the ordering and labeling of the factors is preserved
1345  expectedGraph.push_back(NonlinearFactor::shared_ptr());
1346  expectedGraph.addPrior(1, poseInitial, noisePrior);
1347  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
1348  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
1349  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
1350 
1351  CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
1352 }
1353 
1355 TEST( ConcurrentIncrementalFilter, removeFactors_values )
1356 {
1357  // we try removing the last factor
1358 
1360  parameters.relinearizeThreshold = 0.;
1361  // ISAM2 checks whether to relinearize or not a variable only every relinearizeSkip steps and the
1362  // default value for that is 10 (if you set that to zero the code will crash)
1363  parameters.relinearizeSkip = 1;
1364 
1365  // Create a Concurrent IncrementalFilter
1366  ConcurrentIncrementalFilter filter(parameters);
1367 
1368  // Add some factors to the filter
1369  NonlinearFactorGraph newFactors;
1370  newFactors.addPrior(1, poseInitial, noisePrior);
1371  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1372  newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
1373  newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
1374  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1375 
1376  Values newValues;
1377  newValues.insert(1, Pose3().compose(poseError));
1378  newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
1379  newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
1380  newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
1381 
1382  // Specify a subset of variables to marginalize/move to the smoother
1383  FastList<Key> keysToMove;
1384 
1385  // Update the filter: add all factors
1386  filter.update(newFactors, newValues, keysToMove);
1387 
1388  // factor we want to remove
1389  // NOTE: we can remove factors, paying attention that the remaining graph remains connected
1390  // we remove a single factor, the number 4, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
1391  FactorIndices removeFactorIndices(1,4);
1392 
1393  // Add no factors to the filter (we only want to test the removal)
1394  NonlinearFactorGraph noFactors;
1395  Values noValues;
1396  filter.update(noFactors, noValues, keysToMove, removeFactorIndices);
1397 
1398  NonlinearFactorGraph actualGraph = filter.getFactors();
1399  Values actualValues = filter.getLinearizationPoint();
1400 
1402  expectedGraph.addPrior(1, poseInitial, noisePrior);
1403  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
1404  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
1405  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
1406  // we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
1407  // we should add an empty one, so that the ordering and labeling of the factors is preserved
1408  expectedGraph.push_back(NonlinearFactor::shared_ptr());
1409 
1410  // Calculate expected factor graph and values
1411  Values expectedValues = BatchOptimize(expectedGraph, newValues);
1412 
1413  CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
1414  CHECK(assert_equal(expectedValues, actualValues, 1e-6));
1415 }
1416 
1417 
1418 
1419 /* ************************************************************************* */
1421 /* ************************************************************************* */
const gtsam::Symbol key('X', 0)
Provides additional testing facilities for common data structures.
#define CHECK(condition)
Definition: Test.h:108
std::pair< std::shared_ptr< BayesTreeType >, std::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< FastList< Key > > &keysToMove={}, const std::optional< FactorIndices > &removeFactorIndices={})
A non-templated config holding any types of Manifold-group elements.
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph...
Factor Graph consisting of non-linear factors.
TEST(ConcurrentIncrementalFilter, equals)
const ValueType at(Key j) const
Definition: Values-inl.h:204
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
KeyVector keys() const
Definition: Values.cpp:218
Definition: BFloat16.h:88
NonlinearFactorGraph graph
void getSmootherFactors(NonlinearFactorGraph &smootherFactors, Values &smootherValues) override
static enum @1107 ordering
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
void getSummarizedFactors(NonlinearFactorGraph &filterSummarization, Values &filterSummarizationValues) override
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
static NonlinearFactorGraph expectedGraph(const SharedNoiseModel &model)
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:36
Values result
const NonlinearFactorGraph & getFactors() const
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
OptimizationParams optimizationParams
Definition: ISAM2Params.h:151
Class compose(const Class &g) const
Definition: Lie.h:48
static ConjugateGradientParameters parameters
void synchronize(const NonlinearFactorGraph &smootherSummarization, const Values &smootherSummarizationValues) override
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
RelinearizationThreshold relinearizeThreshold
Definition: ISAM2Params.h:169
traits
Definition: chartTesting.h:28
std::vector< float > Values
The junction tree.
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Vector3 Point3
Definition: Point3.h:38
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
void erase(Key j)
Definition: Values.cpp:210
3D Pose
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
iterator erase(iterator item)
Definition: FactorGraph.h:399
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:59