testConcurrentIncrementalSmootherGN.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 
44 // Set up noise models for the factors
45 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
46 const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished());
47 const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished());
48 
49 /* ************************************************************************* */
50 Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) {
51 
52  // Create an L-M optimizer
55 // parameters.maxIterations = maxIter;
56 // parameters.lambdaUpperBound = 1;
57 // parameters.lambdaInitial = 1;
58 // parameters.verbosity = NonlinearOptimizerParams::ERROR;
59 // parameters.verbosityLM = ISAM2Params::DAMPED;
60 // parameters.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_QR;
61 
62  ISAM2 optimizer(parameters);
63  optimizer.update( graph, theta );
64  Values result = optimizer.calculateEstimate();
65  return result;
66 
67 }
68 
69 } // end namespace
70 
71 
72 
73 
74 /* ************************************************************************* */
75 TEST( ConcurrentIncrementalSmootherGN, equals )
76 {
77  // TODO: Test 'equals' more vigorously
78 
79  // Create a Concurrent Batch Smoother
80  ISAM2Params parameters1;
82  ConcurrentIncrementalSmoother smoother1(parameters1);
83 
84  // Create an identical Concurrent Batch Smoother
85  ISAM2Params parameters2;
87  ConcurrentIncrementalSmoother smoother2(parameters2);
88 
89  // Create a different Concurrent Batch Smoother
90  ISAM2Params parameters3;
92 // parameters3.maxIterations = 1;
93  ConcurrentIncrementalSmoother smoother3(parameters3);
94 
95  CHECK(assert_equal(smoother1, smoother1));
96  CHECK(assert_equal(smoother1, smoother2));
97 // CHECK(assert_inequal(smoother1, smoother3));
98 }
99 
100 /* ************************************************************************* */
101 TEST( ConcurrentIncrementalSmootherGN, getFactors )
102 {
103  // Create a Concurrent Batch Smoother
106  ConcurrentIncrementalSmoother smoother(parameters);
107 
108  // Expected graph is empty
109  NonlinearFactorGraph expected1;
110  // Get actual graph
111  NonlinearFactorGraph actual1 = smoother.getFactors();
112  // Check
113  CHECK(assert_equal(expected1, actual1));
114 
115  // Add some factors to the smoother
116  NonlinearFactorGraph newFactors1;
117  newFactors1.addPrior(1, poseInitial, noisePrior);
118  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
119  Values newValues1;
120  newValues1.insert(1, Pose3());
121  newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
122  smoother.update(newFactors1, newValues1);
123 
124  // Expected graph
125  NonlinearFactorGraph expected2;
126  expected2.push_back(newFactors1);
127  // Get actual graph
128  NonlinearFactorGraph actual2 = smoother.getFactors();
129  // Check
130  CHECK(assert_equal(expected2, actual2));
131 
132  // Add some more factors to the smoother
133  NonlinearFactorGraph newFactors2;
134  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
135  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
136  Values newValues2;
137  newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
138  newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
139  smoother.update(newFactors2, newValues2);
140 
141  // Expected graph
142  NonlinearFactorGraph expected3;
143  expected3.push_back(newFactors1);
144  expected3.push_back(newFactors2);
145  // Get actual graph
146  NonlinearFactorGraph actual3 = smoother.getFactors();
147  // Check
148  CHECK(assert_equal(expected3, actual3));
149 }
150 
151 /* ************************************************************************* */
152 TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
153 {
154  // Create a Concurrent Batch Smoother
157  ConcurrentIncrementalSmoother smoother(parameters);
158 
159  // Expected values is empty
160  Values expected1;
161  // Get Linearization Point
162  Values actual1 = smoother.getLinearizationPoint();
163  // Check
164  CHECK(assert_equal(expected1, actual1));
165 
166  // Add some factors to the smoother
167  NonlinearFactorGraph newFactors1;
168  newFactors1.addPrior(1, poseInitial, noisePrior);
169  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
170  Values newValues1;
171  newValues1.insert(1, Pose3());
172  newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
173  smoother.update(newFactors1, newValues1);
174 
175  // Expected values is equivalent to the provided values only because the provided linearization points were optimal
176  Values expected2;
177  expected2.insert(newValues1);
178  // Get actual linearization point
179  Values actual2 = smoother.getLinearizationPoint();
180  // Check
181  CHECK(assert_equal(expected2, actual2));
182 
183  // Add some more factors to the smoother
184  NonlinearFactorGraph newFactors2;
185  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
186  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
187  Values newValues2;
188  newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
189  newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
190  smoother.update(newFactors2, newValues2);
191 
192  // Expected values is equivalent to the provided values only because the provided linearization points were optimal
193  Values expected3;
194  expected3.insert(newValues1);
195  expected3.insert(newValues2);
196  // Get actual linearization point
197  Values actual3 = smoother.getLinearizationPoint();
198  // Check
199  CHECK(assert_equal(expected3, actual3));
200 }
201 
202 /* ************************************************************************* */
203 TEST( ConcurrentIncrementalSmootherGN, getDelta )
204 {
205  // TODO: Think about how to check ordering...
206 }
207 
208 /* ************************************************************************* */
209 TEST( ConcurrentIncrementalSmootherGN, calculateEstimate )
210 {
211  // Create a Concurrent Batch Smoother
214  ConcurrentIncrementalSmoother smoother(parameters);
215 
216  // Expected values is empty
217  Values expected1;
218  // Get Linearization Point
219  Values actual1 = smoother.calculateEstimate();
220  // Check
221  CHECK(assert_equal(expected1, actual1));
222 
223  // Add some factors to the smoother
224  NonlinearFactorGraph newFactors2;
225  newFactors2.addPrior(1, poseInitial, noisePrior);
226  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
227  Values newValues2;
228  newValues2.insert(1, Pose3().compose(poseError));
229  newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
230  smoother.update(newFactors2, newValues2);
231 
232  // Expected values from full batch optimization
233  NonlinearFactorGraph allFactors2;
234  allFactors2.push_back(newFactors2);
235  Values allValues2;
236  allValues2.insert(newValues2);
237  Values expected2 = BatchOptimize(allFactors2, allValues2);
238  // Get actual linearization point
239  Values actual2 = smoother.calculateEstimate();
240  // Check
241  CHECK(assert_equal(expected2, actual2, 1e-6));
242 
243  // Add some more factors to the smoother
244  NonlinearFactorGraph newFactors3;
245  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
246  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
247  Values newValues3;
248  newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
249  newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
250  smoother.update(newFactors3, newValues3);
251 
252  // Expected values from full batch optimization
253  NonlinearFactorGraph allFactors3;
254  allFactors3.push_back(newFactors2);
255  allFactors3.push_back(newFactors3);
256  Values allValues3;
257  allValues3.insert(newValues2);
258  allValues3.insert(newValues3);
259  Values expected3 = BatchOptimize(allFactors3, allValues3);
260  // Get actual linearization point
261  Values actual3 = smoother.calculateEstimate();
262  // Check
263  CHECK(assert_equal(expected3, actual3, 1e-6));
264 
265  // Also check the single-variable version
266  Pose3 expectedPose1 = expected3.at<Pose3>(1);
267  Pose3 expectedPose2 = expected3.at<Pose3>(2);
268  Pose3 expectedPose3 = expected3.at<Pose3>(3);
269  Pose3 expectedPose4 = expected3.at<Pose3>(4);
270  // Also check the single-variable version
271  Pose3 actualPose1 = smoother.calculateEstimate<Pose3>(1);
272  Pose3 actualPose2 = smoother.calculateEstimate<Pose3>(2);
273  Pose3 actualPose3 = smoother.calculateEstimate<Pose3>(3);
274  Pose3 actualPose4 = smoother.calculateEstimate<Pose3>(4);
275  // Check
276  CHECK(assert_equal(expectedPose1, actualPose1, 1e-6));
277  CHECK(assert_equal(expectedPose2, actualPose2, 1e-6));
278  CHECK(assert_equal(expectedPose3, actualPose3, 1e-6));
279  CHECK(assert_equal(expectedPose4, actualPose4, 1e-6));
280 }
281 
282 /* ************************************************************************* */
283 TEST( ConcurrentIncrementalSmootherGN, update_empty )
284 {
285  // Create a set of optimizer parameters
288  // Create a Concurrent Batch Smoother
289  ConcurrentIncrementalSmoother smoother(parameters);
290 
291  // Call update
292  smoother.update();
293 }
294 
295 /* ************************************************************************* */
296 TEST( ConcurrentIncrementalSmootherGN, update_multiple )
297 {
298  // Create a Concurrent Batch Smoother
301  ConcurrentIncrementalSmoother smoother(parameters);
302 
303  // Expected values is empty
304  Values expected1;
305  // Get Linearization Point
306  Values actual1 = smoother.calculateEstimate();
307  // Check
308  CHECK(assert_equal(expected1, actual1));
309 
310  // Add some factors to the smoother
311  NonlinearFactorGraph newFactors2;
312  newFactors2.addPrior(1, poseInitial, noisePrior);
313  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
314  Values newValues2;
315  newValues2.insert(1, Pose3().compose(poseError));
316  newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
317  smoother.update(newFactors2, newValues2);
318 
319  // Expected values from full batch optimization
320  NonlinearFactorGraph allFactors2;
321  allFactors2.push_back(newFactors2);
322  Values allValues2;
323  allValues2.insert(newValues2);
324  Values expected2 = BatchOptimize(allFactors2, allValues2);
325  // Get actual linearization point
326  Values actual2 = smoother.calculateEstimate();
327  // Check
328  CHECK(assert_equal(expected2, actual2, 1e-6));
329 
330  // Add some more factors to the smoother
331  NonlinearFactorGraph newFactors3;
332  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
333  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
334  Values newValues3;
335  newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
336  newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
337  smoother.update(newFactors3, newValues3);
338 
339  // Expected values from full batch optimization
340  NonlinearFactorGraph allFactors3;
341  allFactors3.push_back(newFactors2);
342  allFactors3.push_back(newFactors3);
343  Values allValues3;
344  allValues3.insert(newValues2);
345  allValues3.insert(newValues3);
346  Values expected3 = BatchOptimize(allFactors3, allValues3);
347  // Get actual linearization point
348  Values actual3 = smoother.calculateEstimate();
349  // Check
350  CHECK(assert_equal(expected3, actual3, 1e-6));
351 }
352 
353 /* ************************************************************************* */
354 TEST( ConcurrentIncrementalSmootherGN, synchronize_empty )
355 {
356  // Create a set of optimizer parameters
359  // Create a Concurrent Batch Smoother
360  ConcurrentIncrementalSmoother smoother(parameters);
361 
362  // Create empty containers *from* the filter
363  NonlinearFactorGraph smootherFactors, filterSumarization;
364  Values smootherValues, filterSeparatorValues;
365 
366  // Create expected values: these will be empty for this case
367  NonlinearFactorGraph expectedSmootherSummarization;
368  Values expectedSmootherSeparatorValues;
369 
370  // Synchronize
371  NonlinearFactorGraph actualSmootherSummarization;
372  Values actualSmootherSeparatorValues;
373  smoother.presync();
374  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
375  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
376  smoother.postsync();
377 
378  // Check
379  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
380  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
381 }
382 
383 /* ************************************************************************* */
384 TEST( ConcurrentIncrementalSmootherGN, synchronize_1 )
385 {
386  // Create a set of optimizer parameters
389 // parameters.maxIterations = 1;
390 
391  // Create a Concurrent Batch Smoother
392  ConcurrentIncrementalSmoother smoother(parameters);
393 
394  // Create a simple separator *from* the filter
395  NonlinearFactorGraph smootherFactors, filterSumarization;
396  Values smootherValues, filterSeparatorValues;
397  filterSeparatorValues.insert(1, Pose3().compose(poseError));
398  filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
400  ordering.push_back(1);
401  ordering.push_back(2);
402  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
403  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
404 
405  // Create expected values: the smoother output will be empty for this case
406  NonlinearFactorGraph expectedSmootherSummarization;
407  Values expectedSmootherSeparatorValues;
408 
409  NonlinearFactorGraph actualSmootherSummarization;
410  Values actualSmootherSeparatorValues;
411  smoother.presync();
412  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
413  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
414  smoother.postsync();
415 
416  // Check
417  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
418  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
419 
420 
421  // Update the smoother
422  smoother.update();
423 
424  // Check the factor graph. It should contain only the filter-provided factors
425  NonlinearFactorGraph expectedFactorGraph = filterSumarization;
426  NonlinearFactorGraph actualFactorGraph = smoother.getFactors();
427  CHECK(assert_equal(expectedFactorGraph, actualFactorGraph, 1e-6));
428 
429  // Check the optimized value of smoother state
430  NonlinearFactorGraph allFactors;
431  allFactors.push_back(filterSumarization);
432  Values allValues;
433  allValues.insert(filterSeparatorValues);
434  Values expectedValues = BatchOptimize(allFactors, allValues,1);
435  Values actualValues = smoother.calculateEstimate();
436  CHECK(assert_equal(expectedValues, actualValues, 1e-6));
437 
438  // Check the linearization point. The separator should remain identical to the filter provided values
439  Values expectedLinearizationPoint = filterSeparatorValues;
440  Values actualLinearizationPoint = smoother.getLinearizationPoint();
441  CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
442 }
443 
444 
445 /* ************************************************************************* */
446 TEST( ConcurrentIncrementalSmootherGN, synchronize_2 )
447 {
448  // Create a set of optimizer parameters
451 // parameters.maxIterations = 1;
452 // parameters.lambdaUpperBound = 1;
453 // parameters.lambdaInitial = 1;
454 // parameters.verbosity = NonlinearOptimizerParams::ERROR;
455 // parameters.verbosityLM = ISAM2Params::DAMPED;
456 
457  // Create a Concurrent Batch Smoother
458  ConcurrentIncrementalSmoother smoother(parameters);
459 
460  // Create a separator and cached smoother factors *from* the filter
461  NonlinearFactorGraph smootherFactors, filterSumarization;
462  Values smootherValues, filterSeparatorValues;
463 
464  filterSeparatorValues.insert(1, Pose3().compose(poseError));
465  filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
466  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
467  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
468  smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
469  smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
470  smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
471  smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
472 
473  // Create expected values: the smoother output will be empty for this case
474  NonlinearFactorGraph expectedSmootherSummarization;
475  Values expectedSmootherSeparatorValues;
476 
477  NonlinearFactorGraph actualSmootherSummarization;
478  Values actualSmootherSeparatorValues;
479  smoother.presync();
480  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
481  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
482  smoother.postsync();
483 
484  // Check
485  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
486  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
487 
488 
489  // Update the smoother
490  smoother.update();
491 
492  // Check the factor graph. It should contain only the filter-provided factors
493  NonlinearFactorGraph expectedFactorGraph;
494  expectedFactorGraph.push_back(smootherFactors);
495  expectedFactorGraph.push_back(filterSumarization);
496  NonlinearFactorGraph actualFactorGraph = smoother.getFactors();
497  CHECK(assert_equal(expectedFactorGraph, actualFactorGraph, 1e-6));
498 
499  // Check the optimized value of smoother state
500  NonlinearFactorGraph allFactors;
501  allFactors.push_back(filterSumarization);
502  allFactors.push_back(smootherFactors);
503  Values allValues;
504  allValues.insert(filterSeparatorValues);
505  allValues.insert(smootherValues);
506 // allValues.print("Batch LinPoint:\n");
507  Values expectedValues = BatchOptimize(allFactors, allValues, 1);
508  Values actualValues = smoother.calculateEstimate();
509  CHECK(assert_equal(expectedValues, actualValues, 1e-6));
510 
511  // Check the linearization point. The separator should remain identical to the filter provided values, but the others should be the optimal values
512  // Isam2 is changing internally the linearization points, so the following check is done only on the separator variables
513 // Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
514  Values expectedLinearizationPoint = filterSeparatorValues;
515  Values actualLinearizationPoint;
516  for(const auto key: filterSeparatorValues.keys()) {
517  actualLinearizationPoint.insert(key, smoother.getLinearizationPoint().at(key));
518  }
519  CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
520 }
521 
522 
523 
524 /* ************************************************************************* */
525 TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
526 {
527  // Create a set of optimizer parameters
530 // parameters.maxIterations = 1;
531 // parameters.lambdaUpperBound = 1;
532 // parameters.lambdaInitial = 1;
533 // parameters.verbosity = NonlinearOptimizerParams::ERROR;
534 // parameters.verbosityLM = ISAM2Params::DAMPED;
535 
536  // Create a Concurrent Batch Smoother
537  ConcurrentIncrementalSmoother smoother(parameters);
538 
539  // Create a separator and cached smoother factors *from* the filter
540  NonlinearFactorGraph smootherFactors, filterSumarization;
541  Values smootherValues, filterSeparatorValues;
542 
543  filterSeparatorValues.insert(1, Pose3().compose(poseError));
544  filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
545  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
546  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
547  smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
548  smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
549  smootherFactors.addPrior(4, poseInitial, noisePrior);
550  smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
551  smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
552 
553  // Create expected values: the smoother output will be empty for this case
554  NonlinearFactorGraph expectedSmootherSummarization;
555  Values expectedSmootherSeparatorValues;
556 
557  NonlinearFactorGraph actualSmootherSummarization;
558  Values actualSmootherSeparatorValues;
559  smoother.presync();
560  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
561  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
562  smoother.postsync();
563 
564  // Check
565  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
566  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
567 
568 
569  // Update the smoother
570  smoother.update();
571 
572  smoother.presync();
573  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
574 
575  // Check the optimized value of smoother state
576  NonlinearFactorGraph allFactors = smootherFactors;
577  Values allValues = smoother.getLinearizationPoint();
578 // ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering...
579 
580  GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(allValues);
581 // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
582 // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
583 
584  KeySet allkeys = LinFactorGraph->keys();
585  for (const auto key : filterSeparatorValues.keys()) allkeys.erase(key);
586  KeyVector variables(allkeys.begin(), allkeys.end());
587  const auto [bn, fg] = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
588 
589  expectedSmootherSummarization.resize(0);
590  for(const GaussianFactor::shared_ptr& factor: *fg) {
591  expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues));
592  }
593 
594  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
595 
596 }
597 
598 // =========================================================================================================
600 TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
601 {
602  // Create a set of optimizer parameters
605 
606  // Create a Concurrent Batch Smoother
607  ConcurrentIncrementalSmoother smoother(parameters);
608 
609  // Add some factors to the smoother
610  NonlinearFactorGraph newFactors;
611  newFactors.addPrior(1, poseInitial, noisePrior);
612  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
613  newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
614  newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
615  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
616 
617  Values newValues;
618  newValues.insert(1, Pose3().compose(poseError));
619  newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
620  newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
621  newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
622 
623  // Update the smoother: add all factors
624  smoother.update(newFactors, newValues);
625 
626  // factor we want to remove
627  // NOTE: we can remove factors, paying attention that the remaining graph remains connected
628  // we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
629  FactorIndices removeFactorIndices(2,1);
630 
631  // Add no factors to the smoother (we only want to test the removal)
632  NonlinearFactorGraph noFactors;
633  Values noValues;
634  smoother.update(noFactors, noValues, removeFactorIndices);
635 
636  NonlinearFactorGraph actualGraph = smoother.getFactors();
637 
638  actualGraph.print("actual graph: \n");
639 
641  expectedGraph.addPrior(1, poseInitial, noisePrior);
642  // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
643  // we should add an empty one, so that the ordering and labeling of the factors is preserved
644  expectedGraph.push_back(NonlinearFactor::shared_ptr());
645  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
646  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
647  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
648 
649 // CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
650 }
651 
653 //TEST( ConcurrentIncrementalSmoother, removeFactors_topology_2 )
654 //{
655 // // we try removing the last factor
656 //
657 // // Create a set of optimizer parameters
658 // LevenbergMarquardtParams parameters;
659 //
660 // // Create a Concurrent Batch Smoother
661 // ConcurrentBatchSmoother smoother(parameters);
662 //
663 // // Add some factors to the smoother
664 // NonlinearFactorGraph newFactors;
665 // newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
666 // newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
667 // newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
668 // newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
669 // newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
670 //
671 // Values newValues;
672 // newValues.insert(1, Pose3().compose(poseError));
673 // newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
674 // newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
675 // newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
676 //
677 // // Update the smoother: add all factors
678 // smoother.update(newFactors, newValues);
679 //
680 // // factor we want to remove
681 // // NOTE: we can remove factors, paying attention that the remaining graph remains connected
682 // // we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
683 // std::vector<size_t> removeFactorIndices(1,4);
684 //
685 // // Add no factors to the smoother (we only want to test the removal)
686 // NonlinearFactorGraph noFactors;
687 // Values noValues;
688 // smoother.update(noFactors, noValues, removeFactorIndices);
689 //
690 // NonlinearFactorGraph actualGraph = smoother.getFactors();
691 //
692 // NonlinearFactorGraph expectedGraph;
693 // expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
694 // expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
695 // expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
696 // expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
697 // // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
698 // // we should add an empty one, so that the ordering and labeling of the factors is preserved
699 // expectedGraph.push_back(NonlinearFactor::shared_ptr());
700 //
701 // CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
702 //}
703 //
704 //
706 //TEST( ConcurrentBatchSmoother, removeFactors_topology_3 )
707 //{
708 // // we try removing the first factor
709 //
710 // // Create a set of optimizer parameters
711 // LevenbergMarquardtParams parameters;
712 // ConcurrentBatchSmoother Smoother(parameters);
713 //
714 // // Add some factors to the Smoother
715 // NonlinearFactorGraph newFactors;
716 // newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
717 // newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
718 // newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
719 // newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
720 // newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
721 //
722 // Values newValues;
723 // newValues.insert(1, Pose3().compose(poseError));
724 // newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
725 // newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
726 // newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
727 //
728 // // Update the Smoother: add all factors
729 // Smoother.update(newFactors, newValues);
730 //
731 // // factor we want to remove
732 // // NOTE: we can remove factors, paying attention that the remaining graph remains connected
733 // // we remove a single factor, the number 0, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
734 // std::vector<size_t> removeFactorIndices(1,0);
735 //
736 // // Add no factors to the Smoother (we only want to test the removal)
737 // NonlinearFactorGraph noFactors;
738 // Values noValues;
739 // Smoother.update(noFactors, noValues, removeFactorIndices);
740 //
741 // NonlinearFactorGraph actualGraph = Smoother.getFactors();
742 //
743 // NonlinearFactorGraph expectedGraph;
744 // // we should add an empty one, so that the ordering and labeling of the factors is preserved
745 // expectedGraph.push_back(NonlinearFactor::shared_ptr());
746 // expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
747 // expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
748 // expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
749 // expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
750 //
751 // CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
752 //}
753 //
755 //TEST( ConcurrentBatchSmoother, removeFactors_values )
756 //{
757 // // we try removing the last factor
758 //
759 // // Create a set of optimizer parameters
760 // LevenbergMarquardtParams parameters;
761 // ConcurrentBatchSmoother Smoother(parameters);
762 //
763 // // Add some factors to the Smoother
764 // NonlinearFactorGraph newFactors;
765 // newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
766 // newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
767 // newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
768 // newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
769 // newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
770 //
771 // Values newValues;
772 // newValues.insert(1, Pose3().compose(poseError));
773 // newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
774 // newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
775 // newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
776 //
777 // // Update the Smoother: add all factors
778 // Smoother.update(newFactors, newValues);
779 //
780 // // factor we want to remove
781 // // NOTE: we can remove factors, paying attention that the remaining graph remains connected
782 // // we remove a single factor, the number 4, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
783 // std::vector<size_t> removeFactorIndices(1,4);
784 //
785 // // Add no factors to the Smoother (we only want to test the removal)
786 // NonlinearFactorGraph noFactors;
787 // Values noValues;
788 // Smoother.update(noFactors, noValues, removeFactorIndices);
789 //
790 // NonlinearFactorGraph actualGraph = Smoother.getFactors();
791 // Values actualValues = Smoother.calculateEstimate();
792 //
793 // // note: factors are removed before the optimization
794 // NonlinearFactorGraph expectedGraph;
795 // expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
796 // expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
797 // expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
798 // expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
799 // // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
800 // // we should add an empty one, so that the ordering and labeling of the factors is preserved
801 // expectedGraph.push_back(NonlinearFactor::shared_ptr());
802 //
803 // // Calculate expected factor graph and values
804 // Values expectedValues = BatchOptimize(expectedGraph, newValues);
805 //
806 // CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
807 // CHECK(assert_equal(expectedValues, actualValues, 1e-6));
808 //}
809 
810 
811 
812 
813 /* ************************************************************************* */
814 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
815 /* ************************************************************************* */
const gtsam::Symbol key('X', 0)
Provides additional testing facilities for common data structures.
#define CHECK(condition)
Definition: Test.h:108
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
An iSAM2-based Smoother that implements the Concurrent Filtering and Smoothing interface.
NonlinearFactorGraph graph
static enum @1107 ordering
void getSummarizedFactors(NonlinearFactorGraph &summarizedFactors, Values &separatorValues) override
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 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
void synchronize(const NonlinearFactorGraph &smootherFactors, const Values &smootherValues, const NonlinearFactorGraph &summarizedFactors, const Values &separatorValues) override
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.
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< FactorIndices > &removeFactorIndices={})
Array< double, 1, 3 > e(1./3., 0.5, 2.)
OptimizationParams optimizationParams
Definition: ISAM2Params.h:151
std::shared_ptr< This > shared_ptr
shared_ptr to this class
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
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
virtual void resize(size_t size)
Definition: FactorGraph.h:389
The junction tree.
const NonlinearFactorGraph & getFactors() const
TEST(ConcurrentIncrementalSmootherGN, equals)
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
3D Pose
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector


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