42 using namespace gtsam;
60 linearizationPoint.insert<
double>(
X(0), 0);
82 auto f0 = std::make_shared<PriorFactor<Pose2>>(
83 1,
Pose2(), noiseModel::Isotropic::Sigma(3, 0.001));
87 auto f1 = std::make_shared<BetweenFactor<Pose2>>(
88 1, 2,
Pose2(), noiseModel::Isotropic::Sigma(3, 0.1));
101 auto nonlinearFactor = std::make_shared<BetweenFactor<double>>();
104 auto discreteFactor = std::make_shared<DecisionTreeFactor>();
107 auto dcFactor = std::make_shared<HybridNonlinearFactor>();
122 std::make_shared<MotionModel>(
X(0),
X(1), 1.0,
noise_model)};
133 auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
134 X(0),
X(1), 0.0, Isotropic::Sigma(1, 0.1));
136 auto discreteFactor = std::make_shared<DecisionTreeFactor>();
139 auto dcFactor = std::make_shared<HybridNonlinearFactor>(
m1,
components);
142 Values linearizationPoint;
143 linearizationPoint.
insert<
double>(
X(0), 0);
144 linearizationPoint.
insert<
double>(
X(1), 1);
161 auto nonlinearFactor = std::make_shared<BetweenFactor<double>>();
168 auto discreteFactor = std::make_shared<DecisionTreeFactor>();
175 auto dcFactor = std::make_shared<HybridNonlinearFactor>();
183 auto gaussianFactor = std::make_shared<JacobianFactor>();
203 auto noise = noiseModel::Isotropic::Sigma(3, 1.0);
222 auto error_tree =
graph.errorTree(
s.linearizationPoint);
224 auto dkeys =
graph.discreteKeys();
228 auto assignments = DiscreteValues::CartesianProduct(discrete_keys);
229 std::vector<double> leaves(assignments.size());
230 for (
auto &&factor :
graph) {
231 for (
size_t i = 0;
i < assignments.size(); ++
i) {
237 double temp = leaves[1];
238 leaves[1] = leaves[2];
263 *
self.nonlinearFactorGraph().linearize(
self.linearizationPoint);
276 for (
size_t k = 0; k <
self.K; k++)
ordering.push_back(
X(k));
286 TEST(GaussianElimination, Eliminate_x0) {
311 TEST(HybridsGaussianElimination, Eliminate_x1) {
334 Values linearizationPoint) {
338 auto between_x0_x1 = std::make_shared<MotionModel>(
X(0),
X(1),
between,
339 Isotropic::Sigma(1, 1.0));
349 TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
352 auto factors =
self.linearizedFactorGraph();
357 const auto [hybridConditional, factorOnModes] =
360 auto hybridGaussianConditional =
361 dynamic_pointer_cast<HybridGaussianConditional>(
362 hybridConditional->inner());
364 CHECK(hybridGaussianConditional);
371 auto discreteFactor = dynamic_pointer_cast<DecisionTreeFactor>(factorOnModes);
372 CHECK(discreteFactor);
374 EXPECT(discreteFactor->root_->isLeaf() ==
false);
385 for (
size_t k = 0; k <
self.K; k++)
ordering.push_back(
X(k));
388 const auto [hybridBayesNet, remainingFactorGraph] =
389 self.linearizedFactorGraph().eliminatePartialSequential(
ordering);
391 CHECK(hybridBayesNet);
400 CHECK(remainingFactorGraph);
414 self.linearizationPoint);
420 self.linearizationPoint);
426 self.linearizationPoint);
432 self.linearizationPoint);
445 fg.
add(
self.linearizedFactorGraph());
449 self.linearizedFactorGraph().eliminateSequential();
472 for (
size_t k = 0; k <
self.K; k++)
ordering.push_back(
X(k));
475 const auto [hybridBayesNet_partial, remainingFactorGraph_partial] =
476 self.linearizedFactorGraph().eliminatePartialSequential(
ordering);
480 for (
auto &factor : (*remainingFactorGraph_partial)) {
481 auto df = dynamic_pointer_cast<DiscreteFactor>(factor);
487 for (
size_t k = 0; k <
self.K - 1; k++)
ordering.push_back(
M(k));
494 for (
size_t k = 0; k <
self.K; k++)
ordering.push_back(
X(k));
495 for (
size_t k = 0; k <
self.K - 1; k++)
ordering.push_back(
M(k));
499 self.linearizedFactorGraph().eliminateSequential(
ordering);
501 CHECK(hybridBayesNet);
516 dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(3)->inner())
517 ->equals(*discreteBayesNet.at(0)));
522 dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(4)->inner())
523 ->equals(*discreteBayesNet.at(1)));
536 for (
size_t k = 0; k <
self.K; k++)
ordering.push_back(
X(k));
539 const auto [hybridBayesNet, remainingFactorGraph] =
540 linearizedFactorGraph.eliminatePartialSequential(
ordering);
542 #ifdef GTSAM_DT_MERGING
543 string expected_hybridFactorGraph = R
"(
573 HybridGaussianFactor:
601 HybridGaussianFactor:
648 string expected_hybridFactorGraph = R
"(
678 HybridGaussianFactor:
706 HybridGaussianFactor:
759 string expected_hybridBayesNet = R
"(
761 conditional 0: P( x0 | x1 m0)
762 Discrete Keys = (m0, 2),
763 logNormalizationConstant: 1.38862
768 S[x1] = [ -0.0995037 ]
770 logNormalizationConstant: 1.38862
775 S[x1] = [ -0.0995037 ]
777 logNormalizationConstant: 1.38862
780 conditional 1: P( x1 | x2 m0 m1)
781 Discrete Keys = (m0, 2), (m1, 2),
782 logNormalizationConstant: 1.3935
788 S[x2] = [ -0.0990196 ]
790 logNormalizationConstant: 1.3935
795 S[x2] = [ -0.0990196 ]
797 logNormalizationConstant: 1.3935
803 S[x2] = [ -0.0990196 ]
805 logNormalizationConstant: 1.3935
810 S[x2] = [ -0.0990196 ]
812 logNormalizationConstant: 1.3935
815 conditional 2: P( x2 | m0 m1)
816 Discrete Keys = (m0, 2), (m1, 2),
817 logNormalizationConstant: 1.38857
826 logNormalizationConstant: 1.38857
834 logNormalizationConstant: 1.38857
843 logNormalizationConstant: 1.38857
851 logNormalizationConstant: 1.38857
871 auto priorNoise = noiseModel::Diagonal::Sigmas(
879 auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
880 std::vector<NoiseModelFactor::shared_ptr> motion_models = {
881 std::make_shared<PlanarMotionModel>(
X(0),
X(1),
Pose2(0, 0, 0),
888 auto measurementNoise = noiseModel::Diagonal::Sigmas(
891 Rot2 bearing11 = Rot2::fromDegrees(45), bearing22 = Rot2::fromDegrees(90);
892 double range11 =
std::sqrt(4.0 + 4.0), range22 = 2.0;
896 X(0),
L(0), bearing11, range11, measurementNoise);
898 X(1),
L(1), bearing22, range22, measurementNoise);
902 initialEstimate.insert(
X(0),
Pose2(0.5, 0.0, 0.2));
903 initialEstimate.insert(
X(1),
Pose2(2.3, 0.1, -0.2));
904 initialEstimate.insert(
L(0),
Point2(1.8, 2.1));
905 initialEstimate.insert(
L(1),
Point2(4.1, 1.8));
913 const auto [hybridBayesNet, remainingFactorGraph] =
933 const std::vector<double> &
means,
const std::vector<double> &
sigmas,
934 DiscreteKey &
m1,
double x0_measurement,
double measurement_noise = 1
e-3) {
935 auto model0 = noiseModel::Isotropic::Sigma(1,
sigmas[0]);
936 auto model1 = noiseModel::Isotropic::Sigma(1,
sigmas[1]);
937 auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise);
940 std::make_shared<BetweenFactor<double>>(
X(0),
X(1),
means[0], model0);
942 std::make_shared<BetweenFactor<double>>(
X(0),
X(1),
means[1],
model1);
947 std::vector<NonlinearFactorValuePair>
factors{{
f0, 0.0}, {
f1, 0.0}};
974 double x0 = 0.0,
x1 = 1.75;
978 std::vector<double>
means = {0.0, 2.0},
sigmas = {1
e-0, 1
e-0};
1007 auto prior_noise = noiseModel::Isotropic::Sigma(1, 1
e-3);
1050 double x0 = 1.0,
x1 = 1.0;
1054 std::vector<double>
means = {0.0, 0.0},
sigmas = {1e2, 1
e-2};
1061 auto hbn = hgfg->eliminateSequential();
1082 double x0 = 0.0,
x1 = 0.8;
1086 std::vector<double>
means = {0.0, 1.0},
sigmas = {1
e-2, 1
e-2};
1088 double prior_sigma = 1
e-2;
1093 X(1), 1.2, noiseModel::Isotropic::Sigma(1, prior_sigma)));
1098 auto hbn = hgfg->eliminateSequential();
1103 Values expected_first_result;
1104 expected_first_result.
insert(
X(0), 0.0666666666667);
1105 expected_first_result.
insert(
X(1), 1.13333333333);
1111 hbn = hgfg->eliminateSequential();
1112 delta = hbn->optimize();