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