30 using namespace gtsam;
41 s.linearizedFactorGraph.eliminateMultifrontal();
45 expectedValues.
insert(
X(0), -0.999904 * Vector1::Ones());
46 expectedValues.
insert(
X(1), -0.99029 * Vector1::Ones());
47 expectedValues.
insert(
X(2), -1.00971 * Vector1::Ones());
48 expectedValues.
insert(
X(3), -1.0001 * Vector1::Ones());
62 for (
size_t i = 1;
i < 4;
i++) {
68 graph1.
push_back(
s.linearizedFactorGraph.at(0));
69 for (
size_t i = 4;
i <= 7;
i++) {
74 for (
size_t i = 7;
i <= 9;
i++) {
91 expected_delta.
insert(make_pair(
X(0), -Vector1::Ones()));
92 expected_delta.
insert(make_pair(
X(1), -Vector1::Ones()));
93 expected_delta.
insert(make_pair(
X(2), -Vector1::Ones()));
94 expected_delta.
insert(make_pair(
X(3), -Vector1::Ones()));
100 for (
size_t k = 0; k <
s.K; k++)
ordering.push_back(
X(k));
102 const auto [hybridBayesNet, remainingFactorGraph] =
103 s.linearizedFactorGraph.eliminatePartialSequential(
ordering);
120 for (
size_t i = 1;
i < 4;
i++) {
126 graph1.
push_back(
s.linearizedFactorGraph.at(0));
127 for (
size_t i = 4;
i <= 6;
i++) {
132 for (
size_t i = 7;
i <= 9;
i++) {
142 for (
size_t k = 0; k <
s.K; k++)
ordering.push_back(
X(k));
144 const auto [hybridBayesNet, remainingFactorGraph] =
145 s.linearizedFactorGraph.eliminatePartialSequential(
ordering);
148 for (
auto&&
f : *remainingFactorGraph) {
149 auto discreteFactor = dynamic_pointer_cast<DiscreteFactor>(
f);
150 assert(discreteFactor);
156 vector<double> probs = {0.012519475, 0.041280228, 0.075018647, 0.081663656,
157 0.037152205, 0.12248971, 0.07349729, 0.08};
161 VectorValues expectedValues = hybridBayesNet->optimize(expectedMPE);
176 for (
size_t i = 1;
i < 4;
i++) {
182 graph1.
push_back(
s.linearizedFactorGraph.at(0));
183 for (
size_t i = 4;
i <= 6;
i++) {
188 for (
size_t i = 7;
i <= 9;
i++) {
195 assignment[
M(0)] = 1;
196 assignment[
M(1)] = 1;
197 assignment[
M(2)] = 1;
203 auto bayesTree =
s.linearizedFactorGraph.eliminateMultifrontal(
ordering);
205 auto expected_gbt = bayesTree->choose(assignment);