Simpler HybridGaussianFoo constructors

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -28,56 +28,47 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
-HybridGaussianFactor::FactorValuePairs GetFactorValuePairs(
+/* *******************************************************************************/
+HybridGaussianConditional::ConstructorHelper::ConstructorHelper(
     const HybridGaussianConditional::Conditionals &conditionals) {
-  auto func = [](const GaussianConditional::shared_ptr &conditional)
-      -> GaussianFactorValuePair {
+  negLogConstant = std::numeric_limits<double>::infinity();
+
+  auto func =
+      [&](const GaussianConditional::shared_ptr &c) -> GaussianFactorValuePair {
     double value = 0.0;
-    // Check if conditional is pruned
-    if (conditional) {
-      // Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|))
-      value = conditional->negLogConstant();
+    if (c) {
+      if (frontals.empty()) {
+        frontals = KeyVector(c->frontals().begin(), c->frontals().end());
+        parents = KeyVector(c->parents().begin(), c->parents().end());
+      }
+      value = c->negLogConstant();
+      negLogConstant = std::min(negLogConstant, value);
     }
-    return {std::dynamic_pointer_cast<GaussianFactor>(conditional), value};
+    return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
   };
-  return HybridGaussianFactor::FactorValuePairs(conditionals, func);
+  pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func);
 }
 
+/* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
-    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKeys &discreteParents,
     const HybridGaussianConditional::Conditionals &conditionals)
-    : BaseFactor(CollectKeys(continuousFrontals, continuousParents),
-                 discreteParents, GetFactorValuePairs(conditionals)),
-      BaseConditional(continuousFrontals.size()),
-      conditionals_(conditionals) {
-  // Calculate negLogConstant_ as the minimum of the negative-log normalizers of
-  // the conditionals, by visiting the decision tree:
-  negLogConstant_ = std::numeric_limits<double>::infinity();
-  conditionals_.visit(
-      [this](const GaussianConditional::shared_ptr &conditional) {
-        if (conditional) {
-          this->negLogConstant_ =
-              std::min(this->negLogConstant_, conditional->negLogConstant());
-        }
-      });
-}
-
-/* *******************************************************************************/
-const HybridGaussianConditional::Conditionals &
-HybridGaussianConditional::conditionals() const {
-  return conditionals_;
-}
+    : HybridGaussianConditional(discreteParents, conditionals,
+                                ConstructorHelper(conditionals)) {}
 
 /* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
-    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKey &discreteParent,
     const std::vector<GaussianConditional::shared_ptr> &conditionals)
-    : HybridGaussianConditional(continuousFrontals, continuousParents,
-                                DiscreteKeys{discreteParent},
+    : HybridGaussianConditional(DiscreteKeys{discreteParent},
                                 Conditionals({discreteParent}, conditionals)) {}
 
+/* *******************************************************************************/
+const HybridGaussianConditional::Conditionals &
+HybridGaussianConditional::conditionals() const {
+  return conditionals_;
+}
+
 /* *******************************************************************************/
 GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree()
     const {
@@ -222,8 +213,8 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
           return {likelihood_m, Cgm_Kgcm};
         }
       });
-  return std::make_shared<HybridGaussianFactor>(
-      continuousParentKeys, discreteParentKeys, likelihoods);
+  return std::make_shared<HybridGaussianFactor>(discreteParentKeys,
+                                                likelihoods);
 }
 
 /* ************************************************************************* */

gtsam/hybrid/HybridGaussianConditional.h

@@ -64,27 +64,11 @@ class GTSAM_EXPORT HybridGaussianConditional
 
  private:
   Conditionals conditionals_;  ///< a decision tree of Gaussian conditionals.
+
   ///< Negative-log of the normalization constant (log(\sqrt(|2πΣ|))).
   ///< Take advantage of the neg-log space so everything is a minimization
   double negLogConstant_;
 
-  /**
-   * @brief Convert a HybridGaussianConditional of conditionals into
-   * a DecisionTree of Gaussian factor graphs.
-   */
-  GaussianFactorGraphTree asGaussianFactorGraphTree() const;
-
-  /**
-   * @brief Helper function to get the pruner functor.
-   *
-   * @param discreteProbs The pruned discrete probabilities.
-   * @return std::function<GaussianConditional::shared_ptr(
-   * const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
-   */
-  std::function<GaussianConditional::shared_ptr(
-      const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
-  prunerFunc(const DecisionTreeFactor &discreteProbs);
-
  public:
   /// @name Constructors
   /// @{
@@ -95,32 +79,25 @@ class GTSAM_EXPORT HybridGaussianConditional
   /**
    * @brief Construct a new HybridGaussianConditional 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. The number of
    * conditionals should be C^(number of discrete parents), where C is the
    * cardinality of the DiscreteKeys in discreteParents, since the
    * discreteParents will be used as the labels in the decision tree.
    */
-  HybridGaussianConditional(const KeyVector &continuousFrontals,
-                            const KeyVector &continuousParents,
-                            const DiscreteKeys &discreteParents,
+  HybridGaussianConditional(const DiscreteKeys &discreteParents,
                             const Conditionals &conditionals);
 
   /**
    * @brief Make a Hybrid Gaussian Conditional from
    * a vector of Gaussian conditionals.
    * The DecisionTree-based constructor is preferred over this one.
    *
-   * @param continuousFrontals The continuous frontal variables
-   * @param continuousParents The continuous parent variables
    * @param discreteParent Single discrete parent variable
    * @param conditionals Vector of conditionals with the same size as the
    * cardinality of the discrete parent.
    */
   HybridGaussianConditional(
-      const KeyVector &continuousFrontals, const KeyVector &continuousParents,
       const DiscreteKey &discreteParent,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
@@ -207,6 +184,33 @@ class GTSAM_EXPORT HybridGaussianConditional
   /// @}
 
  private:
+  /// Helper struct for private constructor.
+  struct ConstructorHelper {
+    KeyVector frontals, parents;
+    HybridGaussianFactor::FactorValuePairs pairs;
+    double negLogConstant;
+    /// Compute all variables needed for the private constructor below.
+    ConstructorHelper(const Conditionals &conditionals);
+  };
+
+  /// Private constructor that uses helper struct above.
+  HybridGaussianConditional(
+      const DiscreteKeys &discreteParents,
+      const HybridGaussianConditional::Conditionals &conditionals,
+      const ConstructorHelper &helper)
+      : BaseFactor(discreteParents, helper.pairs),
+        BaseConditional(helper.frontals.size()),
+        conditionals_(conditionals),
+        negLogConstant_(helper.negLogConstant) {}
+
+  /// Convert to a DecisionTree of Gaussian factor graphs.
+  GaussianFactorGraphTree asGaussianFactorGraphTree() const;
+
+  //// Get the pruner functor from pruned discrete probabilities.
+  std::function<GaussianConditional::shared_ptr(
+      const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
+  prunerFunc(const DecisionTreeFactor &prunedProbabilities);
+
   /// Check whether `given` has values for all frontal keys.
   bool allFrontalsGiven(const VectorValues &given) const;
 

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -26,23 +26,16 @@
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
+#include "gtsam/hybrid/HybridFactor.h"
+
 namespace gtsam {
 
-/**
- * @brief Helper function to augment the [A|b] matrices in the factor components
- * with the additional scalar values.
- * This is done by storing the value in
- * the `b` vector as an additional row.
- *
- * @param factors DecisionTree of GaussianFactors and arbitrary scalars.
- * Gaussian factor in factors.
- * @return HybridGaussianFactor::Factors
- */
-static HybridGaussianFactor::Factors augment(
-    const HybridGaussianFactor::FactorValuePairs &factors) {
+/* *******************************************************************************/
+HybridGaussianFactor::Factors HybridGaussianFactor::augment(
+    const FactorValuePairs &factors) {
   // Find the minimum value so we can "proselytize" to positive values.
   // Done because we can't have sqrt of negative numbers.
-  HybridGaussianFactor::Factors gaussianFactors;
+  Factors gaussianFactors;
   AlgebraicDecisionTree<Key> valueTree;
   std::tie(gaussianFactors, valueTree) = unzip(factors);
 
@@ -73,22 +66,65 @@ static HybridGaussianFactor::Factors augment(
     return std::dynamic_pointer_cast<GaussianFactor>(
         std::make_shared<JacobianFactor>(gfg));
   };
-  return HybridGaussianFactor::Factors(factors, update);
+  return Factors(factors, update);
 }
 
 /* *******************************************************************************/
-HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys,
-                                           const DiscreteKeys &discreteKeys,
-                                           const FactorValuePairs &factors)
-    : Base(continuousKeys, discreteKeys), factors_(augment(factors)) {}
+HybridGaussianFactor::ConstructorHelper::ConstructorHelper(
+    const DiscreteKey &discreteKey,
+    const std::vector<GaussianFactor::shared_ptr> &factors)
+    : discreteKeys({discreteKey}) {
+  // Extract continuous keys from the first non-null factor
+  for (const auto &factor : factors) {
+    if (factor && continuousKeys.empty()) {
+      continuousKeys = factor->keys();
+      break;
+    }
+  }
+
+  // Build the DecisionTree from the factor vector
+  factorsTree = Factors(discreteKeys, factors);
+}
+
+/* *******************************************************************************/
+HybridGaussianFactor::ConstructorHelper::ConstructorHelper(
+    const DiscreteKey &discreteKey,
+    const std::vector<GaussianFactorValuePair> &factorPairs)
+    : discreteKeys({discreteKey}) {
+  // Extract continuous keys from the first non-null factor
+  for (const auto &pair : factorPairs) {
+    if (pair.first && continuousKeys.empty()) {
+      continuousKeys = pair.first->keys();
+      break;
+    }
+  }
+
+  // Build the FactorValuePairs DecisionTree
+  pairs = FactorValuePairs(discreteKeys, factorPairs);
+}
+
+/* *******************************************************************************/
+HybridGaussianFactor::ConstructorHelper::ConstructorHelper(
+    const DiscreteKeys &discreteKeys, const FactorValuePairs &factorPairs)
+    : discreteKeys(discreteKeys) {
+  // Extract continuous keys from the first non-null factor
+  factorPairs.visit([&](const GaussianFactorValuePair &pair) {
+    if (pair.first && continuousKeys.empty()) {
+      continuousKeys = pair.first->keys();
+    }
+  });
+
+  // Build the FactorValuePairs DecisionTree
+  pairs = factorPairs;
+}
 
 /* *******************************************************************************/
 bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
   const This *e = dynamic_cast<const This *>(&lf);
   if (e == nullptr) return false;
 
-  // This will return false if either factors_ is empty or e->factors_ is empty,
-  // but not if both are empty or both are not empty:
+  // This will return false if either factors_ is empty or e->factors_ is
+  // empty, but not if both are empty or both are not empty:
   if (factors_.empty() ^ e->factors_.empty()) return false;
 
   // Check the base and the factors: