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 testConcurrentIncrementalSmoother.cpp
14 * @brief Unit tests for the Concurrent Batch Smoother
15 * @author Stephen Williams (swilliams8@gatech.edu)
16 * @date Jan 5, 2013
17 */
18
19#include <gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.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) );
42const Pose3 poseError( Rot3::RzRyRx(xyz: Vector3(0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) );
43
44// Set up noise models for the factors
45const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.10);
46const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(sigmas: (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished());
47const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(sigmas: (Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished());
48
49/* ************************************************************************* */
50Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int maxIter = 100) {
51
52 // Create an L-M optimizer
53 ISAM2Params parameters;
54 parameters.optimizationParams = ISAM2GaussNewtonParams();
55// parameters.maxIterations = maxIter;
56// parameters.lambdaUpperBound = 1;
57// parameters.lambdaInitial = 1;
58// parameters.verbosity = NonlinearOptimizerParams::ERROR;
59// parameters.verbosityLM = ISAM2Params::DAMPED;
60// parameters.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_QR;
61
62 ISAM2 optimizer(parameters);
63 optimizer.update( newFactors: graph, newTheta: theta );
64 Values result = optimizer.calculateEstimate();
65 return result;
66
67}
68
69} // end namespace
70
71
72
73
74/* ************************************************************************* */
75TEST( ConcurrentIncrementalSmootherGN, equals )
76{
77 // TODO: Test 'equals' more vigorously
78
79 // Create a Concurrent Batch Smoother
80 ISAM2Params parameters1;
81 parameters1.optimizationParams = ISAM2GaussNewtonParams();
82 ConcurrentIncrementalSmoother smoother1(parameters1);
83
84 // Create an identical Concurrent Batch Smoother
85 ISAM2Params parameters2;
86 parameters2.optimizationParams = ISAM2GaussNewtonParams();
87 ConcurrentIncrementalSmoother smoother2(parameters2);
88
89 // Create a different Concurrent Batch Smoother
90 ISAM2Params parameters3;
91 parameters3.optimizationParams = ISAM2GaussNewtonParams();
92// parameters3.maxIterations = 1;
93 ConcurrentIncrementalSmoother smoother3(parameters3);
94
95 CHECK(assert_equal(smoother1, smoother1));
96 CHECK(assert_equal(smoother1, smoother2));
97// CHECK(assert_inequal(smoother1, smoother3));
98}
99
100/* ************************************************************************* */
101TEST( ConcurrentIncrementalSmootherGN, getFactors )
102{
103 // Create a Concurrent Batch Smoother
104 ISAM2Params parameters;
105 parameters.optimizationParams = ISAM2GaussNewtonParams();
106 ConcurrentIncrementalSmoother smoother(parameters);
107
108 // Expected graph is empty
109 NonlinearFactorGraph expected1;
110 // Get actual graph
111 NonlinearFactorGraph actual1 = smoother.getFactors();
112 // Check
113 CHECK(assert_equal(expected1, actual1));
114
115 // Add some factors to the smoother
116 NonlinearFactorGraph newFactors1;
117 newFactors1.addPrior(key: 1, prior: poseInitial, model: noisePrior);
118 newFactors1.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
119 Values newValues1;
120 newValues1.insert(j: 1, val: Pose3());
121 newValues1.insert(j: 2, val: newValues1.at<Pose3>(j: 1).compose(g: poseOdometry));
122 smoother.update(newFactors: newFactors1, newTheta: newValues1);
123
124 // Expected graph
125 NonlinearFactorGraph expected2;
126 expected2.push_back(container: newFactors1);
127 // Get actual graph
128 NonlinearFactorGraph actual2 = smoother.getFactors();
129 // Check
130 CHECK(assert_equal(expected2, actual2));
131
132 // Add some more factors to the smoother
133 NonlinearFactorGraph newFactors2;
134 newFactors2.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
135 newFactors2.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
136 Values newValues2;
137 newValues2.insert(j: 3, val: newValues1.at<Pose3>(j: 2).compose(g: poseOdometry));
138 newValues2.insert(j: 4, val: newValues2.at<Pose3>(j: 3).compose(g: poseOdometry));
139 smoother.update(newFactors: newFactors2, newTheta: newValues2);
140
141 // Expected graph
142 NonlinearFactorGraph expected3;
143 expected3.push_back(container: newFactors1);
144 expected3.push_back(container: newFactors2);
145 // Get actual graph
146 NonlinearFactorGraph actual3 = smoother.getFactors();
147 // Check
148 CHECK(assert_equal(expected3, actual3));
149}
150
151/* ************************************************************************* */
152TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
153{
154 // Create a Concurrent Batch Smoother
155 ISAM2Params parameters;
156 parameters.optimizationParams = ISAM2GaussNewtonParams();
157 ConcurrentIncrementalSmoother smoother(parameters);
158
159 // Expected values is empty
160 Values expected1;
161 // Get Linearization Point
162 Values actual1 = smoother.getLinearizationPoint();
163 // Check
164 CHECK(assert_equal(expected1, actual1));
165
166 // Add some factors to the smoother
167 NonlinearFactorGraph newFactors1;
168 newFactors1.addPrior(key: 1, prior: poseInitial, model: noisePrior);
169 newFactors1.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
170 Values newValues1;
171 newValues1.insert(j: 1, val: Pose3());
172 newValues1.insert(j: 2, val: newValues1.at<Pose3>(j: 1).compose(g: poseOdometry));
173 smoother.update(newFactors: newFactors1, newTheta: newValues1);
174
175 // Expected values is equivalent to the provided values only because the provided linearization points were optimal
176 Values expected2;
177 expected2.insert(values: newValues1);
178 // Get actual linearization point
179 Values actual2 = smoother.getLinearizationPoint();
180 // Check
181 CHECK(assert_equal(expected2, actual2));
182
183 // Add some more factors to the smoother
184 NonlinearFactorGraph newFactors2;
185 newFactors2.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
186 newFactors2.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
187 Values newValues2;
188 newValues2.insert(j: 3, val: newValues1.at<Pose3>(j: 2).compose(g: poseOdometry));
189 newValues2.insert(j: 4, val: newValues2.at<Pose3>(j: 3).compose(g: poseOdometry));
190 smoother.update(newFactors: newFactors2, newTheta: newValues2);
191
192 // Expected values is equivalent to the provided values only because the provided linearization points were optimal
193 Values expected3;
194 expected3.insert(values: newValues1);
195 expected3.insert(values: newValues2);
196 // Get actual linearization point
197 Values actual3 = smoother.getLinearizationPoint();
198 // Check
199 CHECK(assert_equal(expected3, actual3));
200}
201
202/* ************************************************************************* */
203TEST( ConcurrentIncrementalSmootherGN, getDelta )
204{
205 // TODO: Think about how to check ordering...
206}
207
208/* ************************************************************************* */
209TEST( ConcurrentIncrementalSmootherGN, calculateEstimate )
210{
211 // Create a Concurrent Batch Smoother
212 ISAM2Params parameters;
213 parameters.optimizationParams = ISAM2GaussNewtonParams();
214 ConcurrentIncrementalSmoother smoother(parameters);
215
216 // Expected values is empty
217 Values expected1;
218 // Get Linearization Point
219 Values actual1 = smoother.calculateEstimate();
220 // Check
221 CHECK(assert_equal(expected1, actual1));
222
223 // Add some factors to the smoother
224 NonlinearFactorGraph newFactors2;
225 newFactors2.addPrior(key: 1, prior: poseInitial, model: noisePrior);
226 newFactors2.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
227 Values newValues2;
228 newValues2.insert(j: 1, val: Pose3().compose(g: poseError));
229 newValues2.insert(j: 2, val: newValues2.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
230 smoother.update(newFactors: newFactors2, newTheta: newValues2);
231
232 // Expected values from full batch optimization
233 NonlinearFactorGraph allFactors2;
234 allFactors2.push_back(container: newFactors2);
235 Values allValues2;
236 allValues2.insert(values: newValues2);
237 Values expected2 = BatchOptimize(graph: allFactors2, theta: allValues2);
238 // Get actual linearization point
239 Values actual2 = smoother.calculateEstimate();
240 // Check
241 CHECK(assert_equal(expected2, actual2, 1e-6));
242
243 // Add some more factors to the smoother
244 NonlinearFactorGraph newFactors3;
245 newFactors3.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
246 newFactors3.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
247 Values newValues3;
248 newValues3.insert(j: 3, val: newValues2.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
249 newValues3.insert(j: 4, val: newValues3.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
250 smoother.update(newFactors: newFactors3, newTheta: newValues3);
251
252 // Expected values from full batch optimization
253 NonlinearFactorGraph allFactors3;
254 allFactors3.push_back(container: newFactors2);
255 allFactors3.push_back(container: newFactors3);
256 Values allValues3;
257 allValues3.insert(values: newValues2);
258 allValues3.insert(values: newValues3);
259 Values expected3 = BatchOptimize(graph: allFactors3, theta: allValues3);
260 // Get actual linearization point
261 Values actual3 = smoother.calculateEstimate();
262 // Check
263 CHECK(assert_equal(expected3, actual3, 1e-6));
264
265 // Also check the single-variable version
266 Pose3 expectedPose1 = expected3.at<Pose3>(j: 1);
267 Pose3 expectedPose2 = expected3.at<Pose3>(j: 2);
268 Pose3 expectedPose3 = expected3.at<Pose3>(j: 3);
269 Pose3 expectedPose4 = expected3.at<Pose3>(j: 4);
270 // Also check the single-variable version
271 Pose3 actualPose1 = smoother.calculateEstimate<Pose3>(key: 1);
272 Pose3 actualPose2 = smoother.calculateEstimate<Pose3>(key: 2);
273 Pose3 actualPose3 = smoother.calculateEstimate<Pose3>(key: 3);
274 Pose3 actualPose4 = smoother.calculateEstimate<Pose3>(key: 4);
275 // Check
276 CHECK(assert_equal(expectedPose1, actualPose1, 1e-6));
277 CHECK(assert_equal(expectedPose2, actualPose2, 1e-6));
278 CHECK(assert_equal(expectedPose3, actualPose3, 1e-6));
279 CHECK(assert_equal(expectedPose4, actualPose4, 1e-6));
280}
281
282/* ************************************************************************* */
283TEST( ConcurrentIncrementalSmootherGN, update_empty )
284{
285 // Create a set of optimizer parameters
286 ISAM2Params parameters;
287 parameters.optimizationParams = ISAM2GaussNewtonParams();
288 // Create a Concurrent Batch Smoother
289 ConcurrentIncrementalSmoother smoother(parameters);
290
291 // Call update
292 smoother.update();
293}
294
295/* ************************************************************************* */
296TEST( ConcurrentIncrementalSmootherGN, update_multiple )
297{
298 // Create a Concurrent Batch Smoother
299 ISAM2Params parameters;
300 parameters.optimizationParams = ISAM2GaussNewtonParams();
301 ConcurrentIncrementalSmoother smoother(parameters);
302
303 // Expected values is empty
304 Values expected1;
305 // Get Linearization Point
306 Values actual1 = smoother.calculateEstimate();
307 // Check
308 CHECK(assert_equal(expected1, actual1));
309
310 // Add some factors to the smoother
311 NonlinearFactorGraph newFactors2;
312 newFactors2.addPrior(key: 1, prior: poseInitial, model: noisePrior);
313 newFactors2.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
314 Values newValues2;
315 newValues2.insert(j: 1, val: Pose3().compose(g: poseError));
316 newValues2.insert(j: 2, val: newValues2.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
317 smoother.update(newFactors: newFactors2, newTheta: newValues2);
318
319 // Expected values from full batch optimization
320 NonlinearFactorGraph allFactors2;
321 allFactors2.push_back(container: newFactors2);
322 Values allValues2;
323 allValues2.insert(values: newValues2);
324 Values expected2 = BatchOptimize(graph: allFactors2, theta: allValues2);
325 // Get actual linearization point
326 Values actual2 = smoother.calculateEstimate();
327 // Check
328 CHECK(assert_equal(expected2, actual2, 1e-6));
329
330 // Add some more factors to the smoother
331 NonlinearFactorGraph newFactors3;
332 newFactors3.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
333 newFactors3.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
334 Values newValues3;
335 newValues3.insert(j: 3, val: newValues2.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
336 newValues3.insert(j: 4, val: newValues3.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
337 smoother.update(newFactors: newFactors3, newTheta: newValues3);
338
339 // Expected values from full batch optimization
340 NonlinearFactorGraph allFactors3;
341 allFactors3.push_back(container: newFactors2);
342 allFactors3.push_back(container: newFactors3);
343 Values allValues3;
344 allValues3.insert(values: newValues2);
345 allValues3.insert(values: newValues3);
346 Values expected3 = BatchOptimize(graph: allFactors3, theta: allValues3);
347 // Get actual linearization point
348 Values actual3 = smoother.calculateEstimate();
349 // Check
350 CHECK(assert_equal(expected3, actual3, 1e-6));
351}
352
353/* ************************************************************************* */
354TEST( ConcurrentIncrementalSmootherGN, synchronize_empty )
355{
356 // Create a set of optimizer parameters
357 ISAM2Params parameters;
358 parameters.optimizationParams = ISAM2GaussNewtonParams();
359 // Create a Concurrent Batch Smoother
360 ConcurrentIncrementalSmoother smoother(parameters);
361
362 // Create empty containers *from* the filter
363 NonlinearFactorGraph smootherFactors, filterSumarization;
364 Values smootherValues, filterSeparatorValues;
365
366 // Create expected values: these will be empty for this case
367 NonlinearFactorGraph expectedSmootherSummarization;
368 Values expectedSmootherSeparatorValues;
369
370 // Synchronize
371 NonlinearFactorGraph actualSmootherSummarization;
372 Values actualSmootherSeparatorValues;
373 smoother.presync();
374 smoother.getSummarizedFactors(summarizedFactors&: actualSmootherSummarization, separatorValues&: actualSmootherSeparatorValues);
375 smoother.synchronize(smootherFactors, smootherValues, summarizedFactors: filterSumarization, separatorValues: filterSeparatorValues);
376 smoother.postsync();
377
378 // Check
379 CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
380 CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
381}
382
383/* ************************************************************************* */
384TEST( ConcurrentIncrementalSmootherGN, synchronize_1 )
385{
386 // Create a set of optimizer parameters
387 ISAM2Params parameters;
388 parameters.optimizationParams = ISAM2GaussNewtonParams();
389// parameters.maxIterations = 1;
390
391 // Create a Concurrent Batch Smoother
392 ConcurrentIncrementalSmoother smoother(parameters);
393
394 // Create a simple separator *from* the filter
395 NonlinearFactorGraph smootherFactors, filterSumarization;
396 Values smootherValues, filterSeparatorValues;
397 filterSeparatorValues.insert(j: 1, val: Pose3().compose(g: poseError));
398 filterSeparatorValues.insert(j: 2, val: filterSeparatorValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
399 Ordering ordering;
400 ordering.push_back(x: 1);
401 ordering.push_back(x: 2);
402 filterSumarization.push_back(factor: LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(x: filterSeparatorValues), filterSeparatorValues));
403 filterSumarization.push_back(factor: LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(x: filterSeparatorValues), filterSeparatorValues));
404
405 // Create expected values: the smoother output will be empty for this case
406 NonlinearFactorGraph expectedSmootherSummarization;
407 Values expectedSmootherSeparatorValues;
408
409 NonlinearFactorGraph actualSmootherSummarization;
410 Values actualSmootherSeparatorValues;
411 smoother.presync();
412 smoother.getSummarizedFactors(summarizedFactors&: actualSmootherSummarization, separatorValues&: actualSmootherSeparatorValues);
413 smoother.synchronize(smootherFactors, smootherValues, summarizedFactors: filterSumarization, separatorValues: filterSeparatorValues);
414 smoother.postsync();
415
416 // Check
417 CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
418 CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
419
420
421 // Update the smoother
422 smoother.update();
423
424 // Check the factor graph. It should contain only the filter-provided factors
425 NonlinearFactorGraph expectedFactorGraph = filterSumarization;
426 NonlinearFactorGraph actualFactorGraph = smoother.getFactors();
427 CHECK(assert_equal(expectedFactorGraph, actualFactorGraph, 1e-6));
428
429 // Check the optimized value of smoother state
430 NonlinearFactorGraph allFactors;
431 allFactors.push_back(container: filterSumarization);
432 Values allValues;
433 allValues.insert(values: filterSeparatorValues);
434 Values expectedValues = BatchOptimize(graph: allFactors, theta: allValues,maxIter: 1);
435 Values actualValues = smoother.calculateEstimate();
436 CHECK(assert_equal(expectedValues, actualValues, 1e-6));
437
438 // Check the linearization point. The separator should remain identical to the filter provided values
439 Values expectedLinearizationPoint = filterSeparatorValues;
440 Values actualLinearizationPoint = smoother.getLinearizationPoint();
441 CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
442}
443
444
445/* ************************************************************************* */
446TEST( ConcurrentIncrementalSmootherGN, synchronize_2 )
447{
448 // Create a set of optimizer parameters
449 ISAM2Params parameters;
450 parameters.optimizationParams = ISAM2GaussNewtonParams();
451// parameters.maxIterations = 1;
452// parameters.lambdaUpperBound = 1;
453// parameters.lambdaInitial = 1;
454// parameters.verbosity = NonlinearOptimizerParams::ERROR;
455// parameters.verbosityLM = ISAM2Params::DAMPED;
456
457 // Create a Concurrent Batch Smoother
458 ConcurrentIncrementalSmoother smoother(parameters);
459
460 // Create a separator and cached smoother factors *from* the filter
461 NonlinearFactorGraph smootherFactors, filterSumarization;
462 Values smootherValues, filterSeparatorValues;
463
464 filterSeparatorValues.insert(j: 1, val: Pose3().compose(g: poseError));
465 filterSeparatorValues.insert(j: 2, val: filterSeparatorValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
466 filterSumarization.push_back(factor: LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(x: filterSeparatorValues), filterSeparatorValues));
467 filterSumarization.push_back(factor: LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(x: filterSeparatorValues), filterSeparatorValues));
468 smootherFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
469 smootherFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
470 smootherValues.insert(j: 3, val: filterSeparatorValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
471 smootherValues.insert(j: 4, val: smootherValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
472
473 // Create expected values: the smoother output will be empty for this case
474 NonlinearFactorGraph expectedSmootherSummarization;
475 Values expectedSmootherSeparatorValues;
476
477 NonlinearFactorGraph actualSmootherSummarization;
478 Values actualSmootherSeparatorValues;
479 smoother.presync();
480 smoother.getSummarizedFactors(summarizedFactors&: actualSmootherSummarization, separatorValues&: actualSmootherSeparatorValues);
481 smoother.synchronize(smootherFactors, smootherValues, summarizedFactors: filterSumarization, separatorValues: filterSeparatorValues);
482 smoother.postsync();
483
484 // Check
485 CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
486 CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
487
488
489 // Update the smoother
490 smoother.update();
491
492 // Check the factor graph. It should contain only the filter-provided factors
493 NonlinearFactorGraph expectedFactorGraph;
494 expectedFactorGraph.push_back(container: smootherFactors);
495 expectedFactorGraph.push_back(container: filterSumarization);
496 NonlinearFactorGraph actualFactorGraph = smoother.getFactors();
497 CHECK(assert_equal(expectedFactorGraph, actualFactorGraph, 1e-6));
498
499 // Check the optimized value of smoother state
500 NonlinearFactorGraph allFactors;
501 allFactors.push_back(container: filterSumarization);
502 allFactors.push_back(container: smootherFactors);
503 Values allValues;
504 allValues.insert(values: filterSeparatorValues);
505 allValues.insert(values: smootherValues);
506// allValues.print("Batch LinPoint:\n");
507 Values expectedValues = BatchOptimize(graph: allFactors, theta: allValues, maxIter: 1);
508 Values actualValues = smoother.calculateEstimate();
509 CHECK(assert_equal(expectedValues, actualValues, 1e-6));
510
511 // Check the linearization point. The separator should remain identical to the filter provided values, but the others should be the optimal values
512 // Isam2 is changing internally the linearization points, so the following check is done only on the separator variables
513// Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
514 Values expectedLinearizationPoint = filterSeparatorValues;
515 Values actualLinearizationPoint;
516 for(const auto key: filterSeparatorValues.keys()) {
517 actualLinearizationPoint.insert(j: key, val: smoother.getLinearizationPoint().at(j: key));
518 }
519 CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
520}
521
522
523
524/* ************************************************************************* */
525TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
526{
527 // Create a set of optimizer parameters
528 ISAM2Params parameters;
529 parameters.optimizationParams = ISAM2GaussNewtonParams();
530// parameters.maxIterations = 1;
531// parameters.lambdaUpperBound = 1;
532// parameters.lambdaInitial = 1;
533// parameters.verbosity = NonlinearOptimizerParams::ERROR;
534// parameters.verbosityLM = ISAM2Params::DAMPED;
535
536 // Create a Concurrent Batch Smoother
537 ConcurrentIncrementalSmoother smoother(parameters);
538
539 // Create a separator and cached smoother factors *from* the filter
540 NonlinearFactorGraph smootherFactors, filterSumarization;
541 Values smootherValues, filterSeparatorValues;
542
543 filterSeparatorValues.insert(j: 1, val: Pose3().compose(g: poseError));
544 filterSeparatorValues.insert(j: 2, val: filterSeparatorValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
545 filterSumarization.push_back(factor: LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(x: filterSeparatorValues), filterSeparatorValues));
546 filterSumarization.push_back(factor: LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(x: filterSeparatorValues), filterSeparatorValues));
547 smootherFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
548 smootherFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
549 smootherFactors.addPrior(key: 4, prior: poseInitial, model: noisePrior);
550 smootherValues.insert(j: 3, val: filterSeparatorValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
551 smootherValues.insert(j: 4, val: smootherValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
552
553 // Create expected values: the smoother output will be empty for this case
554 NonlinearFactorGraph expectedSmootherSummarization;
555 Values expectedSmootherSeparatorValues;
556
557 NonlinearFactorGraph actualSmootherSummarization;
558 Values actualSmootherSeparatorValues;
559 smoother.presync();
560 smoother.getSummarizedFactors(summarizedFactors&: actualSmootherSummarization, separatorValues&: actualSmootherSeparatorValues);
561 smoother.synchronize(smootherFactors, smootherValues, summarizedFactors: filterSumarization, separatorValues: filterSeparatorValues);
562 smoother.postsync();
563
564 // Check
565 CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
566 CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
567
568
569 // Update the smoother
570 smoother.update();
571
572 smoother.presync();
573 smoother.getSummarizedFactors(summarizedFactors&: actualSmootherSummarization, separatorValues&: actualSmootherSeparatorValues);
574
575 // Check the optimized value of smoother state
576 NonlinearFactorGraph allFactors = smootherFactors;
577 Values allValues = smoother.getLinearizationPoint();
578// ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering...
579
580 GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(linearizationPoint: allValues);
581// GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
582// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
583
584 KeySet allkeys = LinFactorGraph->keys();
585 for (const auto key : filterSeparatorValues.keys()) allkeys.erase(x: key);
586 KeyVector variables(allkeys.begin(), allkeys.end());
587 const auto [bn, fg] = LinFactorGraph->eliminatePartialSequential(variables, function: EliminateCholesky);
588
589 expectedSmootherSummarization.resize(size: 0);
590 for(const GaussianFactor::shared_ptr& factor: *fg) {
591 expectedSmootherSummarization.push_back(factor: LinearContainerFactor(factor, allValues));
592 }
593
594 CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
595
596}
597
598// =========================================================================================================
599///* ************************************************************************* */
600TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
601{
602 // Create a set of optimizer parameters
603 ISAM2Params parameters;
604 parameters.optimizationParams = ISAM2GaussNewtonParams();
605
606 // Create a Concurrent Batch Smoother
607 ConcurrentIncrementalSmoother smoother(parameters);
608
609 // Add some factors to the smoother
610 NonlinearFactorGraph newFactors;
611 newFactors.addPrior(key: 1, prior: poseInitial, model: noisePrior);
612 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
613 newFactors.push_back(factor: BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
614 newFactors.push_back(factor: BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
615 newFactors.push_back(factor: BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
616
617 Values newValues;
618 newValues.insert(j: 1, val: Pose3().compose(g: poseError));
619 newValues.insert(j: 2, val: newValues.at<Pose3>(j: 1).compose(g: poseOdometry).compose(g: poseError));
620 newValues.insert(j: 3, val: newValues.at<Pose3>(j: 2).compose(g: poseOdometry).compose(g: poseError));
621 newValues.insert(j: 4, val: newValues.at<Pose3>(j: 3).compose(g: poseOdometry).compose(g: poseError));
622
623 // Update the smoother: add all factors
624 smoother.update(newFactors, newTheta: newValues);
625
626 // factor we want to remove
627 // NOTE: we can remove factors, paying attention that the remaining graph remains connected
628 // we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
629 FactorIndices removeFactorIndices(2,1);
630
631 // Add no factors to the smoother (we only want to test the removal)
632 NonlinearFactorGraph noFactors;
633 Values noValues;
634 smoother.update(newFactors: noFactors, newTheta: noValues, removeFactorIndices);
635
636 NonlinearFactorGraph actualGraph = smoother.getFactors();
637
638 actualGraph.print(str: "actual graph: \n");
639
640 NonlinearFactorGraph expectedGraph;
641 expectedGraph.addPrior(key: 1, prior: poseInitial, model: noisePrior);
642 // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
643 // we should add an empty one, so that the ordering and labeling of the factors is preserved
644 expectedGraph.push_back(factor: NonlinearFactor::shared_ptr());
645 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 2, args: 3, args: poseOdometry, args: noiseOdometery);
646 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 3, args: 4, args: poseOdometry, args: noiseOdometery);
647 expectedGraph.emplace_shared<BetweenFactor<Pose3> >(args: 1, args: 2, args: poseOdometry, args: noiseOdometery);
648
649// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
650}
651
652/////* ************************************************************************* */
653//TEST( ConcurrentIncrementalSmoother, removeFactors_topology_2 )
654//{
655// // we try removing the last factor
656//
657// // Create a set of optimizer parameters
658// LevenbergMarquardtParams parameters;
659//
660// // Create a Concurrent Batch Smoother
661// ConcurrentBatchSmoother smoother(parameters);
662//
663// // Add some factors to the smoother
664// NonlinearFactorGraph newFactors;
665// newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
666// newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
667// newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
668// newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
669// newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
670//
671// Values newValues;
672// newValues.insert(1, Pose3().compose(poseError));
673// newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
674// newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
675// newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
676//
677// // Update the smoother: add all factors
678// smoother.update(newFactors, newValues);
679//
680// // factor we want to remove
681// // NOTE: we can remove factors, paying attention that the remaining graph remains connected
682// // we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
683// std::vector<size_t> removeFactorIndices(1,4);
684//
685// // Add no factors to the smoother (we only want to test the removal)
686// NonlinearFactorGraph noFactors;
687// Values noValues;
688// smoother.update(noFactors, noValues, removeFactorIndices);
689//
690// NonlinearFactorGraph actualGraph = smoother.getFactors();
691//
692// NonlinearFactorGraph expectedGraph;
693// expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
694// expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
695// expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
696// expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
697// // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
698// // we should add an empty one, so that the ordering and labeling of the factors is preserved
699// expectedGraph.push_back(NonlinearFactor::shared_ptr());
700//
701// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
702//}
703//
704//
705/////* ************************************************************************* */
706//TEST( ConcurrentBatchSmoother, removeFactors_topology_3 )
707//{
708// // we try removing the first factor
709//
710// // Create a set of optimizer parameters
711// LevenbergMarquardtParams parameters;
712// ConcurrentBatchSmoother Smoother(parameters);
713//
714// // Add some factors to the Smoother
715// NonlinearFactorGraph newFactors;
716// newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
717// newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
718// newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
719// newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
720// newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
721//
722// Values newValues;
723// newValues.insert(1, Pose3().compose(poseError));
724// newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
725// newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
726// newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
727//
728// // Update the Smoother: add all factors
729// Smoother.update(newFactors, newValues);
730//
731// // factor we want to remove
732// // NOTE: we can remove factors, paying attention that the remaining graph remains connected
733// // we remove a single factor, the number 0, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
734// std::vector<size_t> removeFactorIndices(1,0);
735//
736// // Add no factors to the Smoother (we only want to test the removal)
737// NonlinearFactorGraph noFactors;
738// Values noValues;
739// Smoother.update(noFactors, noValues, removeFactorIndices);
740//
741// NonlinearFactorGraph actualGraph = Smoother.getFactors();
742//
743// NonlinearFactorGraph expectedGraph;
744// // we should add an empty one, so that the ordering and labeling of the factors is preserved
745// expectedGraph.push_back(NonlinearFactor::shared_ptr());
746// expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
747// expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
748// expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
749// expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
750//
751// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
752//}
753//
754/////* ************************************************************************* */
755//TEST( ConcurrentBatchSmoother, removeFactors_values )
756//{
757// // we try removing the last factor
758//
759// // Create a set of optimizer parameters
760// LevenbergMarquardtParams parameters;
761// ConcurrentBatchSmoother Smoother(parameters);
762//
763// // Add some factors to the Smoother
764// NonlinearFactorGraph newFactors;
765// newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
766// newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
767// newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
768// newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
769// newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
770//
771// Values newValues;
772// newValues.insert(1, Pose3().compose(poseError));
773// newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
774// newValues.insert(3, newValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
775// newValues.insert(4, newValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
776//
777// // Update the Smoother: add all factors
778// Smoother.update(newFactors, newValues);
779//
780// // factor we want to remove
781// // NOTE: we can remove factors, paying attention that the remaining graph remains connected
782// // we remove a single factor, the number 4, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
783// std::vector<size_t> removeFactorIndices(1,4);
784//
785// // Add no factors to the Smoother (we only want to test the removal)
786// NonlinearFactorGraph noFactors;
787// Values noValues;
788// Smoother.update(noFactors, noValues, removeFactorIndices);
789//
790// NonlinearFactorGraph actualGraph = Smoother.getFactors();
791// Values actualValues = Smoother.calculateEstimate();
792//
793// // note: factors are removed before the optimization
794// NonlinearFactorGraph expectedGraph;
795// expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
796// expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
797// expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
798// expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
799// // we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
800// // we should add an empty one, so that the ordering and labeling of the factors is preserved
801// expectedGraph.push_back(NonlinearFactor::shared_ptr());
802//
803// // Calculate expected factor graph and values
804// Values expectedValues = BatchOptimize(expectedGraph, newValues);
805//
806// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
807// CHECK(assert_equal(expectedValues, actualValues, 1e-6));
808//}
809
810
811
812
813/* ************************************************************************* */
814int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
815/* ************************************************************************* */
816