From 28f349c57d7bd9241877fcbf7f0d61f022f99d8e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 30 Dec 2022 10:47:06 +0530 Subject: [PATCH] minor fixes --- gtsam/hybrid/HybridBayesNet.cpp | 4 +- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 23 ------ gtsam/hybrid/tests/testHybridEstimation.cpp | 78 ++------------------- 3 files changed, 6 insertions(+), 99 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 112cf0747..485abbc37 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -108,7 +108,6 @@ void HybridBayesNet::updateDiscreteConditionals( for (size_t i = 0; i < this->size(); i++) { HybridConditional::shared_ptr conditional = this->at(i); if (conditional->isDiscrete()) { - // std::cout << demangle(typeid(conditional).name()) << std::endl; auto discrete = conditional->asDiscrete(); KeyVector frontals(discrete->frontals().begin(), discrete->frontals().end()); @@ -218,8 +217,7 @@ HybridValues HybridBayesNet::optimize() const { DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize(); // Given the MPE, compute the optimal continuous values. - GaussianBayesNet gbn = choose(mpe); - return HybridValues(gbn.optimize(), mpe); + return HybridValues(optimize(mpe), mpe); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 9427eb582..a28f9dea0 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -539,27 +539,4 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::probPrime( return prob_tree; } -/* ************************************************************************ */ -std::pair -HybridGaussianFactorGraph::separateContinuousDiscreteOrdering( - const Ordering &ordering) const { - KeySet all_continuous_keys = this->continuousKeys(); - KeySet all_discrete_keys = this->discreteKeys(); - Ordering continuous_ordering, discrete_ordering; - - for (auto &&key : ordering) { - if (std::find(all_continuous_keys.begin(), all_continuous_keys.end(), - key) != all_continuous_keys.end()) { - continuous_ordering.push_back(key); - } else if (std::find(all_discrete_keys.begin(), all_discrete_keys.end(), - key) != all_discrete_keys.end()) { - discrete_ordering.push_back(key); - } else { - throw std::runtime_error("Key in ordering not present in factors."); - } - } - - return std::make_pair(continuous_ordering, discrete_ordering); -} - } // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 9b3d192ee..dac1f9f54 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -269,26 +269,8 @@ TEST(HybridEstimation, Probability) { std::vector measurements = {0, 1, 2, 2}; double between_sigma = 1.0, measurement_sigma = 0.1; - std::vector expected_errors, expected_prob_primes; - std::map> discrete_seq_map; - for (size_t i = 0; i < pow(2, K - 1); i++) { - discrete_seq_map[i] = getDiscreteSequence(i); - - GaussianFactorGraph::shared_ptr linear_graph = specificModesFactorGraph( - K, measurements, discrete_seq_map[i], measurement_sigma, between_sigma); - - auto bayes_net = linear_graph->eliminateSequential(); - - VectorValues values = bayes_net->optimize(); - - double error = linear_graph->error(values); - expected_errors.push_back(error); - double prob_prime = linear_graph->probPrime(values); - expected_prob_primes.push_back(prob_prime); - } - - // Switching example of robot moving in 1D with given measurements and equal - // mode priors. + // Switching example of robot moving in 1D with + // given measurements and equal mode priors. Switching switching(K, between_sigma, measurement_sigma, measurements, "1/1 1/1"); auto graph = switching.linearizedFactorGraph; @@ -297,18 +279,6 @@ TEST(HybridEstimation, Probability) { HybridBayesNet::shared_ptr bayesNet = graph.eliminateSequential(ordering); auto discreteConditional = bayesNet->atDiscrete(bayesNet->size() - 3); - // Test if the probPrimeTree matches the probability of - // the individual factor graphs - for (size_t i = 0; i < pow(2, K - 1); i++) { - DiscreteValues discrete_assignment; - for (size_t v = 0; v < discrete_seq_map[i].size(); v++) { - discrete_assignment[M(v)] = discrete_seq_map[i][v]; - } - double discrete_transition_prob = 0.25; - EXPECT_DOUBLES_EQUAL(expected_prob_primes.at(i) * discrete_transition_prob, - (*discreteConditional)(discrete_assignment), 1e-8); - } - HybridValues hybrid_values = bayesNet->optimize(); // This is the correct sequence as designed @@ -332,24 +302,6 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { double between_sigma = 1.0, measurement_sigma = 0.1; - // For each discrete mode sequence, create the individual factor graphs and - // optimize each. - std::vector expected_errors, expected_prob_primes; - std::map> discrete_seq_map; - for (size_t i = 0; i < pow(2, K - 1); i++) { - discrete_seq_map[i] = getDiscreteSequence(i); - - GaussianFactorGraph::shared_ptr linear_graph = specificModesFactorGraph( - K, measurements, discrete_seq_map[i], measurement_sigma, between_sigma); - - auto bayes_tree = linear_graph->eliminateMultifrontal(); - - VectorValues values = bayes_tree->optimize(); - - expected_errors.push_back(linear_graph->error(values)); - expected_prob_primes.push_back(linear_graph->probPrime(values)); - } - // Switching example of robot moving in 1D with given measurements and equal // mode priors. Switching switching(K, between_sigma, measurement_sigma, measurements, @@ -373,25 +325,6 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { auto last_conditional = (*bayesTree)[last_continuous_key]->conditional(); DiscreteKeys discrete_keys = last_conditional->discreteKeys(); - // Create a decision tree of all the different VectorValues - AlgebraicDecisionTree probPrimeTree = - graph.continuousProbPrimes(discrete_keys, bayesTree); - - EXPECT(assert_equal(expected_probPrimeTree, probPrimeTree)); - - // Test if the probPrimeTree matches the probability of - // the individual factor graphs - for (size_t i = 0; i < pow(2, K - 1); i++) { - Assignment discrete_assignment; - for (size_t v = 0; v < discrete_seq_map[i].size(); v++) { - discrete_assignment[M(v)] = discrete_seq_map[i][v]; - } - EXPECT_DOUBLES_EQUAL(expected_prob_primes.at(i), - probPrimeTree(discrete_assignment), 1e-8); - } - - discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - Ordering discrete(graph.discreteKeys()); auto discreteBayesTree = discreteGraph->BaseEliminateable::eliminateMultifrontal(discrete); @@ -483,10 +416,9 @@ TEST(HybridEstimation, eliminateSequentialRegression) { HybridBayesNet::shared_ptr bn = fg->eliminateSequential(ordering); // GTSAM_PRINT(*bn); - // TODO(dellaert): dc should be discrete conditional on m0, but it is an unnormalized factor? - // DiscreteKey m(M(0), 2); - // DiscreteConditional expected(m % "0.51341712/1"); - // auto dc = bn->back()->asDiscreteConditional(); + // TODO(dellaert): dc should be discrete conditional on m0, but it is an + // unnormalized factor? DiscreteKey m(M(0), 2); DiscreteConditional expected(m + // % "0.51341712/1"); auto dc = bn->back()->asDiscreteConditional(); // EXPECT(assert_equal(expected, *dc, 1e-9)); }