Merge branch 'develop' into hybrid/model-selection

gtsam/hybrid/HybridBayesNet.cpp

@@ -249,7 +249,7 @@ HybridValues HybridBayesNet::optimize() const {
   DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize();
 
   // Given the MPE, compute the optimal continuous values.
-  return HybridValues(optimize(mpe)), mpe);
+  return HybridValues(optimize(mpe), mpe);
 }
 
 /* ************************************************************************* */

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -540,27 +540,4 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::probPrime(
   return prob_tree;
 }
 
-/* ************************************************************************ */
-std::pair<Ordering, Ordering>
-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

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -291,24 +291,6 @@ TEST(HybridEstimation, Probability) {
   std::vector<double> measurements = {0, 1, 2, 2};
   double between_sigma = 1.0, measurement_sigma = 0.1;
 
-  std::vector<double> expected_errors, expected_prob_primes;
-  std::map<size_t, std::vector<size_t>> discrete_seq_map;
-  for (size_t i = 0; i < pow(2, K - 1); i++) {
-    discrete_seq_map[i] = getDiscreteSequence<K>(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 switching(K, between_sigma, measurement_sigma, measurements,
@@ -332,18 +314,6 @@ TEST(HybridEstimation, Probability) {
   }
   auto discreteConditional = discreteBayesNet->atDiscrete(0);
 
-  // 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
@@ -367,24 +337,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<double> expected_errors, expected_prob_primes;
-  std::map<size_t, std::vector<size_t>> discrete_seq_map;
-  for (size_t i = 0; i < pow(2, K - 1); i++) {
-    discrete_seq_map[i] = getDiscreteSequence<K>(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,
@@ -408,23 +360,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<Key> 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<Key> 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);
-  }
-
   Ordering discrete(graph.discreteKeys());
   auto discreteBayesTree = discreteGraph->eliminateMultifrontal(discrete);
 
@@ -515,10 +450,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?
+  // TODO(dellaert): dc should be discrete conditional on m0, but it is an
-  // DiscreteKey m(M(0), 2);
+  // unnormalized factor? DiscreteKey m(M(0), 2); DiscreteConditional expected(m
-  // DiscreteConditional expected(m % "0.51341712/1");
+  // % "0.51341712/1"); auto dc = bn->back()->asDiscreteConditional();
-  // auto dc = bn->back()->asDiscreteConditional();
   // EXPECT(assert_equal(expected, *dc, 1e-9));
 }