Refactored hybrid inference

.clang-format

@@ -1,8 +1 @@
 BasedOnStyle: Google
-
-BinPackArguments: false
-BinPackParameters: false
-ColumnLimit: 100
-DerivePointerAlignment: false
-IncludeBlocks: Preserve
-PointerAlignment: Left

gtsam/hybrid/HybridBayesTree.cpp

@@ -22,14 +22,13 @@
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/inference/BayesTree-inst.h>
 #include <gtsam/inference/BayesTreeCliqueBase-inst.h>
 #include <gtsam/linear/GaussianJunctionTree.h>
 
 #include <memory>
 
-#include "gtsam/hybrid/HybridConditional.h"
-
 namespace gtsam {
 
 // Instantiate base class

gtsam/hybrid/HybridConditional.cpp

@@ -13,6 +13,7 @@
  *  @file HybridConditional.cpp
  *  @date Mar 11, 2022
  *  @author Fan Jiang
+ *  @author Varun Agrawal
  */
 
 #include <gtsam/hybrid/HybridConditional.h>

gtsam/hybrid/HybridConditional.h

@@ -13,6 +13,7 @@
  *  @file HybridConditional.h
  *  @date Mar 11, 2022
  *  @author Fan Jiang
+ *  @author Varun Agrawal
  */
 
 #pragma once

gtsam/hybrid/HybridFactor.h

@@ -32,9 +32,6 @@ namespace gtsam {
 
 class HybridValues;
 
-/// Alias for DecisionTree of GaussianFactorGraphs
-using GaussianFactorGraphTree = DecisionTree<Key, GaussianFactorGraph>;
-
 KeyVector CollectKeys(const KeyVector &continuousKeys,
                       const DiscreteKeys &discreteKeys);
 KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2);

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -32,6 +32,16 @@
 
 namespace gtsam {
 /* *******************************************************************************/
+/**
+ * @brief Helper struct for constructing HybridGaussianConditional objects
+ *
+ * This struct contains the following fields:
+ * - nrFrontals: Optional size_t for number of frontal variables
+ * - pairs: FactorValuePairs for storing conditionals with their negLogConstant
+ * - conditionals: Conditionals for storing conditionals. TODO(frank): kill!
+ * - minNegLogConstant: minimum negLogConstant, computed here, subtracted in
+ * constructor
+ */
 struct HybridGaussianConditional::Helper {
   std::optional<size_t> nrFrontals;
   FactorValuePairs pairs;
@@ -68,16 +78,12 @@ struct HybridGaussianConditional::Helper {
   explicit Helper(const Conditionals &conditionals)
       : conditionals(conditionals),
         minNegLogConstant(std::numeric_limits<double>::infinity()) {
-    auto func = [this](const GC::shared_ptr &c) -> GaussianFactorValuePair {
-      double value = 0.0;
-      if (c) {
-        if (!nrFrontals.has_value()) {
-          nrFrontals = c->nrFrontals();
-        }
-        value = c->negLogConstant();
-        minNegLogConstant = std::min(minNegLogConstant, value);
-      }
-      return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
+    auto func = [this](const GC::shared_ptr &gc) -> GaussianFactorValuePair {
+      if (!gc) return {nullptr, std::numeric_limits<double>::infinity()};
+      if (!nrFrontals) nrFrontals = gc->nrFrontals();
+      double value = gc->negLogConstant();
+      minNegLogConstant = std::min(minNegLogConstant, value);
+      return {gc, value};
     };
     pairs = FactorValuePairs(conditionals, func);
     if (!nrFrontals.has_value()) {
@@ -91,7 +97,14 @@ struct HybridGaussianConditional::Helper {
 /* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKeys &discreteParents, const Helper &helper)
-    : BaseFactor(discreteParents, helper.pairs),
+    : BaseFactor(discreteParents,
+                 FactorValuePairs(helper.pairs,
+                                  [&](const GaussianFactorValuePair &
+                                          pair) {  // subtract minNegLogConstant
+                                    return GaussianFactorValuePair{
+                                        pair.first,
+                                        pair.second - helper.minNegLogConstant};
+                                  })),
       BaseConditional(*helper.nrFrontals),
       conditionals_(helper.conditionals),
       negLogConstant_(helper.minNegLogConstant) {}
@@ -135,29 +148,6 @@ HybridGaussianConditional::conditionals() const {
   return conditionals_;
 }
 
-/* *******************************************************************************/
-GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree()
-    const {
-  auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
-    // First check if conditional has not been pruned
-    if (gc) {
-      const double Cgm_Kgcm = gc->negLogConstant() - this->negLogConstant_;
-      // If there is a difference in the covariances, we need to account for
-      // that since the error is dependent on the mode.
-      if (Cgm_Kgcm > 0.0) {
-        // We add a constant factor which will be used when computing
-        // the probability of the discrete variables.
-        Vector c(1);
-        c << std::sqrt(2.0 * Cgm_Kgcm);
-        auto constantFactor = std::make_shared<JacobianFactor>(c);
-        return GaussianFactorGraph{gc, constantFactor};
-      }
-    }
-    return GaussianFactorGraph{gc};
-  };
-  return {conditionals_, wrap};
-}
-
 /* *******************************************************************************/
 size_t HybridGaussianConditional::nrComponents() const {
   size_t total = 0;
@@ -192,19 +182,18 @@ bool HybridGaussianConditional::equals(const HybridFactor &lf,
 
   // Check the base and the factors:
   return BaseFactor::equals(*e, tol) &&
-         conditionals_.equals(
-             e->conditionals_, [tol](const auto &f1, const auto &f2) {
-               return (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
-             });
+         conditionals_.equals(e->conditionals_,
+                              [tol](const GaussianConditional::shared_ptr &f1,
+                                    const GaussianConditional::shared_ptr &f2) {
+                                return (!f1 && !f2) ||
+                                       (f1 && f2 && f1->equals(*f2, tol));
+                              });
 }
 
 /* *******************************************************************************/
 void HybridGaussianConditional::print(const std::string &s,
                                       const KeyFormatter &formatter) const {
   std::cout << (s.empty() ? "" : s + "\n");
-  if (isContinuous()) std::cout << "Continuous ";
-  if (isDiscrete()) std::cout << "Discrete ";
-  if (isHybrid()) std::cout << "Hybrid ";
   BaseConditional::print("", formatter);
   std::cout << " Discrete Keys = ";
   for (auto &dk : discreteKeys()) {
@@ -270,13 +259,7 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
           -> GaussianFactorValuePair {
         const auto likelihood_m = conditional->likelihood(given);
         const double Cgm_Kgcm = conditional->negLogConstant() - negLogConstant_;
-        if (Cgm_Kgcm == 0.0) {
-          return {likelihood_m, 0.0};
-        } else {
-          // Add a constant to the likelihood in case the noise models
-          // are not all equal.
-          return {likelihood_m, Cgm_Kgcm};
-        }
+        return {likelihood_m, Cgm_Kgcm};
       });
   return std::make_shared<HybridGaussianFactor>(discreteParentKeys,
                                                 likelihoods);

gtsam/hybrid/HybridGaussianConditional.h

@@ -231,9 +231,6 @@ class GTSAM_EXPORT HybridGaussianConditional
   HybridGaussianConditional(const DiscreteKeys &discreteParents,
                             const Helper &helper);
 
-  /// Convert to a DecisionTree of Gaussian factor graphs.
-  GaussianFactorGraphTree asGaussianFactorGraphTree() const;
-
   /// Check whether `given` has values for all frontal keys.
   bool allFrontalsGiven(const VectorValues &given) const;