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);
387 for (
size_t k = 0; k <
self.K; k++)
ordering.push_back(
X(k));
390 const auto [hybridBayesNet, remainingFactorGraph] =
391 linearizedFactorGraph.eliminatePartialSequential(
ordering);
393 CHECK(hybridBayesNet);
402 CHECK(remainingFactorGraph);
416 self.linearizationPoint);
422 self.linearizationPoint);
428 self.linearizationPoint);
434 self.linearizationPoint);
446 fg.
add(
self.linearizedFactorGraph);
450 self.linearizedFactorGraph.eliminateSequential();
475 for (
size_t k = 0; k <
self.K; k++)
ordering.push_back(
X(k));
478 const auto [hybridBayesNet_partial, remainingFactorGraph_partial] =
479 linearizedFactorGraph.eliminatePartialSequential(
ordering);
483 for (
auto &factor : (*remainingFactorGraph_partial)) {
484 auto df = dynamic_pointer_cast<DiscreteFactor>(factor);
490 for (
size_t k = 0; k <
self.K - 1; k++)
ordering.push_back(
M(k));
497 for (
size_t k = 0; k <
self.K; k++)
ordering.push_back(
X(k));
498 for (
size_t k = 0; k <
self.K - 1; k++)
ordering.push_back(
M(k));
502 linearizedFactorGraph.eliminateSequential(
ordering);
504 CHECK(hybridBayesNet);
519 dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(3)->inner())
520 ->equals(*discreteBayesNet.
at(0)));
525 dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(4)->inner())
526 ->equals(*discreteBayesNet.
at(1)));
539 for (
size_t k = 0; k <
self.K; k++)
ordering.push_back(
X(k));
542 const auto [hybridBayesNet, remainingFactorGraph] =
543 linearizedFactorGraph.eliminatePartialSequential(
ordering);
545 #ifdef GTSAM_DT_MERGING
546 string expected_hybridFactorGraph = R
"(
619 factor 6: P( m1 | m0 ):
630 string expected_hybridFactorGraph = R
"(
703 factor 6: P( m1 | m0 ):
718 string expected_hybridBayesNet = R
"(
720 conditional 0: Hybrid P( x0 | x1 m0)
721 Discrete Keys = (m0, 2),
722 logNormalizationConstant: 1.38862
727 S[x1] = [ -0.0995037 ]
729 logNormalizationConstant: 1.38862
734 S[x1] = [ -0.0995037 ]
736 logNormalizationConstant: 1.38862
739 conditional 1: Hybrid P( x1 | x2 m0 m1)
740 Discrete Keys = (m0, 2), (m1, 2),
741 logNormalizationConstant: 1.3935
747 S[x2] = [ -0.0990196 ]
749 logNormalizationConstant: 1.3935
754 S[x2] = [ -0.0990196 ]
756 logNormalizationConstant: 1.3935
762 S[x2] = [ -0.0990196 ]
764 logNormalizationConstant: 1.3935
769 S[x2] = [ -0.0990196 ]
771 logNormalizationConstant: 1.3935
774 conditional 2: Hybrid P( x2 | m0 m1)
775 Discrete Keys = (m0, 2), (m1, 2),
776 logNormalizationConstant: 1.38857
785 logNormalizationConstant: 1.38857
793 logNormalizationConstant: 1.38857
802 logNormalizationConstant: 1.38857
810 logNormalizationConstant: 1.38857
830 auto priorNoise = noiseModel::Diagonal::Sigmas(
838 auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
839 std::vector<NoiseModelFactor::shared_ptr> motion_models = {
840 std::make_shared<PlanarMotionModel>(
X(0),
X(1),
Pose2(0, 0, 0),
847 auto measurementNoise = noiseModel::Diagonal::Sigmas(
850 Rot2 bearing11 = Rot2::fromDegrees(45), bearing22 = Rot2::fromDegrees(90);
851 double range11 =
std::sqrt(4.0 + 4.0), range22 = 2.0;
855 X(0),
L(0), bearing11, range11, measurementNoise);
857 X(1),
L(1), bearing22, range22, measurementNoise);
861 initialEstimate.insert(
X(0),
Pose2(0.5, 0.0, 0.2));
862 initialEstimate.insert(
X(1),
Pose2(2.3, 0.1, -0.2));
863 initialEstimate.insert(
L(0),
Point2(1.8, 2.1));
864 initialEstimate.insert(
L(1),
Point2(4.1, 1.8));
872 const auto [hybridBayesNet, remainingFactorGraph] =
892 const std::vector<double> &
means,
const std::vector<double> &
sigmas,
893 DiscreteKey &
m1,
double x0_measurement,
double measurement_noise = 1
e-3) {
894 auto model0 = noiseModel::Isotropic::Sigma(1,
sigmas[0]);
895 auto model1 = noiseModel::Isotropic::Sigma(1,
sigmas[1]);
896 auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise);
899 std::make_shared<BetweenFactor<double>>(
X(0),
X(1),
means[0], model0);
901 std::make_shared<BetweenFactor<double>>(
X(0),
X(1),
means[1],
model1);
906 std::vector<NonlinearFactorValuePair>
factors{{
f0, 0.0}, {
f1, 0.0}};
933 double x0 = 0.0,
x1 = 1.75;
937 std::vector<double>
means = {0.0, 2.0},
sigmas = {1
e-0, 1
e-0};
968 auto prior_noise = noiseModel::Isotropic::Sigma(1, 1
e-3);
1011 double x0 = 1.0,
x1 = 1.0;
1015 std::vector<double>
means = {0.0, 0.0},
sigmas = {1e2, 1
e-2};
1022 auto hbn = hgfg->eliminateSequential();
1050 double x0 = 0.0,
x1 = 0.8;
1054 std::vector<double>
means = {0.0, 1.0},
sigmas = {1
e-2, 1
e-2};
1056 double prior_sigma = 1
e-2;
1061 X(1), 1.2, noiseModel::Isotropic::Sigma(1, prior_sigma)));
1066 auto hbn = hgfg->eliminateSequential();
1071 Values expected_first_result;
1072 expected_first_result.
insert(
X(0), 0.0666666666667);
1073 expected_first_result.
insert(
X(1), 1.13333333333);
1079 hbn = hgfg->eliminateSequential();
1080 delta = hbn->optimize();