36 using namespace gtsam;
41 const Pose3 poseInitial;
42 const Pose3 poseOdometry( Rot3::RzRyRx(
Vector3(0.05, 0.10, -0.75)),
Point3(1.0, -0.25, 0.10) );
43 const Pose3 poseError( Rot3::RzRyRx(
Vector3(0.01, 0.02, -0.1)),
Point3(0.05, -0.05, 0.02) );
46 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
47 const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas((
Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished());
48 const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas((
Vector(6) << 0.25, 0.25, 0.25, 1.0, 1.0, 1.0).finished());
64 optimizer.update(
graph, theta );
102 TEST( ConcurrentIncrementalSmootherDL, getFactors )
118 newFactors1.
addPrior(1, poseInitial, noisePrior);
123 smoother.
update(newFactors1, newValues1);
140 smoother.
update(newFactors2, newValues2);
153 TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint )
169 newFactors1.
addPrior(1, poseInitial, noisePrior);
174 smoother.
update(newFactors1, newValues1);
178 expected2.
insert(newValues1);
191 smoother.
update(newFactors2, newValues2);
195 expected3.
insert(newValues1);
196 expected3.
insert(newValues2);
204 TEST( ConcurrentIncrementalSmootherDL, getDelta )
210 TEST( ConcurrentIncrementalSmootherDL, calculateEstimate )
226 newFactors2.
addPrior(1, poseInitial, noisePrior);
231 smoother.
update(newFactors2, newValues2);
237 allValues2.
insert(newValues2);
238 Values expected2 = BatchOptimize(allFactors2, allValues2);
251 smoother.
update(newFactors3, newValues3);
258 allValues3.
insert(newValues2);
259 allValues3.
insert(newValues3);
260 Values expected3 = BatchOptimize(allFactors3, allValues3);
284 TEST( ConcurrentIncrementalSmootherDL, update_empty )
297 TEST( ConcurrentIncrementalSmootherDL, update_multiple )
313 newFactors2.
addPrior(1, poseInitial, noisePrior);
318 smoother.
update(newFactors2, newValues2);
324 allValues2.
insert(newValues2);
325 Values expected2 = BatchOptimize(allFactors2, allValues2);
338 smoother.
update(newFactors3, newValues3);
345 allValues3.
insert(newValues2);
346 allValues3.
insert(newValues3);
347 Values expected3 = BatchOptimize(allFactors3, allValues3);
355 TEST( ConcurrentIncrementalSmootherDL, synchronize_empty )
365 Values smootherValues, filterSeparatorValues;
369 Values expectedSmootherSeparatorValues;
373 Values actualSmootherSeparatorValues;
376 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
380 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
381 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
385 TEST( ConcurrentIncrementalSmootherDL, synchronize_1 )
397 Values smootherValues, filterSeparatorValues;
406 Values expectedSmootherSeparatorValues;
409 Values actualSmootherSeparatorValues;
412 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
416 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
417 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
430 allFactors.
push_back(filterSumarization);
432 allValues.
insert(filterSeparatorValues);
433 Values expectedValues = BatchOptimize(allFactors, allValues,1);
438 Values expectedLinearizationPoint = filterSeparatorValues;
445 TEST( ConcurrentIncrementalSmootherDL, synchronize_2 )
461 Values smootherValues, filterSeparatorValues;
474 Values expectedSmootherSeparatorValues;
477 Values actualSmootherSeparatorValues;
480 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
484 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
485 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
493 expectedFactorGraph.
push_back(smootherFactors);
494 expectedFactorGraph.
push_back(filterSumarization);
500 allFactors.
push_back(filterSumarization);
503 allValues.
insert(filterSeparatorValues);
504 allValues.
insert(smootherValues);
506 Values expectedValues = BatchOptimize(allFactors, allValues, 1);
513 Values expectedLinearizationPoint = filterSeparatorValues;
514 Values actualLinearizationPoint;
515 for(
const auto key: filterSeparatorValues.
keys()) {
524 TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
540 Values smootherValues, filterSeparatorValues;
548 smootherFactors.
addPrior(4, poseInitial, noisePrior);
554 Values expectedSmootherSeparatorValues;
557 Values actualSmootherSeparatorValues;
560 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
564 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
565 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
582 KeySet allkeys = LinFactorGraph->keys();
583 for(
const auto key: filterSeparatorValues.
keys()) {
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));