54 using namespace gtsam;
64 std::mt19937_64
kRng(42);
80 {std::make_shared<GaussianConditional>(
X(0),
Z_3x1, I_3x3,
X(1), I_3x3),
81 std::make_shared<GaussianConditional>(
X(0), Vector3::Ones(), I_3x3,
X(1),
120 return {std::make_shared<JacobianFactor>(
key, I_3x3,
Z_3x1),
121 std::make_shared<JacobianFactor>(
key, I_3x3, Vector3::Ones())};
139 auto factor = std::dynamic_pointer_cast<DecisionTreeFactor>(
result.second);
159 auto dc =
result->
at(2)->asDiscrete();
257 Ordering::ColamdConstrainedLast(hfg, {
M(0),
M(1),
M(2),
M(3)});
308 std::vector<int> naturalX(
N);
309 std::iota(naturalX.begin(), naturalX.end(), 1);
310 std::vector<Key> ordX;
311 std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
312 [](
int x) { return X(x); });
315 std::copy(ndX.begin(), ndX.end(), std::back_inserter(
ordering));
317 for (
auto &
l : lvls) {
322 std::vector<int> naturalC(
N - 1);
323 std::iota(naturalC.begin(), naturalC.end(), 1);
324 std::vector<Key> ordC;
325 std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
326 [](
int x) { return M(x); });
330 std::copy(ndC.begin(), ndC.end(), std::back_inserter(
ordering));
337 const auto [hbt, remaining] =
338 hfg->eliminatePartialMultifrontal(ordering_full);
365 std::vector<int> naturalX(
N);
366 std::iota(naturalX.begin(), naturalX.end(), 1);
367 std::vector<Key> ordX;
368 std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
369 [](
int x) { return X(x); });
372 std::copy(ndX.begin(), ndX.end(), std::back_inserter(
ordering));
374 for (
auto &
l : lvls) {
379 std::vector<int> naturalC(
N - 1);
380 std::iota(naturalC.begin(), naturalC.end(), 1);
381 std::vector<Key> ordC;
382 std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
383 [](
int x) { return M(x); });
387 std::copy(ndC.begin(), ndC.end(), std::back_inserter(
ordering));
391 const auto [hbt, remaining] =
392 hfg->eliminatePartialMultifrontal(ordering_full);
399 factorGraph.
push_back(new_fg->at(new_fg->size() - 2));
400 factorGraph.
push_back(new_fg->at(new_fg->size() - 1));
413 for (
int t = 1;
t <=
N;
t++) {
420 std::iota(naturalX.begin(), naturalX.end(), 1);
422 for (
size_t i = 1;
i <=
N;
i++) {
423 ordX.emplace_back(
X(
i));
424 ordX.emplace_back(
Y(
i));
427 for (
size_t i = 1;
i <=
N - 1;
i++) {
428 ordX.emplace_back(
M(
i));
430 for (
size_t i = 1;
i <=
N - 1;
i++) {
431 ordX.emplace_back(
D(
i));
457 for (
size_t i = 1;
i <=
N;
i++) {
458 ordering_partial.emplace_back(
X(
i));
459 ordering_partial.emplace_back(
Y(
i));
461 const auto [hbn, remaining] =
462 hfg->eliminatePartialSequential(ordering_partial);
569 Values expected_continuous;
570 expected_continuous.
insert<
double>(
X(0), 0);
571 expected_continuous.
insert<
double>(
X(1), 1);
572 expected_continuous.
insert<
double>(
X(2), 2);
573 expected_continuous.
insert<
double>(
X(3), 4);
574 Values result_continuous =
579 expected_discrete[
M(0)] = 1;
580 expected_discrete[
M(1)] = 1;
613 auto error_tree =
graph.errorTree(
delta.continuous());
615 std::vector<DiscreteKey> discrete_keys = {{
M(0), 2}, {
M(1), 2}};
616 std::vector<double> leaves = {0.9998558, 0.4902432, 0.5193694, 0.0097568};
623 std::vector<double> prob_leaves = {0.36793249, 0.61247742, 0.59489556,
650 auto error_tree =
graph.errorTree(
delta.continuous());
652 std::vector<DiscreteKey> discrete_keys = {{
M(0), 2}, {
M(1), 2}};
653 std::vector<double> leaves = {0.99985581, 0.4902432, 0.51936941,
665 hybridBayesNet =
graph.eliminateSequential();
668 delta = hybridBayesNet->optimize();
669 auto error_tree2 =
graph.errorTree(
delta.continuous());
671 discrete_keys = {{
M(0), 2}, {
M(1), 2}, {
M(2), 2}};
672 leaves = {0.50985198, 0.0097577296, 0.50009425, 0,
673 0.52922138, 0.029127133, 0.50985105, 0.0097567964};
684 const int num_measurements = 1;
690 auto actual = fg.assembleGraphTree();
702 const GF
prior = gf->asGaussian();
723 auto compute_ratio = [&](
HybridValues *sample) ->
double {
724 sample->update(measurements);
729 double expected_ratio = compute_ratio(&sample);
732 for (
size_t i = 0;
i < num_samples;
i++) {
734 if (
std::abs(expected_ratio - compute_ratio(&sample)) > 1
e-6)
return false;
744 auto compute_ratio = [&](
HybridValues *sample) ->
double {
745 sample->update(measurements);
750 double expected_ratio = compute_ratio(&sample);
753 for (
size_t i = 0;
i < num_samples;
i++) {
757 if (
std::abs(expected_ratio - compute_ratio(&sample)) > 1
e-6)
return false;
765 const int num_measurements = 1;
768 auto fg = bn.toFactorGraph(measurements);
779 const auto conditional0 = std::make_shared<GaussianConditional>(
780 X(0),
Vector1(14.1421), I_1x1 * 2.82843),
781 conditional1 = std::make_shared<GaussianConditional>(
782 X(0),
Vector1(10.1379), I_1x1 * 2.02759);
784 mode, std::vector{conditional0, conditional1});
790 const auto posterior = fg.eliminateSequential();
806 std::vector<std::pair<Vector, double>>
parms{{Z_1x1, 3}, {Z_1x1, 0.5}};
811 GaussianConditional::sharedMeanAndStddev(
X(0),
Vector1(5.0), 0.5));
829 const auto conditional0 = std::make_shared<GaussianConditional>(
830 X(0),
Vector1(10.1379), I_1x1 * 2.02759),
831 conditional1 = std::make_shared<GaussianConditional>(
832 X(0),
Vector1(14.1421), I_1x1 * 2.82843);
834 m1, std::vector{conditional0, conditional1});
840 const auto posterior = fg.eliminateSequential();
853 const int num_measurements = 2;
856 auto fg = bn.toFactorGraph(measurements);
865 const auto conditional0 = std::make_shared<GaussianConditional>(
866 X(0),
Vector1(17.3205), I_1x1 * 3.4641),
867 conditional1 = std::make_shared<GaussianConditional>(
868 X(0),
Vector1(10.274), I_1x1 * 2.0548);
870 mode, std::vector{conditional0, conditional1});
876 const auto posterior = fg.eliminateSequential();
886 const int num_measurements = 2;
887 const bool manyModes =
true;
892 auto fg = bn.toFactorGraph(measurements);
898 const auto posterior = fg.eliminateSequential();
912 std::vector<std::pair<Vector, double>> measurementModels{{Z_1x1, 3},
914 for (
size_t t : {0, 1, 2}) {
918 X(
t), measurementModels);
925 std::vector<std::pair<Vector, double>> motionModels{{Z_1x1, 0.2},
927 for (
size_t t : {2, 1}) {
932 X(
t - 1), motionModels);
939 bn.
push_back(GaussianConditional::sharedMeanAndStddev(
X(0), Z_1x1, 0.1));
966 fg1->push_back(*bn1);