From 239412956c3632f9da4985cae7583dd3a9c1f31e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 15 Nov 2022 00:50:03 -0500 Subject: [PATCH] address review comments --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 35 ++++-- gtsam/hybrid/HybridGaussianFactorGraph.h | 6 +- gtsam/hybrid/tests/testHybridBayesTree.cpp | 1 - gtsam/hybrid/tests/testHybridEstimation.cpp | 117 ++++++++++++------ gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 2 +- .../hybrid/tests/testHybridNonlinearISAM.cpp | 2 +- 6 files changed, 107 insertions(+), 56 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 4dc309e7b..621830338 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -557,9 +557,9 @@ HybridGaussianFactorGraph::eliminateHybridSequential( const boost::optional continuous, const boost::optional discrete, const Eliminate &function, OptionalVariableIndex variableIndex) const { - Ordering continuous_ordering = + const Ordering continuous_ordering = continuous ? *continuous : Ordering(this->continuousKeys()); - Ordering discrete_ordering = + const Ordering discrete_ordering = discrete ? *discrete : Ordering(this->discreteKeys()); // Eliminate continuous @@ -570,7 +570,8 @@ HybridGaussianFactorGraph::eliminateHybridSequential( function, variableIndex); // Get the last continuous conditional which will have all the discrete keys - auto last_conditional = bayesNet->at(bayesNet->size() - 1); + HybridConditional::shared_ptr last_conditional = + bayesNet->at(bayesNet->size() - 1); DiscreteKeys discrete_keys = last_conditional->discreteKeys(); // If not discrete variables, return the eliminated bayes net. @@ -578,9 +579,11 @@ HybridGaussianFactorGraph::eliminateHybridSequential( return bayesNet; } - AlgebraicDecisionTree probPrimeTree = + // DecisionTree for P'(X|M, Z) for all mode sequences M + const AlgebraicDecisionTree probPrimeTree = this->continuousProbPrimes(discrete_keys, bayesNet); + // Add the model selection factor P(M|Z) discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); // Perform discrete elimination @@ -622,9 +625,9 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal( const boost::optional continuous, const boost::optional discrete, const Eliminate &function, OptionalVariableIndex variableIndex) const { - Ordering continuous_ordering = + const Ordering continuous_ordering = continuous ? *continuous : Ordering(this->continuousKeys()); - Ordering discrete_ordering = + const Ordering discrete_ordering = discrete ? *discrete : Ordering(this->discreteKeys()); // Eliminate continuous @@ -635,9 +638,9 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal( function, variableIndex); // Get the last continuous conditional which will have all the discrete - Key last_continuous_key = - continuous_ordering.at(continuous_ordering.size() - 1); - auto last_conditional = (*bayesTree)[last_continuous_key]->conditional(); + const Key last_continuous_key = continuous_ordering.back(); + HybridConditional::shared_ptr last_conditional = + (*bayesTree)[last_continuous_key]->conditional(); DiscreteKeys discrete_keys = last_conditional->discreteKeys(); // If not discrete variables, return the eliminated bayes net. @@ -645,16 +648,24 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal( return bayesTree; } - AlgebraicDecisionTree probPrimeTree = + // DecisionTree for P'(X|M, Z) for all mode sequences M + const AlgebraicDecisionTree probPrimeTree = this->continuousProbPrimes(discrete_keys, bayesTree); + // Add the model selection factor P(M|Z) discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - auto updatedBayesTree = + // Eliminate discrete variables to get the discrete bayes tree. + // This bayes tree will be updated with the + // continuous variables as the child nodes. + HybridBayesTree::shared_ptr updatedBayesTree = discreteGraph->BaseEliminateable::eliminateMultifrontal(discrete_ordering, function); - auto discrete_clique = (*updatedBayesTree)[discrete_ordering.at(0)]; + // Get the clique with all the discrete keys. + // There should only be 1 clique. + const HybridBayesTree::sharedClique discrete_clique = + (*updatedBayesTree)[discrete_ordering.at(0)]; std::set clique_set; for (auto node : bayesTree->nodes()) { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index d4da66400..47b94070f 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -217,8 +217,10 @@ class GTSAM_EXPORT HybridGaussianFactorGraph const DiscreteValues& discreteValues) const; /** - * @brief Compute the VectorValues solution for the continuous variables for - * each mode. + * @brief Helper method to compute the VectorValues solution for the + * continuous variables for each discrete mode. + * Used as a helper to compute q(\mu | M, Z) which is used by + * both P(X | M, Z) and P(M | Z). * * @tparam BAYES Template on the type of Bayes graph, either a bayes net or a * bayes tree. diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index ab0f3c2d9..9bc12c31d 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -141,7 +141,6 @@ TEST(HybridBayesTree, Optimize) { DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}, {M(2), 2}}; vector probs = {0.012519475, 0.041280228, 0.075018647, 0.081663656, 0.037152205, 0.12248971, 0.07349729, 0.08}; - AlgebraicDecisionTree potentials(discrete_keys, probs); dfg.emplace_shared(discrete_keys, probs); DiscreteValues expectedMPE = dfg.optimize(); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index ec5d6fb7d..431e5769f 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -79,6 +79,8 @@ TEST(HybridEstimation, Incremental) { // Ground truth discrete seq std::vector discrete_seq = {1, 1, 0, 0, 0, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0, 1, 1, 0, 0, 0}; + // Switching example of robot moving in 1D with given measurements and equal + // mode priors. Switching switching(K, 1.0, 0.1, measurements, "1/1 1/1"); HybridSmoother smoother; HybridNonlinearFactorGraph graph; @@ -136,7 +138,7 @@ TEST(HybridEstimation, Incremental) { * @param between_sigma Noise model sigma for the between factor. * @return GaussianFactorGraph::shared_ptr */ -GaussianFactorGraph::shared_ptr specificProblem( +GaussianFactorGraph::shared_ptr specificModesFactorGraph( size_t K, const std::vector& measurements, const std::vector& discrete_seq, double measurement_sigma = 0.1, double between_sigma = 1.0) { @@ -184,7 +186,7 @@ std::vector getDiscreteSequence(size_t x) { } /** - * @brief Helper method to get the probPrimeTree + * @brief Helper method to get the tree of unnormalized probabilities * as per the new elimination scheme. * * @param graph The HybridGaussianFactorGraph to eliminate. @@ -242,18 +244,15 @@ AlgebraicDecisionTree probPrimeTree( TEST(HybridEstimation, Probability) { constexpr size_t K = 4; std::vector measurements = {0, 1, 2, 2}; - - // This is the correct sequence - // std::vector discrete_seq = {1, 1, 0}; - 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++) { - std::vector discrete_seq = getDiscreteSequence(i); + discrete_seq_map[i] = getDiscreteSequence(i); - GaussianFactorGraph::shared_ptr linear_graph = specificProblem( - K, measurements, discrete_seq, measurement_sigma, between_sigma); + GaussianFactorGraph::shared_ptr linear_graph = specificModesFactorGraph( + K, measurements, discrete_seq_map[i], measurement_sigma, between_sigma); auto bayes_net = linear_graph->eliminateSequential(); @@ -263,7 +262,10 @@ TEST(HybridEstimation, Probability) { expected_prob_primes.push_back(linear_graph->probPrime(values)); } - Switching switching(K, between_sigma, measurement_sigma, measurements); + // 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; Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph()); @@ -298,26 +300,30 @@ TEST(HybridEstimation, Probability) { // Test if the probPrimeTree matches the probability of // the individual factor graphs for (size_t i = 0; i < pow(2, K - 1); i++) { - std::vector discrete_seq = getDiscreteSequence(i); Assignment discrete_assignment; - for (size_t v = 0; v < discrete_seq.size(); v++) { - discrete_assignment[M(v)] = discrete_seq[v]; + 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); } - // remainingGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); + discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - // Ordering discrete(graph.discreteKeys()); - // // remainingGraph->print("remainingGraph"); - // // discrete.print(); - // auto discreteBayesNet = remainingGraph->eliminateSequential(discrete); - // bayesNet->add(*discreteBayesNet); - // // bayesNet->print(); + Ordering discrete(graph.discreteKeys()); + auto discreteBayesNet = + discreteGraph->BaseEliminateable::eliminateSequential(discrete); + bayesNet->add(*discreteBayesNet); - // HybridValues hybrid_values = bayesNet->optimize(); - // hybrid_values.discrete().print(); + HybridValues hybrid_values = bayesNet->optimize(); + + // This is the correct sequence as designed + DiscreteValues discrete_seq; + discrete_seq[M(0)] = 1; + discrete_seq[M(1)] = 1; + discrete_seq[M(2)] = 0; + + EXPECT(assert_equal(discrete_seq, hybrid_values.discrete())); } /****************************************************************************/ @@ -330,31 +336,34 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { constexpr size_t K = 4; std::vector measurements = {0, 1, 2, 2}; - // This is the correct sequence - // std::vector discrete_seq = {1, 1, 0}; - 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++) { - std::vector discrete_seq = getDiscreteSequence(i); + discrete_seq_map[i] = getDiscreteSequence(i); - GaussianFactorGraph::shared_ptr linear_graph = specificProblem( - K, measurements, discrete_seq, measurement_sigma, between_sigma); + 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(); - std::cout << i << " " << linear_graph->error(values) << std::endl; expected_errors.push_back(linear_graph->error(values)); expected_prob_primes.push_back(linear_graph->probPrime(values)); } - Switching switching(K, between_sigma, measurement_sigma, measurements); + // 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; Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph()); + // Get the tree of unnormalized probabilities for each mode sequence. AlgebraicDecisionTree expected_probPrimeTree = probPrimeTree(graph); // Eliminate continuous @@ -379,10 +388,9 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { // Test if the probPrimeTree matches the probability of // the individual factor graphs for (size_t i = 0; i < pow(2, K - 1); i++) { - std::vector discrete_seq = getDiscreteSequence(i); Assignment discrete_assignment; - for (size_t v = 0; v < discrete_seq.size(); v++) { - discrete_assignment[M(v)] = discrete_seq[v]; + 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); @@ -390,13 +398,44 @@ TEST(HybridEstimation, ProbabilityMultifrontal) { discreteGraph->add(DecisionTreeFactor(discrete_keys, probPrimeTree)); - // Ordering discrete(graph.discreteKeys()); - // auto discreteBayesTree = discreteGraph->eliminateMultifrontal(discrete); - // // DiscreteBayesTree should have only 1 clique - // bayesTree->addClique((*discreteBayesTree)[discrete.at(0)]); + Ordering discrete(graph.discreteKeys()); + auto discreteBayesTree = + discreteGraph->BaseEliminateable::eliminateMultifrontal(discrete); - // // HybridValues hybrid_values = bayesNet->optimize(); - // // hybrid_values.discrete().print(); + EXPECT_LONGS_EQUAL(1, discreteBayesTree->size()); + // DiscreteBayesTree should have only 1 clique + auto discrete_clique = (*discreteBayesTree)[discrete.at(0)]; + + std::set clique_set; + for (auto node : bayesTree->nodes()) { + clique_set.insert(node.second); + } + + // Set the root of the bayes tree as the discrete clique + for (auto clique : clique_set) { + if (clique->conditional()->parents() == + discrete_clique->conditional()->frontals()) { + discreteBayesTree->addClique(clique, discrete_clique); + + } else { + // Remove the clique from the children of the parents since it will get + // added again in addClique. + auto clique_it = std::find(clique->parent()->children.begin(), + clique->parent()->children.end(), clique); + clique->parent()->children.erase(clique_it); + discreteBayesTree->addClique(clique, clique->parent()); + } + } + + HybridValues hybrid_values = discreteBayesTree->optimize(); + + // This is the correct sequence as designed + DiscreteValues discrete_seq; + discrete_seq[M(0)] = 1; + discrete_seq[M(1)] = 1; + discrete_seq[M(2)] = 0; + + EXPECT(assert_equal(discrete_seq, hybrid_values.discrete())); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 09403853f..4ba6f88a5 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -176,7 +176,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { auto discreteConditional = isam[M(1)]->conditional()->asDiscreteConditional(); - // Test if the probability values are as expected with regression tests. + // Test the probability values with regression tests. DiscreteValues assignment; EXPECT(assert_equal(0.166667, m00_prob, 1e-5)); assignment[M(0)] = 0; diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index e7e65b123..46d5c4324 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -195,7 +195,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { auto discreteConditional = bayesTree[M(1)]->conditional()->asDiscreteConditional(); - // Test if the probability values are as expected with regression tests. + // Test the probability values with regression tests. DiscreteValues assignment; EXPECT(assert_equal(0.166667, m00_prob, 1e-5)); assignment[M(0)] = 0;