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)
-    : Base(keys, discreteKeys), factors_(factors) {}
+                                             const FactorValuePairs& 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.
-  Factors factors_;
+  /// Decision tree of nonlinear factors indexed by discrete keys.
+  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 discreteKeys Vector of discrete keys.
-   * @param factors Decision tree with of shared factors.
+   * @param continuousKeys Vector of keys for continuous factors.
+   * @param discreteKey The discrete key for the "mode", indexing components.
+   * @param factors Vector of gaussian factors, one for each mode.
    */
-  HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys,
-                        const Factors& factors);
+  HybridNonlinearFactor(
+      const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+      const std::vector<NonlinearFactor::shared_ptr>& factors);
 
   /**
-   * @brief Convenience constructor that generates the underlying factor
-   * decision tree for us.
-   *
-   * Here it is important that the vector of factors has the correct number of
-   * elements based on the number of discrete keys and the cardinality of the
-   * keys, so that the decision tree is constructed appropriately.
+   * @brief Construct a new HybridNonlinearFactor on a single discrete key,
+   * 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.
    *
-   * @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.
+   * @param continuousKeys Vector of keys for continuous factors.
+   * @param discreteKey The discrete key for the "mode", indexing components.
+   * @param factors Vector of gaussian factor-scalar pairs, one per mode.
    */
-  template <typename FACTOR>
-  HybridNonlinearFactor(
-      const KeyVector& keys, const DiscreteKey& discreteKey,
-      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!");
-    }
-  }
+  HybridNonlinearFactor(const KeyVector& continuousKeys,
+                        const DiscreteKey& discreteKey,
+                        const std::vector<NonlinearFactorValuePair>& factors);
 
+  /**
+   * @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::DecisionTree<
-          gtsam::Key, std::pair<gtsam::NonlinearFactor*, double>>& factors);
+      const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey,
+      const std::vector<gtsam::NonlinearFactor*>& 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,
-                                                           double sigma = 1.0) {
+  static std::vector<NonlinearFactor::shared_ptr> motionModels(
+      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},
-                                            {one_motion, 0.0}});
+      std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
 
   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},
-                                                      {one_motion, 0.0}};
+  std::vector<NonlinearFactor::shared_ptr> components = {zero_motion,
+                                                         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},
-                                                      {model1, 0.0}};
+  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
 
   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},
-                                                      {model1, 0.0}};
+  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
 
   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),
-                                                   noise_model),
-       moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
-                                                    noise_model);
-  std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
-      {moving, 0.0}, {still, 0.0}};
+  std::vector<NonlinearFactor::shared_ptr> components;
+  components.emplace_back(
+      new PlanarMotionModel(W(0), W(1), odometry, noise_model));  // moving
+  components.emplace_back(
+      new PlanarMotionModel(W(0), W(1), Pose2(0, 0, 0), noise_model));  // still
   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),
-                                              noise_model);
-  moving =
-      std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
-  components = {{moving, 0.0}, {still, 0.0}};
+  components.clear();
+  components.emplace_back(
+      new PlanarMotionModel(W(1), W(2), odometry, noise_model));  // moving
+  components.emplace_back(
+      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),
-                                              noise_model);
-  moving =
-      std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
-  components = {{moving, 0.0}, {still, 0.0}};
+  components.clear();
+  components.emplace_back(
+      new PlanarMotionModel(W(2), W(3), odometry, noise_model));  // moving
+  components.emplace_back(
+      new PlanarMotionModel(W(2), W(3), Pose2(0, 0, 0), noise_model));  // still
   fg.emplace_shared<HybridNonlinearFactor>(
       contKeys, gtsam::DiscreteKey(M(3), 2), components);