35 using namespace gtsam;
40 const Pose3 poseInitial;
41 const Pose3 poseOdometry( Rot3::RzRyRx(
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) );
45 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
46 const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((
Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished());
47 const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((
Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished());
63 optimizer.update(
graph, theta );
101 TEST( ConcurrentIncrementalSmootherGN, getFactors )
117 newFactors1.
addPrior(1, poseInitial, noisePrior);
122 smoother.
update(newFactors1, newValues1);
139 smoother.
update(newFactors2, newValues2);
152 TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
168 newFactors1.
addPrior(1, poseInitial, noisePrior);
173 smoother.
update(newFactors1, newValues1);
177 expected2.
insert(newValues1);
190 smoother.
update(newFactors2, newValues2);
194 expected3.
insert(newValues1);
195 expected3.
insert(newValues2);
203 TEST( ConcurrentIncrementalSmootherGN, getDelta )
209 TEST( ConcurrentIncrementalSmootherGN, calculateEstimate )
225 newFactors2.
addPrior(1, poseInitial, noisePrior);
230 smoother.
update(newFactors2, newValues2);
236 allValues2.
insert(newValues2);
237 Values expected2 = BatchOptimize(allFactors2, allValues2);
250 smoother.
update(newFactors3, newValues3);
257 allValues3.
insert(newValues2);
258 allValues3.
insert(newValues3);
259 Values expected3 = BatchOptimize(allFactors3, allValues3);
283 TEST( ConcurrentIncrementalSmootherGN, update_empty )
296 TEST( ConcurrentIncrementalSmootherGN, update_multiple )
312 newFactors2.
addPrior(1, poseInitial, noisePrior);
317 smoother.
update(newFactors2, newValues2);
323 allValues2.
insert(newValues2);
324 Values expected2 = BatchOptimize(allFactors2, allValues2);
337 smoother.
update(newFactors3, newValues3);
344 allValues3.
insert(newValues2);
345 allValues3.
insert(newValues3);
346 Values expected3 = BatchOptimize(allFactors3, allValues3);
354 TEST( ConcurrentIncrementalSmootherGN, synchronize_empty )
364 Values smootherValues, filterSeparatorValues;
368 Values expectedSmootherSeparatorValues;
372 Values actualSmootherSeparatorValues;
375 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
379 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
380 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
384 TEST( ConcurrentIncrementalSmootherGN, synchronize_1 )
396 Values smootherValues, filterSeparatorValues;
407 Values expectedSmootherSeparatorValues;
410 Values actualSmootherSeparatorValues;
413 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
417 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
418 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
431 allFactors.
push_back(filterSumarization);
433 allValues.
insert(filterSeparatorValues);
434 Values expectedValues = BatchOptimize(allFactors, allValues,1);
439 Values expectedLinearizationPoint = filterSeparatorValues;
446 TEST( ConcurrentIncrementalSmootherGN, synchronize_2 )
462 Values smootherValues, filterSeparatorValues;
475 Values expectedSmootherSeparatorValues;
478 Values actualSmootherSeparatorValues;
481 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
485 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
486 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
494 expectedFactorGraph.
push_back(smootherFactors);
495 expectedFactorGraph.
push_back(filterSumarization);
501 allFactors.
push_back(filterSumarization);
504 allValues.
insert(filterSeparatorValues);
505 allValues.
insert(smootherValues);
507 Values expectedValues = BatchOptimize(allFactors, allValues, 1);
514 Values expectedLinearizationPoint = filterSeparatorValues;
515 Values actualLinearizationPoint;
516 for(
const auto key: filterSeparatorValues.
keys()) {
525 TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
541 Values smootherValues, filterSeparatorValues;
549 smootherFactors.
addPrior(4, poseInitial, noisePrior);
555 Values expectedSmootherSeparatorValues;
558 Values actualSmootherSeparatorValues;
561 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
565 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
566 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
584 KeySet allkeys = LinFactorGraph->keys();
585 for (
const auto key : filterSeparatorValues.
keys()) allkeys.erase(
key);
586 KeyVector variables(allkeys.begin(), allkeys.end());
587 const auto [bn, fg] = LinFactorGraph->eliminatePartialSequential(variables,
EliminateCholesky);
589 expectedSmootherSummarization.
resize(0);
594 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
611 newFactors.
addPrior(1, poseInitial, noisePrior);
624 smoother.
update(newFactors, newValues);
634 smoother.
update(noFactors, noValues, removeFactorIndices);
638 actualGraph.
print(
"actual graph: \n");