Merge pull request #1843 from borglab/feature/easy_hnf

Easy HNF constructors

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -21,10 +21,57 @@
 namespace gtsam {
 
 /* *******************************************************************************/
-HybridNonlinearFactor::HybridNonlinearFactor(const KeyVector& keys,
+static void checkKeys(const KeyVector& continuousKeys,
+                      std::vector<NonlinearFactorValuePair>& pairs) {
+  KeySet factor_keys_set;
+  for (const auto& pair : pairs) {
+    auto f = pair.first;
+    // 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()));
+  }
+
+  KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
+  if (continuous_keys_set != factor_keys_set) {
+    throw std::runtime_error(
+        "HybridNonlinearFactor: The specified continuous keys and the keys in "
+        "the factors do not match!");
+  }
+}
+
+/* *******************************************************************************/
+HybridNonlinearFactor::HybridNonlinearFactor(
+    const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+    const std::vector<NonlinearFactor::shared_ptr>& factors)
+    : Base(continuousKeys, {discreteKey}) {
+  std::vector<NonlinearFactorValuePair> pairs;
+  for (auto&& f : factors) {
+    pairs.emplace_back(f, 0.0);
+  }
+  checkKeys(continuousKeys, pairs);
+  factors_ = FactorValuePairs({discreteKey}, pairs);
+}
+
+/* *******************************************************************************/
+HybridNonlinearFactor::HybridNonlinearFactor(
+    const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+    const std::vector<NonlinearFactorValuePair>& factors)
+    : Base(continuousKeys, {discreteKey}) {
+  std::vector<NonlinearFactorValuePair> pairs;
+  KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
+  KeySet factor_keys_set;
+  for (auto&& [f, val] : factors) {
+    pairs.emplace_back(f, val);
+  }
+  checkKeys(continuousKeys, pairs);
+  factors_ = FactorValuePairs({discreteKey}, pairs);
+}
+
+/* *******************************************************************************/
+HybridNonlinearFactor::HybridNonlinearFactor(const KeyVector& continuousKeys,
                                              const DiscreteKeys& discreteKeys,
-                                             const Factors& factors)
+                                             const FactorValuePairs& factors)
-    : Base(keys, discreteKeys), factors_(factors) {}
+    : Base(continuousKeys, discreteKeys), factors_(factors) {}
 
 /* *******************************************************************************/
 AlgebraicDecisionTree<Key> HybridNonlinearFactor::errorTree(

gtsam/hybrid/HybridNonlinearFactor.h

@@ -68,11 +68,11 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * @brief typedef for DecisionTree which has Keys as node labels and
    * pairs of NonlinearFactor & an arbitrary scalar as leaf nodes.
    */
-  using Factors = DecisionTree<Key, NonlinearFactorValuePair>;
+  using FactorValuePairs = DecisionTree<Key, NonlinearFactorValuePair>;
 
  private:
-  /// Decision tree of Gaussian factors indexed by discrete keys.
+  /// Decision tree of nonlinear factors indexed by discrete keys.
-  Factors factors_;
+  FactorValuePairs factors_;
 
   /// HybridFactor method implementation. Should not be used.
   AlgebraicDecisionTree<Key> errorTree(
@@ -82,62 +82,49 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
   }
 
  public:
+  /// Default constructor, mainly for serialization.
   HybridNonlinearFactor() = default;
 
   /**
-   * @brief Construct from Decision tree.
+   * @brief Construct a new HybridNonlinearFactor on a single discrete key,
+   * providing the factors for each mode m as a vector of factors ϕ_m(x).
+   * The value ϕ(x,m) for the factor is simply ϕ_m(x).
    *
-   * @param keys Vector of keys for continuous factors.
+   * @param continuousKeys Vector of keys for continuous factors.
-   * @param discreteKeys Vector of discrete keys.
+   * @param discreteKey The discrete key for the "mode", indexing components.
-   * @param factors Decision tree with of shared factors.
+   * @param factors Vector of gaussian factors, one for each mode.
    */
-  HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
+  HybridNonlinearFactor(
-                        const Factors& factors);
+      const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+      const std::vector<NonlinearFactor::shared_ptr>& factors);
 
   /**
-   * @brief Convenience constructor that generates the underlying factor
+   * @brief Construct a new HybridNonlinearFactor on a single discrete key,
-   * decision tree for us.
+   * including a scalar error value for each mode m. The factors and scalars are
+   * provided as a vector of pairs (ϕ_m(x), E_m).
+   * The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m.
    *
-   * Here it is important that the vector of factors has the correct number of
+   * @param continuousKeys Vector of keys for continuous factors.
-   * elements based on the number of discrete keys and the cardinality of the
+   * @param discreteKey The discrete key for the "mode", indexing components.
-   * keys, so that the decision tree is constructed appropriately.
+   * @param factors Vector of gaussian factor-scalar pairs, one per mode.
-   *
-   * @tparam FACTOR The type of the factor shared pointers being passed in.
-   * Will be typecast to NonlinearFactor shared pointers.
-   * @param keys Vector of keys for continuous factors.
-   * @param discreteKey The discrete key indexing each component factor.
-   * @param factors Vector of nonlinear factor and scalar pairs.
-   * Same size as the cardinality of discreteKey.
    */
-  template <typename FACTOR>
+  HybridNonlinearFactor(const KeyVector& continuousKeys,
-  HybridNonlinearFactor(
+                        const DiscreteKey& discreteKey,
-      const KeyVector& keys, const DiscreteKey& discreteKey,
+                        const std::vector<NonlinearFactorValuePair>& factors);
-      const std::vector<std::pair<std::shared_ptr<FACTOR>, double>>& factors)
-      : Base(keys, {discreteKey}) {
-    std::vector<NonlinearFactorValuePair> nonlinear_factors;
-    KeySet continuous_keys_set(keys.begin(), keys.end());
-    KeySet factor_keys_set;
-    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.emplace_back(nf, val);
-      } else {
-        throw std::runtime_error(
-            "Factors passed into HybridNonlinearFactor need to be nonlinear!");
-      }
-    }
-    factors_ = Factors({discreteKey}, nonlinear_factors);
-
-    if (continuous_keys_set != factor_keys_set) {
-      throw std::runtime_error(
-          "The specified continuous keys and the keys in the factors don't "
-          "match!");
-    }
-  }
 
+  /**
+   * @brief Construct a new HybridNonlinearFactor on a several discrete keys M,
+   * including a scalar error value for each assignment m. The factors and
+   * scalars are provided as a DecisionTree<Key> of pairs (ϕ_M(x), E_M).
+   * The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m.
+   *
+   * @param continuousKeys A vector of keys representing continuous variables.
+   * @param discreteKeys Discrete variables and their cardinalities.
+   * @param factors The decision tree of nonlinear factor/scalar pairs.
+   */
+  HybridNonlinearFactor(const KeyVector& continuousKeys,
+                        const DiscreteKeys& discreteKeys,
+                        const FactorValuePairs& factors);
   /**
    * @brief Compute error of the HybridNonlinearFactor as a tree.
    *
@@ -196,4 +183,9 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
       const Values& continuousValues) const;
 };
 
+// traits
+template <>
+struct traits<HybridNonlinearFactor> : public Testable<HybridNonlinearFactor> {
+};
+
 }  // namespace gtsam

gtsam/hybrid/hybrid.i

@@ -246,14 +246,18 @@ class HybridNonlinearFactorGraph {
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
 class HybridNonlinearFactor : gtsam::HybridFactor {
   HybridNonlinearFactor(
-      const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
+      const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey,
-      const gtsam::DecisionTree<
+      const std::vector<gtsam::NonlinearFactor*>& factors);
-          gtsam::Key, std::pair<gtsam::NonlinearFactor*, double>>& factors);
 
   HybridNonlinearFactor(
       const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey,
       const std::vector<std::pair<gtsam::NonlinearFactor*, double>>& factors);
 
+  HybridNonlinearFactor(
+      const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
+      const gtsam::DecisionTree<
+          gtsam::Key, std::pair<gtsam::NonlinearFactor*, double>>& factors);
+
   double error(const gtsam::Values& continuousValues,
                const gtsam::DiscreteValues& discreteValues) const;
 

gtsam/hybrid/tests/Switching.h

@@ -161,13 +161,8 @@ 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<NonlinearFactorValuePair> components;
-      for (auto &&f : motion_models) {
-        components.push_back(
-            {std::dynamic_pointer_cast<NonlinearFactor>(f), 0.0});
-      }
       nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(keys, modes[k],
-                                                                 components);
+                                                                 motion_models);
     }
 
     // Add measurement factors
@@ -191,8 +186,8 @@ struct Switching {
   }
 
   // Create motion models for a given time step
-  static std::vector<MotionModel::shared_ptr> motionModels(size_t k,
+  static std::vector<NonlinearFactor::shared_ptr> motionModels(
-                                                           double sigma = 1.0) {
+      size_t k, double sigma = 1.0) {
     auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
     auto still =
              std::make_shared<MotionModel>(X(k), X(k + 1), 0.0, noise_model),

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -391,8 +391,7 @@ TEST(HybridBayesNet, Sampling) {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
   nfg.emplace_shared<HybridNonlinearFactor>(
       KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2),
-      std::vector<NonlinearFactorValuePair>{{zero_motion, 0.0},
+      std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
-                                            {one_motion, 0.0}});
 
   DiscreteKey mode(M(0), 2);
   nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -435,8 +435,8 @@ 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);
-  std::vector<NonlinearFactorValuePair> components = {{zero_motion, 0.0},
+  std::vector<NonlinearFactor::shared_ptr> components = {zero_motion,
-                                                      {one_motion, 0.0}};
+                                                         one_motion};
   nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, m,
                                             components);
 
@@ -566,10 +566,8 @@ std::shared_ptr<HybridGaussianFactor> mixedVarianceFactor(
       [](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair {
         return {gf, 0.0};
       });
-  auto updated_gmf = std::make_shared<HybridGaussianFactor>(
+  return std::make_shared<HybridGaussianFactor>(
       gmf->continuousKeys(), gmf->discreteKeys(), updated_pairs);
-
-  return updated_gmf;
 }
 
 /****************************************************************************/
@@ -591,8 +589,7 @@ TEST(HybridEstimation, ModeSelection) {
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_loose)),
        model1 = std::make_shared<MotionModel>(
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
-  std::vector<NonlinearFactorValuePair> components = {{model0, 0.0},
+  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
-                                                      {model1, 0.0}};
 
   KeyVector keys = {X(0), X(1)};
   DiscreteKey modes(M(0), 2);
@@ -688,8 +685,7 @@ TEST(HybridEstimation, ModeSelection2) {
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_loose)),
        model1 = std::make_shared<BetweenFactor<Vector3>>(
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
-  std::vector<NonlinearFactorValuePair> components = {{model0, 0.0},
+  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
-                                                      {model1, 0.0}};
 
   KeyVector keys = {X(0), X(1)};
   DiscreteKey modes(M(0), 2);

gtsam/hybrid/tests/testHybridGaussianFactor.cpp

@@ -73,7 +73,7 @@ TEST(HybridGaussianFactor, ConstructorVariants) {
   HybridGaussianFactor fromFactors({X(1), X(2)}, m1, {f10, f11});
 
   std::vector<GaussianFactorValuePair> pairs{{f10, 0.0}, {f11, 0.0}};
-  HybridGaussianFactor fromPairs({X(1), X(2)}, {m1}, pairs);
+  HybridGaussianFactor fromPairs({X(1), X(2)}, m1, pairs);
   assert_equal(fromFactors, fromPairs);
 
   HybridGaussianFactor::FactorValuePairs decisionTree({m1}, pairs);

gtsam/hybrid/tests/testHybridGaussianISAM.cpp

@@ -416,12 +416,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
   Pose2 odometry(1.0, 0.0, 0.0);
   KeyVector contKeys = {W(0), W(1)};
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
-  auto still = std::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 0),
+  std::vector<NonlinearFactor::shared_ptr> components;
-                                                   noise_model),
+  components.emplace_back(
-       moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
+      new PlanarMotionModel(W(0), W(1), odometry, noise_model));  // moving
-                                                    noise_model);
+  components.emplace_back(
-  std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
+      new PlanarMotionModel(W(0), W(1), Pose2(0, 0, 0), noise_model));  // still
-      {moving, 0.0}, {still, 0.0}};
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(1), 2), components);
 
@@ -456,11 +455,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
   /*************** Run Round 3 ***************/
   // Add odometry factor with discrete modes.
   contKeys = {W(1), W(2)};
-  still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
+  components.clear();
-                                              noise_model);
+  components.emplace_back(
-  moving =
+      new PlanarMotionModel(W(1), W(2), odometry, noise_model));  // moving
-      std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
+  components.emplace_back(
-  components = {{moving, 0.0}, {still, 0.0}};
+      new PlanarMotionModel(W(1), W(2), Pose2(0, 0, 0), noise_model));  // still
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(2), 2), components);
 
@@ -498,11 +497,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
   /*************** Run Round 4 ***************/
   // Add odometry factor with discrete modes.
   contKeys = {W(2), W(3)};
-  still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
+  components.clear();
-                                              noise_model);
+  components.emplace_back(
-  moving =
+      new PlanarMotionModel(W(2), W(3), odometry, noise_model));  // moving
-      std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
+  components.emplace_back(
-  components = {{moving, 0.0}, {still, 0.0}};
+      new PlanarMotionModel(W(2), W(3), Pose2(0, 0, 0), noise_model));  // still
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(3), 2), components);
 

gtsam/hybrid/tests/testHybridNonlinearFactor.cpp

@@ -43,24 +43,39 @@ TEST(HybridNonlinearFactor, Constructor) {
   HybridNonlinearFactor::iterator it = factor.begin();
   CHECK(it == factor.end());
 }
+/* ************************************************************************* */
+namespace test_constructor {
+DiscreteKey m1(1, 2);
+double between0 = 0.0;
+double between1 = 1.0;
+
+Vector1 sigmas = Vector1(1.0);
+auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
+
+auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
+auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
+}  // namespace test_constructor
+
+/* ************************************************************************* */
+// Test simple to complex constructors...
+TEST(HybridGaussianFactor, ConstructorVariants) {
+  using namespace test_constructor;
+  HybridNonlinearFactor fromFactors({X(1), X(2)}, m1, {f0, f1});
+
+  std::vector<NonlinearFactorValuePair> pairs{{f0, 0.0}, {f1, 0.0}};
+  HybridNonlinearFactor fromPairs({X(1), X(2)}, m1, pairs);
+  assert_equal(fromFactors, fromPairs);
+
+  HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs);
+  HybridNonlinearFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree);
+  assert_equal(fromDecisionTree, fromPairs);
+}
 
 /* ************************************************************************* */
 // Test .print() output.
 TEST(HybridNonlinearFactor, Printing) {
-  DiscreteKey m1(1, 2);
+  using namespace test_constructor;
-  double between0 = 0.0;
+  HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, {f0, f1});
-  double between1 = 1.0;
-
-  Vector1 sigmas = Vector1(1.0);
-  auto model = noiseModel::Diagonal::Sigmas(sigmas, false);
-
-  auto f0 =
-      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<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
-
-  HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, factors);
 
   std::string expected =
       R"(Hybrid [x1 x2; 1]
@@ -86,9 +101,7 @@ 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<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
+  return HybridNonlinearFactor({X(1), X(2)}, m1, {f0, f1});
-
-  return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors);
 }
 
 /* ************************************************************************* */

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -115,35 +115,40 @@ TEST(HybridNonlinearFactorGraph, Resize) {
   EXPECT_LONGS_EQUAL(fg.size(), 0);
 }
 
+/***************************************************************************/
+namespace test_motion {
+KeyVector contKeys = {X(0), X(1)};
+gtsam::DiscreteKey m1(M(1), 2);
+auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
+std::vector<NonlinearFactor::shared_ptr> components = {
+    std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
+    std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model)};
+}  // namespace test_motion
+
 /***************************************************************************
  * Test that the resize method works correctly for a
  * HybridGaussianFactorGraph.
  */
 TEST(HybridGaussianFactorGraph, Resize) {
-  HybridNonlinearFactorGraph nhfg;
+  using namespace test_motion;
+
+  HybridNonlinearFactorGraph hnfg;
   auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
       X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
-  nhfg.push_back(nonlinearFactor);
+  hnfg.push_back(nonlinearFactor);
   auto discreteFactor = std::make_shared<DecisionTreeFactor>();
-  nhfg.push_back(discreteFactor);
+  hnfg.push_back(discreteFactor);
 
-  KeyVector contKeys = {X(0), X(1)};
+  auto dcFactor =
-  auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
+      std::make_shared<HybridNonlinearFactor>(contKeys, m1, components);
-  auto still = std::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
+  hnfg.push_back(dcFactor);
-       moving = std::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
-
-  std::vector<std::pair<MotionModel::shared_ptr, double>> components = {
-      {still, 0.0}, {moving, 0.0}};
-  auto dcFactor = std::make_shared<HybridNonlinearFactor>(
-      contKeys, gtsam::DiscreteKey(M(1), 2), components);
-  nhfg.push_back(dcFactor);
 
   Values linearizationPoint;
   linearizationPoint.insert<double>(X(0), 0);
   linearizationPoint.insert<double>(X(1), 1);
 
   // Generate `HybridGaussianFactorGraph` by linearizing
-  HybridGaussianFactorGraph gfg = *nhfg.linearize(linearizationPoint);
+  HybridGaussianFactorGraph gfg = *hnfg.linearize(linearizationPoint);
 
   EXPECT_LONGS_EQUAL(gfg.size(), 3);
 
@@ -156,26 +161,19 @@ TEST(HybridGaussianFactorGraph, Resize) {
  * continuous keys provided do not match the keys in the factors.
  */
 TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
+  using namespace test_motion;
+
   auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
       X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
   auto discreteFactor = std::make_shared<DecisionTreeFactor>();
 
-  auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
-  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<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)};
+  THROWS_EXCEPTION(
-  THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
+      std::make_shared<HybridNonlinearFactor>(KeyVector{X(0)}, m1, components));
-      contKeys, gtsam::DiscreteKey(M(1), 2), components));
 
   // Check for exception when number of continuous keys are too many.
-  contKeys = {X(0), X(1), X(2)};
   THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
-      contKeys, gtsam::DiscreteKey(M(1), 2), components));
+      KeyVector{X(0), X(1), X(2)}, m1, components));
 }
 
 /*****************************************************************************
@@ -832,12 +830,10 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) {
   Pose2 odometry(2.0, 0.0, 0.0);
   KeyVector contKeys = {X(0), X(1)};
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
-  auto still = std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
+  std::vector<NonlinearFactor::shared_ptr> motion_models = {
-                                                   noise_model),
+      std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
-       moving = std::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
+                                          noise_model),
-                                                    noise_model);
+      std::make_shared<PlanarMotionModel>(X(0), X(1), odometry, noise_model)};
-  std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> motion_models =
-      {{still, 0.0}, {moving, 0.0}};
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(1), 2), motion_models);
 

gtsam/hybrid/tests/testHybridNonlinearISAM.cpp

@@ -439,8 +439,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
                                                    noise_model),
        moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
                                                     noise_model);
-  std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
+  std::vector<NonlinearFactor::shared_ptr> components{moving, still};
-      {moving, 0.0}, {still, 0.0}};
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(1), 2), components);
 
@@ -479,7 +478,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
-  components = {{moving, 0.0}, {still, 0.0}};
+  components = {moving, still};
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(2), 2), components);
 
@@ -521,7 +520,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
-  components = {{moving, 0.0}, {still, 0.0}};
+  components = {moving, still};
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(3), 2), components);
 

gtsam/hybrid/tests/testSerializationHybrid.cpp

@@ -83,7 +83,7 @@ TEST(HybridSerialization, HybridGaussianFactor) {
   auto b1 = Matrix::Ones(2, 1);
   auto f0 = std::make_shared<JacobianFactor>(X(0), A, b0);
   auto f1 = std::make_shared<JacobianFactor>(X(0), A, b1);
-  std::vector<GaussianFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
+  std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
 
   const HybridGaussianFactor factor(continuousKeys, discreteKey, factors);