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
12/**
13 * @file testConcurrentBatchSmoother.cpp
14 * @brief Unit tests for the Concurrent Batch Smoother
15 * @author Stephen Williams (swilliams8@gatech.edu)
16 * @date Jan 5, 2013
17 */
18
19#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
20#include <gtsam/nonlinear/PriorFactor.h>
21#include <gtsam/slam/BetweenFactor.h>
22#include <gtsam/nonlinear/ISAM2.h>
23#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
24#include <gtsam/nonlinear/NonlinearFactorGraph.h>
25#include <gtsam/nonlinear/LinearContainerFactor.h>
26#include <gtsam/nonlinear/Values.h>
27#include <gtsam/inference/Symbol.h>
28#include <gtsam/inference/Ordering.h>
29#include <gtsam/inference/Key.h>
30#include <gtsam/inference/JunctionTree.h>
31#include <gtsam/geometry/Pose3.h>
32#include <gtsam/base/TestableAssertions.h>
33#include <CppUnitLite/TestHarness.h>
34
35using namespace std;
36using namespace gtsam;
37
38namespace {
39
40// Set up initial pose, odometry difference, loop closure difference, and initialization errors
41const Pose3 poseInitial;
42const Pose3 poseOdometry( Rot3::RzRyRx(xyz: Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) );
43const Pose3 poseError( Rot3::RzRyRx(xyz: Vector3(0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
44
45// Set up noise models for the factors
46const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
47const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(sigmas: (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished());
48const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(sigmas: (Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished());
49
50/* ************************************************************************* */
51Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) {
52
53 // Create an L-M optimizer
54 LevenbergMarquardtParams parameters;
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/* ************************************************************************* */
69TEST( ConcurrentBatchSmoother, equals )
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/* ************************************************************************* */
92TEST( ConcurrentBatchSmoother, getFactors )
93{
94 // Create a Concurrent Batch Smoother
95 LevenbergMarquardtParams parameters;
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(key: 1, prior: poseInitial, model: noisePrior);
108 newFactors1.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
109 Values newValues1;
110 newValues1.insert(j: 1, val: Pose3());
111 newValues1.insert(j: 2, val: newValues1.at<Pose3>(j: 1).compose(g: poseOdometry));
112 smoother.update(newFactors: newFactors1, newTheta: newValues1);
113
114 // Expected graph
115 NonlinearFactorGraph expected2;
116 expected2.push_back(container: 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(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
125 newFactors2.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
126 Values newValues2;
127 newValues2.insert(j: 3, val: newValues1.at<Pose3>(j: 2).compose(g: poseOdometry));
128 newValues2.insert(j: 4, val: newValues2.at<Pose3>(j: 3).compose(g: poseOdometry));
129 smoother.update(newFactors: newFactors2, newTheta: newValues2);
130
131 // Expected graph
132 NonlinearFactorGraph expected3;
133 expected3.push_back(container: newFactors1);
134 expected3.push_back(container: newFactors2);
135 // Get actual graph
136 NonlinearFactorGraph actual3 = smoother.getFactors();
137 // Check
138 CHECK(assert_equal(expected3, actual3));
139}
140
141/* ************************************************************************* */
142TEST( ConcurrentBatchSmoother, getLinearizationPoint )
143{
144 // Create a Concurrent Batch Smoother
145 LevenbergMarquardtParams parameters;
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(key: 1, prior: poseInitial, model: noisePrior);
158 newFactors1.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
159 Values newValues1;
160 newValues1.insert(j: 1, val: Pose3());
161 newValues1.insert(j: 2, val: newValues1.at<Pose3>(j: 1).compose(g: poseOdometry));
162 smoother.update(newFactors: newFactors1, newTheta: 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(values: 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(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
175 newFactors2.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
176 Values newValues2;
177 newValues2.insert(j: 3, val: newValues1.at<Pose3>(j: 2).compose(g: poseOdometry));
178 newValues2.insert(j: 4, val: newValues2.at<Pose3>(j: 3).compose(g: poseOdometry));
179 smoother.update(newFactors: newFactors2, newTheta: 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(values: newValues1);
184 expected3.insert(values: newValues2);
185 // Get actual linearization point
186 Values actual3 = smoother.getLinearizationPoint();
187 // Check
188 CHECK(assert_equal(expected3, actual3));
189}
190
191/* ************************************************************************* */
192TEST( ConcurrentBatchSmoother, getOrdering )
193{
194 // TODO: Think about how to check ordering...
195}
196
197/* ************************************************************************* */
198TEST( ConcurrentBatchSmoother, getDelta )
199{
200 // TODO: Think about how to check delta...
201}
202
203/* ************************************************************************* */
204TEST( ConcurrentBatchSmoother, calculateEstimate )
205{
206 // Create a Concurrent Batch Smoother
207 LevenbergMarquardtParams parameters;
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(key: 1, prior: poseInitial, model: noisePrior);
220 newFactors2.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
221 Values newValues2;
222 newValues2.insert(j: 1, val: Pose3().compose(g: poseError));
223 newValues2.insert(j: 2, val: newValues2.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
224 smoother.update(newFactors: newFactors2, newTheta: newValues2);
225
226 // Expected values from full batch optimization
227 NonlinearFactorGraph allFactors2;
228 allFactors2.push_back(container: newFactors2);
229 Values allValues2;
230 allValues2.insert(values: newValues2);
231 Values expected2 = BatchOptimize(graph: allFactors2, theta: 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(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
240 newFactors3.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
241 Values newValues3;
242 newValues3.insert(j: 3, val: newValues2.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
243 newValues3.insert(j: 4, val: newValues3.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
244 smoother.update(newFactors: newFactors3, newTheta: newValues3);
245
246 // Expected values from full batch optimization
247 NonlinearFactorGraph allFactors3;
248 allFactors3.push_back(container: newFactors2);
249 allFactors3.push_back(container: newFactors3);
250 Values allValues3;
251 allValues3.insert(values: newValues2);
252 allValues3.insert(values: newValues3);
253 Values expected3 = BatchOptimize(graph: allFactors3, theta: 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>(j: 1);
261 Pose3 expectedPose2 = expected3.at<Pose3>(j: 2);
262 Pose3 expectedPose3 = expected3.at<Pose3>(j: 3);
263 Pose3 expectedPose4 = expected3.at<Pose3>(j: 4);
264 // Also check the single-variable version
265 Pose3 actualPose1 = smoother.calculateEstimate<Pose3>(key: 1);
266 Pose3 actualPose2 = smoother.calculateEstimate<Pose3>(key: 2);
267 Pose3 actualPose3 = smoother.calculateEstimate<Pose3>(key: 3);
268 Pose3 actualPose4 = smoother.calculateEstimate<Pose3>(key: 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/* ************************************************************************* */
277TEST( ConcurrentBatchSmoother, update_empty )
278{
279 // Create a set of optimizer parameters
280 LevenbergMarquardtParams parameters;
281
282 // Create a Concurrent Batch Smoother
283 ConcurrentBatchSmoother smoother(parameters);
284
285 // Call update
286 smoother.update();
287}
288
289/* ************************************************************************* */
290TEST( ConcurrentBatchSmoother, update_multiple )
291{
292 // Create a Concurrent Batch Smoother
293 LevenbergMarquardtParams parameters;
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(key: 1, prior: poseInitial, model: noisePrior);
306 newFactors2.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
307 Values newValues2;
308 newValues2.insert(j: 1, val: Pose3().compose(g: poseError));
309 newValues2.insert(j: 2, val: newValues2.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
310 smoother.update(newFactors: newFactors2, newTheta: newValues2);
311
312 // Expected values from full batch optimization
313 NonlinearFactorGraph allFactors2;
314 allFactors2.push_back(container: newFactors2);
315 Values allValues2;
316 allValues2.insert(values: newValues2);
317 Values expected2 = BatchOptimize(graph: allFactors2, theta: 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(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
326 newFactors3.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
327 Values newValues3;
328 newValues3.insert(j: 3, val: newValues2.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
329 newValues3.insert(j: 4, val: newValues3.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
330 smoother.update(newFactors: newFactors3, newTheta: newValues3);
331
332 // Expected values from full batch optimization
333 NonlinearFactorGraph allFactors3;
334 allFactors3.push_back(container: newFactors2);
335 allFactors3.push_back(container: newFactors3);
336 Values allValues3;
337 allValues3.insert(values: newValues2);
338 allValues3.insert(values: newValues3);
339 Values expected3 = BatchOptimize(graph: allFactors3, theta: allValues3);
340 // Get actual linearization point
341 Values actual3 = smoother.calculateEstimate();
342 // Check
343 CHECK(assert_equal(expected3, actual3, 1e-6));
344}
345
346/* ************************************************************************* */
347TEST( ConcurrentBatchSmoother, synchronize_empty )
348{
349 // Create a set of optimizer parameters
350 LevenbergMarquardtParams 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(summarizedFactors&: actualSmootherSummarization, separatorValues&: actualSmootherSeparatorValues);
368 smoother.synchronize(smootherFactors, smootherValues, summarizedFactors: filterSumarization, separatorValues: 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/* ************************************************************************* */
377TEST( ConcurrentBatchSmoother, synchronize_1 )
378{
379 // Create a set of optimizer parameters
380 LevenbergMarquardtParams 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(j: 1, val: Pose3().compose(g: poseError));
390 filterSeparatorValues.insert(j: 2, val: filterSeparatorValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
391 Ordering ordering;
392 ordering.push_back(x: 1);
393 ordering.push_back(x: 2);
394 filterSumarization.push_back(factor: LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(x: filterSeparatorValues), filterSeparatorValues));
395 filterSumarization.push_back(factor: LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(x: 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(summarizedFactors&: actualSmootherSummarization, separatorValues&: actualSmootherSeparatorValues);
405 smoother.synchronize(smootherFactors, smootherValues, summarizedFactors: filterSumarization, separatorValues: 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(container: filterSumarization);
424 Values allValues;
425 allValues.insert(values: filterSeparatorValues);
426 Values expectedValues = BatchOptimize(graph: allFactors, theta: allValues, maxIter: 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/* ************************************************************************* */
438TEST( ConcurrentBatchSmoother, synchronize_2 )
439{
440 // Create a set of optimizer parameters
441 LevenbergMarquardtParams 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;
450 Ordering ordering;
451 ordering.push_back(x: 1);
452 ordering.push_back(x: 2);
453 filterSeparatorValues.insert(j: 1, val: Pose3().compose(g: poseError));
454 filterSeparatorValues.insert(j: 2, val: filterSeparatorValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
455 filterSumarization.push_back(factor: LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(x: filterSeparatorValues), filterSeparatorValues));
456 filterSumarization.push_back(factor: LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(x: filterSeparatorValues), filterSeparatorValues));
457 smootherFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
458 smootherFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
459 smootherValues.insert(j: 3, val: filterSeparatorValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
460 smootherValues.insert(j: 4, val: smootherValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: 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(summarizedFactors&: actualSmootherSummarization, separatorValues&: actualSmootherSeparatorValues);
470 smoother.synchronize(smootherFactors, smootherValues, summarizedFactors: filterSumarization, separatorValues: 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(container: smootherFactors);
484 expectedFactorGraph.push_back(container: 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(container: filterSumarization);
491 allFactors.push_back(container: smootherFactors);
492 Values allValues;
493 allValues.insert(values: filterSeparatorValues);
494 allValues.insert(values: smootherValues);
495 Values expectedValues = BatchOptimize(graph: allFactors, theta: allValues, maxIter: 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(graph: allFactors, theta: allValues, maxIter: parameters.maxIterations);
501 expectedLinearizationPoint.update(values: filterSeparatorValues);
502 Values actualLinearizationPoint = smoother.getLinearizationPoint();
503 CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
504}
505
506
507
508/* ************************************************************************* */
509TEST( ConcurrentBatchSmoother, synchronize_3 )
510{
511 // Create a set of optimizer parameters
512 LevenbergMarquardtParams 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;
521 Ordering ordering;
522 ordering.push_back(x: 1);
523 ordering.push_back(x: 2);
524 filterSeparatorValues.insert(j: 1, val: Pose3().compose(g: poseError));
525 filterSeparatorValues.insert(j: 2, val: filterSeparatorValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
526 filterSumarization.push_back(factor: LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(x: filterSeparatorValues), filterSeparatorValues));
527 filterSumarization.push_back(factor: LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(x: filterSeparatorValues), filterSeparatorValues));
528 smootherFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
529 smootherFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
530 smootherFactors.addPrior(key: 4, prior: poseInitial, model: noisePrior);
531 smootherValues.insert(j: 3, val: filterSeparatorValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
532 smootherValues.insert(j: 4, val: smootherValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: 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(summarizedFactors&: actualSmootherSummarization, separatorValues&: actualSmootherSeparatorValues);
542 smoother.synchronize(smootherFactors, smootherValues, summarizedFactors: filterSumarization, separatorValues: 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(summarizedFactors&: actualSmootherSummarization, separatorValues&: 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(linearizationPoint: allValues);
561
562 KeySet eliminateKeys = linearFactors->keys();
563 for(const auto key: filterSeparatorValues.keys()) {
564 eliminateKeys.erase(x: key);
565 }
566 KeyVector variables(eliminateKeys.begin(), eliminateKeys.end());
567 GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, function: EliminateCholesky).second;
568
569 expectedSmootherSummarization.resize(size: 0);
570 for(const GaussianFactor::shared_ptr& factor: result) {
571 expectedSmootherSummarization.push_back(factor: LinearContainerFactor(factor, allValues));
572 }
573
574 CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
575
576}
577
578///* ************************************************************************* */
579TEST( ConcurrentBatchSmoother, removeFactors_topology_1 )
580{
581 std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl;
582
583 // Create a set of optimizer parameters
584 LevenbergMarquardtParams 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(key: 1, prior: poseInitial, model: noisePrior);
592 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
593 newFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
594 newFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
595 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
596
597 Values newValues;
598 newValues.insert(j: 1, val: Pose3().compose(g: poseError));
599 newValues.insert(j: 2, val: newValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
600 newValues.insert(j: 3, val: newValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
601 newValues.insert(j: 4, val: newValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
602
603
604 // Update the smoother: add all factors
605 smoother.update(newFactors, newTheta: 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(newFactors: noFactors, newTheta: noValues, removeFactorIndices);
616
617 NonlinearFactorGraph actualGraph = smoother.getFactors();
618
619 NonlinearFactorGraph expectedGraph;
620 expectedGraph.addPrior(key: 1, prior: poseInitial, model: 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(factor: NonlinearFactor::shared_ptr());
624 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
625 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: noiseOdometery);
626 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
627
628 CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
629}
630
631///* ************************************************************************* */
632TEST( 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
638 LevenbergMarquardtParams 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(key: 1, prior: poseInitial, model: noisePrior);
646 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
647 newFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
648 newFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
649 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
650
651 Values newValues;
652 newValues.insert(j: 1, val: Pose3().compose(g: poseError));
653 newValues.insert(j: 2, val: newValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
654 newValues.insert(j: 3, val: newValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
655 newValues.insert(j: 4, val: newValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
656
657 // Update the smoother: add all factors
658 smoother.update(newFactors, newTheta: 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(newFactors: noFactors, newTheta: noValues, removeFactorIndices);
669
670 NonlinearFactorGraph actualGraph = smoother.getFactors();
671
672 NonlinearFactorGraph expectedGraph;
673 expectedGraph.addPrior(key: 1, prior: poseInitial, model: noisePrior);
674 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
675 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
676 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: 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(factor: NonlinearFactor::shared_ptr());
680
681 CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
682}
683
684
685///* ************************************************************************* */
686TEST( 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
692 LevenbergMarquardtParams parameters;
693 ConcurrentBatchSmoother Smoother(parameters);
694
695 // Add some factors to the Smoother
696 NonlinearFactorGraph newFactors;
697 newFactors.addPrior(key: 1, prior: poseInitial, model: noisePrior);
698 newFactors.addPrior(key: 1, prior: poseInitial, model: noisePrior);
699 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
700 newFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
701 newFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
702
703 Values newValues;
704 newValues.insert(j: 1, val: Pose3().compose(g: poseError));
705 newValues.insert(j: 2, val: newValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
706 newValues.insert(j: 3, val: newValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
707 newValues.insert(j: 4, val: newValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
708
709 // Update the Smoother: add all factors
710 Smoother.update(newFactors, newTheta: 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(newFactors: noFactors, newTheta: noValues, removeFactorIndices);
721
722 NonlinearFactorGraph actualGraph = Smoother.getFactors();
723
724 NonlinearFactorGraph expectedGraph;
725 // we should add an empty one, so that the ordering and labeling of the factors is preserved
726 expectedGraph.push_back(factor: NonlinearFactor::shared_ptr());
727 expectedGraph.addPrior(key: 1, prior: poseInitial, model: noisePrior);
728 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
729 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
730 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: noiseOdometery);
731
732 CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
733}
734
735///* ************************************************************************* */
736TEST( 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
742 LevenbergMarquardtParams parameters;
743 ConcurrentBatchSmoother Smoother(parameters);
744
745 // Add some factors to the Smoother
746 NonlinearFactorGraph newFactors;
747 newFactors.addPrior(key: 1, prior: poseInitial, model: noisePrior);
748 newFactors.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
749 newFactors.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
750 newFactors.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: noiseOdometery);
751 newFactors.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
752
753 Values newValues;
754 newValues.insert(j: 1, val: Pose3().compose(g: poseError));
755 newValues.insert(j: 2, val: newValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
756 newValues.insert(j: 3, val: newValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
757 newValues.insert(j: 4, val: newValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
758
759 // Update the Smoother: add all factors
760 Smoother.update(newFactors, newTheta: 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(newFactors: noFactors, newTheta: noValues, removeFactorIndices);
771
772 NonlinearFactorGraph actualGraph = Smoother.getFactors();
773 Values actualValues = Smoother.calculateEstimate();
774
775 // note: factors are removed before the optimization
776 NonlinearFactorGraph expectedGraph;
777 expectedGraph.addPrior(key: 1, prior: poseInitial, model: noisePrior);
778 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
779 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
780 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: 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(factor: NonlinearFactor::shared_ptr());
784
785 // Calculate expected factor graph and values
786 Values expectedValues = BatchOptimize(graph: expectedGraph, theta: newValues);
787
788 CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
789 CHECK(assert_equal(expectedValues, actualValues, 1e-6));
790}
791
792
793/* ************************************************************************* */
794int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
795/* ************************************************************************* */
796