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
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
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
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
284 
285  // Call update
286  smoother.update();
287 }
288 
289 /* ************************************************************************* */
290 TEST( ConcurrentBatchSmoother, update_multiple )
291 {
292  // Create a Concurrent Batch Smoother
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
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
382 
383  // Create a Concurrent Batch Smoother
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
443 
444  // Create a Concurrent Batch Smoother
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
514 
515  // Create a Concurrent Batch Smoother
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
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
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
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
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
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
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
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
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 /* ************************************************************************* */
gtsam::ConcurrentBatchSmoother::getSummarizedFactors
void getSummarizedFactors(NonlinearFactorGraph &summarizedFactors, Values &separatorValues) override
Definition: ConcurrentBatchSmoother.cpp:115
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
TEST
TEST(ConcurrentBatchSmoother, equals)
Definition: testConcurrentBatchSmoother.cpp:69
gtsam::GaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:82
gtsam::Values::keys
KeyVector keys() const
Definition: Values.cpp:218
ConcurrentBatchSmoother.h
A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoothing interface...
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
LinearContainerFactor.h
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.
TestHarness.h
gtsam::ConcurrentBatchSmoother::getOrdering
const Ordering & getOrdering() const
Definition: ConcurrentBatchSmoother.h:79
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::PriorFactor
Definition: nonlinear/PriorFactor.h:30
gtsam::ConcurrentBatchSmoother
Definition: ConcurrentBatchSmoother.h:31
gtsam::NonlinearFactorGraph::linearize
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:239
Ordering.h
Variable ordering for the elimination algorithm.
gtsam::FastSet< Key >
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Values::update
void update(Key j, const Value &val)
Definition: Values.cpp:169
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::ConcurrentBatchSmoother::getLinearizationPoint
const Values & getLinearizationPoint() const
Definition: ConcurrentBatchSmoother.h:74
expectedGraph
static NonlinearFactorGraph expectedGraph(const SharedNoiseModel &model)
Definition: testDataset.cpp:296
TestableAssertions.h
Provides additional testing facilities for common data structures.
JunctionTree.h
The junction tree.
Key.h
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
PriorFactor.h
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Definition: FactorGraph.h:367
gtsam::ConcurrentBatchSmoother::getFactors
const NonlinearFactorGraph & getFactors() const
Definition: ConcurrentBatchSmoother.h:69
gtsam::ConcurrentBatchSmoother::update
virtual Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< std::vector< size_t > > &removeFactorIndices={})
Definition: ConcurrentBatchSmoother.cpp:49
BetweenFactor.h
gtsam::Pose3
Definition: Pose3.h:37
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
main
int main()
Definition: testConcurrentBatchSmoother.cpp:794
gtsam::ConcurrentBatchSmoother::calculateEstimate
Values calculateEstimate() const
Definition: ConcurrentBatchSmoother.h:91
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::ConcurrentBatchSmoother::postsync
void postsync() override
Definition: ConcurrentBatchSmoother.cpp:178
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::ConcurrentBatchSmoother::synchronize
void synchronize(const NonlinearFactorGraph &smootherFactors, const Values &smootherValues, const NonlinearFactorGraph &summarizedFactors, const Values &separatorValues) override
Definition: ConcurrentBatchSmoother.cpp:129
Symbol.h
ordering
static enum @1096 ordering
gtsam::equals
Definition: Testable.h:112
TestResult
Definition: TestResult.h:26
key
const gtsam::Symbol key('X', 0)
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam
traits
Definition: SFMdata.h:40
gtsam::ConjugateGradientParameters::maxIterations
size_t maxIterations
maximum number of cg iterations
Definition: ConjugateGradientSolver.h:35
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::Values
Definition: Values.h:65
gtsam::testing::compose
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
getOrdering
static std::shared_ptr< Ordering > getOrdering(const vector< GeneralCamera > &cameras, const vector< Point3 > &landmarks)
Definition: testGeneralSFMFactor.cpp:118
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::NonlinearOptimizerParams::maxIterations
size_t maxIterations
The maximum iterations to stop iterating (default 100)
Definition: NonlinearOptimizerParams.h:42
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::LinearContainerFactor
Definition: LinearContainerFactor.h:29
gtsam::EliminateCholesky
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: HessianFactor.cpp:517
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::ConcurrentBatchSmoother::presync
void presync() override
Definition: ConcurrentBatchSmoother.cpp:107
gtsam::Ordering
Definition: inference/Ordering.h:33
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
Values.h
A non-templated config holding any types of Manifold-group elements.
Pose3.h
3D Pose
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::BetweenFactor
Definition: BetweenFactor.h:40


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:46