diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index f5a7bcdfef..a395e17018 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -29,8 +29,7 @@ std::set HybridFactorGraph::discreteKeys() const { for (const DiscreteKey& key : p->discreteKeys()) { keys.insert(key); } - } - if (auto p = std::dynamic_pointer_cast(factor)) { + } else if (auto p = std::dynamic_pointer_cast(factor)) { for (const DiscreteKey& key : p->discreteKeys()) { keys.insert(key); } diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index ef38237f2b..6bd859c1da 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -27,41 +27,59 @@ #include #include +#include + namespace gtsam { -HybridGaussianFactor::FactorValuePairs GetFactorValuePairs( - const HybridGaussianConditional::Conditionals &conditionals) { - auto func = [](const GaussianConditional::shared_ptr &conditional) - -> GaussianFactorValuePair { - double value = 0.0; - // Check if conditional is pruned - if (conditional) { - // Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|)) - value = conditional->negLogConstant(); +/* *******************************************************************************/ +struct HybridGaussianConditional::ConstructorHelper { + std::optional nrFrontals; + HybridGaussianFactor::FactorValuePairs pairs; + double minNegLogConstant; + + /// Compute all variables needed for the private constructor below. + ConstructorHelper(const Conditionals &conditionals) + : minNegLogConstant(std::numeric_limits::infinity()) { + auto func = [this](const GaussianConditional::shared_ptr &c) + -> GaussianFactorValuePair { + double value = 0.0; + if (c) { + if (!nrFrontals.has_value()) { + nrFrontals = c->nrFrontals(); + } + value = c->negLogConstant(); + minNegLogConstant = std::min(minNegLogConstant, value); + } + return {std::dynamic_pointer_cast(c), value}; + }; + pairs = HybridGaussianFactor::FactorValuePairs(conditionals, func); + if (!nrFrontals.has_value()) { + throw std::runtime_error( + "HybridGaussianConditional: need at least one frontal variable."); } - return {std::dynamic_pointer_cast(conditional), value}; - }; - return HybridGaussianFactor::FactorValuePairs(conditionals, func); -} + } +}; + +/* *******************************************************************************/ +HybridGaussianConditional::HybridGaussianConditional( + const DiscreteKeys &discreteParents, + const HybridGaussianConditional::Conditionals &conditionals, + const ConstructorHelper &helper) + : BaseFactor(discreteParents, helper.pairs), + BaseConditional(*helper.nrFrontals), + conditionals_(conditionals), + negLogConstant_(helper.minNegLogConstant) {} HybridGaussianConditional::HybridGaussianConditional( - const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const HybridGaussianConditional::Conditionals &conditionals) - : BaseFactor(CollectKeys(continuousFrontals, continuousParents), - discreteParents, GetFactorValuePairs(conditionals)), - BaseConditional(continuousFrontals.size()), - conditionals_(conditionals) { - // Calculate negLogConstant_ as the minimum of the negative-log normalizers of - // the conditionals, by visiting the decision tree: - negLogConstant_ = std::numeric_limits::infinity(); - conditionals_.visit( - [this](const GaussianConditional::shared_ptr &conditional) { - if (conditional) { - this->negLogConstant_ = - std::min(this->negLogConstant_, conditional->negLogConstant()); - } - }); -} + : HybridGaussianConditional(discreteParents, conditionals, + ConstructorHelper(conditionals)) {} + +HybridGaussianConditional::HybridGaussianConditional( + const DiscreteKey &discreteParent, + const std::vector &conditionals) + : HybridGaussianConditional(DiscreteKeys{discreteParent}, + Conditionals({discreteParent}, conditionals)) {} /* *******************************************************************************/ const HybridGaussianConditional::Conditionals & @@ -69,15 +87,6 @@ HybridGaussianConditional::conditionals() const { return conditionals_; } -/* *******************************************************************************/ -HybridGaussianConditional::HybridGaussianConditional( - const KeyVector &continuousFrontals, const KeyVector &continuousParents, - const DiscreteKey &discreteParent, - const std::vector &conditionals) - : HybridGaussianConditional(continuousFrontals, continuousParents, - DiscreteKeys{discreteParent}, - Conditionals({discreteParent}, conditionals)) {} - /* *******************************************************************************/ GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree() const { @@ -222,8 +231,8 @@ std::shared_ptr HybridGaussianConditional::likelihood( return {likelihood_m, Cgm_Kgcm}; } }); - return std::make_shared( - continuousParentKeys, discreteParentKeys, likelihoods); + return std::make_shared(discreteParentKeys, + likelihoods); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 9c70cf6cb9..e0bf66e529 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -64,27 +64,11 @@ class GTSAM_EXPORT HybridGaussianConditional private: Conditionals conditionals_; ///< a decision tree of Gaussian conditionals. + ///< Negative-log of the normalization constant (log(\sqrt(|2πΣ|))). ///< Take advantage of the neg-log space so everything is a minimization double negLogConstant_; - /** - * @brief Convert a HybridGaussianConditional of conditionals into - * a DecisionTree of Gaussian factor graphs. - */ - GaussianFactorGraphTree asGaussianFactorGraphTree() const; - - /** - * @brief Helper function to get the pruner functor. - * - * @param discreteProbs The pruned discrete probabilities. - * @return std::function &, const GaussianConditional::shared_ptr &)> - */ - std::function &, const GaussianConditional::shared_ptr &)> - prunerFunc(const DecisionTreeFactor &discreteProbs); - public: /// @name Constructors /// @{ @@ -93,37 +77,28 @@ class GTSAM_EXPORT HybridGaussianConditional HybridGaussianConditional() = default; /** - * @brief Construct a new HybridGaussianConditional object. + * @brief Construct from one discrete key and vector of conditionals. * - * @param continuousFrontals the continuous frontals. - * @param continuousParents the continuous parents. - * @param discreteParents the discrete parents. Will be placed last. - * @param conditionals a decision tree of GaussianConditionals. The number of - * conditionals should be C^(number of discrete parents), where C is the - * cardinality of the DiscreteKeys in discreteParents, since the - * discreteParents will be used as the labels in the decision tree. - */ - HybridGaussianConditional(const KeyVector &continuousFrontals, - const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, - const Conditionals &conditionals); - - /** - * @brief Make a Hybrid Gaussian Conditional from - * a vector of Gaussian conditionals. - * The DecisionTree-based constructor is preferred over this one. - * - * @param continuousFrontals The continuous frontal variables - * @param continuousParents The continuous parent variables * @param discreteParent Single discrete parent variable * @param conditionals Vector of conditionals with the same size as the * cardinality of the discrete parent. */ HybridGaussianConditional( - const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKey &discreteParent, const std::vector &conditionals); + /** + * @brief Construct from multiple discrete keys and conditional tree. + * + * @param discreteParents the discrete parents. Will be placed last. + * @param conditionals a decision tree of GaussianConditionals. The number of + * conditionals should be C^(number of discrete parents), where C is the + * cardinality of the DiscreteKeys in discreteParents, since the + * discreteParents will be used as the labels in the decision tree. + */ + HybridGaussianConditional(const DiscreteKeys &discreteParents, + const Conditionals &conditionals); + /// @} /// @name Testable /// @{ @@ -207,6 +182,23 @@ class GTSAM_EXPORT HybridGaussianConditional /// @} private: + /// Helper struct for private constructor. + struct ConstructorHelper; + + /// Private constructor that uses helper struct above. + HybridGaussianConditional( + const DiscreteKeys &discreteParents, + const HybridGaussianConditional::Conditionals &conditionals, + const ConstructorHelper &helper); + + /// Convert to a DecisionTree of Gaussian factor graphs. + GaussianFactorGraphTree asGaussianFactorGraphTree() const; + + //// Get the pruner functor from pruned discrete probabilities. + std::function &, const GaussianConditional::shared_ptr &)> + prunerFunc(const DecisionTreeFactor &prunedProbabilities); + /// Check whether `given` has values for all frontal keys. bool allFrontalsGiven(const VectorValues &given) const; diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index d5773590bb..6f33c53284 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -28,21 +29,12 @@ namespace gtsam { -/** - * @brief Helper function to augment the [A|b] matrices in the factor components - * with the additional scalar values. - * This is done by storing the value in - * the `b` vector as an additional row. - * - * @param factors DecisionTree of GaussianFactors and arbitrary scalars. - * Gaussian factor in factors. - * @return HybridGaussianFactor::Factors - */ -static HybridGaussianFactor::Factors augment( - const HybridGaussianFactor::FactorValuePairs &factors) { +/* *******************************************************************************/ +HybridGaussianFactor::Factors HybridGaussianFactor::augment( + const FactorValuePairs &factors) { // Find the minimum value so we can "proselytize" to positive values. // Done because we can't have sqrt of negative numbers. - HybridGaussianFactor::Factors gaussianFactors; + Factors gaussianFactors; AlgebraicDecisionTree valueTree; std::tie(gaussianFactors, valueTree) = unzip(factors); @@ -73,22 +65,88 @@ static HybridGaussianFactor::Factors augment( return std::dynamic_pointer_cast( std::make_shared(gfg)); }; - return HybridGaussianFactor::Factors(factors, update); + return Factors(factors, update); } /* *******************************************************************************/ -HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, +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) + : HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {} + +HybridGaussianFactor::HybridGaussianFactor( + const DiscreteKey &discreteKey, + const std::vector &factorPairs) + : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {} + +HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys, const FactorValuePairs &factors) - : Base(continuousKeys, discreteKeys), factors_(augment(factors)) {} + : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {} /* *******************************************************************************/ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { const This *e = dynamic_cast(&lf); if (e == nullptr) return false; - // This will return false if either factors_ is empty or e->factors_ is empty, - // but not if both are empty or both are not empty: + // This will return false if either factors_ is empty or e->factors_ is + // empty, but not if both are empty or both are not empty: if (factors_.empty() ^ e->factors_.empty()) return false; // Check the base and the factors: diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 817e54e562..f23d065b6e 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -73,14 +73,6 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// Decision tree of Gaussian factors indexed by discrete keys. Factors factors_; - /** - * @brief Helper function to return factors and functional to create a - * DecisionTree of Gaussian Factor Graphs. - * - * @return GaussianFactorGraphTree - */ - GaussianFactorGraphTree asGaussianFactorGraphTree() const; - public: /// @name Constructors /// @{ @@ -93,14 +85,11 @@ class GTSAM_EXPORT HybridGaussianFactor : 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. */ - HybridGaussianFactor(const KeyVector &continuousKeys, - const DiscreteKey &discreteKey, - const std::vector &factors) - : Base(continuousKeys, {discreteKey}), factors_({discreteKey}, factors) {} + HybridGaussianFactor(const DiscreteKey &discreteKey, + const std::vector &factors); /** * @brief Construct a new HybridGaussianFactor on a single discrete key, @@ -108,15 +97,11 @@ class GTSAM_EXPORT HybridGaussianFactor : 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 factors Vector of gaussian factor-scalar pairs, one per mode. + * @param factorPairs Vector of gaussian factor-scalar pairs, one per mode. */ - HybridGaussianFactor(const KeyVector &continuousKeys, - const DiscreteKey &discreteKey, - const std::vector &factors) - : HybridGaussianFactor(continuousKeys, {discreteKey}, - FactorValuePairs({discreteKey}, factors)) {} + HybridGaussianFactor(const DiscreteKey &discreteKey, + const std::vector &factorPairs); /** * @brief Construct a new HybridGaussianFactor on a several discrete keys M, @@ -124,12 +109,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * scalars are provided as a DecisionTree 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 Gaussian factor/scalar pairs. */ - HybridGaussianFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, + HybridGaussianFactor(const DiscreteKeys &discreteKeys, const FactorValuePairs &factors); /// @} @@ -185,11 +168,37 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { } /// @} + protected: + /** + * @brief Helper function to return factors and functional to create a + * DecisionTree of Gaussian Factor Graphs. + * + * @return GaussianFactorGraphTree + */ + GaussianFactorGraphTree asGaussianFactorGraphTree() const; + private: + /** + * @brief Helper function to augment the [A|b] matrices in the factor + * components with the additional scalar values. This is done by storing the + * value in the `b` vector as an additional row. + * + * @param factors DecisionTree of GaussianFactors and arbitrary scalars. + * Gaussian factor in factors. + * @return HybridGaussianFactor::Factors + */ + static Factors augment(const FactorValuePairs &factors); + /// Helper method to compute the error of a component. double potentiallyPrunedComponentError( const sharedFactor &gf, const VectorValues &continuousValues) const; + /// Helper struct to assist private constructor below. + struct ConstructorHelper; + + // Private constructor using ConstructorHelper above. + HybridGaussianFactor(const ConstructorHelper &helper); + #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 82828bb41e..7ab7893cb1 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -346,7 +346,6 @@ static std::shared_ptr createDiscreteFactor( // for conditional constants. static std::shared_ptr createHybridGaussianFactor( const DecisionTree &eliminationResults, - const KeyVector &continuousSeparator, const DiscreteKeys &discreteSeparator) { // Correct for the normalization constant used up by the conditional auto correct = [&](const Result &pair) -> GaussianFactorValuePair { @@ -364,14 +363,12 @@ static std::shared_ptr createHybridGaussianFactor( DecisionTree newFactors(eliminationResults, correct); - return std::make_shared(continuousSeparator, - discreteSeparator, newFactors); + return std::make_shared(discreteSeparator, newFactors); } static std::pair> hybridElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys, - const KeyVector &continuousSeparator, const std::set &discreteSeparatorSet) { // NOTE: since we use the special JunctionTree, // only possibility is continuous conditioned on discrete. @@ -388,13 +385,18 @@ hybridElimination(const HybridGaussianFactorGraph &factors, factorGraphTree = removeEmpty(factorGraphTree); // This is the elimination method on the leaf nodes + bool someContinuousLeft = false; auto eliminate = [&](const GaussianFactorGraph &graph) -> Result { if (graph.empty()) { return {nullptr, nullptr}; } + // Expensive elimination of product factor. auto result = EliminatePreferCholesky(graph, frontalKeys); + // Record whether there any continuous variables left + someContinuousLeft |= !result.second->empty(); + return result; }; @@ -405,16 +407,15 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // error for each discrete choice. Otherwise, create a HybridGaussianFactor // on the separator, taking care to correct for conditional constants. auto newFactor = - continuousSeparator.empty() - ? createDiscreteFactor(eliminationResults, discreteSeparator) - : createHybridGaussianFactor(eliminationResults, continuousSeparator, - discreteSeparator); + someContinuousLeft + ? createHybridGaussianFactor(eliminationResults, discreteSeparator) + : createDiscreteFactor(eliminationResults, discreteSeparator); // Create the HybridGaussianConditional from the conditionals HybridGaussianConditional::Conditionals conditionals( eliminationResults, [](const Result &pair) { return pair.first; }); auto hybridGaussian = std::make_shared( - frontalKeys, continuousSeparator, discreteSeparator, conditionals); + discreteSeparator, conditionals); return {std::make_shared(hybridGaussian), newFactor}; } @@ -517,22 +518,12 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, // Case 3: We are now in the hybrid land! KeySet frontalKeysSet(frontalKeys.begin(), frontalKeys.end()); - // Find all the keys in the set of continuous keys - // which are not in the frontal keys. This is our continuous separator. - KeyVector continuousSeparator; - auto continuousKeySet = factors.continuousKeySet(); - std::set_difference( - continuousKeySet.begin(), continuousKeySet.end(), - frontalKeysSet.begin(), frontalKeysSet.end(), - std::inserter(continuousSeparator, continuousSeparator.begin())); - - // Similarly for the discrete separator. + // Find all discrete keys. // Since we eliminate all continuous variables first, // the discrete separator will be *all* the discrete keys. std::set discreteSeparator = factors.discreteKeys(); - return hybridElimination(factors, frontalKeys, continuousSeparator, - discreteSeparator); + return hybridElimination(factors, frontalKeys, discreteSeparator); } } diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index f5be6ded87..7b63c0dff3 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -17,56 +17,78 @@ */ #include +#include + +#include namespace gtsam { /* *******************************************************************************/ -static void checkKeys(const KeyVector& continuousKeys, - const std::vector& 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())); +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!"); + } } - 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!"); + 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::HybridNonlinearFactor(const ConstructorHelper& helper) + : Base(helper.continuousKeys, helper.discreteKeys), + factors_(helper.factorTree) {} + HybridNonlinearFactor::HybridNonlinearFactor( - const KeyVector& continuousKeys, const DiscreteKey& discreteKey, + const DiscreteKey& discreteKey, const std::vector& factors) - : Base(continuousKeys, {discreteKey}) { - std::vector pairs; - for (auto&& f : factors) { - pairs.emplace_back(f, 0.0); - } - checkKeys(continuousKeys, pairs); - factors_ = FactorValuePairs({discreteKey}, pairs); -} + : HybridNonlinearFactor(ConstructorHelper(discreteKey, factors)) {} -/* *******************************************************************************/ HybridNonlinearFactor::HybridNonlinearFactor( - const KeyVector& continuousKeys, const DiscreteKey& discreteKey, + const DiscreteKey& discreteKey, const std::vector& pairs) - : Base(continuousKeys, {discreteKey}) { - KeySet continuous_keys_set(continuousKeys.begin(), continuousKeys.end()); - checkKeys(continuousKeys, pairs); - factors_ = FactorValuePairs({discreteKey}, pairs); -} + : HybridNonlinearFactor(ConstructorHelper(discreteKey, pairs)) {} -/* *******************************************************************************/ -HybridNonlinearFactor::HybridNonlinearFactor(const KeyVector& continuousKeys, - const DiscreteKeys& discreteKeys, +HybridNonlinearFactor::HybridNonlinearFactor(const DiscreteKeys& discreteKeys, const FactorValuePairs& factors) - : Base(continuousKeys, discreteKeys), factors_(factors) {} + : HybridNonlinearFactor(ConstructorHelper(discreteKeys, factors)) {} /* *******************************************************************************/ AlgebraicDecisionTree HybridNonlinearFactor::errorTree( @@ -169,7 +191,7 @@ std::shared_ptr HybridNonlinearFactor::linearize( DecisionTree> linearized_factors(factors_, linearizeDT); - return std::make_shared(continuousKeys_, discreteKeys_, + return std::make_shared(discreteKeys_, linearized_factors); } diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 7843afc836..a3b6ad4d94 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -90,12 +90,11 @@ 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( - const KeyVector& continuousKeys, const DiscreteKey& discreteKey, + const DiscreteKey& discreteKey, const std::vector& factors); /** @@ -104,12 +103,10 @@ 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, - const DiscreteKey& discreteKey, + HybridNonlinearFactor(const DiscreteKey& discreteKey, const std::vector& pairs); /** @@ -118,13 +115,12 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { * scalars are provided as a DecisionTree 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, + HybridNonlinearFactor(const DiscreteKeys& discreteKeys, const FactorValuePairs& factors); + /** * @brief Compute error of the HybridNonlinearFactor as a tree. * @@ -181,6 +177,13 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { /// Linearize all the continuous factors to get a HybridGaussianFactor. std::shared_ptr linearize( const Values& continuousValues) const; + + private: + /// Helper struct to assist private constructor below. + struct ConstructorHelper; + + // Private constructor using ConstructorHelper above. + HybridNonlinearFactor(const ConstructorHelper& helper); }; // traits diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index d1b8fbf6d1..9ed96d1154 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -75,10 +75,12 @@ virtual class HybridConditional { #include class HybridGaussianFactor : gtsam::HybridFactor { HybridGaussianFactor( - const gtsam::KeyVector& continuousKeys, + const gtsam::DiscreteKey& discreteKey, + const std::vector& factors); + HybridGaussianFactor( const gtsam::DiscreteKey& discreteKey, const std::vector>& - factorsList); + factorPairs); void print(string s = "HybridGaussianFactor\n", const gtsam::KeyFormatter& keyFormatter = @@ -88,13 +90,9 @@ class HybridGaussianFactor : gtsam::HybridFactor { #include class HybridGaussianConditional : gtsam::HybridFactor { HybridGaussianConditional( - const gtsam::KeyVector& continuousFrontals, - const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, const gtsam::HybridGaussianConditional::Conditionals& conditionals); HybridGaussianConditional( - const gtsam::KeyVector& continuousFrontals, - const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKey& discreteParent, const std::vector& conditionals); @@ -246,15 +244,15 @@ class HybridNonlinearFactorGraph { #include class HybridNonlinearFactor : gtsam::HybridFactor { HybridNonlinearFactor( - const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey, + const gtsam::DiscreteKey& discreteKey, const std::vector& factors); HybridNonlinearFactor( - const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey, + const gtsam::DiscreteKey& discreteKey, const std::vector>& factors); HybridNonlinearFactor( - const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, + const gtsam::DiscreteKeys& discreteKeys, const gtsam::DecisionTree< gtsam::Key, std::pair>& factors); diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 8836d04c88..23a102fd5b 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -65,7 +65,7 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Z_3x1)); components.emplace_back( new JacobianFactor(x(t), I_3x3, x(t + 1), I_3x3, Vector3::Ones())); - hfg.add(HybridGaussianFactor({x(t), x(t + 1)}, {m(t), 2}, components)); + hfg.add(HybridGaussianFactor({m(t), 2}, components)); if (t > 1) { hfg.add(DecisionTreeFactor({{m(t - 1), 2}, {m(t), 2}}, "0 1 1 3")); @@ -159,9 +159,8 @@ struct Switching { // Add "motion models". for (size_t k = 0; k < K - 1; k++) { - KeyVector keys = {X(k), X(k + 1)}; auto motion_models = motionModels(k, between_sigma); - nonlinearFactorGraph.emplace_shared(keys, modes[k], + nonlinearFactorGraph.emplace_shared(modes[k], motion_models); } diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h index 09905bf797..f2ff7dde2c 100644 --- a/gtsam/hybrid/tests/TinyHybridExample.h +++ b/gtsam/hybrid/tests/TinyHybridExample.h @@ -46,8 +46,7 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1, std::vector conditionals{ GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5), GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)}; - bayesNet.emplace_shared( - KeyVector{Z(i)}, KeyVector{X(0)}, mode_i, conditionals); + bayesNet.emplace_shared(mode_i, conditionals); } // Create prior on X(0). diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index be4ba2ff4d..cde7e4063b 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -43,9 +43,6 @@ const DiscreteValues m1Assignment{{M(0), 1}}; DiscreteConditional::shared_ptr mixing = std::make_shared(m, "60/40"); -// define Continuous keys -const KeyVector continuousKeys{Z(0)}; - /** * Create a simple Gaussian Mixture Model represented as p(z|m)P(m) * where m is a discrete variable and z is a continuous variable. @@ -61,8 +58,7 @@ HybridBayesNet GaussianMixtureModel(double mu0, double mu1, double sigma0, model0), c1 = std::make_shared(Z(0), Vector1(mu1), I_1x1, model1); - hbn.emplace_shared(continuousKeys, KeyVector{}, m, - std::vector{c0, c1}); + hbn.emplace_shared(m, std::vector{c0, c1}); hbn.push_back(mixing); return hbn; } diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 6e23db91e3..8659100acb 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -108,8 +108,7 @@ TEST(HybridBayesNet, evaluateHybrid) { HybridBayesNet bayesNet; bayesNet.push_back(continuousConditional); bayesNet.emplace_shared( - KeyVector{X(1)}, KeyVector{}, Asia, - std::vector{conditional0, conditional1}); + Asia, std::vector{conditional0, conditional1}); bayesNet.emplace_shared(Asia, "99/1"); // Create values at which to evaluate. @@ -169,8 +168,7 @@ TEST(HybridBayesNet, Error) { X(1), Vector1::Constant(2), I_1x1, model1); auto gm = std::make_shared( - KeyVector{X(1)}, KeyVector{}, Asia, - std::vector{conditional0, conditional1}); + Asia, std::vector{conditional0, conditional1}); // Create hybrid Bayes net. HybridBayesNet bayesNet; bayesNet.push_back(continuousConditional); @@ -390,7 +388,7 @@ TEST(HybridBayesNet, Sampling) { auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); nfg.emplace_shared( - KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2), + DiscreteKey(M(0), 2), std::vector{zero_motion, one_motion}); DiscreteKey mode(M(0), 2); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 4899205dfc..1b0286084d 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -437,8 +437,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { std::make_shared>(X(0), X(1), 1, noise_model); std::vector components = {zero_motion, one_motion}; - nfg.emplace_shared(KeyVector{X(0), X(1)}, m, - components); + nfg.emplace_shared(m, components); return nfg; } @@ -566,8 +565,8 @@ std::shared_ptr mixedVarianceFactor( [](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair { return {gf, 0.0}; }); - return std::make_shared( - gmf->continuousKeys(), gmf->discreteKeys(), updated_pairs); + return std::make_shared(gmf->discreteKeys(), + updated_pairs); } /****************************************************************************/ @@ -591,9 +590,7 @@ TEST(HybridEstimation, ModeSelection) { X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight)); std::vector components = {model0, model1}; - KeyVector keys = {X(0), X(1)}; - DiscreteKey modes(M(0), 2); - HybridNonlinearFactor mf(keys, modes, components); + HybridNonlinearFactor mf({M(0), 2}, components); initial.insert(X(0), 0.0); initial.insert(X(1), 0.0); @@ -623,10 +620,7 @@ TEST(HybridEstimation, ModeSelection) { Z_1x1, noise_loose), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1), Z_1x1, noise_tight)}; - bn.emplace_shared( - KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode}, - HybridGaussianConditional::Conditionals(DiscreteKeys{mode}, - conditionals)); + bn.emplace_shared(mode, conditionals); VectorValues vv; vv.insert(Z(0), Z_1x1); @@ -658,10 +652,7 @@ TEST(HybridEstimation, ModeSelection2) { Z_3x1, noise_loose), GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1), Z_3x1, noise_tight)}; - bn.emplace_shared( - KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode}, - HybridGaussianConditional::Conditionals(DiscreteKeys{mode}, - conditionals)); + bn.emplace_shared(mode, conditionals); VectorValues vv; vv.insert(Z(0), Z_3x1); @@ -687,9 +678,7 @@ TEST(HybridEstimation, ModeSelection2) { X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight)); std::vector components = {model0, model1}; - KeyVector keys = {X(0), X(1)}; - DiscreteKey modes(M(0), 2); - HybridNonlinearFactor mf(keys, modes, components); + HybridNonlinearFactor mf({M(0), 2}, components); initial.insert(X(0), Z_3x1); initial.insert(X(1), Z_3x1); diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index e4d207af39..c6020de850 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -55,7 +55,7 @@ TEST(HybridFactorGraph, Keys) { std::vector components{ std::make_shared(X(1), I_3x3, Z_3x1), std::make_shared(X(1), I_3x3, Vector3::Ones())}; - hfg.add(HybridGaussianFactor({X(1)}, m1, components)); + hfg.add(HybridGaussianFactor(m1, components)); KeySet expected_continuous{X(0), X(1)}; EXPECT( diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index adcf6c70fd..02163df9e4 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -52,8 +52,7 @@ const std::vector conditionals{ commonSigma), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), commonSigma)}; -const HybridGaussianConditional hybrid_conditional({Z(0)}, {X(0)}, mode, - conditionals); +const HybridGaussianConditional hybrid_conditional(mode, conditionals); } // namespace equal_constants /* ************************************************************************* */ @@ -158,8 +157,7 @@ const std::vector conditionals{ 0.5), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), 3.0)}; -const HybridGaussianConditional hybrid_conditional({Z(0)}, {X(0)}, mode, - conditionals); +const HybridGaussianConditional hybrid_conditional(mode, conditionals); } // namespace mode_dependent_constants /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 55c3bccfb0..080185d88d 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -70,14 +70,14 @@ auto f11 = std::make_shared(X(1), A1, X(2), A2, b); // Test simple to complex constructors... TEST(HybridGaussianFactor, ConstructorVariants) { using namespace test_constructor; - HybridGaussianFactor fromFactors({X(1), X(2)}, m1, {f10, f11}); + HybridGaussianFactor fromFactors(m1, {f10, f11}); std::vector pairs{{f10, 0.0}, {f11, 0.0}}; - HybridGaussianFactor fromPairs({X(1), X(2)}, m1, pairs); + HybridGaussianFactor fromPairs(m1, pairs); assert_equal(fromFactors, fromPairs); HybridGaussianFactor::FactorValuePairs decisionTree({m1}, pairs); - HybridGaussianFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree); + HybridGaussianFactor fromDecisionTree({m1}, decisionTree); assert_equal(fromDecisionTree, fromPairs); } @@ -95,13 +95,12 @@ TEST(HybridGaussianFactor, Sum) { // TODO(Frank): why specify keys at all? And: keys in factor should be *all* // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey? // Design review! - HybridGaussianFactor hybridFactorA({X(1), X(2)}, m1, {f10, f11}); - HybridGaussianFactor hybridFactorB({X(1), X(3)}, m2, {f20, f21, f22}); + HybridGaussianFactor hybridFactorA(m1, {f10, f11}); + HybridGaussianFactor hybridFactorB(m2, {f20, f21, f22}); - // Check that number of keys is 3 + // Check the number of keys matches what we expect EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size()); - - // Check that number of discrete keys is 1 + EXPECT_LONGS_EQUAL(2, hybridFactorA.continuousKeys().size()); EXPECT_LONGS_EQUAL(1, hybridFactorA.discreteKeys().size()); // Create sum of two hybrid factors: it will be a decision tree now on both @@ -122,7 +121,7 @@ TEST(HybridGaussianFactor, Sum) { /* ************************************************************************* */ TEST(HybridGaussianFactor, Printing) { using namespace test_constructor; - HybridGaussianFactor hybridFactor({X(1), X(2)}, m1, {f10, f11}); + HybridGaussianFactor hybridFactor(m1, {f10, f11}); std::string expected = R"(HybridGaussianFactor @@ -159,17 +158,13 @@ Hybrid [x1 x2; 1]{ /* ************************************************************************* */ TEST(HybridGaussianFactor, HybridGaussianConditional) { - KeyVector keys; - keys.push_back(X(0)); - keys.push_back(X(1)); - DiscreteKeys dKeys; dKeys.emplace_back(M(0), 2); dKeys.emplace_back(M(1), 2); auto gaussians = std::make_shared(); HybridGaussianConditional::Conditionals conditionals(gaussians); - HybridGaussianConditional gm({}, keys, dKeys, conditionals); + HybridGaussianConditional gm(dKeys, conditionals); EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size()); } @@ -189,7 +184,7 @@ TEST(HybridGaussianFactor, Error) { auto f0 = std::make_shared(X(1), A01, X(2), A02, b); auto f1 = std::make_shared(X(1), A11, X(2), A12, b); - HybridGaussianFactor hybridFactor({X(1), X(2)}, m1, {f0, f1}); + HybridGaussianFactor hybridFactor(m1, {f0, f1}); VectorValues continuousValues; continuousValues.insert(X(1), Vector2(0, 0)); @@ -234,9 +229,8 @@ static HybridGaussianConditional::shared_ptr CreateHybridMotionModel( -I_1x1, model1); DiscreteKeys discreteParents{m1}; return std::make_shared( - KeyVector{X(1)}, KeyVector{X(0)}, discreteParents, - HybridGaussianConditional::Conditionals(discreteParents, - std::vector{c0, c1})); + discreteParents, HybridGaussianConditional::Conditionals( + discreteParents, std::vector{c0, c1})); } /// Create two state Bayes network with 1 or two measurement models @@ -595,7 +589,7 @@ static HybridGaussianFactorGraph CreateFactorGraph( // the underlying scalar to be log(\sqrt(|2πΣ|)) std::vector factors{{f0, model0->negLogConstant()}, {f1, model1->negLogConstant()}}; - HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors); + HybridGaussianFactor motionFactor(m1, factors); HybridGaussianFactorGraph hfg; hfg.push_back(motionFactor); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index b1c68adf3e..e4acda3871 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -72,13 +72,10 @@ TEST(HybridGaussianFactorGraph, Creation) { // Define a hybrid gaussian conditional P(x0|x1, c0) // and add it to the factor graph. HybridGaussianConditional gm( - {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}), - HybridGaussianConditional::Conditionals( - M(0), - std::make_shared(X(0), Z_3x1, I_3x3, X(1), - I_3x3), - std::make_shared(X(0), Vector3::Ones(), I_3x3, - X(1), I_3x3))); + {M(0), 2}, + {std::make_shared(X(0), Z_3x1, I_3x3, X(1), I_3x3), + std::make_shared(X(0), Vector3::Ones(), I_3x3, X(1), + I_3x3)}); hfg.add(gm); EXPECT_LONGS_EQUAL(2, hfg.size()); @@ -135,7 +132,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { // Add a hybrid gaussian factor ϕ(x1, c1) DiscreteKey m1(M(1), 2); - hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(1)))); + hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); auto result = hfg.eliminateSequential(); @@ -158,7 +155,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { // Add factor between x0 and x1 hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - hfg.add(HybridGaussianFactor({X(1)}, m1, two::components(X(1)))); + hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); // Discrete probability table for c1 hfg.add(DecisionTreeFactor(m1, {2, 8})); @@ -180,7 +177,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - hfg.add(HybridGaussianFactor({X(1)}, {M(1), 2}, two::components(X(1)))); + hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(1)))); hfg.add(DecisionTreeFactor(m1, {2, 8})); // TODO(Varun) Adding extra discrete variable not connected to continuous @@ -207,7 +204,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); // Hybrid factor P(x1|c1) - hfg.add(HybridGaussianFactor({X(1)}, m, two::components(X(1)))); + hfg.add(HybridGaussianFactor(m, two::components(X(1)))); // Prior factor on c1 hfg.add(DecisionTreeFactor(m, {2, 8})); @@ -232,8 +229,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); { - hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, two::components(X(0)))); - hfg.add(HybridGaussianFactor({X(2)}, {M(1), 2}, two::components(X(2)))); + hfg.add(HybridGaussianFactor({M(0), 2}, two::components(X(0)))); + hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(2)))); } hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); @@ -242,8 +239,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); { - hfg.add(HybridGaussianFactor({X(3)}, {M(3), 2}, two::components(X(3)))); - hfg.add(HybridGaussianFactor({X(5)}, {M(2), 2}, two::components(X(5)))); + hfg.add(HybridGaussianFactor({M(3), 2}, two::components(X(3)))); + hfg.add(HybridGaussianFactor({M(2), 2}, two::components(X(5)))); } auto ordering_full = @@ -528,7 +525,7 @@ TEST(HybridGaussianFactorGraph, optimize) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - hfg.add(HybridGaussianFactor({X(1)}, c1, two::components(X(1)))); + hfg.add(HybridGaussianFactor(c1, two::components(X(1)))); auto result = hfg.eliminateSequential(); @@ -654,11 +651,7 @@ TEST(HybridGaussianFactorGraph, ErrorTreeWithConditional) { x0, -I_1x1, model0), c1 = make_shared(f01, Vector1(mu), I_1x1, x1, I_1x1, x0, -I_1x1, model1); - DiscreteKeys discreteParents{m1}; - hbn.emplace_shared( - KeyVector{f01}, KeyVector{x0, x1}, discreteParents, - HybridGaussianConditional::Conditionals(discreteParents, - std::vector{c0, c1})); + hbn.emplace_shared(m1, std::vector{c0, c1}); // Discrete uniform prior. hbn.emplace_shared(m1, "0.5/0.5"); @@ -830,11 +823,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) { X(0), Vector1(14.1421), I_1x1 * 2.82843), conditional1 = std::make_shared( X(0), Vector1(10.1379), I_1x1 * 2.02759); - DiscreteKeys discreteParents{mode}; expectedBayesNet.emplace_shared( - KeyVector{X(0)}, KeyVector{}, discreteParents, - HybridGaussianConditional::Conditionals( - discreteParents, std::vector{conditional0, conditional1})); + mode, std::vector{conditional0, conditional1}); // Add prior on mode. expectedBayesNet.emplace_shared(mode, "74/26"); @@ -860,10 +850,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { std::vector conditionals{ GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 0.5)}; - auto gm = std::make_shared( - KeyVector{Z(0)}, KeyVector{X(0)}, DiscreteKeys{mode}, - HybridGaussianConditional::Conditionals(DiscreteKeys{mode}, - conditionals)); + auto gm = std::make_shared(mode, conditionals); bn.push_back(gm); // Create prior on X(0). @@ -891,9 +878,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { conditional1 = std::make_shared( X(0), Vector1(14.1421), I_1x1 * 2.82843); expectedBayesNet.emplace_shared( - KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, - HybridGaussianConditional::Conditionals( - DiscreteKeys{mode}, std::vector{conditional0, conditional1})); + mode, std::vector{conditional0, conditional1}); // Add prior on mode. expectedBayesNet.emplace_shared(mode, "1/1"); @@ -929,9 +914,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) { conditional1 = std::make_shared( X(0), Vector1(10.274), I_1x1 * 2.0548); expectedBayesNet.emplace_shared( - KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, - HybridGaussianConditional::Conditionals( - DiscreteKeys{mode}, std::vector{conditional0, conditional1})); + mode, std::vector{conditional0, conditional1}); // Add prior on mode. expectedBayesNet.emplace_shared(mode, "23/77"); @@ -980,10 +963,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 0.5), GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 3.0)}; - bn.emplace_shared( - KeyVector{Z(t)}, KeyVector{X(t)}, DiscreteKeys{noise_mode_t}, - HybridGaussianConditional::Conditionals(DiscreteKeys{noise_mode_t}, - conditionals)); + bn.emplace_shared(noise_mode_t, conditionals); // Create prior on discrete mode N(t): bn.emplace_shared(noise_mode_t, "20/80"); @@ -999,10 +979,8 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { 0.2), GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), I_1x1, 0.2)}; - auto gm = std::make_shared( - KeyVector{X(t)}, KeyVector{X(t - 1)}, DiscreteKeys{motion_model_t}, - HybridGaussianConditional::Conditionals(DiscreteKeys{motion_model_t}, - conditionals)); + auto gm = std::make_shared(motion_model_t, + conditionals); bn.push_back(gm); // Create prior on motion model M(t): diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 9ce109647e..280c6cb531 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -414,15 +414,13 @@ TEST(HybridGaussianISAM, NonTrivial) { // Add odometry factor with discrete modes. Pose2 odometry(1.0, 0.0, 0.0); - KeyVector contKeys = {W(0), W(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); std::vector 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( - contKeys, gtsam::DiscreteKey(M(1), 2), components); + fg.emplace_shared(DiscreteKey(M(1), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(0), X(1), Pose2(1.0, 0.0, 0), @@ -454,14 +452,12 @@ TEST(HybridGaussianISAM, NonTrivial) { /*************** Run Round 3 ***************/ // Add odometry factor with discrete modes. - contKeys = {W(1), W(2)}; 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( - contKeys, gtsam::DiscreteKey(M(2), 2), components); + fg.emplace_shared(DiscreteKey(M(2), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(1), X(2), Pose2(1.0, 0.0, 0), @@ -496,14 +492,12 @@ TEST(HybridGaussianISAM, NonTrivial) { /*************** Run Round 4 ***************/ // Add odometry factor with discrete modes. - contKeys = {W(2), W(3)}; 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( - contKeys, gtsam::DiscreteKey(M(3), 2), components); + fg.emplace_shared(DiscreteKey(M(3), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(2), X(3), Pose2(1.0, 0.0, 0), diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp index d949a1e061..2b441ab131 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp @@ -60,14 +60,14 @@ auto f1 = std::make_shared>(X(1), X(2), between1, model); // Test simple to complex constructors... TEST(HybridGaussianFactor, ConstructorVariants) { using namespace test_constructor; - HybridNonlinearFactor fromFactors({X(1), X(2)}, m1, {f0, f1}); + HybridNonlinearFactor fromFactors(m1, {f0, f1}); std::vector pairs{{f0, 0.0}, {f1, 0.0}}; - HybridNonlinearFactor fromPairs({X(1), X(2)}, m1, pairs); + HybridNonlinearFactor fromPairs(m1, pairs); assert_equal(fromFactors, fromPairs); HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs); - HybridNonlinearFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree); + HybridNonlinearFactor fromDecisionTree({m1}, decisionTree); assert_equal(fromDecisionTree, fromPairs); } @@ -75,7 +75,7 @@ TEST(HybridGaussianFactor, ConstructorVariants) { // Test .print() output. TEST(HybridNonlinearFactor, Printing) { using namespace test_constructor; - HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, {f0, f1}); + HybridNonlinearFactor hybridFactor({m1}, {f0, f1}); std::string expected = R"(Hybrid [x1 x2; 1] @@ -101,7 +101,7 @@ static HybridNonlinearFactor getHybridNonlinearFactor() { std::make_shared>(X(1), X(2), between0, model); auto f1 = std::make_shared>(X(1), X(2), between1, model); - return HybridNonlinearFactor({X(1), X(2)}, m1, {f0, f1}); + return HybridNonlinearFactor(m1, {f0, f1}); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 63a41a63a1..f9a546c814 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -117,7 +117,6 @@ TEST(HybridNonlinearFactorGraph, Resize) { /***************************************************************************/ 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 components = { @@ -139,8 +138,7 @@ TEST(HybridGaussianFactorGraph, Resize) { auto discreteFactor = std::make_shared(); hnfg.push_back(discreteFactor); - auto dcFactor = - std::make_shared(contKeys, m1, components); + auto dcFactor = std::make_shared(m1, components); hnfg.push_back(dcFactor); Values linearizationPoint; @@ -156,26 +154,6 @@ TEST(HybridGaussianFactorGraph, Resize) { EXPECT_LONGS_EQUAL(gfg.size(), 0); } -/*************************************************************************** - * Test that the HybridNonlinearFactor reports correctly if the number of - * continuous keys provided do not match the keys in the factors. - */ -TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) { - using namespace test_motion; - - auto nonlinearFactor = std::make_shared>( - X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); - auto discreteFactor = std::make_shared(); - - // Check for exception when number of continuous keys are under-specified. - THROWS_EXCEPTION( - std::make_shared(KeyVector{X(0)}, m1, components)); - - // Check for exception when number of continuous keys are too many. - THROWS_EXCEPTION(std::make_shared( - KeyVector{X(0), X(1), X(2)}, m1, components)); -} - /***************************************************************************** * Test push_back on HFG makes the correct distinction. */ @@ -828,14 +806,12 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) { // Add odometry factor Pose2 odometry(2.0, 0.0, 0.0); - KeyVector contKeys = {X(0), X(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); std::vector motion_models = { std::make_shared(X(0), X(1), Pose2(0, 0, 0), noise_model), std::make_shared(X(0), X(1), odometry, noise_model)}; - fg.emplace_shared( - contKeys, gtsam::DiscreteKey(M(1), 2), motion_models); + fg.emplace_shared(DiscreteKey{M(1), 2}, motion_models); // Add Range-Bearing measurements to from X0 to L0 and X1 to L1. // create a noise model for the landmark measurements @@ -901,7 +877,7 @@ static HybridNonlinearFactorGraph CreateFactorGraph( std::vector factors{{f0, model0->negLogConstant()}, {f1, model1->negLogConstant()}}; - HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors); + HybridNonlinearFactor mixtureFactor(m1, factors); HybridNonlinearFactorGraph hfg; hfg.push_back(mixtureFactor); diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index fe81fc13e5..9e93116fc0 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -433,15 +433,13 @@ TEST(HybridNonlinearISAM, NonTrivial) { /*************** Run Round 2 ***************/ // Add odometry factor with discrete modes. 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(W(0), W(1), Pose2(0, 0, 0), noise_model), moving = std::make_shared(W(0), W(1), odometry, noise_model); std::vector components{moving, still}; - fg.emplace_shared( - contKeys, gtsam::DiscreteKey(M(1), 2), components); + fg.emplace_shared(DiscreteKey(M(1), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(0), X(1), Pose2(1.0, 0.0, 0), @@ -473,14 +471,12 @@ TEST(HybridNonlinearISAM, NonTrivial) { /*************** Run Round 3 ***************/ // Add odometry factor with discrete modes. - contKeys = {W(1), W(2)}; still = std::make_shared(W(1), W(2), Pose2(0, 0, 0), noise_model); moving = std::make_shared(W(1), W(2), odometry, noise_model); components = {moving, still}; - fg.emplace_shared( - contKeys, gtsam::DiscreteKey(M(2), 2), components); + fg.emplace_shared(DiscreteKey(M(2), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(1), X(2), Pose2(1.0, 0.0, 0), @@ -515,14 +511,12 @@ TEST(HybridNonlinearISAM, NonTrivial) { /*************** Run Round 4 ***************/ // Add odometry factor with discrete modes. - contKeys = {W(2), W(3)}; still = std::make_shared(W(2), W(3), Pose2(0, 0, 0), noise_model); moving = std::make_shared(W(2), W(3), odometry, noise_model); components = {moving, still}; - fg.emplace_shared( - contKeys, gtsam::DiscreteKey(M(3), 2), components); + fg.emplace_shared(DiscreteKey(M(3), 2), components); // Add equivalent of ImuFactor fg.emplace_shared>(X(2), X(3), Pose2(1.0, 0.0, 0), diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 4bf90019bd..e09669117e 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -75,7 +75,6 @@ BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet"); /* ****************************************************************************/ // Test HybridGaussianFactor serialization. TEST(HybridSerialization, HybridGaussianFactor) { - KeyVector continuousKeys{X(0)}; DiscreteKey discreteKey{M(0), 2}; auto A = Matrix::Zero(2, 1); @@ -85,7 +84,7 @@ TEST(HybridSerialization, HybridGaussianFactor) { auto f1 = std::make_shared(X(0), A, b1); std::vector factors{f0, f1}; - const HybridGaussianFactor factor(continuousKeys, discreteKey, factors); + const HybridGaussianFactor factor(discreteKey, factors); EXPECT(equalsObj(factor)); EXPECT(equalsXML(factor)); @@ -115,9 +114,7 @@ TEST(HybridSerialization, HybridGaussianConditional) { GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5)); const auto conditional1 = std::make_shared( GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3)); - const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode}, - HybridGaussianConditional::Conditionals( - {mode}, {conditional0, conditional1})); + const HybridGaussianConditional gm(mode, {conditional0, conditional1}); EXPECT(equalsObj(gm)); EXPECT(equalsXML(gm)); diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index bf2b6a0330..57346d4d4b 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -17,7 +17,7 @@ from gtsam.symbol_shorthand import A, X from gtsam.utils.test_case import GtsamTestCase -from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues, +from gtsam import (DiscreteConditional, DiscreteValues, GaussianConditional, HybridBayesNet, HybridGaussianConditional, HybridValues, VectorValues, noiseModel) @@ -48,8 +48,7 @@ def test_evaluate(self): bayesNet = HybridBayesNet() bayesNet.push_back(conditional) bayesNet.push_back( - HybridGaussianConditional([X(1)], [], Asia, - [conditional0, conditional1])) + HybridGaussianConditional(Asia, [conditional0, conditional1])) bayesNet.push_back(DiscreteConditional(Asia, "99/1")) # Create values at which to evaluate. diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 4ec1aec1ed..6d609deb03 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -17,7 +17,7 @@ from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional, +from gtsam import (DiscreteConditional, GaussianConditional, HybridBayesNet, HybridGaussianConditional, HybridGaussianFactor, HybridGaussianFactorGraph, HybridValues, JacobianFactor, noiseModel) @@ -35,7 +35,7 @@ def test_create(self): jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) - gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)]) + gmf = HybridGaussianFactor((C(0), 2), [(jf1, 0), (jf2, 0)]) hfg = HybridGaussianFactorGraph() hfg.push_back(jf1) @@ -60,7 +60,7 @@ def test_optimize(self): jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) - gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)]) + gmf = HybridGaussianFactor((C(0), 2), [(jf1, 0), (jf2, 0)]) hfg = HybridGaussianFactorGraph() hfg.push_back(jf1) @@ -102,8 +102,7 @@ def tiny(num_measurements: int = 1, X(0), [0], sigma=3) bayesNet.push_back( - HybridGaussianConditional([Z(i)], [X(0)], mode, - [conditional0, conditional1])) + HybridGaussianConditional(mode, [conditional0, conditional1])) # Create prior on X(0). prior_on_x0 = GaussianConditional.FromMeanAndStddev( diff --git a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py index 4a1abdcf39..1880c18f27 100644 --- a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py +++ b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py @@ -14,8 +14,6 @@ import unittest -import numpy as np -from gtsam.symbol_shorthand import C, X from gtsam.utils.test_case import GtsamTestCase import gtsam @@ -38,8 +36,9 @@ def test_nonlinear_hybrid(self): noiseModel.Unit.Create(3)), 0.0), (PriorFactorPoint3(1, Point3(1, 2, 1), noiseModel.Unit.Create(3)), 0.0)] - nlfg.push_back(gtsam.HybridNonlinearFactor([1], (10, 2), factors)) - nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3")) + mode = (10, 2) + nlfg.push_back(gtsam.HybridNonlinearFactor(mode, factors)) + nlfg.push_back(gtsam.DecisionTreeFactor(mode, "1 3")) values = gtsam.Values() values.insert_point3(1, Point3(0, 0, 0)) values.insert_point3(2, Point3(2, 3, 1))