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