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


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