Merge pull request #1371 from borglab/hybrid/multimode

gtsam/hybrid/HybridBayesNet.cpp

@@ -377,4 +377,27 @@ AlgebraicDecisionTree<Key> HybridBayesNet::probPrime(
   return error_tree.apply([](double error) { return exp(-error); });
 }
 
+/* ************************************************************************* */
+HybridGaussianFactorGraph HybridBayesNet::toFactorGraph(
+    const VectorValues &measurements) const {
+  HybridGaussianFactorGraph fg;
+
+  // For all nodes in the Bayes net, if its frontal variable is in measurements,
+  // replace it by a likelihood factor:
+  for (auto &&conditional : *this) {
+    if (conditional->frontalsIn(measurements)) {
+      if (auto gc = conditional->asGaussian())
+        fg.push_back(gc->likelihood(measurements));
+      else if (auto gm = conditional->asMixture())
+        fg.push_back(gm->likelihood(measurements));
+      else {
+        throw std::runtime_error("Unknown conditional type");
+      }
+    } else {
+      fg.push_back(conditional);
+    }
+  }
+  return fg;
+}
+
 }  // namespace gtsam

gtsam/hybrid/HybridBayesNet.h

@@ -229,6 +229,12 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   AlgebraicDecisionTree<Key> probPrime(
       const VectorValues &continuousValues) const;
 
+  /**
+   * Convert a hybrid Bayes net to a hybrid Gaussian factor graph by converting
+   * all conditionals with instantiated measurements into likelihood factors.
+   */
+  HybridGaussianFactorGraph toFactorGraph(
+      const VectorValues &measurements) const;
   /// @}
 
  private:

gtsam/hybrid/HybridConditional.h

@@ -178,6 +178,16 @@ class GTSAM_EXPORT HybridConditional
   /// Return the error of the underlying conditional.
   double error(const HybridValues& values) const override;
 
+  /// Check if VectorValues `measurements` contains all frontal keys.
+  bool frontalsIn(const VectorValues& measurements) const {
+    for (Key key : frontals()) {
+      if (!measurements.exists(key)) {
+        return false;
+      }
+    }
+    return true;
+  }
+
   /// @}
 
  private:

gtsam/hybrid/tests/TinyHybridExample.h

@@ -33,46 +33,34 @@ const DiscreteKey mode{M(0), 2};
 /**
  * Create a tiny two variable hybrid model which represents
  * the generative probability P(z,x,mode) = P(z|x,mode)P(x)P(mode).
+ * num_measurements is the number of measurements of the continuous variable x0.
+ * If manyModes is true, then we introduce one mode per measurement.
  */
-inline HybridBayesNet createHybridBayesNet(int num_measurements = 1) {
+inline HybridBayesNet createHybridBayesNet(int num_measurements = 1,
+                                           bool manyModes = false) {
   HybridBayesNet bayesNet;
 
   // Create Gaussian mixture z_i = x0 + noise for each measurement.
   for (int i = 0; i < num_measurements; i++) {
-    const auto conditional0 = boost::make_shared<GaussianConditional>(
-        GaussianConditional::FromMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5));
-    const auto conditional1 = boost::make_shared<GaussianConditional>(
-        GaussianConditional::FromMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3));
-    GaussianMixture gm({Z(i)}, {X(0)}, {mode}, {conditional0, conditional1});
+    const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode;
+    GaussianMixture gm({Z(i)}, {X(0)}, {mode_i},
+                       {GaussianConditional::sharedMeanAndStddev(
+                            Z(i), I_1x1, X(0), Z_1x1, 0.5),
+                        GaussianConditional::sharedMeanAndStddev(
+                            Z(i), I_1x1, X(0), Z_1x1, 3)});
     bayesNet.emplaceMixture(gm);  // copy :-(
   }
 
   // Create prior on X(0).
-  const auto prior_on_x0 =
-      GaussianConditional::FromMeanAndStddev(X(0), Vector1(5.0), 0.5);
-  bayesNet.emplaceGaussian(prior_on_x0);  // copy :-(
+  bayesNet.addGaussian(
+      GaussianConditional::sharedMeanAndStddev(X(0), Vector1(5.0), 0.5));
 
   // Add prior on mode.
-  bayesNet.emplaceDiscrete(mode, "4/6");
-
-  return bayesNet;
-}
-
-/**
- * Convert a hybrid Bayes net to a hybrid Gaussian factor graph.
- */
-inline HybridGaussianFactorGraph convertBayesNet(
-    const HybridBayesNet& bayesNet, const VectorValues& measurements) {
-  HybridGaussianFactorGraph fg;
-  int num_measurements = bayesNet.size() - 2;
-  for (int i = 0; i < num_measurements; i++) {
-    auto conditional = bayesNet.atMixture(i);
-    auto factor = conditional->likelihood({{Z(i), measurements.at(Z(i))}});
-    fg.push_back(factor);
+  const size_t nrModes = manyModes ? num_measurements : 1;
+  for (int i = 0; i < nrModes; i++) {
+    bayesNet.emplaceDiscrete(DiscreteKey{M(i), 2}, "4/6");
   }
-  fg.push_back(bayesNet.atGaussian(num_measurements));
-  fg.push_back(bayesNet.atDiscrete(num_measurements + 1));
-  return fg;
+  return bayesNet;
 }
 
 /**
@@ -83,12 +71,15 @@ inline HybridGaussianFactorGraph convertBayesNet(
  */
 inline HybridGaussianFactorGraph createHybridGaussianFactorGraph(
     int num_measurements = 1,
-    boost::optional<VectorValues> measurements = boost::none) {
-  auto bayesNet = createHybridBayesNet(num_measurements);
+    boost::optional<VectorValues> measurements = boost::none,
+    bool manyModes = false) {
+  auto bayesNet = createHybridBayesNet(num_measurements, manyModes);
   if (measurements) {
-    return convertBayesNet(bayesNet, *measurements);
+    // Use the measurements to create a hybrid factor graph.
+    return bayesNet.toFactorGraph(*measurements);
   } else {
-    return convertBayesNet(bayesNet, bayesNet.sample().continuous());
+    // Sample from the generative model to create a hybrid factor graph.
+    return bayesNet.toFactorGraph(bayesNet.sample().continuous());
   }
 }
 

gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp

@@ -54,8 +54,10 @@ using namespace gtsam;
 
 using gtsam::symbol_shorthand::D;
 using gtsam::symbol_shorthand::M;
+using gtsam::symbol_shorthand::N;
 using gtsam::symbol_shorthand::X;
 using gtsam::symbol_shorthand::Y;
+using gtsam::symbol_shorthand::Z;
 
 /* ************************************************************************* */
 TEST(HybridGaussianFactorGraph, Creation) {
@@ -624,30 +626,36 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) {
       num_measurements, VectorValues{{Z(0), Vector1(5.0)}});
   EXPECT_LONGS_EQUAL(3, fg.size());
 
-  auto sum = fg.assembleGraphTree();
+  // Assemble graph tree:
+  auto actual = fg.assembleGraphTree();
+
+  // Create expected decision tree with two factor graphs:
 
   // Get mixture factor:
   auto mixture = boost::dynamic_pointer_cast<GaussianMixtureFactor>(fg.at(0));
-  using GF = GaussianFactor::shared_ptr;
+  CHECK(mixture);
 
   // Get prior factor:
-  const GF prior =
-      boost::dynamic_pointer_cast<HybridGaussianFactor>(fg.at(1))->inner();
+  const auto gf = boost::dynamic_pointer_cast<HybridConditional>(fg.at(1));
+  CHECK(gf);
+  using GF = GaussianFactor::shared_ptr;
+  const GF prior = gf->asGaussian();
+  CHECK(prior);
 
   // Create DiscreteValues for both 0 and 1:
   DiscreteValues d0{{M(0), 0}}, d1{{M(0), 1}};
 
   // Expected decision tree with two factor graphs:
   // f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0)
-  GaussianFactorGraphTree expectedSum{
+  GaussianFactorGraphTree expected{
       M(0),
       {GaussianFactorGraph(std::vector<GF>{mixture->factor(d0), prior}),
        mixture->constant(d0)},
       {GaussianFactorGraph(std::vector<GF>{mixture->factor(d1), prior}),
        mixture->constant(d1)}};
 
-  EXPECT(assert_equal(expectedSum(d0), sum(d0), 1e-5));
-  EXPECT(assert_equal(expectedSum(d1), sum(d1), 1e-5));
+  EXPECT(assert_equal(expected(d0), actual(d0), 1e-5));
+  EXPECT(assert_equal(expected(d1), actual(d1), 1e-5));
 }
 
 /* ****************************************************************************/
@@ -657,6 +665,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) {
   const int num_measurements = 1;
   auto fg = tiny::createHybridGaussianFactorGraph(
       num_measurements, VectorValues{{Z(0), Vector1(5.0)}});
+  EXPECT_LONGS_EQUAL(3, fg.size());
 
   // Create expected Bayes Net:
   HybridBayesNet expectedBayesNet;
@@ -691,6 +700,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
   auto fg = tiny::createHybridGaussianFactorGraph(
       num_measurements,
       VectorValues{{Z(0), Vector1(4.0)}, {Z(1), Vector1(6.0)}});
+  EXPECT_LONGS_EQUAL(4, fg.size());
 
   // Create expected Bayes Net:
   HybridBayesNet expectedBayesNet;
@@ -716,6 +726,152 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) {
   EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01));
 }
 
+/* ****************************************************************************/
+// Test eliminating tiny net with 1 mode per measurement.
+TEST(HybridGaussianFactorGraph, EliminateTiny22) {
+  // Create factor graph with 2 measurements such that posterior mean = 5.0.
+  using symbol_shorthand::Z;
+  const int num_measurements = 2;
+  const bool manyModes = true;
+
+  // Create Bayes net and convert to factor graph.
+  auto bn = tiny::createHybridBayesNet(num_measurements, manyModes);
+  const VectorValues measurements{{Z(0), Vector1(4.0)}, {Z(1), Vector1(6.0)}};
+  auto fg = bn.toFactorGraph(measurements);
+  EXPECT_LONGS_EQUAL(5, fg.size());
+
+  // Test elimination
+  Ordering ordering;
+  ordering.push_back(X(0));
+  ordering.push_back(M(0));
+  ordering.push_back(M(1));
+  const auto posterior = fg.eliminateSequential(ordering);
+
+  // Compute the log-ratio between the Bayes net and the factor graph.
+  auto compute_ratio = [&](HybridValues *sample) -> double {
+    // update sample with given measurements:
+    sample->update(measurements);
+    return bn.evaluate(*sample) / posterior->evaluate(*sample);
+  };
+
+  // Set up sampling
+  std::mt19937_64 rng(42);
+
+  // The error evaluated by the factor graph and the Bayes net should differ by
+  // the normalizing term computed via the Bayes net determinant.
+  HybridValues sample = bn.sample(&rng);
+  double expected_ratio = compute_ratio(&sample);
+  // regression
+  EXPECT_DOUBLES_EQUAL(0.018253037966018862, expected_ratio, 1e-6);
+
+  // 3. Do sampling
+  constexpr int num_samples = 100;
+  for (size_t i = 0; i < num_samples; i++) {
+    // Sample from the bayes net
+    HybridValues sample = bn.sample(&rng);
+
+    // Check that the ratio is constant.
+    EXPECT_DOUBLES_EQUAL(expected_ratio, compute_ratio(&sample), 1e-6);
+  }
+}
+
+/* ****************************************************************************/
+// Test elimination of a switching network with one mode per measurement.
+TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
+  // Create a switching network with one mode per measurement.
+  HybridBayesNet bn;
+
+  // NOTE: we add reverse topological so we can sample from the Bayes net.:
+
+  // Add measurements:
+  for (size_t t : {0, 1, 2}) {
+    // Create Gaussian mixture on Z(t) conditioned on X(t) and mode N(t):
+    const auto noise_mode_t = DiscreteKey{N(t), 2};
+    GaussianMixture gm({Z(t)}, {X(t)}, {noise_mode_t},
+                       {GaussianConditional::sharedMeanAndStddev(
+                            Z(t), I_1x1, X(t), Z_1x1, 0.5),
+                        GaussianConditional::sharedMeanAndStddev(
+                            Z(t), I_1x1, X(t), Z_1x1, 3.0)});
+    bn.emplaceMixture(gm);  // copy :-(
+
+    // Create prior on discrete mode M(t):
+    bn.emplaceDiscrete(noise_mode_t, "20/80");
+  }
+
+  // Add motion models:
+  for (size_t t : {2, 1}) {
+    // Create Gaussian mixture on X(t) conditioned on X(t-1) and mode M(t-1):
+    const auto motion_model_t = DiscreteKey{M(t), 2};
+    GaussianMixture gm({X(t)}, {X(t - 1)}, {motion_model_t},
+                       {GaussianConditional::sharedMeanAndStddev(
+                            X(t), I_1x1, X(t - 1), Z_1x1, 0.2),
+                        GaussianConditional::sharedMeanAndStddev(
+                            X(t), I_1x1, X(t - 1), I_1x1, 0.2)});
+    bn.emplaceMixture(gm);  // copy :-(
+
+    // Create prior on motion model M(t):
+    bn.emplaceDiscrete(motion_model_t, "40/60");
+  }
+
+  // Create Gaussian prior on continuous X(0) using sharedMeanAndStddev:
+  bn.addGaussian(GaussianConditional::sharedMeanAndStddev(X(0), Z_1x1, 0.1));
+
+  // Make sure we an sample from the Bayes net:
+  EXPECT_LONGS_EQUAL(6, bn.sample().continuous().size());
+
+  // Create measurements consistent with moving right every time:
+  const VectorValues measurements{
+      {Z(0), Vector1(0.0)}, {Z(1), Vector1(1.0)}, {Z(2), Vector1(2.0)}};
+  const auto fg = bn.toFactorGraph(measurements);
+
+  // Create ordering that eliminates in time order, then discrete modes:
+  Ordering ordering;
+  ordering.push_back(X(2));
+  ordering.push_back(X(1));
+  ordering.push_back(X(0));
+  ordering.push_back(N(0));
+  ordering.push_back(N(1));
+  ordering.push_back(N(2));
+  ordering.push_back(M(1));
+  ordering.push_back(M(2));
+
+  // Test elimination result has correct size:
+  const auto posterior = fg.eliminateSequential(ordering);
+  // GTSAM_PRINT(*posterior);
+
+  // Test elimination result has correct size:
+  EXPECT_LONGS_EQUAL(8, posterior->size());
+
+  // TODO(dellaert): below is copy/pasta from above, refactor
+
+  // Compute the log-ratio between the Bayes net and the factor graph.
+  auto compute_ratio = [&](HybridValues *sample) -> double {
+    // update sample with given measurements:
+    sample->update(measurements);
+    return bn.evaluate(*sample) / posterior->evaluate(*sample);
+  };
+
+  // Set up sampling
+  std::mt19937_64 rng(42);
+
+  // The error evaluated by the factor graph and the Bayes net should differ by
+  // the normalizing term computed via the Bayes net determinant.
+  HybridValues sample = bn.sample(&rng);
+  double expected_ratio = compute_ratio(&sample);
+  // regression
+  EXPECT_DOUBLES_EQUAL(0.0094526745785019472, expected_ratio, 1e-6);
+
+  // 3. Do sampling
+  constexpr int num_samples = 100;
+  for (size_t i = 0; i < num_samples; i++) {
+    // Sample from the bayes net
+    HybridValues sample = bn.sample(&rng);
+
+    // Check that the ratio is constant.
+    EXPECT_DOUBLES_EQUAL(expected_ratio, compute_ratio(&sample), 1e-6);
+  }
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/linear/GaussianConditional.cpp

@@ -263,7 +263,7 @@ double GaussianConditional::evaluate(const VectorValues& x) const {
     Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
     frontalVec = R().transpose().triangularView<Eigen::Lower>().solve(frontalVec);
 
-    // Check for indeterminant solution
+    // Check for indeterminate solution
     if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
 
     for (const_iterator it = beginParents(); it!= endParents(); it++)

gtsam/linear/GaussianConditional.h

@@ -100,6 +100,12 @@ namespace gtsam {
                                                  const Matrix& A2, Key parent2,
                                                  const Vector& b, double sigma);
 
+    /// Create shared pointer by forwarding arguments to fromMeanAndStddev.
+    template<typename... Args>
+    static shared_ptr sharedMeanAndStddev(Args&&... args) {
+      return boost::make_shared<This>(FromMeanAndStddev(std::forward<Args>(args)...));
+    }
+
     /** Combine several GaussianConditional into a single dense GC.  The conditionals enumerated by
     *   \c first and \c last must be in increasing order, meaning that the parents of any
     *   conditional may not include a conditional coming before it.