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());
66 std::set<Key> KeysToKeep;
67 for(
const auto key: linPoint.
keys()) {
68 KeysToKeep.insert(
key);
70 for(
Key key: keysToMarginalize) {
75 for(
Key key: keysToMarginalize) {
90 return LinearContainerForGaussianMarginals;
137 newFactors1.
addPrior(1, poseInitial, noisePrior);
142 filter.
update(newFactors1, newValues1);
159 filter.
update(newFactors2, newValues2);
187 newFactors1.
addPrior(1, poseInitial, noisePrior);
192 filter.
update(newFactors1, newValues1);
196 expected2.
insert(newValues1);
209 filter.
update(newFactors2, newValues2);
213 expected3.
insert(newValues1);
214 expected3.
insert(newValues2);
249 newFactors2.
addPrior(1, poseInitial, noisePrior);
254 filter.
update(newFactors2, newValues2);
260 allValues2.
insert(newValues2);
261 Values expected2 = BatchOptimize(allFactors2, allValues2);
274 filter.
update(newFactors3, newValues3);
281 allValues3.
insert(newValues2);
282 allValues3.
insert(newValues3);
283 Values expected3 = BatchOptimize(allFactors3, allValues3);
333 newFactors2.
addPrior(1, poseInitial, noisePrior);
338 filter.
update(newFactors2, newValues2);
344 allValues2.
insert(newValues2);
345 Values expected2 = BatchOptimize(allFactors2, allValues2);
358 filter.
update(newFactors3, newValues3);
365 allValues3.
insert(newValues2);
366 allValues3.
insert(newValues3);
367 Values expected3 = BatchOptimize(allFactors3, allValues3);
383 newFactors.
addPrior(1, poseInitial, noisePrior);
395 keysToMove.push_back(1);
396 keysToMove.push_back(2);
399 filter.
update(newFactors, newValues, keysToMove);
402 Values optimalValues = BatchOptimize(newFactors, newValues);
406 partialGraph.
addPrior(1, poseInitial, noisePrior);
411 partialValues.insert(1, optimalValues.
at<
Pose3>(1));
412 partialValues.insert(2, optimalValues.
at<
Pose3>(2));
413 partialValues.insert(3, optimalValues.
at<
Pose3>(3));
449 Values expectedValues = optimalValues;
452 for(
Key key: keysToMove) {
472 Values smootherSeparatorValues;
476 Values expectedSmootherValues, expectedFilterSeparatorValues;
480 Values actualSmootherValues, actualFilterSeparatorValues;
482 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
491 CHECK(
assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1
e-6));
507 newFactors.
addPrior(1, poseInitial, noisePrior);
512 filter.
update(newFactors, newValues);
516 Values smootherSeparatorValues;
520 Values expectedSmootherValues, expectedFilterSeparatorValues;
524 Values actualSmootherValues, actualFilterSeparatorValues;
526 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
535 CHECK(
assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1
e-6));
541 std::cout <<
"*********************** synchronize_2 ************************" << std::endl;
559 newValues.
insert(1, value1);
560 newValues.
insert(2, value2);
562 keysToMove.push_back(1);
563 filter.
update(newFactors, newValues, keysToMove);
567 Values optimalValues = BatchOptimize(newFactors, newValues);
571 Values smootherSeparatorValues;
579 Values expectedSmootherValues;
581 expectedSmootherValues.
insert(1, optimalValues.
at<
Pose3>(1));
587 Values expectedFilterSeparatorValues;
588 expectedFilterSeparatorValues.
insert(2, optimalValues.
at<
Pose3>(2));
592 Values actualSmootherValues, actualFilterSeparatorValues;
594 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
603 CHECK(
assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1
e-6));
609 std::cout <<
"*********************** synchronize_3 ************************" << std::endl;
631 newValues.
insert(1, value1);
632 newValues.
insert(2, value2);
633 newValues.
insert(3, value3);
636 keysToMove.push_back(1);
638 filter.
update(newFactors, newValues, keysToMove);
640 Values optimalValues = BatchOptimize(newFactors, newValues);
645 Values smootherSeparatorValues;
652 Values expectedSmootherValues;
654 expectedSmootherValues.
insert(1, optimalValues.
at<
Pose3>(1));
660 Values expectedFilterSeparatorValues;
661 expectedFilterSeparatorValues.
insert(2, optimalValues.
at<
Pose3>(2));
671 keysToMarginalize.push_back(3);
673 expectedFilterSummarization = CalculateMarginals(partialGraph, partialValues, keysToMarginalize);
677 Values actualSmootherValues, actualFilterSeparatorValues;
679 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
688 CHECK(
assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1
e-6));
715 newValues.
insert(1, value1);
716 newValues.
insert(2, value2);
717 newValues.
insert(3, value3);
720 keysToMove.push_back(1);
722 filter.
update(newFactors, newValues, keysToMove);
724 Values optimalValuesFilter = BatchOptimize(newFactors, newValues,1);
729 Values smootherSeparatorValues;
736 Values expectedSmootherValues;
738 expectedSmootherValues.
insert(1, optimalValuesFilter.
at<
Pose3>(1));
745 Values expectedFilterSeparatorValues;
746 expectedFilterSeparatorValues.
insert(2, optimalValuesFilter.
at<
Pose3>(2));
751 Values partialValuesFilter;
752 partialValuesFilter.
insert(2, optimalValuesFilter.
at<
Pose3>(2));
753 partialValuesFilter.
insert(3, optimalValuesFilter.
at<
Pose3>(3));
757 orderingFilter.push_back(3);
758 orderingFilter.push_back(2);
761 keysToMarginalize.push_back(3);
763 expectedFilterSummarization = CalculateMarginals(partialGraphFilter, partialValuesFilter, keysToMarginalize);
768 Values actualSmootherValues, actualFilterSeparatorValues;
770 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
780 CHECK(
assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1
e-6));
787 std::cout <<
"*********************** synchronize_5 ************************" << std::endl;
815 newValues.
insert(1, value1);
816 newValues.
insert(2, value2);
817 newValues.
insert(3, value3);
818 newValues.
insert(4, value4);
821 keysToMove.push_back(1);
823 filter.
update(newFactors, newValues, keysToMove);
827 Values smootherSeparatorValues;
832 Values actualSmootherValues, actualFilterSeparatorValues;
834 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
843 Values optimalValues = BatchOptimize(newFactors, newValues, 1);
844 Values expectedSmootherValues;
857 keysToMove2.push_back(2);
861 filter.
update(newFactors, newValues, keysToMove2);
866 Values smootherSeparatorValues2;
883 partialValues.
insert(2, optimalValues.
at(2));
889 keysToMarginalize.push_back(1);
891 smootherSummarization2 = CalculateMarginals(partialGraph, partialValues, keysToMarginalize);
892 smootherSeparatorValues2.
insert(2, partialValues.
at(2));
898 Values actualSmootherValues2, actualFilterSeparatorValues2;
900 filter.
synchronize(smootherSummarization2, smootherSeparatorValues2);
907 expectedSmootherFactors2.
push_back(factor4);
909 Values expectedSmootherValues2;
910 expectedSmootherValues2.
insert(2, optimalValues.
at(2));
925 Values partialValuesFilter;
926 partialValuesFilter.
insert(3, optimalValues2.
at(3));
927 partialValuesFilter.
insert(4, optimalValues2.
at(4));
930 keysToMarginalize2.push_back(4);
932 NonlinearFactorGraph expectedFilterSummarization2 = CalculateMarginals(partialGraphFilter, partialValuesFilter, keysToMarginalize2);
933 Values expectedFilterSeparatorValues2;
934 expectedFilterSeparatorValues2.
insert(3, optimalValues2.
at(3));
937 CHECK(
assert_equal(expectedFilterSeparatorValues2, actualFilterSeparatorValues2, 1
e-6));
943 partialGraphTransition.
push_back(factor4);
944 partialGraphTransition.
push_back(smootherSummarization2);
946 Values partialValuesTransition;
947 partialValuesTransition.
insert(2,optimalValues.
at(2));
948 partialValuesTransition.
insert(3,optimalValues2.
at(3));
951 keysToMarginalize3.push_back(2);
961 expectedFilterGraph.
push_back(factorEmpty);
962 expectedFilterGraph.
push_back(factorEmpty);
963 expectedFilterGraph.
push_back(factorEmpty);
964 expectedFilterGraph.
push_back(factorEmpty);
966 expectedFilterGraph.
push_back(CalculateMarginals(partialGraphTransition, partialValuesTransition, keysToMarginalize3));
995 newValues.
insert(1, value1);
996 newValues.
insert(2, value2);
997 newValues.
insert(3, value3);
1020 keysToMarginalize.push_back(1);
1022 actualMarginals = CalculateMarginals(factorGraph, newValues, keysToMarginalize);
1048 newValues.
insert(1, value1);
1049 newValues.
insert(2, value2);
1050 newValues.
insert(3, value3);
1073 keysToMarginalize.push_back(1);
1074 keysToMarginalize.push_back(2);
1076 actualMarginals = CalculateMarginals(factorGraph, newValues, keysToMarginalize);
1085 std::cout <<
"*********************** removeFactors_topology_1 ************************" << std::endl;
1093 newFactors.
addPrior(1, poseInitial, noisePrior);
1109 filter.
update(newFactors, newValues, keysToMove);
1114 std::vector<size_t> removeFactorIndices(1,1);
1119 filter.
update(noFactors, noValues, keysToMove, removeFactorIndices);
1138 std::cout <<
"*********************** removeFactors_topology_2 ************************" << std::endl;
1148 newFactors.
addPrior(1, poseInitial, noisePrior);
1164 filter.
update(newFactors, newValues, keysToMove);
1169 std::vector<size_t> removeFactorIndices(1,4);
1174 filter.
update(noFactors, noValues, keysToMove, removeFactorIndices);
1194 std::cout <<
"*********************** removeFactors_topology_3 ************************" << std::endl;
1203 newFactors.
addPrior(1, poseInitial, noisePrior);
1204 newFactors.
addPrior(1, poseInitial, noisePrior);
1219 filter.
update(newFactors, newValues, keysToMove);
1224 std::vector<size_t> removeFactorIndices(1,0);
1229 filter.
update(noFactors, noValues, keysToMove, removeFactorIndices);
1247 std::cout <<
"*********************** removeFactors_values ************************" << std::endl;
1256 newFactors.
addPrior(1, poseInitial, noisePrior);
1272 filter.
update(newFactors, newValues, keysToMove);
1277 std::vector<size_t> removeFactorIndices(1,4);
1282 filter.
update(noFactors, noValues, keysToMove, removeFactorIndices);