testConcurrentIncrementalSmootherDL.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>
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
56 // parameters.maxIterations = maxIter;
57 // parameters.lambdaUpperBound = 1;
58 // parameters.lambdaInitial = 1;
59 // parameters.verbosity = NonlinearOptimizerParams::ERROR;
60 // parameters.verbosityLM = ISAM2Params::DAMPED;
61 // parameters.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_QR;
62 
63  ISAM2 optimizer(parameters);
64  optimizer.update( graph, theta );
65  Values result = optimizer.calculateEstimate();
66  return result;
67 
68 }
69 
70 } // end namespace
71 
72 
73 
74 
75 /* ************************************************************************* */
76 TEST( ConcurrentIncrementalSmootherDL, equals )
77 {
78  // TODO: Test 'equals' more vigorously
79 
80  // Create a Concurrent Batch Smoother
81  ISAM2Params parameters1;
82  parameters1.optimizationParams = ISAM2DoglegParams();
83  ConcurrentIncrementalSmoother smoother1(parameters1);
84 
85  // Create an identical Concurrent Batch Smoother
86  ISAM2Params parameters2;
87  parameters2.optimizationParams = ISAM2DoglegParams();
88  ConcurrentIncrementalSmoother smoother2(parameters2);
89 
90  // Create a different Concurrent Batch Smoother
91  ISAM2Params parameters3;
92  parameters3.optimizationParams = ISAM2DoglegParams();
93 // parameters3.maxIterations = 1;
94  ConcurrentIncrementalSmoother smoother3(parameters3);
95 
96  CHECK(assert_equal(smoother1, smoother1));
97  CHECK(assert_equal(smoother1, smoother2));
98 // CHECK(assert_inequal(smoother1, smoother3));
99 }
100 
101 /* ************************************************************************* */
102 TEST( ConcurrentIncrementalSmootherDL, getFactors )
103 {
104  // Create a Concurrent Batch Smoother
106  parameters.optimizationParams = ISAM2DoglegParams();
107  ConcurrentIncrementalSmoother smoother(parameters);
108 
109  // Expected graph is empty
110  NonlinearFactorGraph expected1;
111  // Get actual graph
112  NonlinearFactorGraph actual1 = smoother.getFactors();
113  // Check
114  CHECK(assert_equal(expected1, actual1));
115 
116  // Add some factors to the smoother
117  NonlinearFactorGraph newFactors1;
118  newFactors1.addPrior(1, poseInitial, noisePrior);
119  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
120  Values newValues1;
121  newValues1.insert(1, Pose3());
122  newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
123  smoother.update(newFactors1, newValues1);
124 
125  // Expected graph
126  NonlinearFactorGraph expected2;
127  expected2.push_back(newFactors1);
128  // Get actual graph
129  NonlinearFactorGraph actual2 = smoother.getFactors();
130  // Check
131  CHECK(assert_equal(expected2, actual2));
132 
133  // Add some more factors to the smoother
134  NonlinearFactorGraph newFactors2;
135  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
136  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
137  Values newValues2;
138  newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
139  newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
140  smoother.update(newFactors2, newValues2);
141 
142  // Expected graph
143  NonlinearFactorGraph expected3;
144  expected3.push_back(newFactors1);
145  expected3.push_back(newFactors2);
146  // Get actual graph
147  NonlinearFactorGraph actual3 = smoother.getFactors();
148  // Check
149  CHECK(assert_equal(expected3, actual3));
150 }
151 
152 /* ************************************************************************* */
153 TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint )
154 {
155  // Create a Concurrent Batch Smoother
157  parameters.optimizationParams = ISAM2DoglegParams();
158  ConcurrentIncrementalSmoother smoother(parameters);
159 
160  // Expected values is empty
161  Values expected1;
162  // Get Linearization Point
163  Values actual1 = smoother.getLinearizationPoint();
164  // Check
165  CHECK(assert_equal(expected1, actual1));
166 
167  // Add some factors to the smoother
168  NonlinearFactorGraph newFactors1;
169  newFactors1.addPrior(1, poseInitial, noisePrior);
170  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
171  Values newValues1;
172  newValues1.insert(1, Pose3());
173  newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
174  smoother.update(newFactors1, newValues1);
175 
176  // Expected values is equivalent to the provided values only because the provided linearization points were optimal
177  Values expected2;
178  expected2.insert(newValues1);
179  // Get actual linearization point
180  Values actual2 = smoother.getLinearizationPoint();
181  // Check
182  CHECK(assert_equal(expected2, actual2));
183 
184  // Add some more factors to the smoother
185  NonlinearFactorGraph newFactors2;
186  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
187  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
188  Values newValues2;
189  newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
190  newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
191  smoother.update(newFactors2, newValues2);
192 
193  // Expected values is equivalent to the provided values only because the provided linearization points were optimal
194  Values expected3;
195  expected3.insert(newValues1);
196  expected3.insert(newValues2);
197  // Get actual linearization point
198  Values actual3 = smoother.getLinearizationPoint();
199  // Check
200  CHECK(assert_equal(expected3, actual3));
201 }
202 
203 /* ************************************************************************* */
204 TEST( ConcurrentIncrementalSmootherDL, getDelta )
205 {
206  // TODO: Think about how to check ordering...
207 }
208 
209 /* ************************************************************************* */
210 TEST( ConcurrentIncrementalSmootherDL, calculateEstimate )
211 {
212  // Create a Concurrent Batch Smoother
214  parameters.optimizationParams = ISAM2DoglegParams();
215  ConcurrentIncrementalSmoother smoother(parameters);
216 
217  // Expected values is empty
218  Values expected1;
219  // Get Linearization Point
220  Values actual1 = smoother.calculateEstimate();
221  // Check
222  CHECK(assert_equal(expected1, actual1));
223 
224  // Add some factors to the smoother
225  NonlinearFactorGraph newFactors2;
226  newFactors2.addPrior(1, poseInitial, noisePrior);
227  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
228  Values newValues2;
229  newValues2.insert(1, Pose3().compose(poseError));
230  newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
231  smoother.update(newFactors2, newValues2);
232 
233  // Expected values from full batch optimization
234  NonlinearFactorGraph allFactors2;
235  allFactors2.push_back(newFactors2);
236  Values allValues2;
237  allValues2.insert(newValues2);
238  Values expected2 = BatchOptimize(allFactors2, allValues2);
239  // Get actual linearization point
240  Values actual2 = smoother.calculateEstimate();
241  // Check
242  CHECK(assert_equal(expected2, actual2, 1e-6));
243 
244  // Add some more factors to the smoother
245  NonlinearFactorGraph newFactors3;
246  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
247  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
248  Values newValues3;
249  newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
250  newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
251  smoother.update(newFactors3, newValues3);
252 
253  // Expected values from full batch optimization
254  NonlinearFactorGraph allFactors3;
255  allFactors3.push_back(newFactors2);
256  allFactors3.push_back(newFactors3);
257  Values allValues3;
258  allValues3.insert(newValues2);
259  allValues3.insert(newValues3);
260  Values expected3 = BatchOptimize(allFactors3, allValues3);
261  // Get actual linearization point
262  Values actual3 = smoother.calculateEstimate();
263  // Check
264  CHECK(assert_equal(expected3, actual3, 1e-6));
265 
266  // Also check the single-variable version
267  Pose3 expectedPose1 = expected3.at<Pose3>(1);
268  Pose3 expectedPose2 = expected3.at<Pose3>(2);
269  Pose3 expectedPose3 = expected3.at<Pose3>(3);
270  Pose3 expectedPose4 = expected3.at<Pose3>(4);
271  // Also check the single-variable version
272  Pose3 actualPose1 = smoother.calculateEstimate<Pose3>(1);
273  Pose3 actualPose2 = smoother.calculateEstimate<Pose3>(2);
274  Pose3 actualPose3 = smoother.calculateEstimate<Pose3>(3);
275  Pose3 actualPose4 = smoother.calculateEstimate<Pose3>(4);
276  // Check
277  CHECK(assert_equal(expectedPose1, actualPose1, 1e-6));
278  CHECK(assert_equal(expectedPose2, actualPose2, 1e-6));
279  CHECK(assert_equal(expectedPose3, actualPose3, 1e-6));
280  CHECK(assert_equal(expectedPose4, actualPose4, 1e-6));
281 }
282 
283 /* ************************************************************************* */
284 TEST( ConcurrentIncrementalSmootherDL, update_empty )
285 {
286  // Create a set of optimizer parameters
288  parameters.optimizationParams = ISAM2DoglegParams();
289  // Create a Concurrent Batch Smoother
290  ConcurrentIncrementalSmoother smoother(parameters);
291 
292  // Call update
293  smoother.update();
294 }
295 
296 /* ************************************************************************* */
297 TEST( ConcurrentIncrementalSmootherDL, update_multiple )
298 {
299  // Create a Concurrent Batch Smoother
301  parameters.optimizationParams = ISAM2DoglegParams();
302  ConcurrentIncrementalSmoother smoother(parameters);
303 
304  // Expected values is empty
305  Values expected1;
306  // Get Linearization Point
307  Values actual1 = smoother.calculateEstimate();
308  // Check
309  CHECK(assert_equal(expected1, actual1));
310 
311  // Add some factors to the smoother
312  NonlinearFactorGraph newFactors2;
313  newFactors2.addPrior(1, poseInitial, noisePrior);
314  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
315  Values newValues2;
316  newValues2.insert(1, Pose3().compose(poseError));
317  newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
318  smoother.update(newFactors2, newValues2);
319 
320  // Expected values from full batch optimization
321  NonlinearFactorGraph allFactors2;
322  allFactors2.push_back(newFactors2);
323  Values allValues2;
324  allValues2.insert(newValues2);
325  Values expected2 = BatchOptimize(allFactors2, allValues2);
326  // Get actual linearization point
327  Values actual2 = smoother.calculateEstimate();
328  // Check
329  CHECK(assert_equal(expected2, actual2, 1e-6));
330 
331  // Add some more factors to the smoother
332  NonlinearFactorGraph newFactors3;
333  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
334  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
335  Values newValues3;
336  newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
337  newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
338  smoother.update(newFactors3, newValues3);
339 
340  // Expected values from full batch optimization
341  NonlinearFactorGraph allFactors3;
342  allFactors3.push_back(newFactors2);
343  allFactors3.push_back(newFactors3);
344  Values allValues3;
345  allValues3.insert(newValues2);
346  allValues3.insert(newValues3);
347  Values expected3 = BatchOptimize(allFactors3, allValues3);
348  // Get actual linearization point
349  Values actual3 = smoother.calculateEstimate();
350  // Check
351  CHECK(assert_equal(expected3, actual3, 1e-6));
352 }
353 
354 /* ************************************************************************* */
355 TEST( ConcurrentIncrementalSmootherDL, synchronize_empty )
356 {
357  // Create a set of optimizer parameters
359  parameters.optimizationParams = ISAM2DoglegParams();
360  // Create a Concurrent Batch Smoother
361  ConcurrentIncrementalSmoother smoother(parameters);
362 
363  // Create empty containers *from* the filter
364  NonlinearFactorGraph smootherFactors, filterSumarization;
365  Values smootherValues, filterSeparatorValues;
366 
367  // Create expected values: these will be empty for this case
368  NonlinearFactorGraph expectedSmootherSummarization;
369  Values expectedSmootherSeparatorValues;
370 
371  // Synchronize
372  NonlinearFactorGraph actualSmootherSummarization;
373  Values actualSmootherSeparatorValues;
374  smoother.presync();
375  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
376  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
377  smoother.postsync();
378 
379  // Check
380  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
381  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
382 }
383 
384 /* ************************************************************************* */
385 TEST( ConcurrentIncrementalSmootherDL, synchronize_1 )
386 {
387  // Create a set of optimizer parameters
389  parameters.optimizationParams = ISAM2DoglegParams();
390 // parameters.maxIterations = 1;
391 
392  // Create a Concurrent Batch Smoother
393  ConcurrentIncrementalSmoother smoother(parameters);
394 
395  // Create a simple separator *from* the filter
396  NonlinearFactorGraph smootherFactors, filterSumarization;
397  Values smootherValues, filterSeparatorValues;
398  filterSeparatorValues.insert(1, Pose3().compose(poseError));
399  filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
400 
401  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
402  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
403 
404  // Create expected values: the smoother output will be empty for this case
405  NonlinearFactorGraph expectedSmootherSummarization;
406  Values expectedSmootherSeparatorValues;
407 
408  NonlinearFactorGraph actualSmootherSummarization;
409  Values actualSmootherSeparatorValues;
410  smoother.presync();
411  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
412  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
413  smoother.postsync();
414 
415  // Check
416  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
417  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
418 
419 
420  // Update the smoother
421  smoother.update();
422 
423  // Check the factor graph. It should contain only the filter-provided factors
424  NonlinearFactorGraph expectedFactorGraph = filterSumarization;
425  NonlinearFactorGraph actualFactorGraph = smoother.getFactors();
426  CHECK(assert_equal(expectedFactorGraph, actualFactorGraph, 1e-6));
427 
428  // Check the optimized value of smoother state
429  NonlinearFactorGraph allFactors;
430  allFactors.push_back(filterSumarization);
431  Values allValues;
432  allValues.insert(filterSeparatorValues);
433  Values expectedValues = BatchOptimize(allFactors, allValues,1);
434  Values actualValues = smoother.calculateEstimate();
435  CHECK(assert_equal(expectedValues, actualValues, 1e-6));
436 
437  // Check the linearization point. The separator should remain identical to the filter provided values
438  Values expectedLinearizationPoint = filterSeparatorValues;
439  Values actualLinearizationPoint = smoother.getLinearizationPoint();
440  CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
441 }
442 
443 
444 /* ************************************************************************* */
445 TEST( ConcurrentIncrementalSmootherDL, synchronize_2 )
446 {
447  // Create a set of optimizer parameters
449  parameters.optimizationParams = ISAM2DoglegParams();
450 // parameters.maxIterations = 1;
451 // parameters.lambdaUpperBound = 1;
452 // parameters.lambdaInitial = 1;
453 // parameters.verbosity = NonlinearOptimizerParams::ERROR;
454 // parameters.verbosityLM = ISAM2Params::DAMPED;
455 
456  // Create a Concurrent Batch Smoother
457  ConcurrentIncrementalSmoother smoother(parameters);
458 
459  // Create a separator and cached smoother factors *from* the filter
460  NonlinearFactorGraph smootherFactors, filterSumarization;
461  Values smootherValues, filterSeparatorValues;
462 
463  filterSeparatorValues.insert(1, Pose3().compose(poseError));
464  filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
465  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
466  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
467  smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
468  smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
469  smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
470  smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
471 
472  // Create expected values: the smoother output will be empty for this case
473  NonlinearFactorGraph expectedSmootherSummarization;
474  Values expectedSmootherSeparatorValues;
475 
476  NonlinearFactorGraph actualSmootherSummarization;
477  Values actualSmootherSeparatorValues;
478  smoother.presync();
479  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
480  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
481  smoother.postsync();
482 
483  // Check
484  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
485  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
486 
487 
488  // Update the smoother
489  smoother.update();
490 
491  // Check the factor graph. It should contain only the filter-provided factors
492  NonlinearFactorGraph expectedFactorGraph;
493  expectedFactorGraph.push_back(smootherFactors);
494  expectedFactorGraph.push_back(filterSumarization);
495  NonlinearFactorGraph actualFactorGraph = smoother.getFactors();
496  CHECK(assert_equal(expectedFactorGraph, actualFactorGraph, 1e-6));
497 
498  // Check the optimized value of smoother state
499  NonlinearFactorGraph allFactors;
500  allFactors.push_back(filterSumarization);
501  allFactors.push_back(smootherFactors);
502  Values allValues;
503  allValues.insert(filterSeparatorValues);
504  allValues.insert(smootherValues);
505 // allValues.print("Batch LinPoint:\n");
506  Values expectedValues = BatchOptimize(allFactors, allValues, 1);
507  Values actualValues = smoother.calculateEstimate();
508  CHECK(assert_equal(expectedValues, actualValues, 1e-6));
509 
510  // Check the linearization point. The separator should remain identical to the filter provided values, but the others should be the optimal values
511  // Isam2 is changing internally the linearization points, so the following check is done only on the separator variables
512 // Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
513  Values expectedLinearizationPoint = filterSeparatorValues;
514  Values actualLinearizationPoint;
515  for(const auto key_value: filterSeparatorValues) {
516  actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key));
517  }
518  CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
519 }
520 
521 
522 
523 /* ************************************************************************* */
524 TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
525 {
526  // Create a set of optimizer parameters
528  parameters.optimizationParams = ISAM2DoglegParams();
529 // parameters.maxIterations = 1;
530 // parameters.lambdaUpperBound = 1;
531 // parameters.lambdaInitial = 1;
532 // parameters.verbosity = NonlinearOptimizerParams::ERROR;
533 // parameters.verbosityLM = ISAM2Params::DAMPED;
534 
535  // Create a Concurrent Batch Smoother
536  ConcurrentIncrementalSmoother smoother(parameters);
537 
538  // Create a separator and cached smoother factors *from* the filter
539  NonlinearFactorGraph smootherFactors, filterSumarization;
540  Values smootherValues, filterSeparatorValues;
541 
542  filterSeparatorValues.insert(1, Pose3().compose(poseError));
543  filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
544  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
545  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
546  smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
547  smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
548  smootherFactors.addPrior(4, poseInitial, noisePrior);
549  smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
550  smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
551 
552  // Create expected values: the smoother output will be empty for this case
553  NonlinearFactorGraph expectedSmootherSummarization;
554  Values expectedSmootherSeparatorValues;
555 
556  NonlinearFactorGraph actualSmootherSummarization;
557  Values actualSmootherSeparatorValues;
558  smoother.presync();
559  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
560  smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
561  smoother.postsync();
562 
563  // Check
564  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
565  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
566 
567 
568  // Update the smoother
569  smoother.update();
570 
571  smoother.presync();
572  smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
573 
574  // Check the optimized value of smoother state
575  NonlinearFactorGraph allFactors = smootherFactors;
576  Values allValues = smoother.getLinearizationPoint();
577 
578  GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(allValues);
579 // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
580 // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
581 
582  KeySet allkeys = LinFactorGraph->keys();
583  for(const auto key_value: filterSeparatorValues) {
584  allkeys.erase(key_value.key);
585  }
586  KeyVector variables(allkeys.begin(), allkeys.end());
587  std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
588 
589  expectedSmootherSummarization.resize(0);
590  for(const GaussianFactor::shared_ptr& factor: *result.second) {
591  expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues));
592  }
593 
594  CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
595 
596 }
597 
598 /* ************************************************************************* */
599 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
600 /* ************************************************************************* */
Provides additional testing facilities for common data structures.
#define CHECK(condition)
Definition: Test.h:109
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.
void insert(Key j, const Value &val)
Definition: Values.cpp:140
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: Half.h:150
An iSAM2-based Smoother that implements the Concurrent Filtering and Smoothing interface.
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:166
Rot2 theta
NonlinearFactorGraph graph
void getSummarizedFactors(NonlinearFactorGraph &summarizedFactors, Values &separatorValues) override
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const boost::optional< FactorIndices > &removeFactorIndices=boost::none)
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
TEST(ConcurrentIncrementalSmootherDL, equals)
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
Class compose(const Class &g) const
Definition: Lie.h:47
void synchronize(const NonlinearFactorGraph &smootherFactors, const Values &smootherValues, const NonlinearFactorGraph &summarizedFactors, const Values &separatorValues) override
Values result
const ValueType at(Key j) const
Definition: Values-inl.h:342
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
OptimizationParams optimizationParams
Definition: ISAM2Params.h:150
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
static ConjugateGradientParameters parameters
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
The junction tree.
void resize(size_t size)
Definition: FactorGraph.h:358
Vector3 Point3
Definition: Point3.h:35
boost::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
const NonlinearFactorGraph & getFactors() const
3D Pose
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:46:21