38 using namespace gtsam;
127 auto discreteConditional_m0 = bayesTree[
M(0)]->conditional()->asDiscrete();
149 const auto [expectedHybridBayesTree, expectedRemainingGraph] =
151 .BaseEliminateable::eliminatePartialMultifrontal(
ordering);
154 auto x0_conditional = dynamic_pointer_cast<GaussianMixture>(
155 bayesTree[
X(0)]->conditional()->inner());
156 auto expected_x0_conditional = dynamic_pointer_cast<GaussianMixture>(
157 (*expectedHybridBayesTree)[
X(0)]->conditional()->inner());
161 auto x1_conditional = dynamic_pointer_cast<GaussianMixture>(
162 bayesTree[
X(1)]->conditional()->inner());
163 auto expected_x1_conditional = dynamic_pointer_cast<GaussianMixture>(
164 (*expectedHybridBayesTree)[
X(1)]->conditional()->inner());
168 auto x2_conditional = dynamic_pointer_cast<GaussianMixture>(
169 bayesTree[
X(2)]->conditional()->inner());
170 auto expected_x2_conditional = dynamic_pointer_cast<GaussianMixture>(
171 (*expectedHybridBayesTree)[
X(2)]->conditional()->inner());
178 expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal(
182 m00[
M(0)] = 0, m00[
M(1)] = 0;
184 *(*discreteBayesTree)[
M(1)]->conditional()->asDiscrete();
185 double m00_prob = decisionTree(m00);
187 auto discreteConditional = bayesTree[
M(1)]->conditional()->asDiscrete();
192 assignment[
M(0)] = 0;
193 assignment[
M(1)] = 0;
195 assignment[
M(0)] = 1;
196 assignment[
M(1)] = 0;
198 assignment[
M(0)] = 0;
199 assignment[
M(1)] = 1;
201 assignment[
M(0)] = 1;
202 assignment[
M(1)] = 1;
207 auto expectedChordal = expectedRemainingGraph->eliminateMultifrontal();
208 auto actualConditional = dynamic_pointer_cast<DecisionTreeFactor>(
209 bayesTree[
M(1)]->conditional()->inner());
212 vector<double> probs = {0.095292197, 0.31417524, 0.28275772, 0.30777485};
213 auto expectedConditional =
214 std::make_shared<DecisionTreeFactor>(discrete_keys, probs);
227 for (
size_t i = 1;
i < 4;
i++) {
234 for (
size_t i = 4;
i <= 7;
i++) {
241 for (
size_t j = 0;
j < 4;
j++) {
246 const auto [unprunedHybridBayesTree, unprunedRemainingGraph] =
248 .BaseEliminateable::eliminatePartialMultifrontal(
ordering);
250 size_t maxNrLeaves = 5;
254 bayesTree.
prune(maxNrLeaves);
290 auto discreteConditional_m0 = *dynamic_pointer_cast<DiscreteConditional>(
291 bayesTree[
M(0)]->conditional()->inner());
295 auto count = [](
const double &
value,
int count) {
296 return value > 0 ? count + 1 : count;
303 auto &unprunedLastDensity = *dynamic_pointer_cast<GaussianMixture>(
304 unprunedHybridBayesTree->clique(
X(3))->conditional()->inner());
305 auto &lastDensity = *dynamic_pointer_cast<GaussianMixture>(
306 bayesTree[
X(3)]->conditional()->inner());
308 std::vector<std::pair<DiscreteValues, double>> assignments =
309 discreteConditional_m0.enumerate();
311 for (
auto &&av : assignments) {
313 const double value = av.second;
316 EXPECT(lastDensity(assignment) ==
nullptr);
318 CHECK(lastDensity(assignment));
320 *lastDensity(assignment)));
335 for (
size_t i = 1;
i < 4;
i++) {
343 for (
size_t i = 5;
i <= 7;
i++) {
349 size_t maxComponents = 5;
351 incrementalHybrid.
prune(maxComponents);
358 2, bayesTree[
X(0)]->conditional()->asMixture()->nrComponents());
360 3, bayesTree[
X(1)]->conditional()->asMixture()->nrComponents());
362 5, bayesTree[
X(2)]->conditional()->asMixture()->nrComponents());
364 5, bayesTree[
X(3)]->conditional()->asMixture()->nrComponents());
375 incrementalHybrid.
prune(maxComponents);
376 bayesTree = incrementalHybrid.
bayesTree();
382 5, bayesTree[
X(3)]->conditional()->asMixture()->nrComponents());
384 5, bayesTree[
X(4)]->conditional()->asMixture()->nrComponents());
401 auto priorNoise = noiseModel::Diagonal::Sigmas(
406 auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1);
434 auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
435 auto still = std::make_shared<PlanarMotionModel>(
W(0),
W(1),
Pose2(0, 0, 0),
437 moving = std::make_shared<PlanarMotionModel>(
W(0),
W(1),
odometry,
439 std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
440 auto mixtureFactor = std::make_shared<MixtureFactor>(
474 contKeys = {
W(1),
W(2)};
475 still = std::make_shared<PlanarMotionModel>(
W(1),
W(2),
Pose2(0, 0, 0),
478 std::make_shared<PlanarMotionModel>(
W(1),
W(2),
odometry, noise_model);
479 components = {moving, still};
480 mixtureFactor = std::make_shared<MixtureFactor>(
517 contKeys = {
W(2),
W(3)};
518 still = std::make_shared<PlanarMotionModel>(
W(2),
W(3),
Pose2(0, 0, 0),
521 std::make_shared<PlanarMotionModel>(
W(2),
W(3),
odometry, noise_model);
522 components = {moving, still};
523 mixtureFactor = std::make_shared<MixtureFactor>(
554 auto discreteTree = bayesTree[
M(3)]->conditional()->asDiscrete();
563 expected_assignment[
M(1)] = 1;
564 expected_assignment[
M(2)] = 1;
565 expected_assignment[
M(3)] = 1;
571 auto lastConditional = bayesTree[
X(3)]->conditional()->asMixture();