50 using namespace gtsam;
58 std::mt19937_64
kRng(42);
74 {std::make_shared<GaussianConditional>(
X(0),
Z_3x1, I_3x3,
X(1), I_3x3),
75 std::make_shared<GaussianConditional>(
X(0), Vector3::Ones(), I_3x3,
X(1),
98 return {std::make_shared<JacobianFactor>(
key, I_3x3,
Z_3x1),
99 std::make_shared<JacobianFactor>(
key, I_3x3, Vector3::Ones())};
117 auto factor = std::dynamic_pointer_cast<DecisionTreeFactor>(
result.second);
138 auto dc =
result->
at(2)->asDiscrete();
174 const double betweenSigma = 0.3, priorSigma = 0.1;
187 auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(
graph.
at(2));
191 auto [factor0, scalar0] = (*hgf)(modeZero);
192 auto [
factor1, scalar1] = (*hgf)(modeOne);
197 auto betweenModel = noiseModel::Isotropic::Sigma(1, betweenSigma);
203 double expectedError0 = 0;
204 for (
const auto &factor :
graph) expectedError0 += factor->error(values0);
209 double expectedError1 = 0;
210 for (
const auto &factor :
graph) expectedError1 += factor->error(values1);
231 graph.discretePosterior(continuousValues);
232 const double sum = probPrime0 + probPrime1;
246 auto [gaussianFactor0, actualScalar0] = productFactor(modeZero);
247 EXPECT(gaussianFactor0.size() == 2);
251 auto [gaussianFactor1, actualScalar1] = productFactor(modeOne);
252 EXPECT(gaussianFactor1.size() == 2);
261 EXPECT(conditional->isHybrid());
262 auto p_x0_given_x1_m = conditional->asHybrid();
263 CHECK(p_x0_given_x1_m);
264 EXPECT(HybridGaussianConditional::CheckInvariants(*p_x0_given_x1_m, values1));
270 EXPECT(std::dynamic_pointer_cast<HybridGaussianFactor>(factor));
271 auto phi_x1_m = std::dynamic_pointer_cast<HybridGaussianFactor>(factor);
276 (*phi_x1_m)(modeZero).second, 1
e-9);
278 (*phi_x1_m)(modeOne).second, 1
e-9);
282 for (
auto &&
mode : {modeZero, modeOne}) {
283 const auto gc = (*p_x0_given_x1_m)(
mode);
289 double originalError = factors_x0.
error({continuousValues,
mode});
290 const double actualError =
gc->negLogConstant() +
291 gc->error(continuousValues) +
292 gf->error(continuousValues) +
scalar;
307 auto [gaussianFactor_x1_0, actualScalar_x1_0] = productFactor_x1(modeZero);
313 auto [gaussianFactor_x1_1, actualScalar_x1_1] = productFactor_x1(modeOne);
320 auto [conditional_x1, factor_x1] = factors_x1.
eliminate(ordering_x1);
323 CHECK(conditional_x1);
324 EXPECT(conditional_x1->isHybrid());
325 auto p_x1_given_m = conditional_x1->asHybrid();
332 auto phi_x1 = std::dynamic_pointer_cast<DecisionTreeFactor>(factor_x1);
339 auto hybridBayesNet =
graph.eliminateSequential();
340 CHECK(hybridBayesNet);
346 hybridBayesNet->discretePosterior(continuousValues);
377 hybridBayesNet->discretePosterior(
delta.continuous());
477 auto pruned =
product.removeEmpty();
481 auto [hybridConditional, factor] = fragment.
eliminate({
X(0)});
482 EXPECT(hybridConditional->isHybrid());
485 EXPECT(dynamic_pointer_cast<HybridGaussianFactor>(factor));
492 Values expected_continuous;
493 expected_continuous.
insert<
double>(
X(0), 0);
494 expected_continuous.
insert<
double>(
X(1), 1);
495 expected_continuous.
insert<
double>(
X(2), 2);
496 expected_continuous.
insert<
double>(
X(3), 4);
497 Values result_continuous =
502 expected_discrete[
M(0)] = 1;
503 expected_discrete[
M(1)] = 1;
529 hybridBayesNet->discretePosterior(
delta.continuous());
537 hybridBayesNet =
graph.eliminateSequential();
540 delta = hybridBayesNet->optimize();
541 graphPosterior =
graph.discretePosterior(
delta.continuous());
542 bnPosterior = hybridBayesNet->discretePosterior(
delta.continuous());
549 const int num_measurements = 1;
555 auto actual = fg.collectProductFactor();
567 const GF
prior = gf->asGaussian();
590 auto compute_ratio = [&](
HybridValues *sample) ->
double {
596 double expected_ratio = compute_ratio(&sample);
599 for (
size_t i = 0;
i < num_samples;
i++) {
601 if (
std::abs(expected_ratio - compute_ratio(&sample)) > 1
e-6)
return false;
611 auto compute_ratio = [&](
HybridValues *sample) ->
double {
617 double expected_ratio = compute_ratio(&sample);
620 for (
size_t i = 0;
i < num_samples;
i++) {
623 if (
std::abs(expected_ratio - compute_ratio(&sample)) > 1
e-6)
return false;
631 const int num_measurements = 1;
645 const auto conditional0 = std::make_shared<GaussianConditional>(
646 X(0),
Vector1(14.1421), I_1x1 * 2.82843),
647 conditional1 = std::make_shared<GaussianConditional>(
648 X(0),
Vector1(10.1379), I_1x1 * 2.02759);
650 mode, std::vector{conditional0, conditional1});
656 const auto posterior = fg.eliminateSequential();
672 std::vector<std::pair<Vector, double>>
parms{{Z_1x1, 3}, {Z_1x1, 0.5}};
677 GaussianConditional::sharedMeanAndStddev(
X(0),
Vector1(5.0), 0.5));
695 const auto conditional0 = std::make_shared<GaussianConditional>(
696 X(0),
Vector1(10.1379), I_1x1 * 2.02759),
697 conditional1 = std::make_shared<GaussianConditional>(
698 X(0),
Vector1(14.1421), I_1x1 * 2.82843);
700 m1, std::vector{conditional0, conditional1});
706 const auto posterior = fg.eliminateSequential();
719 const int num_measurements = 2;
731 const auto conditional0 = std::make_shared<GaussianConditional>(
732 X(0),
Vector1(17.3205), I_1x1 * 3.4641),
733 conditional1 = std::make_shared<GaussianConditional>(
734 X(0),
Vector1(10.274), I_1x1 * 2.0548);
736 mode, std::vector{conditional0, conditional1});
742 const auto posterior = fg.eliminateSequential();
752 const int num_measurements = 2;
753 const bool manyModes =
true;
764 const auto posterior = fg.eliminateSequential();
778 std::vector<std::pair<Vector, double>> measurementModels{{Z_1x1, 3},
780 for (
size_t t : {0, 1, 2}) {
784 X(
t), measurementModels);
791 std::vector<std::pair<Vector, double>> motionModels{{Z_1x1, 0.2},
793 for (
size_t t : {2, 1}) {
798 X(
t - 1), motionModels);
805 bn.
push_back(GaussianConditional::sharedMeanAndStddev(
X(0), Z_1x1, 0.1));
832 fg1->push_back(*bn1);