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 testConcurrentBatchFilter.cpp
14 * @brief Unit tests for the Concurrent Batch Filter
15 * @author Stephen Williams (swilliams8@gatech.edu)
16 * @date Jan 5, 2013
17 */
18
19#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.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/* ************************************************************************* */
63NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, const Values& linPoint, const FastList<Key>& keysToMarginalize){
64
65
66 std::set<Key> KeysToKeep;
67 for(const auto key: linPoint.keys()) { // we cycle over all the keys of factorGraph
68 KeysToKeep.insert(x: key);
69 } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize
70 for(Key key: keysToMarginalize) {
71 KeysToKeep.erase(x: key);
72 } // we removed the keys that we have to marginalize
73
74 Ordering ordering;
75 for(Key key: keysToMarginalize) {
76 ordering.push_back(x: key);
77 } // the keys that we marginalize should be at the beginning in the ordering
78 for(Key key: KeysToKeep) {
79 ordering.push_back(x: key);
80 }
81
82 GaussianFactorGraph linearGraph = *factorGraph.linearize(linearizationPoint: linPoint);
83
84 GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(variables: KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), function: EliminateCholesky).second;
85
86 NonlinearFactorGraph LinearContainerForGaussianMarginals;
87 for(const GaussianFactor::shared_ptr& factor: marginal) {
88 LinearContainerForGaussianMarginals.push_back(factor: LinearContainerFactor(factor, linPoint));
89 }
90 return LinearContainerForGaussianMarginals;
91}
92
93
94} // end namespace
95
96
97
98/* ************************************************************************* */
99TEST( ConcurrentBatchFilter, equals )
100{
101 // TODO: Test 'equals' more vigorously
102
103 // Create a Concurrent Batch Filter
104 LevenbergMarquardtParams parameters1;
105 ConcurrentBatchFilter filter1(parameters1);
106
107 // Create an identical Concurrent Batch Filter
108 LevenbergMarquardtParams parameters2;
109 ConcurrentBatchFilter filter2(parameters2);
110
111 // Create a different Concurrent Batch Filter
112 LevenbergMarquardtParams parameters3;
113 parameters3.maxIterations = 1;
114 ConcurrentBatchFilter filter3(parameters3);
115
116 CHECK(assert_equal(filter1, filter1));
117 CHECK(assert_equal(filter1, filter2));
118// CHECK(assert_inequal(filter1, filter3));
119}
120
121/* ************************************************************************* */
122TEST( ConcurrentBatchFilter, getFactors )
123{
124 // Create a Concurrent Batch Filter
125 LevenbergMarquardtParams parameters;
126 ConcurrentBatchFilter filter(parameters);
127
128 // Expected graph is empty
129 NonlinearFactorGraph expected1;
130 // Get actual graph
131 NonlinearFactorGraph actual1 = filter.getFactors();
132 // Check
133 CHECK(assert_equal(expected1, actual1));
134
135 // Add some factors to the filter
136 NonlinearFactorGraph newFactors1;
137 newFactors1.addPrior(key: 1, prior: poseInitial, model: noisePrior);
138 newFactors1.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
139 Values newValues1;
140 newValues1.insert(j: 1, val: Pose3());
141 newValues1.insert(j: 2, val: newValues1.at<Pose3>(j: 1).compose(g: poseOdometry));
142 filter.update(newFactors: newFactors1, newTheta: newValues1);
143
144 // Expected graph
145 NonlinearFactorGraph expected2;
146 expected2.push_back(container: newFactors1);
147 // Get actual graph
148 NonlinearFactorGraph actual2 = filter.getFactors();
149 // Check
150 CHECK(assert_equal(expected2, actual2));
151
152 // Add some more factors to the filter
153 NonlinearFactorGraph newFactors2;
154 newFactors2.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
155 newFactors2.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
156 Values newValues2;
157 newValues2.insert(j: 3, val: newValues1.at<Pose3>(j: 2).compose(g: poseOdometry));
158 newValues2.insert(j: 4, val: newValues2.at<Pose3>(j: 3).compose(g: poseOdometry));
159 filter.update(newFactors: newFactors2, newTheta: newValues2);
160
161 // Expected graph
162 NonlinearFactorGraph expected3;
163 expected3.push_back(container: newFactors1);
164 expected3.push_back(container: newFactors2);
165 // Get actual graph
166 NonlinearFactorGraph actual3 = filter.getFactors();
167 // Check
168 CHECK(assert_equal(expected3, actual3));
169}
170
171/* ************************************************************************* */
172TEST( ConcurrentBatchFilter, getLinearizationPoint )
173{
174 // Create a Concurrent Batch Filter
175 LevenbergMarquardtParams parameters;
176 ConcurrentBatchFilter filter(parameters);
177
178 // Expected values is empty
179 Values expected1;
180 // Get Linearization Point
181 Values actual1 = filter.getLinearizationPoint();
182 // Check
183 CHECK(assert_equal(expected1, actual1));
184
185 // Add some factors to the filter
186 NonlinearFactorGraph newFactors1;
187 newFactors1.addPrior(key: 1, prior: poseInitial, model: noisePrior);
188 newFactors1.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
189 Values newValues1;
190 newValues1.insert(j: 1, val: Pose3());
191 newValues1.insert(j: 2, val: newValues1.at<Pose3>(j: 1).compose(g: poseOdometry));
192 filter.update(newFactors: newFactors1, newTheta: newValues1);
193
194 // Expected values is equivalent to the provided values only because the provided linearization points were optimal
195 Values expected2;
196 expected2.insert(values: newValues1);
197 // Get actual linearization point
198 Values actual2 = filter.getLinearizationPoint();
199 // Check
200 CHECK(assert_equal(expected2, actual2));
201
202 // Add some more factors to the filter
203 NonlinearFactorGraph newFactors2;
204 newFactors2.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
205 newFactors2.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
206 Values newValues2;
207 newValues2.insert(j: 3, val: newValues1.at<Pose3>(j: 2).compose(g: poseOdometry));
208 newValues2.insert(j: 4, val: newValues2.at<Pose3>(j: 3).compose(g: poseOdometry));
209 filter.update(newFactors: newFactors2, newTheta: newValues2);
210
211 // Expected values is equivalent to the provided values only because the provided linearization points were optimal
212 Values expected3;
213 expected3.insert(values: newValues1);
214 expected3.insert(values: newValues2);
215 // Get actual linearization point
216 Values actual3 = filter.getLinearizationPoint();
217 // Check
218 CHECK(assert_equal(expected3, actual3));
219}
220
221/* ************************************************************************* */
222TEST( ConcurrentBatchFilter, getOrdering )
223{
224 // TODO: Think about how to check ordering...
225}
226
227/* ************************************************************************* */
228TEST( ConcurrentBatchFilter, getDelta )
229{
230 // TODO: Think about how to check delta...
231}
232
233/* ************************************************************************* */
234TEST( ConcurrentBatchFilter, calculateEstimate )
235{
236 // Create a Concurrent Batch Filter
237 LevenbergMarquardtParams parameters;
238 ConcurrentBatchFilter filter(parameters);
239
240 // Expected values is empty
241 Values expected1;
242 // Get Linearization Point
243 Values actual1 = filter.calculateEstimate();
244 // Check
245 CHECK(assert_equal(expected1, actual1));
246
247 // Add some factors to the filter
248 NonlinearFactorGraph newFactors2;
249 newFactors2.addPrior(key: 1, prior: poseInitial, model: noisePrior);
250 newFactors2.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
251 Values newValues2;
252 newValues2.insert(j: 1, val: Pose3().compose(g: poseError));
253 newValues2.insert(j: 2, val: newValues2.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
254 filter.update(newFactors: newFactors2, newTheta: newValues2);
255
256 // Expected values from full batch optimization
257 NonlinearFactorGraph allFactors2;
258 allFactors2.push_back(container: newFactors2);
259 Values allValues2;
260 allValues2.insert(values: newValues2);
261 Values expected2 = BatchOptimize(graph: allFactors2, theta: allValues2);
262 // Get actual linearization point
263 Values actual2 = filter.calculateEstimate();
264 // Check
265 CHECK(assert_equal(expected2, actual2, 1e-6));
266
267 // Add some more factors to the filter
268 NonlinearFactorGraph newFactors3;
269 newFactors3.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
270 newFactors3.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
271 Values newValues3;
272 newValues3.insert(j: 3, val: newValues2.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
273 newValues3.insert(j: 4, val: newValues3.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
274 filter.update(newFactors: newFactors3, newTheta: newValues3);
275
276 // Expected values from full batch optimization
277 NonlinearFactorGraph allFactors3;
278 allFactors3.push_back(container: newFactors2);
279 allFactors3.push_back(container: newFactors3);
280 Values allValues3;
281 allValues3.insert(values: newValues2);
282 allValues3.insert(values: newValues3);
283 Values expected3 = BatchOptimize(graph: allFactors3, theta: allValues3);
284 // Get actual linearization point
285 Values actual3 = filter.calculateEstimate();
286 // Check
287 CHECK(assert_equal(expected3, actual3, 1e-6));
288
289 // Also check the single-variable version
290 Pose3 expectedPose1 = expected3.at<Pose3>(j: 1);
291 Pose3 expectedPose2 = expected3.at<Pose3>(j: 2);
292 Pose3 expectedPose3 = expected3.at<Pose3>(j: 3);
293 Pose3 expectedPose4 = expected3.at<Pose3>(j: 4);
294 // Also check the single-variable version
295 Pose3 actualPose1 = filter.calculateEstimate<Pose3>(key: 1);
296 Pose3 actualPose2 = filter.calculateEstimate<Pose3>(key: 2);
297 Pose3 actualPose3 = filter.calculateEstimate<Pose3>(key: 3);
298 Pose3 actualPose4 = filter.calculateEstimate<Pose3>(key: 4);
299 // Check
300 CHECK(assert_equal(expectedPose1, actualPose1, 1e-6));
301 CHECK(assert_equal(expectedPose2, actualPose2, 1e-6));
302 CHECK(assert_equal(expectedPose3, actualPose3, 1e-6));
303 CHECK(assert_equal(expectedPose4, actualPose4, 1e-6));
304}
305
306/* ************************************************************************* */
307TEST( ConcurrentBatchFilter, update_empty )
308{
309 // Create a set of optimizer parameters
310 LevenbergMarquardtParams parameters;
311 ConcurrentBatchFilter filter(parameters);
312
313 // Call update
314 filter.update();
315}
316
317/* ************************************************************************* */
318TEST( ConcurrentBatchFilter, update_multiple )
319{
320 // Create a Concurrent Batch Filter
321 LevenbergMarquardtParams parameters;
322 ConcurrentBatchFilter filter(parameters);
323
324 // Expected values is empty
325 Values expected1;
326 // Get Linearization Point
327 Values actual1 = filter.calculateEstimate();
328 // Check
329 CHECK(assert_equal(expected1, actual1));
330
331 // Add some factors to the filter
332 NonlinearFactorGraph newFactors2;
333 newFactors2.addPrior(key: 1, prior: poseInitial, model: noisePrior);
334 newFactors2.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
335 Values newValues2;
336 newValues2.insert(j: 1, val: Pose3().compose(g: poseError));
337 newValues2.insert(j: 2, val: newValues2.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
338 filter.update(newFactors: newFactors2, newTheta: newValues2);
339
340 // Expected values from full batch optimization
341 NonlinearFactorGraph allFactors2;
342 allFactors2.push_back(container: newFactors2);
343 Values allValues2;
344 allValues2.insert(values: newValues2);
345 Values expected2 = BatchOptimize(graph: allFactors2, theta: allValues2);
346 // Get actual linearization point
347 Values actual2 = filter.calculateEstimate();
348 // Check
349 CHECK(assert_equal(expected2, actual2, 1e-6));
350
351 // Add some more factors to the filter
352 NonlinearFactorGraph newFactors3;
353 newFactors3.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
354 newFactors3.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
355 Values newValues3;
356 newValues3.insert(j: 3, val: newValues2.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
357 newValues3.insert(j: 4, val: newValues3.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
358 filter.update(newFactors: newFactors3, newTheta: newValues3);
359
360 // Expected values from full batch optimization
361 NonlinearFactorGraph allFactors3;
362 allFactors3.push_back(container: newFactors2);
363 allFactors3.push_back(container: newFactors3);
364 Values allValues3;
365 allValues3.insert(values: newValues2);
366 allValues3.insert(values: newValues3);
367 Values expected3 = BatchOptimize(graph: allFactors3, theta: allValues3);
368 // Get actual linearization point
369 Values actual3 = filter.calculateEstimate();
370 // Check
371 CHECK(assert_equal(expected3, actual3, 1e-6));
372}
373
374/* ************************************************************************* */
375TEST( ConcurrentBatchFilter, update_and_marginalize )
376{
377 // Create a set of optimizer parameters
378 LevenbergMarquardtParams parameters;
379 ConcurrentBatchFilter filter(parameters);
380
381 // Add some factors to the filter
382 NonlinearFactorGraph newFactors;
383 newFactors.addPrior(key: 1, prior: poseInitial, model: noisePrior);
384 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
385 newFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
386 newFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
387 Values newValues;
388 newValues.insert(j: 1, val: Pose3().compose(g: poseError));
389 newValues.insert(j: 2, val: newValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
390 newValues.insert(j: 3, val: newValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
391 newValues.insert(j: 4, val: newValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
392
393 // Specify a subset of variables to marginalize/move to the smoother
394 FastList<Key> keysToMove;
395 keysToMove.push_back(x: 1);
396 keysToMove.push_back(x: 2);
397
398 // Update the filter
399 filter.update(newFactors, newTheta: newValues, keysToMove);
400
401 // Calculate expected factor graph and values
402 Values optimalValues = BatchOptimize(graph: newFactors, theta: newValues);
403
404
405 NonlinearFactorGraph partialGraph;
406 partialGraph.addPrior(key: 1, prior: poseInitial, model: noisePrior);
407 partialGraph.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
408 partialGraph.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
409
410 Values partialValues;
411 partialValues.insert(j: 1, val: optimalValues.at<Pose3>(j: 1));
412 partialValues.insert(j: 2, val: optimalValues.at<Pose3>(j: 2));
413 partialValues.insert(j: 3, val: optimalValues.at<Pose3>(j: 3));
414
415 // Create an ordering
416 Ordering ordering;
417 ordering.push_back(x: 1);
418 ordering.push_back(x: 2);
419 ordering.push_back(x: 3);
420
421 // Create the set of marginalizable variables
422 KeyVector linearIndices {1, 2};
423
424 GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(linearizationPoint: partialValues);
425 GaussianFactorGraph result = *linearPartialGraph.eliminatePartialMultifrontal(variables: linearIndices, function: EliminateCholesky).second;
426
427
428 NonlinearFactorGraph expectedGraph;
429
430 // These three lines create three empty factors in expectedGraph:
431 // this is done since the equal function in NonlinearFactorGraph also cares about the ordering of the factors
432 // and in the actualGraph produced by the HMF we first insert 5 nonlinear factors, then we delete 3 of them, by
433 // substituting them with a linear marginal
434 expectedGraph.push_back(factor: NonlinearFactor::shared_ptr());
435 expectedGraph.push_back(factor: NonlinearFactor::shared_ptr());
436 expectedGraph.push_back(factor: NonlinearFactor::shared_ptr());
437 // ==========================================================
438 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: noiseOdometery);
439
440 for(const GaussianFactor::shared_ptr& factor: result) {
441 expectedGraph.emplace_shared<LinearContainerFactor>(args: factor, args&: partialValues);
442 }
443
444
445 // Get the actual factor graph and optimal point
446 NonlinearFactorGraph actualGraph = filter.getFactors();
447 Values actualValues = filter.calculateEstimate();
448
449 Values expectedValues = optimalValues;
450
451 // Check
452 for(Key key: keysToMove) {
453 expectedValues.erase(j: key);
454 }
455
456
457 CHECK(assert_equal(expectedValues, actualValues, 1e-6));
458 CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
459}
460
461/* ************************************************************************* */
462TEST( ConcurrentBatchFilter, synchronize_0 )
463{
464 // Create a set of optimizer parameters
465 LevenbergMarquardtParams parameters;
466
467 // Create a Concurrent Batch Filter
468 ConcurrentBatchFilter filter(parameters);
469
470 // Create empty containers *from* the smoother
471 NonlinearFactorGraph smootherSummarization;
472 Values smootherSeparatorValues;
473
474 // Create expected values from the filter. For the case where the filter is empty, the expected values are also empty
475 NonlinearFactorGraph expectedSmootherFactors, expectedFilterSummarization;
476 Values expectedSmootherValues, expectedFilterSeparatorValues;
477
478 // Synchronize
479 NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
480 Values actualSmootherValues, actualFilterSeparatorValues;
481 filter.presync();
482 filter.synchronize(smootherSummarization, smootherSummarizationValues: smootherSeparatorValues);
483 filter.getSmootherFactors(smootherFactors&: actualSmootherFactors, smootherValues&: actualSmootherValues);
484 filter.getSummarizedFactors(filterSummarization&: actualFilterSummarization, filterSummarizationValues&: actualFilterSeparatorValues);
485 filter.postsync();
486
487 // Check
488 CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
489 CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
490 CHECK(assert_equal(expectedFilterSummarization, actualFilterSummarization, 1e-6));
491 CHECK(assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1e-6));
492}
493
494/* ************************************************************************* */
495TEST( ConcurrentBatchFilter, synchronize_1 )
496{
497 // Create a set of optimizer parameters
498 LevenbergMarquardtParams parameters;
499 parameters.maxIterations = 1;
500
501 // Create a Concurrent Batch Filter
502 ConcurrentBatchFilter filter(parameters);
503
504 // Insert factors into the filter, but do not marginalize out any variables.
505 // The synchronization should still be empty
506 NonlinearFactorGraph newFactors;
507 newFactors.addPrior(key: 1, prior: poseInitial, model: noisePrior);
508 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
509 Values newValues;
510 newValues.insert(j: 1, val: Pose3().compose(g: poseError));
511 newValues.insert(j: 2, val: newValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
512 filter.update(newFactors, newTheta: newValues);
513
514 // Create empty containers *from* the smoother
515 NonlinearFactorGraph smootherSummarization;
516 Values smootherSeparatorValues;
517
518 // Create expected values from the filter. For the case when nothing has been marginalized from the filter, the expected values are empty
519 NonlinearFactorGraph expectedSmootherFactors, expectedFilterSummarization;
520 Values expectedSmootherValues, expectedFilterSeparatorValues;
521
522 // Synchronize
523 NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
524 Values actualSmootherValues, actualFilterSeparatorValues;
525 filter.presync();
526 filter.synchronize(smootherSummarization, smootherSummarizationValues: smootherSeparatorValues);
527 filter.getSmootherFactors(smootherFactors&: actualSmootherFactors, smootherValues&: actualSmootherValues);
528 filter.getSummarizedFactors(filterSummarization&: actualFilterSummarization, filterSummarizationValues&: actualFilterSeparatorValues);
529 filter.postsync();
530
531 // Check
532 CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
533 CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
534 CHECK(assert_equal(expectedFilterSummarization, actualFilterSummarization, 1e-6));
535 CHECK(assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1e-6));
536}
537
538/* ************************************************************************* */
539TEST( ConcurrentBatchFilter, synchronize_2 )
540{
541 std::cout << "*********************** synchronize_2 ************************" << std::endl;
542 // Create a set of optimizer parameters
543 LevenbergMarquardtParams parameters;
544 //parameters.maxIterations = 1;
545
546 // Create a Concurrent Batch Filter
547 ConcurrentBatchFilter filter(parameters);
548
549 // Insert factors into the filter, and marginalize out one variable.
550 // There should not be information transmitted to the smoother from the filter
551 NonlinearFactorGraph newFactors;
552 NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
553 NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
554 newFactors.push_back(factor: factor1);
555 newFactors.push_back(factor: factor2);
556 Values newValues;
557 Pose3 value1 = Pose3().compose(g: poseError);
558 Pose3 value2 = value1.compose(g: poseOdometry).compose(g: poseError);
559 newValues.insert(j: 1, val: value1);
560 newValues.insert(j: 2, val: value2);
561 FastList<Key> keysToMove;
562 keysToMove.push_back(x: 1);
563 filter.update(newFactors, newTheta: newValues, keysToMove);
564 // this will not work, as in the filter only remains node 2, while 1 was marginalized out
565 // Values optimalValues = filter.calculateEstimate();
566
567 Values optimalValues = BatchOptimize(graph: newFactors, theta: newValues);
568
569 // Create empty containers *from* the smoother
570 NonlinearFactorGraph smootherSummarization;
571 Values smootherSeparatorValues;
572
573
574 // Create expected values from the filter.
575 // The smoother factors include any factor adjacent to a marginalized variable
576 NonlinearFactorGraph expectedSmootherFactors;
577 expectedSmootherFactors.push_back(factor: factor1);
578 expectedSmootherFactors.push_back(factor: factor2);
579 Values expectedSmootherValues;
580 // We only pass linearization points for the marginalized variables
581 expectedSmootherValues.insert(j: 1, val: optimalValues.at<Pose3>(j: 1));
582
583 // The filter summarization is the remaining factors from marginalizing out the requested variable
584 // In the current example, after marginalizing out 1, the filter only contains the separator (2), with
585 // no nonlinear factor attached to it, therefore no filter summarization needs to be passed to the smoother
586 NonlinearFactorGraph expectedFilterSummarization;
587 Values expectedFilterSeparatorValues;
588 expectedFilterSeparatorValues.insert(j: 2, val: optimalValues.at<Pose3>(j: 2));
589
590 // Synchronize
591 NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
592 Values actualSmootherValues, actualFilterSeparatorValues;
593 filter.presync();
594 filter.synchronize(smootherSummarization, smootherSummarizationValues: smootherSeparatorValues);
595 filter.getSmootherFactors(smootherFactors&: actualSmootherFactors, smootherValues&: actualSmootherValues);
596 filter.getSummarizedFactors(filterSummarization&: actualFilterSummarization, filterSummarizationValues&: actualFilterSeparatorValues);
597 filter.postsync();
598
599 // Check
600 CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
601 CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
602 CHECK(assert_equal(expectedFilterSummarization, actualFilterSummarization, 1e-6));
603 CHECK(assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1e-6));
604}
605
606/* ************************************************************************* */
607TEST( ConcurrentBatchFilter, synchronize_3 )
608{
609 std::cout << "*********************** synchronize_3 ************************" << std::endl;
610 // Create a set of optimizer parameters
611 LevenbergMarquardtParams parameters;
612 //parameters.maxIterations = 1;
613
614 // Create a Concurrent Batch Filter
615 ConcurrentBatchFilter filter(parameters);
616
617 // Insert factors into the filter, and marginalize out one variable.
618 // There should not be information transmitted to the smoother from the filter
619 NonlinearFactorGraph newFactors;
620 NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
621 NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
622 NonlinearFactor::shared_ptr factor3(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
623 newFactors.push_back(factor: factor1);
624 newFactors.push_back(factor: factor2);
625 newFactors.push_back(factor: factor3);
626
627 Values newValues;
628 Pose3 value1 = Pose3().compose(g: poseError);
629 Pose3 value2 = value1.compose(g: poseOdometry).compose(g: poseError);
630 Pose3 value3 = value2.compose(g: poseOdometry).compose(g: poseError);
631 newValues.insert(j: 1, val: value1);
632 newValues.insert(j: 2, val: value2);
633 newValues.insert(j: 3, val: value3);
634
635 FastList<Key> keysToMove;
636 keysToMove.push_back(x: 1);
637 // we add factors to the filter while marginalizing node 1
638 filter.update(newFactors, newTheta: newValues, keysToMove);
639
640 Values optimalValues = BatchOptimize(graph: newFactors, theta: newValues);
641
642 // In this example the smoother is empty
643 // Create empty containers *from* the smoother
644 NonlinearFactorGraph smootherSummarization;
645 Values smootherSeparatorValues;
646
647 // Create expected values from the filter.
648 // The smoother factors include any factor adjacent to a marginalized variable
649 NonlinearFactorGraph expectedSmootherFactors;
650 expectedSmootherFactors.push_back(factor: factor1);
651 expectedSmootherFactors.push_back(factor: factor2);
652 Values expectedSmootherValues;
653 // We only pass linearization points for the marginalized variables
654 expectedSmootherValues.insert(j: 1, val: optimalValues.at<Pose3>(j: 1));
655
656 // In the current example, after marginalizing out 1, the filter contains the separator 2 and node 3, with
657 // a nonlinear factor attached to them
658 // Why there is no summarization from filter ????
659 NonlinearFactorGraph expectedFilterSummarization;
660 Values expectedFilterSeparatorValues;
661 expectedFilterSeparatorValues.insert(j: 2, val: optimalValues.at<Pose3>(j: 2));
662 // ------------------------------------------------------------------------------
663 NonlinearFactorGraph partialGraph;
664 partialGraph.push_back(factor: factor3);
665
666 Values partialValues;
667 partialValues.insert(j: 2, val: optimalValues.at<Pose3>(j: 2));
668 partialValues.insert(j: 3, val: optimalValues.at<Pose3>(j: 3));
669
670 FastList<Key> keysToMarginalize;
671 keysToMarginalize.push_back(x: 3);
672
673 expectedFilterSummarization = CalculateMarginals(factorGraph: partialGraph, linPoint: partialValues, keysToMarginalize);
674 // ------------------------------------------------------------------------------
675 // Synchronize
676 NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
677 Values actualSmootherValues, actualFilterSeparatorValues;
678 filter.presync();
679 filter.synchronize(smootherSummarization, smootherSummarizationValues: smootherSeparatorValues);
680 filter.getSmootherFactors(smootherFactors&: actualSmootherFactors, smootherValues&: actualSmootherValues);
681 filter.getSummarizedFactors(filterSummarization&: actualFilterSummarization, filterSummarizationValues&: actualFilterSeparatorValues);
682 filter.postsync();
683
684 // Check
685 CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
686 CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
687 CHECK(assert_equal(expectedFilterSummarization, actualFilterSummarization, 1e-6));
688 CHECK(assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1e-6));
689}
690
691/* ************************************************************************* */
692TEST( ConcurrentBatchFilter, synchronize_4 )
693{
694 // Create a set of optimizer parameters
695 LevenbergMarquardtParams parameters;
696 parameters.maxIterations = 1;
697
698 // Create a Concurrent Batch Filter
699 ConcurrentBatchFilter filter(parameters);
700
701 // Insert factors into the filter, and marginalize out one variable.
702 // There should not be information transmitted to the smoother from the filter
703 NonlinearFactorGraph newFactors;
704 NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
705 NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
706 NonlinearFactor::shared_ptr factor3(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
707 newFactors.push_back(factor: factor1);
708 newFactors.push_back(factor: factor2);
709 newFactors.push_back(factor: factor3);
710
711 Values newValues;
712 Pose3 value1 = Pose3().compose(g: poseError);
713 Pose3 value2 = value1.compose(g: poseOdometry).compose(g: poseError);
714 Pose3 value3 = value2.compose(g: poseOdometry).compose(g: poseError);
715 newValues.insert(j: 1, val: value1);
716 newValues.insert(j: 2, val: value2);
717 newValues.insert(j: 3, val: value3);
718
719 FastList<Key> keysToMove;
720 keysToMove.push_back(x: 1);
721 // we add factors to the filter while marginalizing node 1
722 filter.update(newFactors, newTheta: newValues, keysToMove);
723
724 Values optimalValuesFilter = BatchOptimize(graph: newFactors, theta: newValues,maxIter: 1);
725
726 // In this example the smoother contains a between factor and a prior factor
727 // COMPUTE SUMMARIZATION ON THE SMOOTHER SIDE
728 NonlinearFactorGraph smootherSummarization;
729 Values smootherSeparatorValues;
730
731 // Create expected values from the filter.
732 // The smoother factors include any factor adjacent to a marginalized variable
733 NonlinearFactorGraph expectedSmootherFactors;
734 expectedSmootherFactors.push_back(factor: factor1);
735 expectedSmootherFactors.push_back(factor: factor2);
736 Values expectedSmootherValues;
737 // We only pass linearization points for the marginalized variables
738 expectedSmootherValues.insert(j: 1, val: optimalValuesFilter.at<Pose3>(j: 1));
739
740 // COMPUTE SUMMARIZATION ON THE FILTER SIDE
741 // In the current example, after marginalizing out 1, the filter contains the separator 2 and node 3, with
742 // a nonlinear factor attached to them
743 // Why there is no summarization from filter ????
744 NonlinearFactorGraph expectedFilterSummarization;
745 Values expectedFilterSeparatorValues;
746 expectedFilterSeparatorValues.insert(j: 2, val: optimalValuesFilter.at<Pose3>(j: 2));
747 // ------------------------------------------------------------------------------
748 NonlinearFactorGraph partialGraphFilter;
749 partialGraphFilter.push_back(factor: factor3);
750
751 Values partialValuesFilter;
752 partialValuesFilter.insert(j: 2, val: optimalValuesFilter.at<Pose3>(j: 2));
753 partialValuesFilter.insert(j: 3, val: optimalValuesFilter.at<Pose3>(j: 3));
754
755 // Create an ordering
756 Ordering orderingFilter;
757 orderingFilter.push_back(x: 3);
758 orderingFilter.push_back(x: 2);
759
760 FastList<Key> keysToMarginalize;
761 keysToMarginalize.push_back(x: 3);
762
763 expectedFilterSummarization = CalculateMarginals(factorGraph: partialGraphFilter, linPoint: partialValuesFilter, keysToMarginalize);
764 // ------------------------------------------------------------------------------
765 // Synchronize
766 // This is only an information compression/exchange: to actually incorporate the info we should call update
767 NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
768 Values actualSmootherValues, actualFilterSeparatorValues;
769 filter.presync();
770 filter.synchronize(smootherSummarization, smootherSummarizationValues: smootherSeparatorValues);
771 filter.getSmootherFactors(smootherFactors&: actualSmootherFactors, smootherValues&: actualSmootherValues);
772 filter.getSummarizedFactors(filterSummarization&: actualFilterSummarization, filterSummarizationValues&: actualFilterSeparatorValues);
773 filter.postsync();
774
775
776 // Check
777 CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
778 CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
779 CHECK(assert_equal(expectedFilterSummarization, actualFilterSummarization, 1e-6));
780 CHECK(assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1e-6));
781}
782
783
784/* ************************************************************************* */
785TEST( ConcurrentBatchFilter, synchronize_5 )
786{
787 std::cout << "*********************** synchronize_5 ************************" << std::endl;
788 // Create a set of optimizer parameters
789 LevenbergMarquardtParams parameters;
790 parameters.maxIterations = 1;
791
792 // Create a Concurrent Batch Filter
793 ConcurrentBatchFilter filter(parameters);
794
795 // Insert factors into the filter, and marginalize out one variable.
796 // There should not be information transmitted to the smoother from the filter
797 NonlinearFactorGraph newFactors;
798 NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
799 NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
800 NonlinearFactor::shared_ptr factor3(new PriorFactor<Pose3>(2, poseInitial, noisePrior));
801 NonlinearFactor::shared_ptr factor4(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
802 NonlinearFactor::shared_ptr factor5(new BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
803 newFactors.push_back(factor: factor1);
804 newFactors.push_back(factor: factor2);
805 newFactors.push_back(factor: factor3);
806 newFactors.push_back(factor: factor4);
807 newFactors.push_back(factor: factor5);
808
809 Values newValues;
810 Pose3 value1 = Pose3().compose(g: poseError);
811 Pose3 value2 = value1.compose(g: poseOdometry).compose(g: poseError);
812 Pose3 value3 = value2.compose(g: poseOdometry).compose(g: poseError);
813 Pose3 value4 = value3.compose(g: poseOdometry).compose(g: poseError);
814
815 newValues.insert(j: 1, val: value1);
816 newValues.insert(j: 2, val: value2);
817 newValues.insert(j: 3, val: value3);
818 newValues.insert(j: 4, val: value4);
819
820 FastList<Key> keysToMove;
821 keysToMove.push_back(x: 1);
822 // we add factors to the filter while marginalizing node 1
823 filter.update(newFactors, newTheta: newValues, keysToMove);
824
825 // At the beginning the smoother is empty
826 NonlinearFactorGraph smootherSummarization;
827 Values smootherSeparatorValues;
828
829 // Synchronize
830 // This is only an information compression/exchange: to actually incorporate the info we should call update
831 NonlinearFactorGraph actualSmootherFactors, actualFilterSummarization;
832 Values actualSmootherValues, actualFilterSeparatorValues;
833 filter.presync();
834 filter.synchronize(smootherSummarization, smootherSummarizationValues: smootherSeparatorValues);
835 filter.getSmootherFactors(smootherFactors&: actualSmootherFactors, smootherValues&: actualSmootherValues);
836 filter.getSummarizedFactors(filterSummarization&: actualFilterSummarization, filterSummarizationValues&: actualFilterSeparatorValues);
837 filter.postsync();
838
839 NonlinearFactorGraph expectedSmootherFactors;
840 expectedSmootherFactors.push_back(factor: factor1);
841 expectedSmootherFactors.push_back(factor: factor2);
842
843 Values optimalValues = BatchOptimize(graph: newFactors, theta: newValues, maxIter: 1);
844 Values expectedSmootherValues;
845 // Pose3 cast is useless in this case (but we still put it as an example): values and graphs can handle generic
846 // geometric objects. You really need the <Pose3> when you need to fill in a Pose3 object with the .at()
847 expectedSmootherValues.insert(j: 1,val: optimalValues.at<Pose3>(j: 1));
848
849 // Check
850 CHECK(assert_equal(expectedSmootherFactors, actualSmootherFactors, 1e-6));
851 CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-6));
852
853 // at this point the filter contains: nodes 2 3 4 and factors 3 4 5 + marginal on 2
854 Values optimalValues2 = BatchOptimize(graph: filter.getFactors(),theta: filter.getLinearizationPoint(),maxIter: 1);
855
856 FastList<Key> keysToMove2;
857 keysToMove2.push_back(x: 2);
858 newFactors.resize(size: 0);
859 newValues.clear();
860 // we add factors to the filter while marginalizing node 1
861 filter.update(newFactors, newTheta: newValues, keysToMove: keysToMove2);
862
863
864 // At the beginning the smoother is empty
865 NonlinearFactorGraph smootherSummarization2;
866 Values smootherSeparatorValues2;
867
868 // ------------------------------------------------------------------------------
869 // We fake the computation of the smoother separator
870 // currently the smoother contains factor 1 and 2 and node 1 and 2
871
872 NonlinearFactorGraph partialGraph;
873 partialGraph.push_back(factor: factor1);
874 partialGraph.push_back(factor: factor2);
875
876 // we also assume that the smoother received an extra factor (e.g., a prior on 1)
877 partialGraph.push_back(factor: factor1);
878
879 Values partialValues;
880 // Batch optimization updates all linearization points but the ones of the separator
881 // In this case, we start with no separator (everything is in the filter), therefore,
882 // we update all linearization point
883 partialValues.insert(j: 2, val: optimalValues.at(j: 2)); //<-- does not actually exist
884 //The linearization point of 1 is controlled by the smoother and
885 // we are artificially setting that to something different to what was in the filter
886 partialValues.insert(j: 1, val: Pose3().compose(g: poseError.inverse()));
887
888 FastList<Key> keysToMarginalize;
889 keysToMarginalize.push_back(x: 1);
890
891 smootherSummarization2 = CalculateMarginals(factorGraph: partialGraph, linPoint: partialValues, keysToMarginalize);
892 smootherSeparatorValues2.insert(j: 2, val: partialValues.at(j: 2));
893
894 // ------------------------------------------------------------------------------
895 // Synchronize
896 // This is only an information compression/exchange: to actually incorporate the info we should call update
897 NonlinearFactorGraph actualSmootherFactors2, actualFilterSummarization2;
898 Values actualSmootherValues2, actualFilterSeparatorValues2;
899 filter.presync();
900 filter.synchronize(smootherSummarization: smootherSummarization2, smootherSummarizationValues: smootherSeparatorValues2);
901 filter.getSmootherFactors(smootherFactors&: actualSmootherFactors2, smootherValues&: actualSmootherValues2);
902 filter.getSummarizedFactors(filterSummarization&: actualFilterSummarization2, filterSummarizationValues&: actualFilterSeparatorValues2);
903 filter.postsync();
904
905 NonlinearFactorGraph expectedSmootherFactors2;
906 expectedSmootherFactors2.push_back(factor: factor3);
907 expectedSmootherFactors2.push_back(factor: factor4);
908
909 Values expectedSmootherValues2;
910 expectedSmootherValues2.insert(j: 2, val: optimalValues.at(j: 2));
911
912 // Check
913 CHECK(assert_equal(expectedSmootherFactors2, actualSmootherFactors2, 1e-6));
914 CHECK(assert_equal(expectedSmootherValues2, actualSmootherValues2, 1e-6));
915
916
917 // In this example the smoother contains a between factor and a prior factor
918 // COMPUTE SUMMARIZATION ON THE FILTER SIDE
919 // ------------------------------------------------------------------------------
920 // This cannot be nonempty for the first call to synchronize
921 NonlinearFactorGraph partialGraphFilter;
922 partialGraphFilter.push_back(factor: factor5);
923
924
925 Values partialValuesFilter;
926 partialValuesFilter.insert(j: 3, val: optimalValues2.at(j: 3));
927 partialValuesFilter.insert(j: 4, val: optimalValues2.at(j: 4));
928
929 FastList<Key> keysToMarginalize2;
930 keysToMarginalize2.push_back(x: 4);
931
932 NonlinearFactorGraph expectedFilterSummarization2 = CalculateMarginals(factorGraph: partialGraphFilter, linPoint: partialValuesFilter, keysToMarginalize: keysToMarginalize2);
933 Values expectedFilterSeparatorValues2;
934 expectedFilterSeparatorValues2.insert(j: 3, val: optimalValues2.at(j: 3));
935
936 CHECK(assert_equal(expectedFilterSummarization2, actualFilterSummarization2, 1e-6));
937 CHECK(assert_equal(expectedFilterSeparatorValues2, actualFilterSeparatorValues2, 1e-6));
938
939 // Now we should check that the smooother summarization on the old separator is correctly propagated
940 // on the new separator by the filter
941 NonlinearFactorGraph partialGraphTransition;
942 partialGraphTransition.push_back(factor: factor3);
943 partialGraphTransition.push_back(factor: factor4);
944 partialGraphTransition.push_back(container: smootherSummarization2);
945
946 Values partialValuesTransition;
947 partialValuesTransition.insert(j: 2,val: optimalValues.at(j: 2));
948 partialValuesTransition.insert(j: 3,val: optimalValues2.at(j: 3));
949
950 FastList<Key> keysToMarginalize3;
951 keysToMarginalize3.push_back(x: 2);
952
953 NonlinearFactorGraph expectedFilterGraph;
954
955 // The assert equal will check if the expected and the actual graphs are the same, taking into account
956 // orders of the factors, and empty factors:
957 // in the filter we originally had 5 factors, and by marginalizing 1 and 2 we got rid of factors 1 2 3 4,
958 // therefore in the expected Factor we should include 4 empty factors.
959 // Note that the unit test will fail also if we change the order of the factors, due to the definition of assert_equal
960 NonlinearFactor::shared_ptr factorEmpty;
961 expectedFilterGraph.push_back(factor: factorEmpty);
962 expectedFilterGraph.push_back(factor: factorEmpty);
963 expectedFilterGraph.push_back(factor: factorEmpty);
964 expectedFilterGraph.push_back(factor: factorEmpty);
965 expectedFilterGraph.push_back(factor: factor5);
966 expectedFilterGraph.push_back(container: CalculateMarginals(factorGraph: partialGraphTransition, linPoint: partialValuesTransition, keysToMarginalize: keysToMarginalize3));
967
968 NonlinearFactorGraph actualFilterGraph;
969 actualFilterGraph = filter.getFactors();
970
971 CHECK(assert_equal(expectedFilterGraph, actualFilterGraph, 1e-6));
972}
973
974
975///* ************************************************************************* */
976TEST( ConcurrentBatchFilter, CalculateMarginals_1 )
977{
978 // We compare the manual computation of the linear marginals from a factor graph, with the function CalculateMarginals
979 NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
980 NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
981 NonlinearFactor::shared_ptr factor3(new PriorFactor<Pose3>(2, poseInitial, noisePrior));
982 NonlinearFactor::shared_ptr factor4(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
983
984 NonlinearFactorGraph factorGraph;
985 factorGraph.push_back(factor: factor1);
986 factorGraph.push_back(factor: factor2);
987 factorGraph.push_back(factor: factor3);
988 factorGraph.push_back(factor: factor4);
989
990 Values newValues;
991 Pose3 value1 = Pose3().compose(g: poseError);
992 Pose3 value2 = value1.compose(g: poseOdometry).compose(g: poseError);
993 Pose3 value3 = value2.compose(g: poseOdometry).compose(g: poseError);
994
995 newValues.insert(j: 1, val: value1);
996 newValues.insert(j: 2, val: value2);
997 newValues.insert(j: 3, val: value3);
998
999 // We first manually
1000 // Create an ordering
1001 Ordering ordering;
1002 ordering.push_back(x: 1);
1003 ordering.push_back(x: 2);
1004 ordering.push_back(x: 3);
1005
1006 GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(linearizationPoint: newValues);
1007
1008 // Create the set of marginalizable variables
1009 KeyVector linearIndices {1};
1010
1011 GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(variables: linearIndices, function: EliminateCholesky).second;
1012
1013 NonlinearFactorGraph expectedMarginals;
1014 for(const GaussianFactor::shared_ptr& factor: result) {
1015 expectedMarginals.push_back(factor: LinearContainerFactor(factor, newValues));
1016
1017 }
1018
1019 FastList<Key> keysToMarginalize;
1020 keysToMarginalize.push_back(x: 1);
1021 NonlinearFactorGraph actualMarginals;
1022 actualMarginals = CalculateMarginals(factorGraph, linPoint: newValues, keysToMarginalize);
1023
1024 // Check
1025 CHECK(assert_equal(expectedMarginals, actualMarginals, 1e-6));
1026}
1027
1028///* ************************************************************************* */
1029TEST( ConcurrentBatchFilter, CalculateMarginals_2 )
1030{
1031 // We compare the manual computation of the linear marginals from a factor graph, with the function CalculateMarginals
1032 NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
1033 NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1034 NonlinearFactor::shared_ptr factor3(new PriorFactor<Pose3>(2, poseInitial, noisePrior));
1035 NonlinearFactor::shared_ptr factor4(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
1036
1037 NonlinearFactorGraph factorGraph;
1038 factorGraph.push_back(factor: factor1);
1039 factorGraph.push_back(factor: factor2);
1040 factorGraph.push_back(factor: factor3);
1041 factorGraph.push_back(factor: factor4);
1042
1043 Values newValues;
1044 Pose3 value1 = Pose3().compose(g: poseError);
1045 Pose3 value2 = value1.compose(g: poseOdometry).compose(g: poseError);
1046 Pose3 value3 = value2.compose(g: poseOdometry).compose(g: poseError);
1047
1048 newValues.insert(j: 1, val: value1);
1049 newValues.insert(j: 2, val: value2);
1050 newValues.insert(j: 3, val: value3);
1051
1052 // We first manually
1053 // Create an ordering
1054 Ordering ordering;
1055 ordering.push_back(x: 1);
1056 ordering.push_back(x: 2);
1057 ordering.push_back(x: 3);
1058
1059 GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(linearizationPoint: newValues);
1060
1061 // Create the set of marginalizable variables
1062 KeyVector linearIndices {1, 2};
1063
1064 GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(variables: linearIndices, function: EliminateCholesky).second;
1065
1066 NonlinearFactorGraph expectedMarginals;
1067 for(const GaussianFactor::shared_ptr& factor: result) {
1068 expectedMarginals.push_back(factor: LinearContainerFactor(factor, newValues));
1069
1070 }
1071
1072 FastList<Key> keysToMarginalize;
1073 keysToMarginalize.push_back(x: 1);
1074 keysToMarginalize.push_back(x: 2);
1075 NonlinearFactorGraph actualMarginals;
1076 actualMarginals = CalculateMarginals(factorGraph, linPoint: newValues, keysToMarginalize);
1077
1078 // Check
1079 CHECK(assert_equal(expectedMarginals, actualMarginals, 1e-6));
1080}
1081
1082///* ************************************************************************* */
1083TEST( ConcurrentBatchFilter, removeFactors_topology_1 )
1084{
1085 std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl;
1086
1087 // Create a set of optimizer parameters
1088 LevenbergMarquardtParams parameters;
1089 ConcurrentBatchFilter filter(parameters);
1090
1091 // Add some factors to the filter
1092 NonlinearFactorGraph newFactors;
1093 newFactors.addPrior(key: 1, prior: poseInitial, model: noisePrior);
1094 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1095 newFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
1096 newFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
1097 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1098
1099 Values newValues;
1100 newValues.insert(j: 1, val: Pose3().compose(g: poseError));
1101 newValues.insert(j: 2, val: newValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
1102 newValues.insert(j: 3, val: newValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
1103 newValues.insert(j: 4, val: newValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
1104
1105 // Specify a subset of variables to marginalize/move to the smoother
1106 FastList<Key> keysToMove;
1107
1108 // Update the filter: add all factors
1109 filter.update(newFactors, newTheta: newValues, keysToMove);
1110
1111 // factor we want to remove
1112 // NOTE: we can remove factors, paying attention that the remaining graph remains connected
1113 // we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
1114 std::vector<size_t> removeFactorIndices(1,1);
1115
1116 // Add no factors to the filter (we only want to test the removal)
1117 NonlinearFactorGraph noFactors;
1118 Values noValues;
1119 filter.update(newFactors: noFactors, newTheta: noValues, keysToMove, removeFactorIndices);
1120
1121 NonlinearFactorGraph actualGraph = filter.getFactors();
1122
1123 NonlinearFactorGraph expectedGraph;
1124 expectedGraph.addPrior(key: 1, prior: poseInitial, model: noisePrior);
1125 // we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
1126 // we should add an empty one, so that the ordering and labeling of the factors is preserved
1127 expectedGraph.push_back(factor: NonlinearFactor::shared_ptr());
1128 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
1129 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: noiseOdometery);
1130 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
1131
1132 CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
1133}
1134
1135///* ************************************************************************* */
1136TEST( ConcurrentBatchFilter, removeFactors_topology_2 )
1137{
1138 std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl;
1139 // we try removing the last factor
1140
1141
1142 // Create a set of optimizer parameters
1143 LevenbergMarquardtParams parameters;
1144 ConcurrentBatchFilter filter(parameters);
1145
1146 // Add some factors to the filter
1147 NonlinearFactorGraph newFactors;
1148 newFactors.addPrior(key: 1, prior: poseInitial, model: noisePrior);
1149 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1150 newFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
1151 newFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
1152 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1153
1154 Values newValues;
1155 newValues.insert(j: 1, val: Pose3().compose(g: poseError));
1156 newValues.insert(j: 2, val: newValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
1157 newValues.insert(j: 3, val: newValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
1158 newValues.insert(j: 4, val: newValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
1159
1160 // Specify a subset of variables to marginalize/move to the smoother
1161 FastList<Key> keysToMove;
1162
1163 // Update the filter: add all factors
1164 filter.update(newFactors, newTheta: newValues, keysToMove);
1165
1166 // factor we want to remove
1167 // NOTE: we can remove factors, paying attention that the remaining graph remains connected
1168 // we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
1169 std::vector<size_t> removeFactorIndices(1,4);
1170
1171 // Add no factors to the filter (we only want to test the removal)
1172 NonlinearFactorGraph noFactors;
1173 Values noValues;
1174 filter.update(newFactors: noFactors, newTheta: noValues, keysToMove, removeFactorIndices);
1175
1176 NonlinearFactorGraph actualGraph = filter.getFactors();
1177
1178 NonlinearFactorGraph expectedGraph;
1179 expectedGraph.addPrior(key: 1, prior: poseInitial, model: noisePrior);
1180 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
1181 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
1182 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: noiseOdometery);
1183 // we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
1184 // we should add an empty one, so that the ordering and labeling of the factors is preserved
1185 expectedGraph.push_back(factor: NonlinearFactor::shared_ptr());
1186
1187 CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
1188}
1189
1190
1191///* ************************************************************************* */
1192TEST( ConcurrentBatchFilter, removeFactors_topology_3 )
1193{
1194 std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl;
1195 // we try removing the first factor
1196
1197 // Create a set of optimizer parameters
1198 LevenbergMarquardtParams parameters;
1199 ConcurrentBatchFilter filter(parameters);
1200
1201 // Add some factors to the filter
1202 NonlinearFactorGraph newFactors;
1203 newFactors.addPrior(key: 1, prior: poseInitial, model: noisePrior);
1204 newFactors.addPrior(key: 1, prior: poseInitial, model: noisePrior);
1205 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
1206 newFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
1207 newFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
1208
1209 Values newValues;
1210 newValues.insert(j: 1, val: Pose3().compose(g: poseError));
1211 newValues.insert(j: 2, val: newValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
1212 newValues.insert(j: 3, val: newValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
1213 newValues.insert(j: 4, val: newValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
1214
1215 // Specify a subset of variables to marginalize/move to the smoother
1216 FastList<Key> keysToMove;
1217
1218 // Update the filter: add all factors
1219 filter.update(newFactors, newTheta: newValues, keysToMove);
1220
1221 // factor we want to remove
1222 // NOTE: we can remove factors, paying attention that the remaining graph remains connected
1223 // we remove a single factor, the number 0, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
1224 std::vector<size_t> removeFactorIndices(1,0);
1225
1226 // Add no factors to the filter (we only want to test the removal)
1227 NonlinearFactorGraph noFactors;
1228 Values noValues;
1229 filter.update(newFactors: noFactors, newTheta: noValues, keysToMove, removeFactorIndices);
1230
1231 NonlinearFactorGraph actualGraph = filter.getFactors();
1232
1233 NonlinearFactorGraph expectedGraph;
1234 // we should add an empty one, so that the ordering and labeling of the factors is preserved
1235 expectedGraph.push_back(factor: NonlinearFactor::shared_ptr());
1236 expectedGraph.addPrior(key: 1, prior: poseInitial, model: noisePrior);
1237 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
1238 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
1239 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: noiseOdometery);
1240
1241 CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
1242}
1243
1244///* ************************************************************************* */
1245TEST( ConcurrentBatchFilter, removeFactors_values )
1246{
1247 std::cout << "*********************** removeFactors_values ************************" << std::endl;
1248 // we try removing the last factor
1249
1250 // Create a set of optimizer parameters
1251 LevenbergMarquardtParams parameters;
1252 ConcurrentBatchFilter filter(parameters);
1253
1254 // Add some factors to the filter
1255 NonlinearFactorGraph newFactors;
1256 newFactors.addPrior(key: 1, prior: poseInitial, model: noisePrior);
1257 newFactors.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
1258 newFactors.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
1259 newFactors.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: noiseOdometery);
1260 newFactors.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
1261
1262 Values newValues;
1263 newValues.insert(j: 1, val: Pose3().compose(g: poseError));
1264 newValues.insert(j: 2, val: newValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
1265 newValues.insert(j: 3, val: newValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
1266 newValues.insert(j: 4, val: newValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
1267
1268 // Specify a subset of variables to marginalize/move to the smoother
1269 FastList<Key> keysToMove;
1270
1271 // Update the filter: add all factors
1272 filter.update(newFactors, newTheta: newValues, keysToMove);
1273
1274 // factor we want to remove
1275 // NOTE: we can remove factors, paying attention that the remaining graph remains connected
1276 // we remove a single factor, the number 4, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
1277 std::vector<size_t> removeFactorIndices(1,4);
1278
1279 // Add no factors to the filter (we only want to test the removal)
1280 NonlinearFactorGraph noFactors;
1281 Values noValues;
1282 filter.update(newFactors: noFactors, newTheta: noValues, keysToMove, removeFactorIndices);
1283
1284 NonlinearFactorGraph actualGraph = filter.getFactors();
1285 Values actualValues = filter.calculateEstimate();
1286
1287 // note: factors are removed before the optimization
1288 NonlinearFactorGraph expectedGraph;
1289 expectedGraph.addPrior(key: 1, prior: poseInitial, model: noisePrior);
1290 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
1291 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
1292 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: noiseOdometery);
1293 // we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
1294 // we should add an empty one, so that the ordering and labeling of the factors is preserved
1295 expectedGraph.push_back(factor: NonlinearFactor::shared_ptr());
1296
1297 // Calculate expected factor graph and values
1298 Values expectedValues = BatchOptimize(graph: expectedGraph, theta: newValues);
1299
1300 CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
1301 CHECK(assert_equal(expectedValues, actualValues, 1e-6));
1302}
1303
1304///* ************************************************************************* */
1305//TEST( ConcurrentBatchFilter, synchronize_10 )
1306//{
1307// // Create a set of optimizer parameters
1308// LevenbergMarquardtParams parameters;
1309// parameters.maxIterations = 1;
1310//
1311// // Create a Concurrent Batch Filter
1312// ConcurrentBatchFilter filter(parameters);
1313//
1314// // Insert factors into the filter, and marginalize out several variables.
1315// // This test places several factors in the smoother side, while leaving
1316// // several factors on the filter side
1317//}
1318//
1319///* ************************************************************************* */
1320//TEST( ConcurrentBatchFilter, synchronize_11 )
1321//{
1322// // Create a set of optimizer parameters
1323// LevenbergMarquardtParams parameters;
1324// parameters.maxIterations = 1;
1325//
1326// // Create a Concurrent Batch Filter
1327// ConcurrentBatchFilter filter(parameters);
1328//
1329// // Insert factors into the filter, and marginalize out several variables.
1330//
1331// // Generate a non-empty smoother update, simulating synchronizing with a non-empty smoother
1332//}
1333
1334/* ************************************************************************* */
1335int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
1336/* ************************************************************************* */
1337