HybridNonlinearFactor Relinearization

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -55,23 +55,14 @@ HybridGaussianConditional::conditionals() const {
   return conditionals_;
 }
 
-/* *******************************************************************************/
-HybridGaussianConditional::HybridGaussianConditional(
-    KeyVector &&continuousFrontals, KeyVector &&continuousParents,
-    DiscreteKeys &&discreteParents,
-    std::vector<GaussianConditional::shared_ptr> &&conditionals)
-    : HybridGaussianConditional(continuousFrontals, continuousParents,
-                                discreteParents,
-                                Conditionals(discreteParents, conditionals)) {}
-
 /* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
     const KeyVector &continuousFrontals, const KeyVector &continuousParents,
-    const DiscreteKeys &discreteParents,
+    const DiscreteKey &discreteParent,
     const std::vector<GaussianConditional::shared_ptr> &conditionals)
     : HybridGaussianConditional(continuousFrontals, continuousParents,
-                                discreteParents,
-                                Conditionals(discreteParents, conditionals)) {}
+                                DiscreteKeys{discreteParent},
+                                Conditionals({discreteParent}, conditionals)) {}
 
 /* *******************************************************************************/
 // TODO(dellaert): This is copy/paste: HybridGaussianConditional should be
@@ -219,23 +210,20 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
 
   const DiscreteKeys discreteParentKeys = discreteKeys();
   const KeyVector continuousParentKeys = continuousParents();
-  const HybridGaussianFactor::Factors likelihoods(
-      conditionals_, [&](const GaussianConditional::shared_ptr &conditional) {
+  const HybridGaussianFactor::FactorValuePairs likelihoods(
+      conditionals_,
+      [&](const GaussianConditional::shared_ptr &conditional)
+          -> GaussianFactorValuePair {
         const auto likelihood_m = conditional->likelihood(given);
         const double Cgm_Kgcm =
             logConstant_ - conditional->logNormalizationConstant();
         if (Cgm_Kgcm == 0.0) {
-          return likelihood_m;
+          return {likelihood_m, 0.0};
         } else {
-          // Add a constant factor to the likelihood in case the noise models
+          // Add a constant to the likelihood in case the noise models
           // are not all equal.
-          GaussianFactorGraph gfg;
-          gfg.push_back(likelihood_m);
-          Vector c(1);
-          c << std::sqrt(2.0 * Cgm_Kgcm);
-          auto constantFactor = std::make_shared<JacobianFactor>(c);
-          gfg.push_back(constantFactor);
-          return std::make_shared<JacobianFactor>(gfg);
+          double c = 2.0 * Cgm_Kgcm;
+          return {likelihood_m, c};
         }
       });
   return std::make_shared<HybridGaussianFactor>(

gtsam/hybrid/HybridGaussianConditional.h

@@ -107,29 +107,18 @@ class GTSAM_EXPORT HybridGaussianConditional
                             const Conditionals &conditionals);
 
   /**
-   * @brief Make a Gaussian Mixture from a list of Gaussian conditionals
+   * @brief Make a Gaussian Mixture 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 discreteParents Discrete parents variables
-   * @param conditionals List of conditionals
-   */
-  HybridGaussianConditional(
-      KeyVector &&continuousFrontals, KeyVector &&continuousParents,
-      DiscreteKeys &&discreteParents,
-      std::vector<GaussianConditional::shared_ptr> &&conditionals);
-
-  /**
-   * @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
+   * @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 DiscreteKeys &discreteParents,
+      const DiscreteKey &discreteParent,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
   /// @}

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -28,11 +28,56 @@
 
 namespace gtsam {
 
+/**
+ * @brief Helper function to augment the [A|b] matrices in the factor components
+ * with the normalizer values.
+ * This is done by storing the normalizer value in
+ * the `b` vector as an additional row.
+ *
+ * @param factors DecisionTree of GaussianFactors and arbitrary scalars.
+ * Gaussian factor in factors.
+ * @return HybridGaussianFactor::Factors
+ */
+HybridGaussianFactor::Factors augment(
+    const HybridGaussianFactor::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;
+  AlgebraicDecisionTree<Key> valueTree;
+  std::tie(gaussianFactors, valueTree) = unzip(factors);
+
+  // Normalize
+  double min_value = valueTree.min();
+  AlgebraicDecisionTree<Key> values =
+      valueTree.apply([&min_value](double n) { return n - min_value; });
+
+  // Finally, update the [A|b] matrices.
+  auto update = [&values](const Assignment<Key> &assignment,
+                          const HybridGaussianFactor::sharedFactor &gf) {
+    auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
+    if (!jf) return gf;
+    // If the log_normalizer is 0, do nothing
+    if (values(assignment) == 0.0) return gf;
+
+    GaussianFactorGraph gfg;
+    gfg.push_back(jf);
+
+    Vector c(1);
+    c << std::sqrt(values(assignment));
+    auto constantFactor = std::make_shared<JacobianFactor>(c);
+
+    gfg.push_back(constantFactor);
+    return std::dynamic_pointer_cast<GaussianFactor>(
+        std::make_shared<JacobianFactor>(gfg));
+  };
+  return gaussianFactors.apply(update);
+}
+
 /* *******************************************************************************/
 HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys,
                                            const DiscreteKeys &discreteKeys,
-                                           const Factors &factors)
-    : Base(continuousKeys, discreteKeys), factors_(factors) {}
+                                           const FactorValuePairs &factors)
+    : Base(continuousKeys, discreteKeys), factors_(augment(factors)) {}
 
 /* *******************************************************************************/
 bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {

gtsam/hybrid/HybridGaussianFactor.h

@@ -33,6 +33,9 @@ class HybridValues;
 class DiscreteValues;
 class VectorValues;
 
+/// Alias for pair of GaussianFactor::shared_pointer and a double value.
+using GaussianFactorValuePair = std::pair<GaussianFactor::shared_ptr, double>;
+
 /**
  * @brief Implementation of a discrete conditional mixture factor.
  * Implements a joint discrete-continuous factor where the discrete variable
@@ -52,7 +55,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
 
   using sharedFactor = std::shared_ptr<GaussianFactor>;
 
-  /// typedef for Decision Tree of Gaussian factors and log-constant.
+  /// typedef for Decision Tree of Gaussian factors and arbitrary value.
+  using FactorValuePairs = DecisionTree<Key, GaussianFactorValuePair>;
+  /// typedef for Decision Tree of Gaussian factors.
   using Factors = DecisionTree<Key, sharedFactor>;
 
  private:
@@ -80,26 +85,26 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @param continuousKeys A vector of keys representing continuous variables.
    * @param discreteKeys A vector of keys representing discrete variables and
    * their cardinalities.
-   * @param factors The decision tree of Gaussian factors stored
-   * as the mixture density.
+   * @param factors The decision tree of Gaussian factors and arbitrary scalars.
    */
   HybridGaussianFactor(const KeyVector &continuousKeys,
                        const DiscreteKeys &discreteKeys,
-                       const Factors &factors);
+                       const FactorValuePairs &factors);
 
   /**
    * @brief Construct a new HybridGaussianFactor object using a vector of
    * GaussianFactor shared pointers.
    *
    * @param continuousKeys Vector of keys for continuous factors.
-   * @param discreteKeys Vector of discrete keys.
-   * @param factors Vector of gaussian factor shared pointers.
+   * @param discreteKey The discrete key to index each component.
+   * @param factors Vector of gaussian factor shared pointers
+   *  and arbitrary scalars. Same size as the cardinality of discreteKey.
    */
   HybridGaussianFactor(const KeyVector &continuousKeys,
-                       const DiscreteKeys &discreteKeys,
-                       const std::vector<sharedFactor> &factors)
-      : HybridGaussianFactor(continuousKeys, discreteKeys,
-                             Factors(discreteKeys, factors)) {}
+                       const DiscreteKey &discreteKey,
+                       const std::vector<GaussianFactorValuePair> &factors)
+      : HybridGaussianFactor(continuousKeys, {discreteKey},
+                             FactorValuePairs({discreteKey}, factors)) {}
 
   /// @}
   /// @name Testable

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -92,13 +92,13 @@ void HybridGaussianFactorGraph::printErrors(
     // Clear the stringstream
     ss.str(std::string());
 
-    if (auto gmf = std::dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
+    if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
       if (factor == nullptr) {
         std::cout << "nullptr"
                   << "\n";
       } else {
-        gmf->operator()(values.discrete())->print(ss.str(), keyFormatter);
-        std::cout << "error = " << gmf->error(values) << std::endl;
+        hgf->operator()(values.discrete())->print(ss.str(), keyFormatter);
+        std::cout << "error = " << factor->error(values) << std::endl;
       }
     } else if (auto hc = std::dynamic_pointer_cast<HybridConditional>(factor)) {
       if (factor == nullptr) {
@@ -348,7 +348,7 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
     const KeyVector &continuousSeparator,
     const DiscreteKeys &discreteSeparator) {
   // Correct for the normalization constant used up by the conditional
-  auto correct = [&](const Result &pair) -> GaussianFactor::shared_ptr {
+  auto correct = [&](const Result &pair) -> GaussianFactorValuePair {
     const auto &[conditional, factor] = pair;
     if (factor) {
       auto hf = std::dynamic_pointer_cast<HessianFactor>(factor);
@@ -357,10 +357,10 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
       // as per the Hessian definition
       hf->constantTerm() += 2.0 * conditional->logNormalizationConstant();
     }
-    return factor;
+    return {factor, 0.0};
   };
-  DecisionTree<Key, GaussianFactor::shared_ptr> newFactors(eliminationResults,
-                                                           correct);
+  DecisionTree<Key, GaussianFactorValuePair> newFactors(eliminationResults,
+                                                        correct);
 
   return std::make_shared<HybridGaussianFactor>(continuousSeparator,
                                                 discreteSeparator, newFactors);
@@ -597,10 +597,10 @@ GaussianFactorGraph HybridGaussianFactorGraph::operator()(
       gfg.push_back(gf);
     } else if (auto gc = std::dynamic_pointer_cast<GaussianConditional>(f)) {
       gfg.push_back(gf);
-    } else if (auto gmf = std::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
-      gfg.push_back((*gmf)(assignment));
-    } else if (auto gm = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
-      gfg.push_back((*gm)(assignment));
+    } else if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
+      gfg.push_back((*hgf)(assignment));
+    } else if (auto hgc = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
+      gfg.push_back((*hgc)(assignment));
     } else {
       continue;
     }