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) );
43 const Pose3 poseError( Rot3::RzRyRx(
Vector3(0.1, 0.02, -0.1)),
Point3(0.5, -0.05, 0.2) );
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());
71 optimizer.update( graphForISAM2, theta );
82 std::set<Key> KeysToKeep;
83 for(
const auto key: linPoint.
keys()) {
84 KeysToKeep.insert(
key);
86 for(
Key key: keysToMarginalize) {
91 for(
Key key: keysToMarginalize) {
108 return LinearContainerForGaussianMarginals;
156 newFactors1.
addPrior(1, poseInitial, noisePrior);
161 filter.
update(newFactors1, newValues1);
178 filter.
update(newFactors2, newValues2);
206 newFactors1.
addPrior(1, poseInitial, noisePrior);
211 filter.
update(newFactors1, newValues1);
215 expected2.
insert(newValues1);
228 filter.
update(newFactors2, newValues2);
232 expected3.
insert(newValues1);
233 expected3.
insert(newValues2);
262 newFactors2.
addPrior(1, poseInitial, noisePrior);
267 filter.
update(newFactors2, newValues2);
273 allValues2.
insert(newValues2);
274 Values expected2 = BatchOptimize(allFactors2, allValues2);
287 filter.
update(newFactors3, newValues3);
294 allValues3.
insert(newValues2);
295 allValues3.
insert(newValues3);
296 Values expected3 = BatchOptimize(allFactors3, allValues3);
346 newFactors2.
addPrior(1, poseInitial, noisePrior);
351 filter.
update(newFactors2, newValues2);
357 allValues2.
insert(newValues2);
358 Values expected2 = BatchOptimize(allFactors2, allValues2);
371 filter.
update(newFactors3, newValues3);
378 allValues3.
insert(newValues2);
379 allValues3.
insert(newValues3);
380 Values expected3 = BatchOptimize(allFactors3, allValues3);
396 newFactors.
addPrior(1, poseInitial, noisePrior);
408 keysToMove.push_back(1);
409 keysToMove.push_back(2);
412 filter.
update(newFactors, newValues, keysToMove);
415 Values optimalValues = BatchOptimize(newFactors, newValues);
417 Values expectedValues = optimalValues;
420 for(
Key key: keysToMove) {
426 partialGraph.
addPrior(1, poseInitial, noisePrior);
479 newFactors.
addPrior(1, poseInitial, noisePrior);
491 keysToMove.push_back(1);
492 keysToMove.push_back(2);
495 filter.
update(newFactors, newValues);
499 Values optimalValues = BatchOptimize(newFactors, newValues);
501 Values expectedValues = optimalValues;
504 for(
Key key: keysToMove) {
510 partialGraph.
addPrior(1, poseInitial, noisePrior);
543 Values optimalValues2 = BatchOptimize(newFactors, optimalValues);
544 Values expectedValues2 = optimalValues2;
546 for(
Key key: keysToMove) {
570 Values smootherSeparatorValues;
574 Values expectedSmootherValues, expectedFilterSeparatorValues;
578 Values actualSmootherValues, actualFilterSeparatorValues;
580 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
589 CHECK(
assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1
e-6));
608 newFactors.
addPrior(1, poseInitial, noisePrior);
613 filter.
update(newFactors, newValues);
617 Values smootherSeparatorValues;
621 Values expectedSmootherValues, expectedFilterSeparatorValues;
625 Values actualSmootherValues, actualFilterSeparatorValues;
627 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
636 CHECK(
assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1
e-6));
662 newValues.
insert(1, value1);
663 newValues.
insert(2, value2);
665 keysToMove.push_back(1);
666 filter.
update(newFactors, newValues, keysToMove);
670 Values optimalValues = BatchOptimize(newFactors, newValues);
674 Values smootherSeparatorValues;
682 Values expectedSmootherValues;
684 expectedSmootherValues.
insert(1, newValues.
at(1));
690 Values expectedFilterSeparatorValues;
691 expectedFilterSeparatorValues.
insert(2, newValues.
at(2));
695 Values actualSmootherValues, actualFilterSeparatorValues;
697 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
706 CHECK(
assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1
e-6));
736 newValues.
insert(1, value1);
737 newValues.
insert(2, value2);
738 newValues.
insert(3, value3);
741 keysToMove.push_back(1);
743 filter.
update(newFactors, newValues, keysToMove);
745 Values optimalValues = BatchOptimize(newFactors, newValues);
750 Values smootherSeparatorValues;
757 Values expectedSmootherValues;
765 Values expectedFilterSeparatorValues;
766 expectedFilterSeparatorValues.
insert(2, newValues.
at(2));
776 keysToMarginalize.push_back(3);
778 expectedFilterSummarization = CalculateMarginals(partialGraph, partialValues, keysToMarginalize);
782 Values actualSmootherValues, actualFilterSeparatorValues;
784 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
793 CHECK(
assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1
e-6));
823 newValues.
insert(1, value1);
824 newValues.
insert(2, value2);
825 newValues.
insert(3, value3);
828 keysToMove.push_back(1);
830 filter.
update(newFactors, newValues, keysToMove);
832 Values optimalValuesFilter = BatchOptimize(newFactors, newValues,1);
837 Values smootherSeparatorValues;
844 Values expectedSmootherValues;
853 Values expectedFilterSeparatorValues;
854 expectedFilterSeparatorValues.
insert(2, newValues.
at<
Pose3>(2));
859 Values partialValuesFilter;
865 orderingFilter.push_back(3);
866 orderingFilter.push_back(2);
869 keysToMarginalize.push_back(3);
871 expectedFilterSummarization = CalculateMarginals(partialGraphFilter, partialValuesFilter, keysToMarginalize);
876 Values actualSmootherValues, actualFilterSeparatorValues;
878 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
887 CHECK(
assert_equal(expectedFilterSeparatorValues, actualFilterSeparatorValues, 1
e-6));
924 newValues.
insert(1, value1);
925 newValues.
insert(2, value2);
926 newValues.
insert(3, value3);
927 newValues.
insert(4, value4);
930 keysToMove.push_back(1);
932 filter.
update(newFactors, newValues, keysToMove);
936 Values smootherSeparatorValues;
941 Values actualSmootherValues, actualFilterSeparatorValues;
943 filter.
synchronize(smootherSummarization, smootherSeparatorValues);
952 Values optimalValues = BatchOptimize(newFactors, newValues, 1);
953 Values expectedSmootherValues;
956 expectedSmootherValues.
insert(1,newValues.
at(1));
966 keysToMove2.push_back(2);
973 Values smootherSeparatorValues2;
990 partialValues.
insert(2, newValues.
at(2));
996 keysToMarginalize.push_back(1);
998 smootherSummarization2 = CalculateMarginals(partialGraph, partialValues, keysToMarginalize);
999 smootherSeparatorValues2.
insert(2, partialValues.
at(2));
1005 Values actualSmootherValues2, actualFilterSeparatorValues2;
1007 filter.
synchronize(smootherSummarization2, smootherSeparatorValues2);
1014 expectedSmootherFactors2.
push_back(factor4);
1016 Values expectedSmootherValues2;
1017 expectedSmootherValues2.
insert(2, newValues.
at(2));
1032 Values partialValuesFilter;
1033 partialValuesFilter.
insert(3, optimalValues.
at(3));
1034 partialValuesFilter.
insert(4, optimalValues.
at(4));
1037 keysToMarginalize2.push_back(4);
1039 NonlinearFactorGraph expectedFilterSummarization2 = CalculateMarginals(partialGraphFilter, partialValuesFilter, keysToMarginalize2);
1040 Values expectedFilterSeparatorValues2;
1041 expectedFilterSeparatorValues2.
insert(3, optimalValues.
at(3));
1043 CHECK(
assert_equal(expectedFilterSeparatorValues2, actualFilterSeparatorValues2, 1
e-6));
1051 partialGraphTransition.
push_back(factor4);
1052 partialGraphTransition.
push_back(smootherSummarization2);
1054 Values partialValuesTransition;
1055 partialValuesTransition.
insert(2,newValues.
at(2));
1056 partialValuesTransition.
insert(3,optimalValues.
at(3));
1059 keysToMarginalize3.push_back(2);
1069 expectedFilterGraph.
push_back(factorEmpty);
1070 expectedFilterGraph.
push_back(factorEmpty);
1071 expectedFilterGraph.
push_back(factorEmpty);
1072 expectedFilterGraph.
push_back(factorEmpty);
1074 expectedFilterGraph.
push_back(factorEmpty);
1075 expectedFilterGraph.
push_back(factorEmpty);
1076 expectedFilterGraph.
push_back(factorEmpty);
1077 expectedFilterGraph.
push_back(CalculateMarginals(partialGraphTransition, partialValuesTransition, keysToMarginalize3));
1106 newValues.
insert(1, value1);
1107 newValues.
insert(2, value2);
1108 newValues.
insert(3, value3);
1122 keysToMarginalize.push_back(1);
1124 actualMarginals = CalculateMarginals(factorGraph, newValues, keysToMarginalize);
1152 newValues.
insert(1, value1);
1153 newValues.
insert(2, value2);
1154 newValues.
insert(3, value3);
1169 keysToMarginalize.push_back(1);
1170 keysToMarginalize.push_back(2);
1172 actualMarginals = CalculateMarginals(factorGraph, newValues, keysToMarginalize);
1195 newFactors.
addPrior(1, poseInitial, noisePrior);
1211 filter.
update(newFactors, newValues, keysToMove);
1217 removeFactorIndices.push_back(1);
1222 filter.
update(noFactors, noValues, keysToMove, removeFactorIndices);
1254 newFactors.
addPrior(1, poseInitial, noisePrior);
1270 filter.
update(newFactors, newValues, keysToMove);
1280 filter.
update(noFactors, noValues, keysToMove, removeFactorIndices);
1313 newFactors.
addPrior(1, poseInitial, noisePrior);
1314 newFactors.
addPrior(1, poseInitial, noisePrior);
1329 filter.
update(newFactors, newValues, keysToMove);
1339 filter.
update(noFactors, noValues, keysToMove, removeFactorIndices);
1370 newFactors.
addPrior(1, poseInitial, noisePrior);
1386 filter.
update(newFactors, newValues, keysToMove);
1396 filter.
update(noFactors, noValues, keysToMove, removeFactorIndices);