update HybridNonlinearFactor to accept a tree of nonlinear factors and arbitrary scalars

gtsam/hybrid/HybridNonlinearFactor.h

@@ -53,9 +53,9 @@ class HybridNonlinearFactor : public HybridFactor {
 
   /**
    * @brief typedef for DecisionTree which has Keys as node labels and
-   * NonlinearFactor as leaf nodes.
+   * pairs of NonlinearFactor & an arbitrary scalar as leaf nodes.
    */
-  using Factors = DecisionTree<Key, sharedFactor>;
+  using Factors = DecisionTree<Key, std::pair<sharedFactor, double>>;
 
  private:
   /// Decision tree of Gaussian factors indexed by discrete keys.
@@ -90,25 +90,27 @@ class HybridNonlinearFactor : public HybridFactor {
    * Will be typecast to NonlinearFactor shared pointers.
    * @param keys Vector of keys for continuous factors.
    * @param discreteKeys Vector of discrete keys.
-   * @param factors Vector of nonlinear factors.
+   * @param factors Vector of nonlinear factor and scalar pairs.
    * @param normalized Flag indicating if the factor error is already
    * normalized.
    */
   template <typename FACTOR>
-  HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
+  HybridNonlinearFactor(
-                        const std::vector<std::shared_ptr<FACTOR>>& factors,
+      const KeyVector& keys, const DiscreteKeys& discreteKeys,
-                        bool normalized = false)
+      const std::vector<std::pair<std::shared_ptr<FACTOR>, double>>& factors,
+      bool normalized = false)
       : Base(keys, discreteKeys), normalized_(normalized) {
-    std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
+    std::vector<std::pair<NonlinearFactor::shared_ptr, double>>
+        nonlinear_factors;
     KeySet continuous_keys_set(keys.begin(), keys.end());
     KeySet factor_keys_set;
-    for (auto&& f : factors) {
+    for (auto&& [f, val] : factors) {
       // Insert all factor continuous keys in the continuous keys set.
       std::copy(f->keys().begin(), f->keys().end(),
                 std::inserter(factor_keys_set, factor_keys_set.end()));
 
       if (auto nf = std::dynamic_pointer_cast<NonlinearFactor>(f)) {
-        nonlinear_factors.push_back(nf);
+        nonlinear_factors.push_back(std::make_pair(nf, val));
       } else {
         throw std::runtime_error(
             "Factors passed into HybridNonlinearFactor need to be nonlinear!");
@@ -133,9 +135,11 @@ class HybridNonlinearFactor : public HybridFactor {
    */
   AlgebraicDecisionTree<Key> errorTree(const Values& continuousValues) const {
     // functor to convert from sharedFactor to double error value.
-    auto errorFunc = [continuousValues](const sharedFactor& factor) {
+    auto errorFunc =
-      return factor->error(continuousValues);
+        [continuousValues](const std::pair<sharedFactor, double>& f) {
-    };
+          auto [factor, val] = f;
+          return factor->error(continuousValues) + val;
+        };
     DecisionTree<Key, double> result(factors_, errorFunc);
     return result;
   }
@@ -150,12 +154,10 @@ class HybridNonlinearFactor : public HybridFactor {
   double error(const Values& continuousValues,
                const DiscreteValues& discreteValues) const {
     // Retrieve the factor corresponding to the assignment in discreteValues.
-    auto factor = factors_(discreteValues);
+    auto [factor, val] = factors_(discreteValues);
     // Compute the error for the selected factor
     const double factorError = factor->error(continuousValues);
-    if (normalized_) return factorError;
+    return factorError + val;
-    return factorError + this->nonlinearFactorLogNormalizingConstant(
-                             factor, continuousValues);
   }
 
   /**
@@ -175,7 +177,7 @@ class HybridNonlinearFactor : public HybridFactor {
    */
   size_t dim() const {
     const auto assignments = DiscreteValues::CartesianProduct(discreteKeys_);
-    auto factor = factors_(assignments.at(0));
+    auto [factor, val] = factors_(assignments.at(0));
     return factor->dim();
   }
 
@@ -189,9 +191,11 @@ class HybridNonlinearFactor : public HybridFactor {
     std::cout << (s.empty() ? "" : s + " ");
     Base::print("", keyFormatter);
     std::cout << "\nHybridNonlinearFactor\n";
-    auto valueFormatter = [](const sharedFactor& v) {
+    auto valueFormatter = [](const std::pair<sharedFactor, double>& v) {
-      if (v) {
+      auto [factor, val] = v;
-        return "Nonlinear factor on " + std::to_string(v->size()) + " keys";
+      if (factor) {
+        return "Nonlinear factor on " + std::to_string(factor->size()) +
+               " keys";
       } else {
         return std::string("nullptr");
       }
@@ -211,8 +215,10 @@ class HybridNonlinearFactor : public HybridFactor {
         static_cast<const HybridNonlinearFactor&>(other));
 
     // Ensure that this HybridNonlinearFactor and `f` have the same `factors_`.
-    auto compare = [tol](const sharedFactor& a, const sharedFactor& b) {
+    auto compare = [tol](const std::pair<sharedFactor, double>& a,
-      return traits<NonlinearFactor>::Equals(*a, *b, tol);
+                         const std::pair<sharedFactor, double>& b) {
+      return traits<NonlinearFactor>::Equals(*a.first, *b.first, tol) &&
+             (a.second == b.second);
     };
     if (!factors_.equals(f.factors_, compare)) return false;
 
@@ -230,7 +236,7 @@ class HybridNonlinearFactor : public HybridFactor {
   GaussianFactor::shared_ptr linearize(
       const Values& continuousValues,
       const DiscreteValues& discreteValues) const {
-    auto factor = factors_(discreteValues);
+    auto [factor, val] = factors_(discreteValues);
     return factor->linearize(continuousValues);
   }
 
@@ -238,12 +244,14 @@ class HybridNonlinearFactor : public HybridFactor {
   std::shared_ptr<HybridGaussianFactor> linearize(
       const Values& continuousValues) const {
     // functional to linearize each factor in the decision tree
-    auto linearizeDT = [continuousValues](const sharedFactor& factor) {
+    auto linearizeDT =
-      return factor->linearize(continuousValues);
+        [continuousValues](const std::pair<sharedFactor, double>& f) {
-    };
+          auto [factor, val] = f;
+          return {factor->linearize(continuousValues), val};
+        };
 
-    DecisionTree<Key, GaussianFactor::shared_ptr> linearized_factors(
+    DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>>
-        factors_, linearizeDT);
+        linearized_factors(factors_, linearizeDT);
 
     return std::make_shared<HybridGaussianFactor>(
         continuousKeys_, discreteKeys_, linearized_factors);

gtsam/hybrid/tests/Switching.h

@@ -159,9 +159,10 @@ struct Switching {
     for (size_t k = 0; k < K - 1; k++) {
       KeyVector keys = {X(k), X(k + 1)};
       auto motion_models = motionModels(k, between_sigma);
-      std::vector<NonlinearFactor::shared_ptr> components;
+      std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components;
       for (auto &&f : motion_models) {
-        components.push_back(std::dynamic_pointer_cast<NonlinearFactor>(f));
+        components.push_back(
+            {std::dynamic_pointer_cast<NonlinearFactor>(f), 0.0});
       }
       nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(
           keys, DiscreteKeys{modes[k]}, components);

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -387,7 +387,8 @@ TEST(HybridBayesNet, Sampling) {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
   auto one_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
-  std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion};
+  std::vector<std::pair<NonlinearFactor::shared_ptr, double>> factors = {
+      {zero_motion, 0.0}, {one_motion, 0.0}};
   nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
   nfg.emplace_shared<HybridNonlinearFactor>(
       KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -435,9 +435,10 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
   const auto one_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
-  nfg.emplace_shared<HybridNonlinearFactor>(
+  std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components = {
-      KeyVector{X(0), X(1)}, DiscreteKeys{m},
+      {zero_motion, 0.0}, {one_motion, 0.0}};
-      std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
+  nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)},
+                                            DiscreteKeys{m}, components);
 
   return nfg;
 }
@@ -589,7 +590,8 @@ TEST(HybridEstimation, ModeSelection) {
        model1 = std::make_shared<MotionModel>(
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
 
-  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
+  std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components = {
+      {model0, 0.0}, {model1, 0.0}};
 
   KeyVector keys = {X(0), X(1)};
   HybridNonlinearFactor mf(keys, modes, components);
@@ -680,7 +682,8 @@ TEST(HybridEstimation, ModeSelection2) {
        model1 = std::make_shared<BetweenFactor<Vector3>>(
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
 
-  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
+  std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components = {
+      {model0, 0.0}, {model1, 0.0}};
 
   KeyVector keys = {X(0), X(1)};
   HybridNonlinearFactor mf(keys, modes, components);

gtsam/hybrid/tests/testHybridGaussianISAM.cpp

@@ -420,7 +420,8 @@ TEST(HybridGaussianISAM, NonTrivial) {
                                                    noise_model),
        moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
                                                     noise_model);
-  std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
+  std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
+      {moving, 0.0}, {still, 0.0}};
   auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
       contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
   fg.push_back(mixtureFactor);
@@ -460,7 +461,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
-  components = {moving, still};
+  components = {{moving, 0.0}, {still, 0.0}};
   mixtureFactor = std::make_shared<HybridNonlinearFactor>(
       contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
   fg.push_back(mixtureFactor);
@@ -503,7 +504,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
-  components = {moving, still};
+  components = {{moving, 0.0}, {still, 0.0}};
   mixtureFactor = std::make_shared<HybridNonlinearFactor>(
       contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
   fg.push_back(mixtureFactor);

gtsam/hybrid/tests/testHybridNonlinearFactor.cpp

@@ -58,7 +58,8 @@ TEST(HybridNonlinearFactor, Printing) {
       std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
   auto f1 =
       std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
-  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
+  std::vector<std::pair<NonlinearFactor::shared_ptr, double>> factors{
+      {f0, 0.0}, {f1, 0.0}};
 
   HybridNonlinearFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
 
@@ -86,7 +87,8 @@ static HybridNonlinearFactor getHybridNonlinearFactor() {
       std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
   auto f1 =
       std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
-  std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
+  std::vector<std::pair<NonlinearFactor::shared_ptr, double>> factors{
+      {f0, 0.0}, {f1, 0.0}};
 
   return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors);
 }

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -131,7 +131,8 @@ TEST(HybridGaussianFactorGraph, Resize) {
   auto still = std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
        moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
 
-  std::vector<MotionModel::shared_ptr> components = {still, moving};
+  std::vector<std::pair<MotionModel::shared_ptr, double>> components = {
+      {still, 0.0}, {moving, 0.0}};
   auto dcFactor = std::make_shared<HybridNonlinearFactor>(
       contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
   nhfg.push_back(dcFactor);
@@ -162,7 +163,8 @@ TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
   auto still = std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
        moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
 
-  std::vector<MotionModel::shared_ptr> components = {still, moving};
+  std::vector<std::pair<MotionModel::shared_ptr, double>> components = {
+      {still, 0.0}, {moving, 0.0}};
 
   // Check for exception when number of continuous keys are under-specified.
   KeyVector contKeys = {X(0)};
@@ -801,7 +803,8 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
                                                    noise_model),
        moving = std::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
                                                     noise_model);
-  std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
+  std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> motion_models =
+      {{still, 0.0}, {moving, 0.0}};
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
 

gtsam/hybrid/tests/testHybridNonlinearISAM.cpp

@@ -439,7 +439,8 @@ TEST(HybridNonlinearISAM, NonTrivial) {
                                                    noise_model),
        moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
                                                     noise_model);
-  std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
+  std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
+      {moving, 0.0}, {still, 0.0}};
   auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
       contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
   fg.push_back(mixtureFactor);
@@ -479,7 +480,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
-  components = {moving, still};
+  components = {{moving, 0.0}, {still, 0.0}};
   mixtureFactor = std::make_shared<HybridNonlinearFactor>(
       contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
   fg.push_back(mixtureFactor);
@@ -522,7 +523,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
-  components = {moving, still};
+  components = {{moving, 0.0}, {still, 0.0}};
   mixtureFactor = std::make_shared<HybridNonlinearFactor>(
       contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
   fg.push_back(mixtureFactor);