update HybridGaussianFactor to allow for tree of pairs

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -222,8 +222,8 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
   const HybridGaussianFactor::Factors likelihoods(
       conditionals_,
       [&](const GaussianConditional::shared_ptr &conditional)
-          -> std::pair<GaussianFactor::shared_ptr, double> {
+          -> GaussianFactorValuePair {
-        auto likelihood_m = conditional->likelihood(given);
+        const auto likelihood_m = conditional->likelihood(given);
         const double Cgm_Kgcm =
             logConstant_ - conditional->logNormalizationConstant();
         if (Cgm_Kgcm == 0.0) {
@@ -231,8 +231,13 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
         } else {
           // Add a constant factor to the likelihood in case the noise models
           // are not all equal.
-          double c = std::sqrt(2.0 * Cgm_Kgcm);
+          GaussianFactorGraph gfg;
-          return {likelihood_m, c};
+          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), 0.0};
         }
       });
   return std::make_shared<HybridGaussianFactor>(

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -45,12 +45,10 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
 
   // Check the base and the factors:
   return Base::equals(*e, tol) &&
-         factors_.equals(e->factors_,
+         factors_.equals(e->factors_, [tol](const GaussianFactorValuePair &f1,
-                         [tol](const std::pair<sharedFactor, double> &f1,
+                                            const GaussianFactorValuePair &f2) {
-                               const std::pair<sharedFactor, double> &f2) {
+           return f1.first->equals(*f2.first, tol) && (f1.second == f2.second);
-                           return f1.first->equals(*f2.first, tol) &&
+         });
-                                  (f1.second == f2.second);
-                         });
 }
 
 /* *******************************************************************************/
@@ -65,7 +63,7 @@ void HybridGaussianFactor::print(const std::string &s,
   } else {
     factors_.print(
         "", [&](Key k) { return formatter(k); },
-        [&](const std::pair<sharedFactor, double> &gfv) -> std::string {
+        [&](const GaussianFactorValuePair &gfv) -> std::string {
           auto [gf, val] = gfv;
           RedirectCout rd;
           std::cout << ":\n";
@@ -82,8 +80,8 @@ void HybridGaussianFactor::print(const std::string &s,
 }
 
 /* *******************************************************************************/
-std::pair<HybridGaussianFactor::sharedFactor, double>
+GaussianFactorValuePair HybridGaussianFactor::operator()(
-HybridGaussianFactor::operator()(const DiscreteValues &assignment) const {
+    const DiscreteValues &assignment) const {
   return factors_(assignment);
 }
 
@@ -103,7 +101,7 @@ GaussianFactorGraphTree HybridGaussianFactor::add(
 /* *******************************************************************************/
 GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree()
     const {
-  auto wrap = [](const std::pair<sharedFactor, double> &gfv) {
+  auto wrap = [](const GaussianFactorValuePair &gfv) {
     return GaussianFactorGraph{gfv.first};
   };
   return {factors_, wrap};
@@ -113,11 +111,10 @@ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree()
 AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
     const VectorValues &continuousValues) const {
   // functor to convert from sharedFactor to double error value.
-  auto errorFunc =
+  auto errorFunc = [&continuousValues](const GaussianFactorValuePair &gfv) {
-      [&continuousValues](const std::pair<sharedFactor, double> &gfv) {
+    auto [gf, v] = gfv;
-        auto [gf, val] = gfv;
+    return gf->error(continuousValues) + (0.5 * v * v);
-        return gf->error(continuousValues) + val;
+  };
-      };
   DecisionTree<Key, double> error_tree(factors_, errorFunc);
   return error_tree;
 }

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
@@ -53,7 +56,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   using sharedFactor = std::shared_ptr<GaussianFactor>;
 
   /// typedef for Decision Tree of Gaussian factors and log-constant.
-  using Factors = DecisionTree<Key, std::pair<sharedFactor, double>>;
+  using Factors = DecisionTree<Key, GaussianFactorValuePair>;
 
  private:
   /// Decision tree of Gaussian factors indexed by discrete keys.
@@ -95,9 +98,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @param factors Vector of gaussian factor shared pointers
    *  and arbitrary scalars.
    */
-  HybridGaussianFactor(
+  HybridGaussianFactor(const KeyVector &continuousKeys,
-      const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
+                       const DiscreteKeys &discreteKeys,
-      const std::vector<std::pair<sharedFactor, double>> &factors)
+                       const std::vector<GaussianFactorValuePair> &factors)
       : HybridGaussianFactor(continuousKeys, discreteKeys,
                              Factors(discreteKeys, factors)) {}
 
@@ -115,8 +118,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   /// @{
 
   /// Get the factor and scalar at a given discrete assignment.
-  std::pair<sharedFactor, double> operator()(
+  GaussianFactorValuePair operator()(const DiscreteValues &assignment) const;
-      const DiscreteValues &assignment) const;
 
   /**
    * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -263,9 +263,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
     } else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
       // Case where we have a HybridGaussianFactor with no continuous keys.
       // In this case, compute discrete probabilities.
-      auto logProbability =
+      auto logProbability = [&](const GaussianFactorValuePair &fv) -> double {
-          [&](const std::pair<GaussianFactor::shared_ptr, double> &fv)
-          -> double {
         auto [factor, val] = fv;
         double v = 0.5 * val * val;
         if (!factor) return -v;
@@ -353,8 +351,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 =
+  auto correct = [&](const Result &pair) -> GaussianFactorValuePair {
-      [&](const Result &pair) -> std::pair<GaussianFactor::shared_ptr, double> {
     const auto &[conditional, factor] = pair;
     if (factor) {
       auto hf = std::dynamic_pointer_cast<HessianFactor>(factor);
@@ -365,8 +362,8 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
     }
     return {factor, 0.0};
   };
-  DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>> newFactors(
+  DecisionTree<Key, GaussianFactorValuePair> newFactors(eliminationResults,
-      eliminationResults, correct);
+                                                        correct);
 
   return std::make_shared<HybridGaussianFactor>(continuousSeparator,
                                                 discreteSeparator, newFactors);

gtsam/hybrid/HybridNonlinearFactor.h

@@ -246,7 +246,7 @@ class HybridNonlinearFactor : public HybridFactor {
     // functional to linearize each factor in the decision tree
     auto linearizeDT =
         [continuousValues](const std::pair<sharedFactor, double>& f)
-        -> std::pair<GaussianFactor::shared_ptr, double> {
+        -> GaussianFactorValuePair {
       auto [factor, val] = f;
       return {factor->linearize(continuousValues), val};
     };