diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index f3a699094..87dff7e59 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -29,34 +29,47 @@ namespace gtsam { /* *******************************************************************************/ -HybridGaussianConditional::ConstructorHelper::ConstructorHelper( - const HybridGaussianConditional::Conditionals &conditionals) { - negLogConstant = std::numeric_limits::infinity(); +struct HybridGaussianConditional::ConstructorHelper { + KeyVector frontals, parents; + HybridGaussianFactor::FactorValuePairs pairs; + double negLogConstant; + /// Compute all variables needed for the private constructor below. + ConstructorHelper(const Conditionals &conditionals) { + negLogConstant = std::numeric_limits::infinity(); - auto func = - [&](const GaussianConditional::shared_ptr &c) -> GaussianFactorValuePair { - double value = 0.0; - if (c) { - if (frontals.empty()) { - frontals = KeyVector(c->frontals().begin(), c->frontals().end()); - parents = KeyVector(c->parents().begin(), c->parents().end()); + auto func = [&](const GaussianConditional::shared_ptr &c) + -> GaussianFactorValuePair { + double value = 0.0; + if (c) { + if (frontals.empty()) { + frontals = KeyVector(c->frontals().begin(), c->frontals().end()); + parents = KeyVector(c->parents().begin(), c->parents().end()); + } + value = c->negLogConstant(); + negLogConstant = std::min(negLogConstant, value); } - value = c->negLogConstant(); - negLogConstant = std::min(negLogConstant, value); - } - return {std::dynamic_pointer_cast(c), value}; - }; - pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func); -} + return {std::dynamic_pointer_cast(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 &conditionals) diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 15ef43173..b7e2ff024 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -185,23 +185,13 @@ class GTSAM_EXPORT HybridGaussianConditional private: /// Helper struct for private constructor. - struct ConstructorHelper { - KeyVector frontals, parents; - HybridGaussianFactor::FactorValuePairs pairs; - double negLogConstant; - /// Compute all variables needed for the private constructor below. - ConstructorHelper(const Conditionals &conditionals); - }; + struct ConstructorHelper; /// Private constructor that uses helper struct above. HybridGaussianConditional( const DiscreteKeys &discreteParents, const HybridGaussianConditional::Conditionals &conditionals, - const ConstructorHelper &helper) - : BaseFactor(discreteParents, helper.pairs), - BaseConditional(helper.frontals.size()), - conditionals_(conditionals), - negLogConstant_(helper.negLogConstant) {} + const ConstructorHelper &helper); /// Convert to a DecisionTree of Gaussian factor graphs. GaussianFactorGraphTree asGaussianFactorGraphTree() const; diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 938a79b19..3672eb2e2 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/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 &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 &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 &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; - } - } + : HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {} - // Build the DecisionTree from the factor vector - factorsTree = Factors(discreteKeys, factors); -} - -/* *******************************************************************************/ -HybridGaussianFactor::ConstructorHelper::ConstructorHelper( +HybridGaussianFactor::HybridGaussianFactor( const DiscreteKey &discreteKey, const std::vector &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; - } - } + : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {} - // Build the FactorValuePairs DecisionTree - pairs = FactorValuePairs(discreteKeys, factorPairs); -} - -/* *******************************************************************************/ -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; -} +HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys, + const FactorValuePairs &factors) + : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {} /* *******************************************************************************/ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 46b21b8aa..972ee47ba 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/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 &factors) - : HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {} + const std::vector &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 &factorPairs) - : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {} + const std::vector &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) - : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {} + const FactorValuePairs &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 { - 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 &factors); - - ConstructorHelper(const DiscreteKey &discreteKey, - const std::vector &factorPairs); - - ConstructorHelper(const DiscreteKeys &discreteKeys, - const FactorValuePairs &factorPairs); - }; + struct ConstructorHelper; // Private constructor using ConstructorHelper above. - HybridGaussianFactor(const ConstructorHelper &helper) - : Base(helper.continuousKeys, helper.discreteKeys), - factors_(helper.factorsTree.empty() ? augment(helper.pairs) - : helper.factorsTree) {} + HybridGaussianFactor(const ConstructorHelper &helper); #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 351a7fea4..ac7e943b4 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -25,52 +25,71 @@ namespace gtsam { /* *******************************************************************************/ -static void CopyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor, - KeyVector* continuousKeys) { - if (!factor) return; - if (continuousKeys->empty()) { - *continuousKeys = factor->keys(); - } else if (factor->keys() != *continuousKeys) { - throw std::runtime_error( - "HybridNonlinearFactor: all factors should have the same keys!"); +struct HybridNonlinearFactor::ConstructorHelper { + KeyVector continuousKeys; // Continuous keys extracted from factors + DiscreteKeys discreteKeys; // Discrete keys provided to the constructors + FactorValuePairs factorTree; + + void copyOrCheckContinuousKeys(const NonlinearFactor::shared_ptr& factor) { + if (!factor) return; + 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& factors) + : discreteKeys({discreteKey}) { + std::vector 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& 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& factors) - : discreteKeys({discreteKey}) { - std::vector 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(ConstructorHelper(discreteKey, factors)) {} -/* *******************************************************************************/ -HybridNonlinearFactor::ConstructorHelper::ConstructorHelper( +HybridNonlinearFactor::HybridNonlinearFactor( const DiscreteKey& discreteKey, const std::vector& pairs) - : discreteKeys({discreteKey}) { - // Extract continuous keys from the first non-null factor - for (const auto& pair : pairs) { - CopyOrCheckContinuousKeys(pair.first, &continuousKeys); - } - factorTree = FactorValuePairs({discreteKey}, pairs); -} + : HybridNonlinearFactor(ConstructorHelper(discreteKey, pairs)) {} -/* *******************************************************************************/ -HybridNonlinearFactor::ConstructorHelper::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, &continuousKeys); - }); -} +HybridNonlinearFactor::HybridNonlinearFactor(const DiscreteKeys& discreteKeys, + const FactorValuePairs& factors) + : HybridNonlinearFactor(ConstructorHelper(discreteKeys, factors)) {} /* *******************************************************************************/ AlgebraicDecisionTree HybridNonlinearFactor::errorTree( diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 161a7b357..a3b6ad4d9 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/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, - const std::vector& factors) - : HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {} + HybridNonlinearFactor( + const DiscreteKey& discreteKey, + const std::vector& 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& pairs) - : HybridNonlinearFactor(ConstructorHelper(discreteKey, pairs)) {} + const std::vector& 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) - : HybridNonlinearFactor(ConstructorHelper(discreteKeys, factors)) {} + const FactorValuePairs& 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 { - KeyVector continuousKeys; // Continuous keys extracted from factors - DiscreteKeys discreteKeys; // Discrete keys provided to the constructors - FactorValuePairs factorTree; - - ConstructorHelper(const DiscreteKey& discreteKey, - const std::vector& factors); - - ConstructorHelper(const DiscreteKey& discreteKey, - const std::vector& factorPairs); - - ConstructorHelper(const DiscreteKeys& discreteKeys, - const FactorValuePairs& factorPairs); - }; + struct ConstructorHelper; // Private constructor using ConstructorHelper above. - HybridNonlinearFactor(const ConstructorHelper& helper) - : Base(helper.continuousKeys, helper.discreteKeys), - factors_(helper.factorTree) {} + HybridNonlinearFactor(const ConstructorHelper& helper); }; // traits