Add various tests for Hybrid

gtsam/discrete/DecisionTree-inl.h

@@ -39,10 +39,10 @@
 #include <string>
 #include <vector>
 
-using boost::assign::operator+=;
-
 namespace gtsam {
 
+  using boost::assign::operator+=;
+
   /****************************************************************************/
   // Node
   /****************************************************************************/

gtsam/hybrid/GaussianMixture.cpp

@@ -19,7 +19,7 @@
  */
 
 #include <gtsam/base/utilities.h>
-#include <gtsam/discrete/DecisionTree-inl.h>
+#include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/inference/Conditional-inst.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
@@ -36,8 +36,7 @@ GaussianMixture::GaussianMixture(
       conditionals_(conditionals) {}
 
 /* *******************************************************************************/
-const GaussianMixture::Conditionals &
-GaussianMixture::conditionals() {
+const GaussianMixture::Conditionals &GaussianMixture::conditionals() {
   return conditionals_;
 }
 
@@ -48,8 +47,8 @@ GaussianMixture GaussianMixture::FromConditionals(
     const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
   Conditionals dt(discreteParents, conditionalsList);
 
-  return GaussianMixture(continuousFrontals, continuousParents,
-                                    discreteParents, dt);
+  return GaussianMixture(continuousFrontals, continuousParents, discreteParents,
+                         dt);
 }
 
 /* *******************************************************************************/
@@ -66,8 +65,7 @@ GaussianMixture::Sum GaussianMixture::add(
 }
 
 /* *******************************************************************************/
-GaussianMixture::Sum
-GaussianMixture::asGaussianFactorGraphTree() const {
+GaussianMixture::Sum GaussianMixture::asGaussianFactorGraphTree() const {
   auto lambda = [](const GaussianFactor::shared_ptr &factor) {
     GaussianFactorGraph result;
     result.push_back(factor);
@@ -77,21 +75,42 @@ GaussianMixture::asGaussianFactorGraphTree() const {
 }
 
 /* *******************************************************************************/
-bool GaussianMixture::equals(const HybridFactor &lf,
-                                        double tol) const {
+size_t GaussianMixture::nrComponents() const {
+  size_t total = 0;
+  conditionals_.visit([&total](const GaussianFactor::shared_ptr &node) {
+    if (node) total += 1;
+  });
+  return total;
+}
+
+/* *******************************************************************************/
+GaussianConditional::shared_ptr GaussianMixture::operator()(
+    const DiscreteValues &discreteVals) const {
+  auto &ptr = conditionals_(discreteVals);
+  if (!ptr) return nullptr;
+  auto conditional = boost::dynamic_pointer_cast<GaussianConditional>(ptr);
+  if (conditional)
+    return conditional;
+  else
+    throw std::logic_error(
+        "A GaussianMixture unexpectedly contained a non-conditional");
+}
+
+/* *******************************************************************************/
+bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
   const This *e = dynamic_cast<const This *>(&lf);
   return e != nullptr && BaseFactor::equals(*e, tol);
 }
 
 /* *******************************************************************************/
 void GaussianMixture::print(const std::string &s,
-                                       const KeyFormatter &formatter) const {
+                            const KeyFormatter &formatter) const {
   std::cout << s;
   if (isContinuous()) std::cout << "Continuous ";
   if (isDiscrete()) std::cout << "Discrete ";
   if (isHybrid()) std::cout << "Hybrid ";
   BaseConditional::print("", formatter);
-  std::cout << "\nDiscrete Keys = ";
+  std::cout << " Discrete Keys = ";
   for (auto &dk : discreteKeys()) {
     std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
   }

gtsam/hybrid/GaussianMixture.h

@@ -19,7 +19,9 @@
 
 #pragma once
 
+#include <gtsam/discrete/DecisionTree-inl.h>
 #include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/inference/Conditional.h>
 #include <gtsam/linear/GaussianConditional.h>
@@ -99,6 +101,16 @@ class GTSAM_EXPORT GaussianMixture
       const DiscreteKeys &discreteParents,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
+  /// @}
+  /// @name Standard API
+  /// @{
+
+  GaussianConditional::shared_ptr operator()(
+      const DiscreteValues &discreteVals) const;
+
+  /// Returns the total number of continuous components
+  size_t nrComponents() const;
+
   /// @}
   /// @name Testable
   /// @{

gtsam/hybrid/GaussianMixtureFactor.cpp

@@ -51,16 +51,19 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactors(
 void GaussianMixtureFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
+  std::cout << "]{\n";
   factors_.print(
-      "mixture = ", [&](Key k) { return formatter(k); },
+      "", [&](Key k) { return formatter(k); },
       [&](const GaussianFactor::shared_ptr &gf) -> std::string {
         RedirectCout rd;
-        if (!gf->empty())
+        std::cout << ":\n";
+        if (gf)
           gf->print("", formatter);
         else
           return {"nullptr"};
         return rd.str();
       });
+  std::cout << "}" << std::endl;
 }
 
 /* *******************************************************************************/

gtsam/hybrid/GaussianMixtureFactor.h

@@ -84,6 +84,19 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
                         const DiscreteKeys &discreteKeys,
                         const Factors &factors);
 
+  /**
+   * @brief Construct a new GaussianMixtureFactor object using a vector of
+   * GaussianFactor shared pointers.
+   *
+   * @param keys Vector of keys for continuous factors.
+   * @param discreteKeys Vector of discrete keys.
+   * @param factors Vector of gaussian factor shared pointers.
+   */
+  GaussianMixtureFactor(const KeyVector &keys, const DiscreteKeys &discreteKeys,
+                        const std::vector<GaussianFactor::shared_ptr> &factors)
+      : GaussianMixtureFactor(keys, discreteKeys,
+                              Factors(discreteKeys, factors)) {}
+
   static This FromFactors(
       const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
       const std::vector<GaussianFactor::shared_ptr> &factors);
@@ -111,6 +124,12 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    * @return Sum
    */
   Sum add(const Sum &sum) const;
+
+  /// Add MixtureFactor to a Sum, syntactic sugar.
+  friend Sum &operator+=(Sum &sum, const GaussianMixtureFactor &factor) {
+    sum = factor.add(sum);
+    return sum;
+  }
 };
 
 // traits

gtsam/hybrid/HybridDiscreteFactor.cpp

@@ -47,7 +47,7 @@ bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const {
 void HybridDiscreteFactor::print(const std::string &s,
                                  const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
-  inner_->print("inner: ", formatter);
+  inner_->print("\n", formatter);
 };
 
 }  // namespace gtsam

gtsam/hybrid/HybridFactor.cpp

@@ -50,7 +50,10 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &keys)
-    : Base(keys), isContinuous_(true), nrContinuous_(keys.size()) {}
+    : Base(keys),
+      isContinuous_(true),
+      nrContinuous_(keys.size()),
+      continuousKeys_(keys) {}
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &continuousKeys,
@@ -60,13 +63,15 @@ HybridFactor::HybridFactor(const KeyVector &continuousKeys,
       isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)),
       isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)),
       nrContinuous_(continuousKeys.size()),
-      discreteKeys_(discreteKeys) {}
+      discreteKeys_(discreteKeys),
+      continuousKeys_(continuousKeys) {}
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
     : Base(CollectKeys({}, discreteKeys)),
       isDiscrete_(true),
-      discreteKeys_(discreteKeys) {}
+      discreteKeys_(discreteKeys),
+      continuousKeys_({}) {}
 
 /* ************************************************************************ */
 bool HybridFactor::equals(const HybridFactor &lf, double tol) const {
@@ -83,7 +88,17 @@ void HybridFactor::print(const std::string &s,
   if (isContinuous_) std::cout << "Continuous ";
   if (isDiscrete_) std::cout << "Discrete ";
   if (isHybrid_) std::cout << "Hybrid ";
-  this->printKeys("", formatter);
+  for (size_t c=0; c<continuousKeys_.size(); c++) {
+    std::cout << formatter(continuousKeys_.at(c));
+    if (c < continuousKeys_.size() - 1) {
+      std::cout << " ";
+    } else {
+      std::cout << "; ";
+    }
+  }
+  for(auto && discreteKey: discreteKeys_) {
+    std::cout << formatter(discreteKey.first) << " ";
+  }
 }
 
 }  // namespace gtsam

gtsam/hybrid/HybridFactor.h

@@ -52,6 +52,9 @@ class GTSAM_EXPORT HybridFactor : public Factor {
  protected:
   DiscreteKeys discreteKeys_;
 
+  /// Record continuous keys for book-keeping
+  KeyVector continuousKeys_;
+
  public:
   // typedefs needed to play nice with gtsam
   typedef HybridFactor This;  ///< This class

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -41,7 +41,7 @@ bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const {
 void HybridGaussianFactor::print(const std::string &s,
                                  const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
-  inner_->print("inner: ", formatter);
+  inner_->print("\n", formatter);
 };
 
 }  // namespace gtsam

gtsam/hybrid/tests/testGaussianMixture.cpp

@@ -0,0 +1,92 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testGaussianMixture.cpp
+ * @brief   Unit tests for GaussianMixture class
+ * @author  Varun Agrawal
+ * @author  Fan Jiang
+ * @author  Frank Dellaert
+ * @date    December 2021
+ */
+
+#include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianConditional.h>
+
+#include <vector>
+
+// Include for test suite
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using noiseModel::Isotropic;
+using symbol_shorthand::M;
+using symbol_shorthand::X;
+
+/* ************************************************************************* */
+TEST(GaussianConditional, Equals) {
+  // create a conditional gaussian node
+  Matrix S1(2, 2);
+  S1(0, 0) = 1;
+  S1(1, 0) = 2;
+  S1(0, 1) = 3;
+  S1(1, 1) = 4;
+
+  Matrix S2(2, 2);
+  S2(0, 0) = 6;
+  S2(1, 0) = 0.2;
+  S2(0, 1) = 8;
+  S2(1, 1) = 0.4;
+
+  Matrix R1(2, 2);
+  R1(0, 0) = 0.1;
+  R1(1, 0) = 0.3;
+  R1(0, 1) = 0.0;
+  R1(1, 1) = 0.34;
+
+  Matrix R2(2, 2);
+  R2(0, 0) = 0.1;
+  R2(1, 0) = 0.3;
+  R2(0, 1) = 0.0;
+  R2(1, 1) = 0.34;
+
+  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34));
+
+  Vector2 d1(0.2, 0.5), d2(0.5, 0.2);
+
+  auto conditional0 = boost::make_shared<GaussianConditional>(X(1), d1, R1,
+                                                              X(2), S1, model),
+       conditional1 = boost::make_shared<GaussianConditional>(X(1), d2, R2,
+                                                              X(2), S2, model);
+
+  // Create decision tree
+  DiscreteKey m1(1, 2);
+  GaussianMixture::Conditionals conditionals(
+      {m1},
+      vector<GaussianConditional::shared_ptr>{conditional0, conditional1});
+  GaussianMixture mixtureFactor({X(1)}, {X(2)}, {m1}, conditionals);
+
+  // Let's check that this worked:
+  DiscreteValues mode;
+  mode[m1.first] = 1;
+  auto actual = mixtureFactor(mode);
+  EXPECT(actual == conditional1);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */