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());
107 newFactors1.
addPrior(1, poseInitial, noisePrior);
112 smoother.
update(newFactors1, newValues1);
129 smoother.
update(newFactors2, newValues2);
157 newFactors1.
addPrior(1, poseInitial, noisePrior);
162 smoother.
update(newFactors1, newValues1);
166 expected2.
insert(newValues1);
179 smoother.
update(newFactors2, newValues2);
183 expected3.
insert(newValues1);
184 expected3.
insert(newValues2);
219 newFactors2.
addPrior(1, poseInitial, noisePrior);
224 smoother.
update(newFactors2, newValues2);
230 allValues2.
insert(newValues2);
231 Values expected2 = BatchOptimize(allFactors2, allValues2);
244 smoother.
update(newFactors3, newValues3);
251 allValues3.
insert(newValues2);
252 allValues3.
insert(newValues3);
253 Values expected3 = BatchOptimize(allFactors3, allValues3);
305 newFactors2.
addPrior(1, poseInitial, noisePrior);
310 smoother.
update(newFactors2, newValues2);
316 allValues2.
insert(newValues2);
317 Values expected2 = BatchOptimize(allFactors2, allValues2);
330 smoother.
update(newFactors3, newValues3);
337 allValues3.
insert(newValues2);
338 allValues3.
insert(newValues3);
339 Values expected3 = BatchOptimize(allFactors3, allValues3);
357 Values smootherValues, filterSeparatorValues;
361 Values expectedSmootherSeparatorValues;
365 Values actualSmootherSeparatorValues;
368 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
372 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
373 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
388 Values smootherValues, filterSeparatorValues;
399 Values expectedSmootherSeparatorValues;
402 Values actualSmootherSeparatorValues;
405 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
409 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
410 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
423 allFactors.
push_back(filterSumarization);
425 allValues.
insert(filterSeparatorValues);
431 Values expectedLinearizationPoint = filterSeparatorValues;
449 Values smootherValues, filterSeparatorValues;
464 Values expectedSmootherSeparatorValues;
467 Values actualSmootherSeparatorValues;
470 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
474 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
475 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
483 expectedFactorGraph.
push_back(smootherFactors);
484 expectedFactorGraph.
push_back(filterSumarization);
490 allFactors.
push_back(filterSumarization);
493 allValues.
insert(filterSeparatorValues);
494 allValues.
insert(smootherValues);
501 expectedLinearizationPoint.
update(filterSeparatorValues);
520 Values smootherValues, filterSeparatorValues;
530 smootherFactors.
addPrior(4, poseInitial, noisePrior);
536 Values expectedSmootherSeparatorValues;
539 Values actualSmootherSeparatorValues;
542 smoother.
synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
546 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
547 CHECK(
assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1
e-6));
562 KeySet eliminateKeys = linearFactors->keys();
563 for(
const auto key: filterSeparatorValues.
keys()) {
564 eliminateKeys.erase(
key);
566 KeyVector variables(eliminateKeys.begin(), eliminateKeys.end());
569 expectedSmootherSummarization.
resize(0);
574 CHECK(
assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1
e-6));
581 std::cout <<
"*********************** removeFactors_topology_1 ************************" << std::endl;
591 newFactors.
addPrior(1, poseInitial, noisePrior);
605 smoother.
update(newFactors, newValues);
610 std::vector<size_t> removeFactorIndices(1,1);
615 smoother.
update(noFactors, noValues, removeFactorIndices);
634 std::cout <<
"*********************** removeFactors_topology_2 ************************" << std::endl;
645 newFactors.
addPrior(1, poseInitial, noisePrior);
658 smoother.
update(newFactors, newValues);
663 std::vector<size_t> removeFactorIndices(1,4);
668 smoother.
update(noFactors, noValues, removeFactorIndices);
688 std::cout <<
"*********************** removeFactors_topology_3 ************************" << std::endl;
697 newFactors.
addPrior(1, poseInitial, noisePrior);
698 newFactors.
addPrior(1, poseInitial, noisePrior);
710 Smoother.
update(newFactors, newValues);
715 std::vector<size_t> removeFactorIndices(1,0);
720 Smoother.
update(noFactors, noValues, removeFactorIndices);
738 std::cout <<
"*********************** removeFactors_values ************************" << std::endl;
747 newFactors.
addPrior(1, poseInitial, noisePrior);
760 Smoother.
update(newFactors, newValues);
765 std::vector<size_t> removeFactorIndices(1,4);
770 Smoother.
update(noFactors, noValues, removeFactorIndices);