HybridGaussianFactor tests passing

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -220,22 +220,19 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
   const DiscreteKeys discreteParentKeys = discreteKeys();
   const KeyVector continuousParentKeys = continuousParents();
   const HybridGaussianFactor::Factors likelihoods(
-      conditionals_, [&](const GaussianConditional::shared_ptr &conditional) {
+      conditionals_,
-        const auto likelihood_m = conditional->likelihood(given);
+      [&](const GaussianConditional::shared_ptr &conditional)
+          -> std::pair<GaussianFactor::shared_ptr, double> {
+        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
           // are not all equal.
-          GaussianFactorGraph gfg;
+          double c = std::sqrt(2.0 * Cgm_Kgcm);
-          gfg.push_back(likelihood_m);
+          return {likelihood_m, c};
-          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);
         }
       });
   return std::make_shared<HybridGaussianFactor>(

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -46,8 +46,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_,
-                         [tol](const sharedFactor &f1, const sharedFactor &f2) {
+                         [tol](const std::pair<sharedFactor, double> &f1,
-                           return f1->equals(*f2, tol);
+                               const std::pair<sharedFactor, double> &f2) {
+                           return f1.first->equals(*f2.first, tol) &&
+                                  (f1.second == f2.second);
                          });
 }
 
@@ -63,11 +65,13 @@ void HybridGaussianFactor::print(const std::string &s,
   } else {
     factors_.print(
         "", [&](Key k) { return formatter(k); },
-        [&](const sharedFactor &gf) -> std::string {
+        [&](const std::pair<sharedFactor, double> &gfv) -> std::string {
+          auto [gf, val] = gfv;
           RedirectCout rd;
           std::cout << ":\n";
           if (gf) {
             gf->print("", formatter);
+            std::cout << "value: " << val << std::endl;
             return rd.str();
           } else {
             return "nullptr";
@@ -78,8 +82,8 @@ void HybridGaussianFactor::print(const std::string &s,
 }
 
 /* *******************************************************************************/
-HybridGaussianFactor::sharedFactor HybridGaussianFactor::operator()(
+std::pair<HybridGaussianFactor::sharedFactor, double>
-    const DiscreteValues &assignment) const {
+HybridGaussianFactor::operator()(const DiscreteValues &assignment) const {
   return factors_(assignment);
 }
 
@@ -99,7 +103,9 @@ GaussianFactorGraphTree HybridGaussianFactor::add(
 /* *******************************************************************************/
 GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree()
     const {
-  auto wrap = [](const sharedFactor &gf) { return GaussianFactorGraph{gf}; };
+  auto wrap = [](const std::pair<sharedFactor, double> &gfv) {
+    return GaussianFactorGraph{gfv.first};
+  };
   return {factors_, wrap};
 }
 
@@ -107,17 +113,19 @@ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree()
 AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
     const VectorValues &continuousValues) const {
   // functor to convert from sharedFactor to double error value.
-  auto errorFunc = [&continuousValues](const sharedFactor &gf) {
+  auto errorFunc =
-    return gf->error(continuousValues);
+      [&continuousValues](const std::pair<sharedFactor, double> &gfv) {
-  };
+        auto [gf, val] = gfv;
+        return gf->error(continuousValues) + val;
+      };
   DecisionTree<Key, double> error_tree(factors_, errorFunc);
   return error_tree;
 }
 
 /* *******************************************************************************/
 double HybridGaussianFactor::error(const HybridValues &values) const {
-  const sharedFactor gf = factors_(values.discrete());
+  auto &&[gf, val] = factors_(values.discrete());
-  return gf->error(values.continuous());
+  return gf->error(values.continuous()) + val;
 }
 
 }  // namespace gtsam

gtsam/hybrid/HybridGaussianFactor.h

@@ -53,7 +53,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, sharedFactor>;
+  using Factors = DecisionTree<Key, std::pair<sharedFactor, double>>;
 
  private:
   /// Decision tree of Gaussian factors indexed by discrete keys.
@@ -80,8 +80,7 @@ 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
+   * @param factors The decision tree of Gaussian factors and arbitrary scalars.
-   * as the mixture density.
    */
   HybridGaussianFactor(const KeyVector &continuousKeys,
                        const DiscreteKeys &discreteKeys,
@@ -93,11 +92,12 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    *
    * @param continuousKeys Vector of keys for continuous factors.
    * @param discreteKeys Vector of discrete keys.
-   * @param factors Vector of gaussian factor shared pointers.
+   * @param factors Vector of gaussian factor shared pointers
+   *  and arbitrary scalars.
    */
-  HybridGaussianFactor(const KeyVector &continuousKeys,
+  HybridGaussianFactor(
-                       const DiscreteKeys &discreteKeys,
+      const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
-                       const std::vector<sharedFactor> &factors)
+      const std::vector<std::pair<sharedFactor, double>> &factors)
       : HybridGaussianFactor(continuousKeys, discreteKeys,
                              Factors(discreteKeys, factors)) {}
 
@@ -114,8 +114,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   /// @name Standard API
   /// @{
 
-  /// Get factor at a given discrete assignment.
+  /// Get the factor and scalar at a given discrete assignment.
-  sharedFactor operator()(const DiscreteValues &assignment) const;
+  std::pair<sharedFactor, double> operator()(
+      const DiscreteValues &assignment) const;
 
   /**
    * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -92,13 +92,15 @@ 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);
+        auto [factor, val] = hgf->operator()(values.discrete());
-        std::cout << "error = " << gmf->error(values) << std::endl;
+        factor->print(ss.str(), keyFormatter);
+        std::cout << "value: " << val << std::endl;
+        std::cout << "error = " << factor->error(values) << std::endl;
       }
     } else if (auto hc = std::dynamic_pointer_cast<HybridConditional>(factor)) {
       if (factor == nullptr) {
@@ -262,9 +264,12 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
       // Case where we have a HybridGaussianFactor with no continuous keys.
       // In this case, compute discrete probabilities.
       auto logProbability =
-          [&](const GaussianFactor::shared_ptr &factor) -> double {
+          [&](const std::pair<GaussianFactor::shared_ptr, double> &fv)
-        if (!factor) return 0.0;
+          -> double {
-        return -factor->error(VectorValues());
+        auto [factor, val] = fv;
+        double v = 0.5 * val * val;
+        if (!factor) return -v;
+        return -(factor->error(VectorValues()) + v);
       };
       AlgebraicDecisionTree<Key> logProbabilities =
           DecisionTree<Key, double>(gmf->factors(), logProbability);
@@ -348,7 +353,8 @@ 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) -> std::pair<GaussianFactor::shared_ptr, double> {
     const auto &[conditional, factor] = pair;
     if (factor) {
       auto hf = std::dynamic_pointer_cast<HessianFactor>(factor);
@@ -357,10 +363,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,
+  DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>> newFactors(
-                                                           correct);
+      eliminationResults, correct);
 
   return std::make_shared<HybridGaussianFactor>(continuousSeparator,
                                                 discreteSeparator, newFactors);
@@ -597,10 +603,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)) {
+    } else if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
-      gfg.push_back((*gmf)(assignment));
+      gfg.push_back((*hgf)(assignment).first);
-    } else if (auto gm = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
+    } else if (auto hgc = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
-      gfg.push_back((*gm)(assignment));
+      gfg.push_back((*hgc)(assignment));
     } else {
       continue;
     }

gtsam/hybrid/HybridNonlinearFactor.h

@@ -245,10 +245,11 @@ class HybridNonlinearFactor : public HybridFactor {
       const Values& continuousValues) const {
     // functional to linearize each factor in the decision tree
     auto linearizeDT =
-        [continuousValues](const std::pair<sharedFactor, double>& f) {
+        [continuousValues](const std::pair<sharedFactor, double>& f)
-          auto [factor, val] = f;
+        -> std::pair<GaussianFactor::shared_ptr, double> {
-          return {factor->linearize(continuousValues), val};
+      auto [factor, val] = f;
-        };
+      return {factor->linearize(continuousValues), val};
+    };
 
     DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>>
         linearized_factors(factors_, linearizeDT);

gtsam/hybrid/tests/testHybridGaussianFactor.cpp

@@ -71,8 +71,10 @@ TEST(HybridGaussianFactor, Sum) {
   auto f20 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
   auto f21 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
   auto f22 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
-  std::vector<GaussianFactor::shared_ptr> factorsA{f10, f11};
+  std::vector<std::pair<GaussianFactor::shared_ptr, double>> factorsA{
-  std::vector<GaussianFactor::shared_ptr> factorsB{f20, f21, f22};
+      {f10, 0.0}, {f11, 0.0}};
+  std::vector<std::pair<GaussianFactor::shared_ptr, double>> factorsB{
+      {f20, 0.0}, {f21, 0.0}, {f22, 0.0}};
 
   // TODO(Frank): why specify keys at all? And: keys in factor should be *all*
   // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey?
@@ -109,7 +111,8 @@ TEST(HybridGaussianFactor, Printing) {
   auto b = Matrix::Zero(2, 1);
   auto f10 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
   auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
-  std::vector<GaussianFactor::shared_ptr> factors{f10, f11};
+  std::vector<std::pair<GaussianFactor::shared_ptr, double>> factors{
+      {f10, 0.0}, {f11, 0.0}};
 
   HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
 
@@ -128,6 +131,7 @@ Hybrid [x1 x2; 1]{
 ]
   b = [ 0 0 ]
   No noise model
+value: 0
 
  1 Leaf :
   A[x1] = [
@@ -140,6 +144,7 @@ Hybrid [x1 x2; 1]{
 ]
   b = [ 0 0 ]
   No noise model
+value: 0
 
 }
 )";
@@ -178,7 +183,8 @@ TEST(HybridGaussianFactor, Error) {
 
   auto f0 = std::make_shared<JacobianFactor>(X(1), A01, X(2), A02, b);
   auto f1 = std::make_shared<JacobianFactor>(X(1), A11, X(2), A12, b);
-  std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
+  std::vector<std::pair<GaussianFactor::shared_ptr, double>> factors{{f0, 0.0},
+                                                                     {f1, 0.0}};
 
   HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors);