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


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