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);
479 remainingFactorGraph_partial->discreteFactors();
482 for (
size_t k = 0; k <
self.K - 1; k++)
ordering.push_back(
M(k));
489 for (
size_t k = 0; k <
self.K; k++)
ordering.push_back(
X(k));
490 for (
size_t k = 0; k <
self.K - 1; k++)
ordering.push_back(
M(k));
494 self.linearizedFactorGraph().eliminateSequential(
ordering);
496 CHECK(hybridBayesNet);
511 dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(3)->inner())
512 ->equals(*discreteBayesNet.at(0)));
517 dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(4)->inner())
518 ->equals(*discreteBayesNet.at(1)));
531 for (
size_t k = 0; k <
self.K; k++)
ordering.push_back(
X(k));
534 const auto [hybridBayesNet, remainingFactorGraph] =
535 linearizedFactorGraph.eliminatePartialSequential(
ordering);
537 #ifdef GTSAM_DT_MERGING
538 string expected_hybridFactorGraph = R
"(
568 HybridGaussianFactor:
596 HybridGaussianFactor:
643 string expected_hybridFactorGraph = R
"(
673 HybridGaussianFactor:
701 HybridGaussianFactor:
754 string expected_hybridBayesNet = R
"(
756 conditional 0: P( x0 | x1 m0)
757 Discrete Keys = (m0, 2),
758 logNormalizationConstant: 1.38862
763 S[x1] = [ -0.0995037 ]
765 logNormalizationConstant: 1.38862
770 S[x1] = [ -0.0995037 ]
772 logNormalizationConstant: 1.38862
775 conditional 1: P( x1 | x2 m0 m1)
776 Discrete Keys = (m0, 2), (m1, 2),
777 logNormalizationConstant: 1.3935
783 S[x2] = [ -0.0990196 ]
785 logNormalizationConstant: 1.3935
790 S[x2] = [ -0.0990196 ]
792 logNormalizationConstant: 1.3935
798 S[x2] = [ -0.0990196 ]
800 logNormalizationConstant: 1.3935
805 S[x2] = [ -0.0990196 ]
807 logNormalizationConstant: 1.3935
810 conditional 2: P( x2 | m0 m1)
811 Discrete Keys = (m0, 2), (m1, 2),
812 logNormalizationConstant: 1.38857
821 logNormalizationConstant: 1.38857
829 logNormalizationConstant: 1.38857
838 logNormalizationConstant: 1.38857
846 logNormalizationConstant: 1.38857
866 auto priorNoise = noiseModel::Diagonal::Sigmas(
874 auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
875 std::vector<NoiseModelFactor::shared_ptr> motion_models = {
876 std::make_shared<PlanarMotionModel>(
X(0),
X(1),
Pose2(0, 0, 0),
883 auto measurementNoise = noiseModel::Diagonal::Sigmas(
886 Rot2 bearing11 = Rot2::fromDegrees(45), bearing22 = Rot2::fromDegrees(90);
887 double range11 =
std::sqrt(4.0 + 4.0), range22 = 2.0;
891 X(0),
L(0), bearing11, range11, measurementNoise);
893 X(1),
L(1), bearing22, range22, measurementNoise);
897 initialEstimate.insert(
X(0),
Pose2(0.5, 0.0, 0.2));
898 initialEstimate.insert(
X(1),
Pose2(2.3, 0.1, -0.2));
899 initialEstimate.insert(
L(0),
Point2(1.8, 2.1));
900 initialEstimate.insert(
L(1),
Point2(4.1, 1.8));
908 const auto [hybridBayesNet, remainingFactorGraph] =
928 const std::vector<double> &
means,
const std::vector<double> &
sigmas,
929 DiscreteKey &
m1,
double x0_measurement,
double measurement_noise = 1
e-3) {
930 auto model0 = noiseModel::Isotropic::Sigma(1,
sigmas[0]);
931 auto model1 = noiseModel::Isotropic::Sigma(1,
sigmas[1]);
932 auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise);
935 std::make_shared<BetweenFactor<double>>(
X(0),
X(1),
means[0], model0);
937 std::make_shared<BetweenFactor<double>>(
X(0),
X(1),
means[1],
model1);
942 std::vector<NonlinearFactorValuePair>
factors{{
f0, 0.0}, {
f1, 0.0}};
969 double x0 = 0.0,
x1 = 1.75;
973 std::vector<double>
means = {0.0, 2.0},
sigmas = {1
e-0, 1
e-0};
1002 auto prior_noise = noiseModel::Isotropic::Sigma(1, 1
e-3);
1045 double x0 = 1.0,
x1 = 1.0;
1049 std::vector<double>
means = {0.0, 0.0},
sigmas = {1e2, 1
e-2};
1056 auto hbn = hgfg->eliminateSequential();
1077 double x0 = 0.0,
x1 = 0.8;
1081 std::vector<double>
means = {0.0, 1.0},
sigmas = {1
e-2, 1
e-2};
1083 double prior_sigma = 1
e-2;
1088 X(1), 1.2, noiseModel::Isotropic::Sigma(1, prior_sigma)));
1093 auto hbn = hgfg->eliminateSequential();
1098 Values expected_first_result;
1099 expected_first_result.
insert(
X(0), 0.0666666666667);
1100 expected_first_result.
insert(
X(1), 1.13333333333);
1106 hbn = hgfg->eliminateSequential();
1107 delta = hbn->optimize();