From 1789bb74fe045b45e20e480a06053231b7e68ae7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 25 Oct 2022 14:02:36 -0400 Subject: [PATCH] showing difference in computed probabilities --- gtsam/hybrid/tests/Switching.h | 1 + gtsam/hybrid/tests/testHybridEstimation.cpp | 10 +++++++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index a28db4894..76721edc4 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -130,6 +130,7 @@ struct Switching { * @param K The total number of timesteps. * @param between_sigma The stddev between poses. * @param prior_sigma The stddev on priors (also used for measurements). + * @param measurements Vector of measurements for each timestep. */ Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1, std::vector measurements = {}) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index d786528d3..9104ad584 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -201,10 +201,14 @@ TEST(HybridEstimation, Probability) { VectorValues values = bayes_net->optimize(); std::cout << i << " : " << linear_graph->probPrime(values) << std::endl; } - // std::cout << linear_graph->error(values) << std::endl; - // // values.at(); - // // linearizationPoint.retract(values).print(); + Switching switching(K, between_sigma, measurement_sigma, measurements); + auto graph = switching.linearizedFactorGraph; + Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph()); + HybridBayesNet::shared_ptr bayesNet = graph.eliminateSequential(ordering); + const DecisionTreeFactor::shared_ptr decisionTree = + bayesNet->discreteConditionals(); + decisionTree->print(); } /* ************************************************************************* */