52 using namespace gtsam;
62 std::mt19937_64
kRng(42);
77 std::make_shared<GaussianConditional>(
78 X(0),
Z_3x1, I_3x3,
X(1), I_3x3),
79 std::make_shared<GaussianConditional>(
80 X(0), Vector3::Ones(), I_3x3,
X(1), I_3x3)));
130 M(1), std::make_shared<JacobianFactor>(
X(1), I_3x3,
Z_3x1),
131 std::make_shared<JacobianFactor>(
X(1), I_3x3, Vector3::Ones()));
136 auto dc =
result->
at(2)->asDiscrete();
155 std::vector<GaussianFactor::shared_ptr>
factors = {
156 std::make_shared<JacobianFactor>(
X(1), I_3x3,
Z_3x1),
157 std::make_shared<JacobianFactor>(
X(1), I_3x3, Vector3::Ones())};
182 {std::make_shared<JacobianFactor>(
X(1), I_3x3,
Z_3x1),
183 std::make_shared<JacobianFactor>(
X(1), I_3x3, Vector3::Ones())}));
211 M(1), std::make_shared<JacobianFactor>(
X(1), I_3x3,
Z_3x1),
212 std::make_shared<JacobianFactor>(
X(1), I_3x3, Vector3::Ones()));
242 {std::make_shared<JacobianFactor>(
X(0), I_3x3,
Z_3x1),
243 std::make_shared<JacobianFactor>(
X(0), I_3x3, Vector3::Ones())}));
246 M(1), std::make_shared<JacobianFactor>(
X(2), I_3x3,
Z_3x1),
247 std::make_shared<JacobianFactor>(
X(2), I_3x3, Vector3::Ones()));
259 M(3), std::make_shared<JacobianFactor>(
X(3), I_3x3,
Z_3x1),
260 std::make_shared<JacobianFactor>(
X(3), I_3x3, Vector3::Ones()));
265 M(2), std::make_shared<JacobianFactor>(
X(5), I_3x3,
Z_3x1),
266 std::make_shared<JacobianFactor>(
X(5), I_3x3, Vector3::Ones()));
272 Ordering::ColamdConstrainedLast(hfg, {
M(0),
M(1),
M(2),
M(3)});
323 std::vector<int> naturalX(
N);
324 std::iota(naturalX.begin(), naturalX.end(), 1);
325 std::vector<Key> ordX;
326 std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
327 [](
int x) { return X(x); });
330 std::copy(ndX.begin(), ndX.end(), std::back_inserter(
ordering));
332 for (
auto &
l : lvls) {
337 std::vector<int> naturalC(
N - 1);
338 std::iota(naturalC.begin(), naturalC.end(), 1);
339 std::vector<Key> ordC;
340 std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
341 [](
int x) { return M(x); });
345 std::copy(ndC.begin(), ndC.end(), std::back_inserter(
ordering));
352 const auto [hbt, remaining] =
353 hfg->eliminatePartialMultifrontal(ordering_full);
380 std::vector<int> naturalX(
N);
381 std::iota(naturalX.begin(), naturalX.end(), 1);
382 std::vector<Key> ordX;
383 std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
384 [](
int x) { return X(x); });
387 std::copy(ndX.begin(), ndX.end(), std::back_inserter(
ordering));
389 for (
auto &
l : lvls) {
394 std::vector<int> naturalC(
N - 1);
395 std::iota(naturalC.begin(), naturalC.end(), 1);
396 std::vector<Key> ordC;
397 std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
398 [](
int x) { return M(x); });
402 std::copy(ndC.begin(), ndC.end(), std::back_inserter(
ordering));
406 const auto [hbt, remaining] =
407 hfg->eliminatePartialMultifrontal(ordering_full);
414 factorGraph.
push_back(new_fg->at(new_fg->size() - 2));
415 factorGraph.
push_back(new_fg->at(new_fg->size() - 1));
428 for (
int t = 1;
t <=
N;
t++) {
435 std::iota(naturalX.begin(), naturalX.end(), 1);
437 for (
size_t i = 1;
i <=
N;
i++) {
438 ordX.emplace_back(
X(
i));
439 ordX.emplace_back(
Y(
i));
442 for (
size_t i = 1;
i <=
N - 1;
i++) {
443 ordX.emplace_back(
M(
i));
445 for (
size_t i = 1;
i <=
N - 1;
i++) {
446 ordX.emplace_back(
D(
i));
472 for (
size_t i = 1;
i <=
N;
i++) {
473 ordering_partial.emplace_back(
X(
i));
474 ordering_partial.emplace_back(
Y(
i));
476 const auto [hbn, remaining] =
477 hfg->eliminatePartialSequential(ordering_partial);
503 C(1), std::make_shared<JacobianFactor>(
X(1), I_3x3,
Z_3x1),
504 std::make_shared<JacobianFactor>(
X(1), I_3x3, Vector3::Ones()));
539 Values expected_continuous;
540 expected_continuous.
insert<
double>(
X(0), 0);
541 expected_continuous.
insert<
double>(
X(1), 1);
542 expected_continuous.
insert<
double>(
X(2), 2);
543 expected_continuous.
insert<
double>(
X(3), 4);
544 Values result_continuous =
549 expected_discrete[
M(0)] = 1;
550 expected_discrete[
M(1)] = 1;
583 auto error_tree =
graph.errorTree(
delta.continuous());
585 std::vector<DiscreteKey> discrete_keys = {{
M(0), 2}, {
M(1), 2}};
586 std::vector<double> leaves = {0.9998558, 0.4902432, 0.5193694, 0.0097568};
593 std::vector<double> prob_leaves = {0.36793249, 0.61247742, 0.59489556,
605 const int num_measurements = 1;
611 auto actual = fg.assembleGraphTree();
623 const GF
prior = gf->asGaussian();
644 auto compute_ratio = [&](
HybridValues *sample) ->
double {
645 sample->update(measurements);
650 double expected_ratio = compute_ratio(&sample);
653 for (
size_t i = 0;
i < num_samples;
i++) {
655 if (
std::abs(expected_ratio - compute_ratio(&sample)) > 1
e-6)
return false;
665 auto compute_ratio = [&](
HybridValues *sample) ->
double {
666 sample->update(measurements);
671 double expected_ratio = compute_ratio(&sample);
674 for (
size_t i = 0;
i < num_samples;
i++) {
678 if (
std::abs(expected_ratio - compute_ratio(&sample)) > 1
e-6)
return false;
686 const int num_measurements = 1;
689 auto fg = bn.toFactorGraph(measurements);
700 const auto conditional0 = std::make_shared<GaussianConditional>(
701 X(0),
Vector1(14.1421), I_1x1 * 2.82843),
702 conditional1 = std::make_shared<GaussianConditional>(
703 X(0),
Vector1(10.1379), I_1x1 * 2.02759);
711 const auto posterior = fg.eliminateSequential();
730 {GaussianConditional::sharedMeanAndStddev(
Z(0), I_1x1,
X(0), Z_1x1, 3),
731 GaussianConditional::sharedMeanAndStddev(
Z(0), I_1x1,
X(0), Z_1x1,
736 GaussianConditional::sharedMeanAndStddev(
X(0),
Vector1(5.0), 0.5));
754 const auto conditional0 = std::make_shared<GaussianConditional>(
755 X(0),
Vector1(10.1379), I_1x1 * 2.02759),
756 conditional1 = std::make_shared<GaussianConditional>(
757 X(0),
Vector1(14.1421), I_1x1 * 2.82843);
765 const auto posterior = fg.eliminateSequential();
778 const int num_measurements = 2;
781 auto fg = bn.toFactorGraph(measurements);
790 const auto conditional0 = std::make_shared<GaussianConditional>(
791 X(0),
Vector1(17.3205), I_1x1 * 3.4641),
792 conditional1 = std::make_shared<GaussianConditional>(
793 X(0),
Vector1(10.274), I_1x1 * 2.0548);
801 const auto posterior = fg.eliminateSequential();
811 const int num_measurements = 2;
812 const bool manyModes =
true;
817 auto fg = bn.toFactorGraph(measurements);
823 const auto posterior = fg.eliminateSequential();
837 for (
size_t t : {0, 1, 2}) {
842 {GaussianConditional::sharedMeanAndStddev(
843 Z(
t), I_1x1,
X(
t), Z_1x1, 0.5),
844 GaussianConditional::sharedMeanAndStddev(
845 Z(
t), I_1x1,
X(
t), Z_1x1, 3.0)}));
852 for (
size_t t : {2, 1}) {
857 {GaussianConditional::sharedMeanAndStddev(
858 X(
t), I_1x1,
X(
t - 1), Z_1x1, 0.2),
859 GaussianConditional::sharedMeanAndStddev(
860 X(
t), I_1x1,
X(
t - 1), I_1x1, 0.2)}));
867 bn.
push_back(GaussianConditional::sharedMeanAndStddev(
X(0), Z_1x1, 0.1));
894 fg1->push_back(*bn1);