Fix HybridNonlinearFactor

gtsam/hybrid/HybridGaussianFactor.h

@@ -196,16 +196,15 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   double potentiallyPrunedComponentError(
       const sharedFactor &gf, const VectorValues &continuousValues) const;
 
-  /// Helper struct to assist in constructing the HybridGaussianFactor
+  /// Helper struct to assist private constructor below.
   struct ConstructorHelper {
     KeyVector continuousKeys;   // Continuous keys extracted from factors
     DiscreteKeys discreteKeys;  // Discrete keys provided to the constructors
     FactorValuePairs pairs;     // Used only if factorsTree is empty
     Factors factorsTree;
 
-    ConstructorHelper(
+    ConstructorHelper(const DiscreteKey &discreteKey,
-        const DiscreteKey &discreteKey,
+                      const std::vector<GaussianFactor::shared_ptr> &factors);
-        const std::vector<GaussianFactor::shared_ptr> &factorsVec);
 
     ConstructorHelper(const DiscreteKey &discreteKey,
                       const std::vector<GaussianFactorValuePair> &factorPairs);
@@ -214,7 +213,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
                       const FactorValuePairs &factorPairs);
   };
 
-  // Private constructor using ConstructorHelper
+  // Private constructor using ConstructorHelper above.
   HybridGaussianFactor(const ConstructorHelper &helper)
       : Base(helper.continuousKeys, helper.discreteKeys),
         factors_(helper.factorsTree.empty() ? augment(helper.pairs)

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -18,55 +18,59 @@
 
 #include <gtsam/hybrid/HybridNonlinearFactor.h>
 
+#include <memory>
+
+#include "gtsam/nonlinear/NonlinearFactor.h"
+
 namespace gtsam {
 
 /* *******************************************************************************/
-static void checkKeys(const KeyVector& continuousKeys,
+static void CopyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor,
-                      const std::vector<NonlinearFactorValuePair>& pairs) {
+                                      KeyVector* continuousKeys) {
-  KeySet factor_keys_set;
+  if (!factor) return;
-  for (const auto& pair : pairs) {
+  if (continuousKeys->empty()) {
-    auto f = pair.first;
+    *continuousKeys = factor->keys();
-    // Insert all factor continuous keys in the continuous keys set.
+  } else if (factor->keys() != *continuousKeys) {
-    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 "
+        "HybridNonlinearFactor: all factors should have the same keys!");
-        "the factors do not match!");
   }
 }
 
 /* *******************************************************************************/
-HybridNonlinearFactor::HybridNonlinearFactor(
+HybridNonlinearFactor::ConstructorHelper::ConstructorHelper(
-    const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+    const DiscreteKey& discreteKey,
     const std::vector<NonlinearFactor::shared_ptr>& factors)
-    : Base(continuousKeys, {discreteKey}) {
+    : discreteKeys({discreteKey}) {
   std::vector<NonlinearFactorValuePair> pairs;
-  for (auto&& f : factors) {
+  // Extract continuous keys from the first non-null factor
-    pairs.emplace_back(f, 0.0);
+  for (const auto& factor : factors) {
+    pairs.emplace_back(factor, 0.0);
+    CopyOrCheckContinuousKeys(factor, &continuousKeys);
   }
-  checkKeys(continuousKeys, pairs);
+  factorTree = FactorValuePairs({discreteKey}, pairs);
-  factors_ = FactorValuePairs({discreteKey}, pairs);
 }
 
 /* *******************************************************************************/
-HybridNonlinearFactor::HybridNonlinearFactor(
+HybridNonlinearFactor::ConstructorHelper::ConstructorHelper(
-    const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+    const DiscreteKey& discreteKey,
     const std::vector<NonlinearFactorValuePair>& pairs)
-    : Base(continuousKeys, {discreteKey}) {
+    : discreteKeys({discreteKey}) {
-  KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end());
+  // Extract continuous keys from the first non-null factor
-  checkKeys(continuousKeys, pairs);
+  for (const auto& pair : pairs) {
-  factors_ = FactorValuePairs({discreteKey}, pairs);
+    CopyOrCheckContinuousKeys(pair.first, &continuousKeys);
+  }
+  factorTree = FactorValuePairs({discreteKey}, pairs);
 }
 
 /* *******************************************************************************/
-HybridNonlinearFactor::HybridNonlinearFactor(const KeyVector& continuousKeys,
+HybridNonlinearFactor::ConstructorHelper::ConstructorHelper(
-                                             const DiscreteKeys& discreteKeys,
+    const DiscreteKeys& discreteKeys, const FactorValuePairs& factorPairs)
-                                             const FactorValuePairs& factors)
+    : discreteKeys(discreteKeys), factorTree(factorPairs) {
-    : Base(continuousKeys, discreteKeys), factors_(factors) {}
+  // Extract continuous keys from the first non-null factor
+  factorPairs.visit([&](const NonlinearFactorValuePair& pair) {
+    CopyOrCheckContinuousKeys(pair.first, &continuousKeys);
+  });
+}
 
 /* *******************************************************************************/
 AlgebraicDecisionTree<Key> HybridNonlinearFactor::errorTree(

gtsam/hybrid/HybridNonlinearFactor.h

@@ -90,13 +90,12 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * 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 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(
+  HybridNonlinearFactor(const DiscreteKey& discreteKey,
-      const KeyVector& continuousKeys, const DiscreteKey& discreteKey,
+                        const std::vector<NonlinearFactor::shared_ptr>& factors)
-      const std::vector<NonlinearFactor::shared_ptr>& factors);
+      : HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {}
 
   /**
    * @brief Construct a new HybridNonlinearFactor on a single discrete key,
@@ -104,13 +103,12 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * provided as a vector of pairs (ϕ_m(x), E_m).
    * The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m.
    *
-   * @param continuousKeys Vector of keys for continuous factors.
    * @param discreteKey The discrete key for the "mode", indexing components.
    * @param pairs Vector of gaussian factor-scalar pairs, one per mode.
    */
-  HybridNonlinearFactor(const KeyVector& continuousKeys,
+  HybridNonlinearFactor(const DiscreteKey& discreteKey,
-                        const DiscreteKey& discreteKey,
+                        const std::vector<NonlinearFactorValuePair>& pairs)
-                        const std::vector<NonlinearFactorValuePair>& pairs);
+      : HybridNonlinearFactor(ConstructorHelper(discreteKey, pairs)) {}
 
   /**
    * @brief Construct a new HybridNonlinearFactor on a several discrete keys M,
@@ -118,13 +116,12 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * 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,
+  HybridNonlinearFactor(const DiscreteKeys& discreteKeys,
-                        const DiscreteKeys& discreteKeys,
+                        const FactorValuePairs& factors)
-                        const FactorValuePairs& factors);
+      : HybridNonlinearFactor(ConstructorHelper(discreteKeys, factors)) {}
   /**
    * @brief Compute error of the HybridNonlinearFactor as a tree.
    *
@@ -181,6 +178,28 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
   /// Linearize all the continuous factors to get a HybridGaussianFactor.
   std::shared_ptr<HybridGaussianFactor> linearize(
       const Values& continuousValues) const;
+
+ private:
+  /// Helper struct to assist private constructor below.
+  struct ConstructorHelper {
+    KeyVector continuousKeys;   // Continuous keys extracted from factors
+    DiscreteKeys discreteKeys;  // Discrete keys provided to the constructors
+    FactorValuePairs factorTree;
+
+    ConstructorHelper(const DiscreteKey& discreteKey,
+                      const std::vector<NonlinearFactor::shared_ptr>& factors);
+
+    ConstructorHelper(const DiscreteKey& discreteKey,
+                      const std::vector<NonlinearFactorValuePair>& factorPairs);
+
+    ConstructorHelper(const DiscreteKeys& discreteKeys,
+                      const FactorValuePairs& factorPairs);
+  };
+
+  // Private constructor using ConstructorHelper above.
+  HybridNonlinearFactor(const ConstructorHelper& helper)
+      : Base(helper.continuousKeys, helper.discreteKeys),
+        factors_(helper.factorTree) {}
 };
 
 // traits