GaussianMixtureFactor tests

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/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/tests/testGaussianMixtureFactor.cpp

@@ -0,0 +1,159 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    GaussianMixtureFactor.cpp
+ * @brief   Unit tests for GaussianMixtureFactor
+ * @author  Varun Agrawal
+ * @author  Fan Jiang
+ * @author  Frank Dellaert
+ * @date    December 2021
+ */
+
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+
+// 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;
+
+/* ************************************************************************* */
+// Check iterators of empty mixture.
+TEST(GaussianMixtureFactor, Constructor) {
+  GaussianMixtureFactor factor;
+  GaussianMixtureFactor::const_iterator const_it = factor.begin();
+  CHECK(const_it == factor.end());
+  GaussianMixtureFactor::iterator it = factor.begin();
+  CHECK(it == factor.end());
+}
+
+/* ************************************************************************* */
+// "Add" two mixture factors together.
+TEST(GaussianMixtureFactor, Sum) {
+  DiscreteKey m1(1, 2), m2(2, 3);
+
+  auto A1 = Matrix::Zero(2, 1);
+  auto A2 = Matrix::Zero(2, 2);
+  auto A3 = Matrix::Zero(2, 3);
+  auto b = Matrix::Zero(2, 1);
+  Vector2 sigmas;
+  sigmas << 1, 2;
+  auto model = noiseModel::Diagonal::Sigmas(sigmas, true);
+
+  auto f10 = boost::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
+  auto f11 = boost::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
+  auto f20 = boost::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
+  auto f21 = boost::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
+  auto f22 = boost::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
+  std::vector<GaussianFactor::shared_ptr> factorsA{f10, f11};
+  std::vector<GaussianFactor::shared_ptr> factorsB{f20, f21, f22};
+
+  // TODO(Frank): why specify keys at all? And: keys in factor should be *all*
+  // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey?
+  // Design review!
+  GaussianMixtureFactor mixtureFactorA({X(1), X(2)}, {m1}, factorsA);
+  GaussianMixtureFactor mixtureFactorB({X(1), X(3)}, {m2}, factorsB);
+
+  // Check that number of keys is 3
+  EXPECT_LONGS_EQUAL(3, mixtureFactorA.keys().size());
+
+  // Check that number of discrete keys is 1 // TODO(Frank): should not exist?
+  EXPECT_LONGS_EQUAL(1, mixtureFactorA.discreteKeys().size());
+
+  // Create sum of two mixture factors: it will be a decision tree now on both
+  // discrete variables m1 and m2:
+  GaussianMixtureFactor::Sum sum;
+  sum += mixtureFactorA;
+  sum += mixtureFactorB;
+
+  // Let's check that this worked:
+  Assignment<Key> mode;
+  mode[m1.first] = 1;
+  mode[m2.first] = 2;
+  auto actual = sum(mode);
+  EXPECT(actual.at(0) == f11);
+  EXPECT(actual.at(1) == f22);
+}
+
+TEST(GaussianMixtureFactor, Printing) {
+  DiscreteKey m1(1, 2);
+  auto A1 = Matrix::Zero(2, 1);
+  auto A2 = Matrix::Zero(2, 2);
+  auto b = Matrix::Zero(2, 1);
+  auto f10 = boost::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
+  auto f11 = boost::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
+  std::vector<GaussianFactor::shared_ptr> factors{f10, f11};
+
+  GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
+
+  std::string expected =
+      R"(Hybrid x1 x2; 1 ]{
+ Choice(1) 
+ 0 Leaf :
+  A[x1] = [
+	0;
+	0
+]
+  A[x2] = [
+	0, 0;
+	0, 0
+]
+  b = [ 0 0 ]
+  No noise model
+
+ 1 Leaf :
+  A[x1] = [
+	0;
+	0
+]
+  A[x2] = [
+	0, 0;
+	0, 0
+]
+  b = [ 0 0 ]
+  No noise model
+
+}
+)";
+  EXPECT(assert_print_equal(expected, mixtureFactor));
+}
+
+TEST_UNSAFE(GaussianMixtureFactor, GaussianMixture) {
+  KeyVector keys;
+  keys.push_back(X(0));
+  keys.push_back(X(1));
+
+  DiscreteKeys dKeys;
+  dKeys.emplace_back(M(0), 2);
+  dKeys.emplace_back(M(1), 2);
+
+  auto gaussians = boost::make_shared<GaussianConditional>();
+  GaussianMixture::Conditionals conditionals(gaussians);
+  GaussianMixture gm({}, keys, dKeys, conditionals);
+
+  EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size());
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */