Hybrid Factor Graph Implementation

gtsam/CMakeLists.txt

@@ -10,6 +10,7 @@ set (gtsam_subdirs
     inference
     symbolic
     discrete
+    hybrid
     linear
     nonlinear
     sam

gtsam/hybrid/CMakeLists.txt

@@ -0,0 +1,8 @@
+# Install headers
+set(subdir hybrid)
+file(GLOB hybrid_headers "*.h")
+# FIXME: exclude headers
+install(FILES ${hybrid_headers} DESTINATION include/gtsam/hybrid)
+
+# Add all tests
+add_subdirectory(tests)

gtsam/hybrid/GaussianMixtureConditional.cpp

@@ -0,0 +1,106 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   GaussianMixtureConditional.cpp
+ * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
+ * @author Fan Jiang
+ * @author Varun Agrawal
+ * @author Frank Dellaert
+ * @date   Mar 12, 2022
+ */
+
+#include <gtsam/base/utilities.h>
+#include <gtsam/discrete/DecisionTree-inl.h>
+#include <gtsam/hybrid/GaussianMixtureConditional.h>
+#include <gtsam/inference/Conditional-inst.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+
+namespace gtsam {
+
+GaussianMixtureConditional::GaussianMixtureConditional(
+    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
+    const DiscreteKeys &discreteParents,
+    const GaussianMixtureConditional::Conditionals &conditionals)
+    : BaseFactor(CollectKeys(continuousFrontals, continuousParents),
+                 discreteParents),
+      BaseConditional(continuousFrontals.size()),
+      conditionals_(conditionals) {}
+
+/* *******************************************************************************/
+const GaussianMixtureConditional::Conditionals &GaussianMixtureConditional::conditionals() {
+  return conditionals_;
+}
+
+/* *******************************************************************************/
+GaussianMixtureConditional GaussianMixtureConditional::FromConditionalList(
+    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
+    const DiscreteKeys &discreteParents,
+    const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
+  Conditionals dt(discreteParents, conditionalsList);
+
+  return GaussianMixtureConditional(continuousFrontals, continuousParents, discreteParents,
+                         dt);
+}
+
+/* *******************************************************************************/
+GaussianMixtureConditional::Sum GaussianMixtureConditional::add(
+    const GaussianMixtureConditional::Sum &sum) const {
+  using Y = GaussianFactorGraph;
+  auto add = [](const Y &graph1, const Y &graph2) {
+    auto result = graph1;
+    result.push_back(graph2);
+    return result;
+  };
+  const Sum wrapped = asGraph();
+  return sum.empty() ? wrapped : sum.apply(wrapped, add);
+}
+
+/* *******************************************************************************/
+GaussianMixtureConditional::Sum GaussianMixtureConditional::asGraph() const {
+  auto lambda = [](const GaussianFactor::shared_ptr &factor) {
+    GaussianFactorGraph result;
+    result.push_back(factor);
+    return result;
+  };
+  return {conditionals_, lambda};
+}
+
+/* TODO(fan): this (for Testable) is not implemented! */
+bool GaussianMixtureConditional::equals(const HybridFactor &lf, double tol) const {
+  return false;
+}
+
+/* *******************************************************************************/
+void GaussianMixtureConditional::print(const std::string &s,
+                            const KeyFormatter &formatter) const {
+  std::cout << s << ": ";
+  if (isContinuous_) std::cout << "Cont. ";
+  if (isDiscrete_) std::cout << "Disc. ";
+  if (isHybrid_) std::cout << "Hybr. ";
+  BaseConditional::print("", formatter);
+  std::cout << "Discrete Keys = ";
+  for (auto &dk : discreteKeys_) {
+    std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
+  }
+  std::cout << "\n";
+  conditionals_.print(
+      "", [&](Key k) { return formatter(k); },
+      [&](const GaussianConditional::shared_ptr &gf) -> std::string {
+        RedirectCout rd;
+        if (!gf->empty())
+          gf->print("", formatter);
+        else
+          return {"nullptr"};
+        return rd.str();
+      });
+}
+}  // namespace gtsam

gtsam/hybrid/GaussianMixtureConditional.h

@@ -0,0 +1,89 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   GaussianMixtureConditional.h
+ * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
+ * @author Fan Jiang
+ * @author Varun Agrawal
+ * @date   Mar 12, 2022
+ */
+
+#pragma once
+
+#include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/inference/Conditional.h>
+#include <gtsam/linear/GaussianConditional.h>
+
+namespace gtsam {
+class GaussianMixtureConditional : public HybridFactor,
+                        public Conditional<HybridFactor, GaussianMixtureConditional> {
+ public:
+  using This = GaussianMixtureConditional;
+  using shared_ptr = boost::shared_ptr<GaussianMixtureConditional>;
+  using BaseFactor = HybridFactor;
+  using BaseConditional = Conditional<HybridFactor, GaussianMixtureConditional>;
+
+  using Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr>;
+
+  Conditionals conditionals_;
+
+ public:
+  /**
+   * @brief Construct a new Gaussian Mixture object
+   *
+   * @param continuousFrontals the continuous frontals.
+   * @param continuousParents the continuous parents.
+   * @param discreteParents the discrete parents. Will be placed last.
+   * @param conditionals a decision tree of GaussianConditionals.
+   */
+  GaussianMixtureConditional(const KeyVector &continuousFrontals,
+                  const KeyVector &continuousParents,
+                  const DiscreteKeys &discreteParents,
+                  const Conditionals &conditionals);
+
+  using Sum = DecisionTree<Key, GaussianFactorGraph>;
+
+  const Conditionals &conditionals();
+
+  /**
+   * @brief Combine Decision Trees
+   */
+  Sum add(const Sum &sum) const;
+
+	/**
+	 * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs.
+	 */
+  Sum asGraph() const;
+
+	/**
+	 * @brief Make a Gaussian Mixture from a list of Gaussian conditionals
+	 * 
+	 * @param continuousFrontals The continuous frontal variables
+	 * @param continuousParents The continuous parent variables
+	 * @param discreteParents Discrete parents variables
+	 * @param conditionals List of conditionals
+	 */
+  static This FromConditionalList(
+      const KeyVector &continuousFrontals, const KeyVector &continuousParents,
+      const DiscreteKeys &discreteParents,
+      const std::vector<GaussianConditional::shared_ptr> &conditionals);
+
+	/* TODO: this is only a stub */
+  bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
+
+	/* print utility */
+  void print(
+      const std::string &s = "GaussianMixtureConditional\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+};
+}  // namespace gtsam

gtsam/hybrid/GaussianMixtureFactor.cpp

@@ -0,0 +1,90 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  A set of Gaussian factors indexed by a set of discrete keys.
+ * @author Fan Jiang
+ * @author Varun Agrawal
+ * @author Frank Dellaert
+ * @date   Mar 12, 2022
+ */
+
+#include <gtsam/base/utilities.h>
+#include <gtsam/discrete/DecisionTree-inl.h>
+#include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+
+namespace gtsam {
+
+GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
+                                             const DiscreteKeys &discreteKeys,
+                                             const Factors &factors)
+    : Base(continuousKeys, discreteKeys), factors_(factors) {}
+bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
+  return false;
+}
+
+/* *******************************************************************************/
+GaussianMixtureFactor GaussianMixtureFactor::FromFactorList(
+    const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
+    const std::vector<GaussianFactor::shared_ptr> &factorsList) {
+  Factors dt(discreteKeys, factorsList);
+
+  return GaussianMixtureFactor(continuousKeys, discreteKeys, dt);
+}
+
+
+/* *******************************************************************************/
+void GaussianMixtureFactor::print(const std::string &s,
+                                  const KeyFormatter &formatter) const {
+  HybridFactor::print(s, formatter);
+  factors_.print(
+      "mixture = ", [&](Key k) { return formatter(k); },
+      [&](const GaussianFactor::shared_ptr &gf) -> std::string {
+        RedirectCout rd;
+        if (!gf->empty())
+          gf->print("", formatter);
+        else
+          return {"nullptr"};
+        return rd.str();
+      });
+}
+
+/* *******************************************************************************/
+const GaussianMixtureFactor::Factors &GaussianMixtureFactor::factors() {
+  return factors_;
+}
+
+/* *******************************************************************************/
+GaussianMixtureFactor::Sum GaussianMixtureFactor::add(
+    const GaussianMixtureFactor::Sum &sum) const {
+  using Y = GaussianFactorGraph;
+  auto add = [](const Y &graph1, const Y &graph2) {
+    auto result = graph1;
+    result.push_back(graph2);
+    return result;
+  };
+  const Sum wrapped = wrappedFactors();
+  return sum.empty() ? wrapped : sum.apply(wrapped, add);
+}
+
+/* *******************************************************************************/
+GaussianMixtureFactor::Sum GaussianMixtureFactor::wrappedFactors() const {
+  auto wrap = [](const GaussianFactor::shared_ptr &factor) {
+    GaussianFactorGraph result;
+    result.push_back(factor);
+    return result;
+  };
+  return {factors_, wrap};
+}
+}  // namespace gtsam

gtsam/hybrid/GaussianMixtureFactor.h

@@ -0,0 +1,69 @@
+/* ----------------------------------------------------------------------------
+
+ * 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.h
+ * @brief  A set of Gaussian factors indexed by a set of discrete keys.
+ * @author Fan Jiang
+ * @author Varun Agrawal
+ * @author Frank Dellaert
+ * @date   Mar 12, 2022
+ */
+
+#pragma once
+
+#include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/linear/GaussianFactor.h>
+
+namespace gtsam {
+
+class GaussianFactorGraph;
+
+typedef std::vector<gtsam::GaussianFactor::shared_ptr> GaussianFactorVector;
+
+class GaussianMixtureFactor : public HybridFactor {
+ public:
+  using Base = HybridFactor;
+  using This = GaussianMixtureFactor;
+  using shared_ptr = boost::shared_ptr<This>;
+
+  using Factors = DecisionTree<Key, GaussianFactor::shared_ptr>;
+
+  Factors factors_;
+
+  GaussianMixtureFactor() = default;
+
+  GaussianMixtureFactor(const KeyVector &continuousKeys,
+                        const DiscreteKeys &discreteKeys,
+                        const Factors &factors);
+
+  using Sum = DecisionTree<Key, GaussianFactorGraph>;
+
+  const Factors &factors();
+
+  static This FromFactorList(
+      const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
+      const std::vector<GaussianFactor::shared_ptr> &factors);
+
+  Sum add(const Sum &sum) const;
+
+  Sum wrappedFactors() const;
+
+  bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
+
+  void print(
+      const std::string &s = "HybridFactor\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridBayesNet.cpp

@@ -0,0 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * 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   HybridBayesNet.cpp
+ * @brief  A bayes net of Gaussian Conditionals indexed by discrete keys.
+ * @author Fan Jiang
+ * @date   January 2022
+ */
+
+#include <gtsam/hybrid/HybridBayesNet.h>