Nonlinear Hybrid

gtsam/discrete/discrete.i

@@ -219,6 +219,16 @@ class DiscreteBayesTree {
 };
 
 #include <gtsam/discrete/DiscreteLookupDAG.h>
+
+class DiscreteLookupTable : gtsam::DiscreteConditional{
+  DiscreteLookupTable(size_t nFrontals, const gtsam::DiscreteKeys& keys,
+                      const gtsam::DecisionTreeFactor::ADT& potentials);
+  void print(
+    const std::string& s = "Discrete Lookup Table: ",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+  size_t argmax(const gtsam::DiscreteValues& parentsValues) const;
+};
+
 class DiscreteLookupDAG {
   DiscreteLookupDAG();
   void push_back(const gtsam::DiscreteLookupTable* table);

gtsam/hybrid/GaussianMixtureFactor.cpp

@@ -51,7 +51,7 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactors(
 void GaussianMixtureFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
-  std::cout << "]{\n";
+  std::cout << "{\n";
   factors_.print(
       "", [&](Key k) { return formatter(k); },
       [&](const GaussianFactor::shared_ptr &gf) -> std::string {

gtsam/hybrid/GaussianMixtureFactor.h

@@ -22,7 +22,7 @@
 
 #include <gtsam/discrete/DecisionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
-#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/linear/GaussianFactor.h>
 
 namespace gtsam {

gtsam/hybrid/HybridBayesTree.h

@@ -76,7 +76,10 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
 /**
  * @brief Class for Hybrid Bayes tree orphan subtrees.
  *
- * This does special stuff for the hybrid case
+ * This object stores parent keys in our base type factor so that
+ * eliminating those parent keys will pull this subtree into the
+ * elimination.
+ * This does special stuff for the hybrid case.
  *
  * @tparam CLIQUE
  */

gtsam/hybrid/HybridFactor.cpp

@@ -89,17 +89,23 @@ void HybridFactor::print(const std::string &s,
   if (isContinuous_) std::cout << "Continuous ";
   if (isDiscrete_) std::cout << "Discrete ";
   if (isHybrid_) std::cout << "Hybrid ";
-  for (size_t c=0; c<continuousKeys_.size(); c++) {
+  std::cout << "[";
+  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 << "; ";
+      std::cout << (discreteKeys_.size() > 0 ? "; " : "");
     }
   }
-  for(auto && discreteKey: discreteKeys_) {
-    std::cout << formatter(discreteKey.first) << " ";
+  for (size_t d = 0; d < discreteKeys_.size(); d++) {
+    std::cout << formatter(discreteKeys_.at(d).first);
+    if (d < discreteKeys_.size() - 1) {
+      std::cout << " ";
+    }
+
   }
+  std::cout << "]";
 }
 
 }  // namespace gtsam

gtsam/hybrid/HybridGaussianFactor.h

@@ -37,6 +37,8 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   using This = HybridGaussianFactor;
   using shared_ptr = boost::shared_ptr<This>;
 
+  HybridGaussianFactor() = default;
+
   // Explicit conversion from a shared ptr of GF
   explicit HybridGaussianFactor(GaussianFactor::shared_ptr other);
 
@@ -52,7 +54,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
 
   /// GTSAM print utility.
   void print(
-      const std::string &s = "HybridFactor\n",
+      const std::string &s = "HybridGaussianFactor\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 
   /// @}

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -98,6 +98,12 @@ GaussianMixtureFactor::Sum sumFrontals(
     } else if (f->isContinuous()) {
       deferredFactors.push_back(
           boost::dynamic_pointer_cast<HybridGaussianFactor>(f)->inner());
+
+    } else if (f->isDiscrete()) {
+      // Don't do anything for discrete-only factors
+      // since we want to eliminate continuous values only.
+      continue;
+
     } else {
       // We need to handle the case where the object is actually an
       // BayesTreeOrphanWrapper!
@@ -106,8 +112,8 @@ GaussianMixtureFactor::Sum sumFrontals(
       if (!orphan) {
         auto &fr = *f;
         throw std::invalid_argument(
-            std::string("factor is discrete in continuous elimination") +
-            typeid(fr).name());
+            std::string("factor is discrete in continuous elimination ") +
+            demangle(typeid(fr).name()));
       }
     }
   }
@@ -158,7 +164,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
     }
   }
 
-  auto result = EliminateDiscrete(dfg, frontalKeys);
+  auto result = EliminateForMPE(dfg, frontalKeys);
 
   return {boost::make_shared<HybridConditional>(result.first),
           boost::make_shared<HybridDiscreteFactor>(result.second)};

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -24,6 +24,7 @@
 #include <gtsam/inference/EliminateableFactorGraph.h>
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/Ordering.h>
+#include <gtsam/linear/GaussianFactor.h>
 
 namespace gtsam {
 

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -0,0 +1,41 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridNonlinearFactor.cpp
+ *  @date May 28, 2022
+ *  @author Varun Agrawal
+ */
+
+#include <gtsam/hybrid/HybridNonlinearFactor.h>
+
+#include <boost/make_shared.hpp>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+HybridNonlinearFactor::HybridNonlinearFactor(
+    const NonlinearFactor::shared_ptr &other)
+    : Base(other->keys()), inner_(other) {}
+
+/* ************************************************************************* */
+bool HybridNonlinearFactor::equals(const HybridFactor &lf, double tol) const {
+  return Base::equals(lf, tol);
+}
+
+/* ************************************************************************* */
+void HybridNonlinearFactor::print(const std::string &s,
+                                  const KeyFormatter &formatter) const {
+  HybridFactor::print(s, formatter);
+  inner_->print("\n", formatter);
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridNonlinearFactor.h

@@ -0,0 +1,62 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridNonlinearFactor.h
+ *  @date May 28, 2022
+ *  @author Varun Agrawal
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * A HybridNonlinearFactor is a layer over NonlinearFactor so that we do not
+ * have a diamond inheritance.
+ */
+class HybridNonlinearFactor : public HybridFactor {
+ private:
+  NonlinearFactor::shared_ptr inner_;
+
+ public:
+  using Base = HybridFactor;
+  using This = HybridNonlinearFactor;
+  using shared_ptr = boost::shared_ptr<This>;
+
+  // Explicit conversion from a shared ptr of NonlinearFactor
+  explicit HybridNonlinearFactor(const NonlinearFactor::shared_ptr &other);
+
+  /// @name Testable
+  /// @{
+
+  /// Check equality.
+  virtual bool equals(const HybridFactor &lf, double tol) const override;
+
+  /// GTSAM print utility.
+  void print(
+      const std::string &s = "HybridNonlinearFactor\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+
+  /// @}
+
+  NonlinearFactor::shared_ptr inner() const { return inner_; }
+
+  /// Linearize to a HybridGaussianFactor at the linearization point `c`.
+  boost::shared_ptr<HybridGaussianFactor> linearize(const Values &c) const {
+    return boost::make_shared<HybridGaussianFactor>(inner_->linearize(c));
+  }
+};
+}  // namespace gtsam

gtsam/hybrid/HybridNonlinearFactorGraph.cpp

@@ -0,0 +1,94 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   HybridNonlinearFactorGraph.cpp
+ * @brief  Nonlinear hybrid factor graph that uses type erasure
+ * @author Varun Agrawal
+ * @date   May 28, 2022
+ */
+
+#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+void HybridNonlinearFactorGraph::add(
+    boost::shared_ptr<NonlinearFactor> factor) {
+  FactorGraph::add(boost::make_shared<HybridNonlinearFactor>(factor));
+}
+
+/* ************************************************************************* */
+void HybridNonlinearFactorGraph::print(const std::string& s,
+                                       const KeyFormatter& keyFormatter) const {
+  // Base::print(str, keyFormatter);
+  std::cout << (s.empty() ? "" : s + " ") << std::endl;
+  std::cout << "size: " << size() << std::endl;
+  for (size_t i = 0; i < factors_.size(); i++) {
+    std::stringstream ss;
+    ss << "factor " << i << ": ";
+    if (factors_[i]) {
+      factors_[i]->print(ss.str(), keyFormatter);
+      std::cout << std::endl;
+    }
+  }
+}
+
+/* ************************************************************************* */
+HybridGaussianFactorGraph HybridNonlinearFactorGraph::linearize(
+    const Values& continuousValues) const {
+  // create an empty linear FG
+  HybridGaussianFactorGraph linearFG;
+
+  linearFG.reserve(size());
+
+  // linearize all hybrid factors
+  for (auto&& factor : factors_) {
+    // First check if it is a valid factor
+    if (factor) {
+      // Check if the factor is a hybrid factor.
+      // It can be either a nonlinear MixtureFactor or a linear
+      // GaussianMixtureFactor.
+      if (factor->isHybrid()) {
+        // Check if it is a nonlinear mixture factor
+        if (auto nlmf = boost::dynamic_pointer_cast<MixtureFactor>(factor)) {
+          linearFG.push_back(nlmf->linearize(continuousValues));
+        } else {
+          linearFG.push_back(factor);
+        }
+
+        // Now check if the factor is a continuous only factor.
+      } else if (factor->isContinuous()) {
+        // In this case, we check if factor->inner() is nonlinear since
+        // HybridFactors wrap over continuous factors.
+        auto nlhf = boost::dynamic_pointer_cast<HybridNonlinearFactor>(factor);
+        if (auto nlf =
+                boost::dynamic_pointer_cast<NonlinearFactor>(nlhf->inner())) {
+          auto hgf = boost::make_shared<HybridGaussianFactor>(
+              nlf->linearize(continuousValues));
+          linearFG.push_back(hgf);
+        } else {
+          linearFG.push_back(factor);
+        }
+        // Finally if nothing else, we are discrete-only which doesn't need
+        // lineariztion.
+      } else {
+        linearFG.push_back(factor);
+      }
+
+    } else {
+      linearFG.push_back(GaussianFactor::shared_ptr());
+    }
+  }
+  return linearFG;
+}
+
+}  // namespace gtsam