Move code to cpp file.

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -29,34 +29,47 @@
 
 namespace gtsam {
 /* *******************************************************************************/
-HybridGaussianConditional::ConstructorHelper::ConstructorHelper(
+struct HybridGaussianConditional::ConstructorHelper {
-    const HybridGaussianConditional::Conditionals &conditionals) {
+  KeyVector frontals, parents;
-  negLogConstant = std::numeric_limits<double>::infinity();
+  HybridGaussianFactor::FactorValuePairs pairs;
+  double negLogConstant;
+  /// Compute all variables needed for the private constructor below.
+  ConstructorHelper(const Conditionals &conditionals) {
+    negLogConstant = std::numeric_limits<double>::infinity();
 
-  auto func =
+    auto func = [&](const GaussianConditional::shared_ptr &c)
-      [&](const GaussianConditional::shared_ptr &c) -> GaussianFactorValuePair {
+        -> GaussianFactorValuePair {
-    double value = 0.0;
+      double value = 0.0;
-    if (c) {
+      if (c) {
-      if (frontals.empty()) {
+        if (frontals.empty()) {
-        frontals = KeyVector(c->frontals().begin(), c->frontals().end());
+          frontals = KeyVector(c->frontals().begin(), c->frontals().end());
-        parents = KeyVector(c->parents().begin(), c->parents().end());
+          parents = KeyVector(c->parents().begin(), c->parents().end());
+        }
+        value = c->negLogConstant();
+        negLogConstant = std::min(negLogConstant, value);
       }
-      value = c->negLogConstant();
+      return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
-      negLogConstant = std::min(negLogConstant, value);
+    };
-    }
+    pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func);
-    return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
+  }
-  };
+};
-  pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func);
-}
 
 /* *******************************************************************************/
+HybridGaussianConditional::HybridGaussianConditional(
+    const DiscreteKeys &discreteParents,
+    const HybridGaussianConditional::Conditionals &conditionals,
+    const ConstructorHelper &helper)
+    : BaseFactor(discreteParents, helper.pairs),
+      BaseConditional(helper.frontals.size()),
+      conditionals_(conditionals),
+      negLogConstant_(helper.negLogConstant) {}
+
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKeys &discreteParents,
     const HybridGaussianConditional::Conditionals &conditionals)
     : HybridGaussianConditional(discreteParents, conditionals,
                                 ConstructorHelper(conditionals)) {}
 
-/* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
     const DiscreteKey &discreteParent,
     const std::vector<GaussianConditional::shared_ptr> &conditionals)

gtsam/hybrid/HybridGaussianConditional.h

@@ -185,23 +185,13 @@ class GTSAM_EXPORT HybridGaussianConditional
 
  private:
   /// Helper struct for private constructor.
-  struct ConstructorHelper {
+  struct ConstructorHelper;
-    KeyVector frontals, parents;
-    HybridGaussianFactor::FactorValuePairs pairs;
-    double negLogConstant;
-    /// Compute all variables needed for the private constructor below.
-    ConstructorHelper(const Conditionals &conditionals);
-  };
 
   /// Private constructor that uses helper struct above.
   HybridGaussianConditional(
       const DiscreteKeys &discreteParents,
       const HybridGaussianConditional::Conditionals &conditionals,
-      const ConstructorHelper &helper)
+      const ConstructorHelper &helper);
-      : BaseFactor(discreteParents, helper.pairs),
-        BaseConditional(helper.frontals.size()),
-        conditionals_(conditionals),
-        negLogConstant_(helper.negLogConstant) {}
 
   /// Convert to a DecisionTree of Gaussian factor graphs.
   GaussianFactorGraphTree asGaussianFactorGraphTree() const;

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -70,53 +70,76 @@ HybridGaussianFactor::Factors HybridGaussianFactor::augment(
 }
 
 /* *******************************************************************************/
-HybridGaussianFactor::ConstructorHelper::ConstructorHelper(
+struct HybridGaussianFactor::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(const DiscreteKey &discreteKey,
+                    const std::vector<GaussianFactor::shared_ptr> &factors)
+      : discreteKeys({discreteKey}) {
+    // Extract continuous keys from the first non-null factor
+    for (const auto &factor : factors) {
+      if (factor && continuousKeys.empty()) {
+        continuousKeys = factor->keys();
+        break;
+      }
+    }
+
+    // Build the DecisionTree from the factor vector
+    factorsTree = Factors(discreteKeys, factors);
+  }
+
+  ConstructorHelper(const DiscreteKey &discreteKey,
+                    const std::vector<GaussianFactorValuePair> &factorPairs)
+      : discreteKeys({discreteKey}) {
+    // Extract continuous keys from the first non-null factor
+    for (const auto &pair : factorPairs) {
+      if (pair.first && continuousKeys.empty()) {
+        continuousKeys = pair.first->keys();
+        break;
+      }
+    }
+
+    // Build the FactorValuePairs DecisionTree
+    pairs = FactorValuePairs(discreteKeys, factorPairs);
+  }
+
+  ConstructorHelper(const DiscreteKeys &discreteKeys,
+                    const FactorValuePairs &factorPairs)
+      : discreteKeys(discreteKeys) {
+    // Extract continuous keys from the first non-null factor
+    factorPairs.visit([&](const GaussianFactorValuePair &pair) {
+      if (pair.first && continuousKeys.empty()) {
+        continuousKeys = pair.first->keys();
+      }
+    });
+
+    // Build the FactorValuePairs DecisionTree
+    pairs = factorPairs;
+  }
+};
+
+/* *******************************************************************************/
+HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper &helper)
+    : Base(helper.continuousKeys, helper.discreteKeys),
+      factors_(helper.factorsTree.empty() ? augment(helper.pairs)
+                                          : helper.factorsTree) {}
+
+HybridGaussianFactor::HybridGaussianFactor(
     const DiscreteKey &discreteKey,
     const std::vector<GaussianFactor::shared_ptr> &factors)
-    : discreteKeys({discreteKey}) {
+    : HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {}
-  // Extract continuous keys from the first non-null factor
-  for (const auto &factor : factors) {
-    if (factor && continuousKeys.empty()) {
-      continuousKeys = factor->keys();
-      break;
-    }
-  }
 
-  // Build the DecisionTree from the factor vector
+HybridGaussianFactor::HybridGaussianFactor(
-  factorsTree = Factors(discreteKeys, factors);
-}
-
-/* *******************************************************************************/
-HybridGaussianFactor::ConstructorHelper::ConstructorHelper(
     const DiscreteKey &discreteKey,
     const std::vector<GaussianFactorValuePair> &factorPairs)
-    : discreteKeys({discreteKey}) {
+    : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {}
-  // Extract continuous keys from the first non-null factor
-  for (const auto &pair : factorPairs) {
-    if (pair.first && continuousKeys.empty()) {
-      continuousKeys = pair.first->keys();
-      break;
-    }
-  }
 
-  // Build the FactorValuePairs DecisionTree
+HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys,
-  pairs = FactorValuePairs(discreteKeys, factorPairs);
+                                           const FactorValuePairs &factors)
-}
+    : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
-
-/* *******************************************************************************/
-HybridGaussianFactor::ConstructorHelper::ConstructorHelper(
-    const DiscreteKeys &discreteKeys, const FactorValuePairs &factorPairs)
-    : discreteKeys(discreteKeys) {
-  // Extract continuous keys from the first non-null factor
-  factorPairs.visit([&](const GaussianFactorValuePair &pair) {
-    if (pair.first && continuousKeys.empty()) {
-      continuousKeys = pair.first->keys();
-    }
-  });
-
-  // Build the FactorValuePairs DecisionTree
-  pairs = factorPairs;
-}
 
 /* *******************************************************************************/
 bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {

gtsam/hybrid/HybridGaussianFactor.h

@@ -89,8 +89,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @param factors Vector of gaussian factors, one for each mode.
    */
   HybridGaussianFactor(const DiscreteKey &discreteKey,
-                       const std::vector<GaussianFactor::shared_ptr> &factors)
+                       const std::vector<GaussianFactor::shared_ptr> &factors);
-      : HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {}
 
   /**
    * @brief Construct a new HybridGaussianFactor on a single discrete key,
@@ -102,8 +101,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @param factorPairs Vector of gaussian factor-scalar pairs, one per mode.
    */
   HybridGaussianFactor(const DiscreteKey &discreteKey,
-                       const std::vector<GaussianFactorValuePair> &factorPairs)
+                       const std::vector<GaussianFactorValuePair> &factorPairs);
-      : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {}
 
   /**
    * @brief Construct a new HybridGaussianFactor on a several discrete keys M,
@@ -115,8 +113,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @param factors The decision tree of Gaussian factor/scalar pairs.
    */
   HybridGaussianFactor(const DiscreteKeys &discreteKeys,
-                       const FactorValuePairs &factors)
+                       const FactorValuePairs &factors);
-      : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {}
 
   /// @}
   /// @name Testable
@@ -197,27 +194,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
       const sharedFactor &gf, const VectorValues &continuousValues) const;
 
   /// Helper struct to assist private constructor below.
-  struct ConstructorHelper {
+  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(const DiscreteKey &discreteKey,
-                      const std::vector<GaussianFactor::shared_ptr> &factors);
-
-    ConstructorHelper(const DiscreteKey &discreteKey,
-                      const std::vector<GaussianFactorValuePair> &factorPairs);
-
-    ConstructorHelper(const DiscreteKeys &discreteKeys,
-                      const FactorValuePairs &factorPairs);
-  };
 
   // Private constructor using ConstructorHelper above.
-  HybridGaussianFactor(const ConstructorHelper &helper)
+  HybridGaussianFactor(const ConstructorHelper &helper);
-      : Base(helper.continuousKeys, helper.discreteKeys),
-        factors_(helper.factorsTree.empty() ? augment(helper.pairs)
-                                            : helper.factorsTree) {}
 
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -25,52 +25,71 @@
 namespace gtsam {
 
 /* *******************************************************************************/
-static void CopyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor,
+struct HybridNonlinearFactor::ConstructorHelper {
-                                      KeyVector* continuousKeys) {
+  KeyVector continuousKeys;   // Continuous keys extracted from factors
-  if (!factor) return;
+  DiscreteKeys discreteKeys;  // Discrete keys provided to the constructors
-  if (continuousKeys->empty()) {
+  FactorValuePairs factorTree;
-    *continuousKeys = factor->keys();
+
-  } else if (factor->keys() != *continuousKeys) {
+  void copyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor) {
-    throw std::runtime_error(
+    if (!factor) return;
-        "HybridNonlinearFactor: all factors should have the same keys!");
+    if (continuousKeys.empty()) {
+      continuousKeys = factor->keys();
+    } else if (factor->keys() != continuousKeys) {
+      throw std::runtime_error(
+          "HybridNonlinearFactor: all factors should have the same keys!");
+    }
   }
-}
+
+  ConstructorHelper(const DiscreteKey& discreteKey,
+                    const std::vector<NonlinearFactor::shared_ptr>& factors)
+      : discreteKeys({discreteKey}) {
+    std::vector<NonlinearFactorValuePair> pairs;
+    // Extract continuous keys from the first non-null factor
+    for (const auto& factor : factors) {
+      pairs.emplace_back(factor, 0.0);
+      copyOrCheckContinuousKeys(factor);
+    }
+    factorTree = FactorValuePairs({discreteKey}, pairs);
+  }
+
+  ConstructorHelper(const DiscreteKey& discreteKey,
+                    const std::vector<NonlinearFactorValuePair>& pairs)
+      : discreteKeys({discreteKey}) {
+    // Extract continuous keys from the first non-null factor
+    for (const auto& pair : pairs) {
+      copyOrCheckContinuousKeys(pair.first);
+    }
+    factorTree = FactorValuePairs({discreteKey}, pairs);
+  }
+
+  ConstructorHelper(const DiscreteKeys& discreteKeys,
+                    const FactorValuePairs& factorPairs)
+      : discreteKeys(discreteKeys), factorTree(factorPairs) {
+    // Extract continuous keys from the first non-null factor
+    factorPairs.visit([&](const NonlinearFactorValuePair& pair) {
+      copyOrCheckContinuousKeys(pair.first);
+    });
+  }
+};
 
 /* *******************************************************************************/
-HybridNonlinearFactor::ConstructorHelper::ConstructorHelper(
+HybridNonlinearFactor::HybridNonlinearFactor(const ConstructorHelper& helper)
+    : Base(helper.continuousKeys, helper.discreteKeys),
+      factors_(helper.factorTree) {}
+
+HybridNonlinearFactor::HybridNonlinearFactor(
     const DiscreteKey& discreteKey,
     const std::vector<NonlinearFactor::shared_ptr>& factors)
-    : discreteKeys({discreteKey}) {
+    : HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {}
-  std::vector<NonlinearFactorValuePair> pairs;
-  // Extract continuous keys from the first non-null factor
-  for (const auto& factor : factors) {
-    pairs.emplace_back(factor, 0.0);
-    CopyOrCheckContinuousKeys(factor, &continuousKeys);
-  }
-  factorTree = FactorValuePairs({discreteKey}, pairs);
-}
 
-/* *******************************************************************************/
+HybridNonlinearFactor::HybridNonlinearFactor(
-HybridNonlinearFactor::ConstructorHelper::ConstructorHelper(
     const DiscreteKey& discreteKey,
     const std::vector<NonlinearFactorValuePair>& pairs)
-    : discreteKeys({discreteKey}) {
+    : HybridNonlinearFactor(ConstructorHelper(discreteKey, pairs)) {}
-  // Extract continuous keys from the first non-null factor
-  for (const auto& pair : pairs) {
-    CopyOrCheckContinuousKeys(pair.first, &continuousKeys);
-  }
-  factorTree = FactorValuePairs({discreteKey}, pairs);
-}
 
-/* *******************************************************************************/
+HybridNonlinearFactor::HybridNonlinearFactor(const DiscreteKeys& discreteKeys,
-HybridNonlinearFactor::ConstructorHelper::ConstructorHelper(
+                                             const FactorValuePairs& factors)
-    const DiscreteKeys& discreteKeys, const FactorValuePairs& factorPairs)
+    : HybridNonlinearFactor(ConstructorHelper(discreteKeys, factors)) {}
-    : discreteKeys(discreteKeys), factorTree(factorPairs) {
-  // 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

@@ -93,9 +93,9 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * @param discreteKey The discrete key for the "mode", indexing components.
    * @param factors Vector of gaussian factors, one for each mode.
    */
-  HybridNonlinearFactor(const DiscreteKey& discreteKey,
+  HybridNonlinearFactor(
-                        const std::vector<NonlinearFactor::shared_ptr>& factors)
+      const DiscreteKey& discreteKey,
-      : HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {}
+      const std::vector<NonlinearFactor::shared_ptr>& factors);
 
   /**
    * @brief Construct a new HybridNonlinearFactor on a single discrete key,
@@ -107,8 +107,7 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * @param pairs Vector of gaussian factor-scalar pairs, one per mode.
    */
   HybridNonlinearFactor(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,
@@ -120,8 +119,8 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
    * @param factors The decision tree of nonlinear factor/scalar pairs.
    */
   HybridNonlinearFactor(const DiscreteKeys& discreteKeys,
-                        const FactorValuePairs& factors)
+                        const FactorValuePairs& factors);
-      : HybridNonlinearFactor(ConstructorHelper(discreteKeys, factors)) {}
+
   /**
    * @brief Compute error of the HybridNonlinearFactor as a tree.
    *
@@ -181,25 +180,10 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
 
  private:
   /// Helper struct to assist private constructor below.
-  struct ConstructorHelper {
+  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)
+  HybridNonlinearFactor(const ConstructorHelper& helper);
-      : Base(helper.continuousKeys, helper.discreteKeys),
-        factors_(helper.factorTree) {}
 };
 
 // traits