diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 42e7021556..4c2739c865 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -55,23 +55,14 @@ HybridGaussianConditional::conditionals() const { return conditionals_; } -/* *******************************************************************************/ -HybridGaussianConditional::HybridGaussianConditional( - KeyVector &&continuousFrontals, KeyVector &&continuousParents, - DiscreteKeys &&discreteParents, - std::vector &&conditionals) - : HybridGaussianConditional(continuousFrontals, continuousParents, - discreteParents, - Conditionals(discreteParents, conditionals)) {} - /* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( const KeyVector &continuousFrontals, const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, + const DiscreteKey &discreteParent, const std::vector &conditionals) : HybridGaussianConditional(continuousFrontals, continuousParents, - discreteParents, - Conditionals(discreteParents, conditionals)) {} + DiscreteKeys{discreteParent}, + Conditionals({discreteParent}, conditionals)) {} /* *******************************************************************************/ // TODO(dellaert): This is copy/paste: HybridGaussianConditional should be @@ -219,23 +210,20 @@ std::shared_ptr HybridGaussianConditional::likelihood( const DiscreteKeys discreteParentKeys = discreteKeys(); const KeyVector continuousParentKeys = continuousParents(); - const HybridGaussianFactor::Factors likelihoods( - conditionals_, [&](const GaussianConditional::shared_ptr &conditional) { + const HybridGaussianFactor::FactorValuePairs likelihoods( + conditionals_, + [&](const GaussianConditional::shared_ptr &conditional) + -> GaussianFactorValuePair { const auto likelihood_m = conditional->likelihood(given); const double Cgm_Kgcm = logConstant_ - conditional->logNormalizationConstant(); if (Cgm_Kgcm == 0.0) { - return likelihood_m; + return {likelihood_m, 0.0}; } else { - // Add a constant factor to the likelihood in case the noise models + // Add a constant to the likelihood in case the noise models // are not all equal. - GaussianFactorGraph gfg; - gfg.push_back(likelihood_m); - Vector c(1); - c << std::sqrt(2.0 * Cgm_Kgcm); - auto constantFactor = std::make_shared(c); - gfg.push_back(constantFactor); - return std::make_shared(gfg); + double c = 2.0 * Cgm_Kgcm; + return {likelihood_m, c}; } }); return std::make_shared( diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index fc315c6f9e..240160744b 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -107,29 +107,18 @@ class GTSAM_EXPORT HybridGaussianConditional const Conditionals &conditionals); /** - * @brief Make a Gaussian Mixture from a list of Gaussian conditionals + * @brief Make a Gaussian Mixture 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 discreteParents Discrete parents variables - * @param conditionals List of conditionals - */ - HybridGaussianConditional( - KeyVector &&continuousFrontals, KeyVector &&continuousParents, - DiscreteKeys &&discreteParents, - std::vector &&conditionals); - - /** - * @brief Make a Gaussian Mixture from a list of Gaussian conditionals - * - * @param continuousFrontals The continuous frontal variables - * @param continuousParents The continuous parent variables - * @param discreteParents Discrete parents variables - * @param conditionals List of conditionals + * @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 DiscreteKeys &discreteParents, + const DiscreteKey &discreteParent, const std::vector &conditionals); /// @} diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index f7207bf81d..cabe6defcc 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -28,11 +28,56 @@ namespace gtsam { +/** + * @brief Helper function to augment the [A|b] matrices in the factor components + * with the normalizer values. + * This is done by storing the normalizer value in + * the `b` vector as an additional row. + * + * @param factors DecisionTree of GaussianFactors and arbitrary scalars. + * Gaussian factor in factors. + * @return HybridGaussianFactor::Factors + */ +HybridGaussianFactor::Factors augment( + const HybridGaussianFactor::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; + AlgebraicDecisionTree valueTree; + std::tie(gaussianFactors, valueTree) = unzip(factors); + + // Normalize + double min_value = valueTree.min(); + AlgebraicDecisionTree values = + valueTree.apply([&min_value](double n) { return n - min_value; }); + + // Finally, update the [A|b] matrices. + auto update = [&values](const Assignment &assignment, + const HybridGaussianFactor::sharedFactor &gf) { + auto jf = std::dynamic_pointer_cast(gf); + if (!jf) return gf; + // If the log_normalizer is 0, do nothing + if (values(assignment) == 0.0) return gf; + + GaussianFactorGraph gfg; + gfg.push_back(jf); + + Vector c(1); + c << std::sqrt(values(assignment)); + auto constantFactor = std::make_shared(c); + + gfg.push_back(constantFactor); + return std::dynamic_pointer_cast( + std::make_shared(gfg)); + }; + return gaussianFactors.apply(update); +} + /* *******************************************************************************/ HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const Factors &factors) - : Base(continuousKeys, discreteKeys), factors_(factors) {} + const FactorValuePairs &factors) + : Base(continuousKeys, discreteKeys), factors_(augment(factors)) {} /* *******************************************************************************/ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 650f7a5a7c..5f9cd925e3 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -33,6 +33,9 @@ class HybridValues; class DiscreteValues; class VectorValues; +/// Alias for pair of GaussianFactor::shared_pointer and a double value. +using GaussianFactorValuePair = std::pair; + /** * @brief Implementation of a discrete conditional mixture factor. * Implements a joint discrete-continuous factor where the discrete variable @@ -52,7 +55,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { using sharedFactor = std::shared_ptr; - /// typedef for Decision Tree of Gaussian factors and log-constant. + /// typedef for Decision Tree of Gaussian factors and arbitrary value. + using FactorValuePairs = DecisionTree; + /// typedef for Decision Tree of Gaussian factors. using Factors = DecisionTree; private: @@ -80,26 +85,26 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * @param continuousKeys A vector of keys representing continuous variables. * @param discreteKeys A vector of keys representing discrete variables and * their cardinalities. - * @param factors The decision tree of Gaussian factors stored - * as the mixture density. + * @param factors The decision tree of Gaussian factors and arbitrary scalars. */ HybridGaussianFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const Factors &factors); + const FactorValuePairs &factors); /** * @brief Construct a new HybridGaussianFactor object using a vector of * GaussianFactor shared pointers. * * @param continuousKeys Vector of keys for continuous factors. - * @param discreteKeys Vector of discrete keys. - * @param factors Vector of gaussian factor shared pointers. + * @param discreteKey The discrete key to index each component. + * @param factors Vector of gaussian factor shared pointers + * and arbitrary scalars. Same size as the cardinality of discreteKey. */ HybridGaussianFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, - const std::vector &factors) - : HybridGaussianFactor(continuousKeys, discreteKeys, - Factors(discreteKeys, factors)) {} + const DiscreteKey &discreteKey, + const std::vector &factors) + : HybridGaussianFactor(continuousKeys, {discreteKey}, + FactorValuePairs({discreteKey}, factors)) {} /// @} /// @name Testable diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 900ec90c78..74091bf956 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -92,13 +92,13 @@ void HybridGaussianFactorGraph::printErrors( // Clear the stringstream ss.str(std::string()); - if (auto gmf = std::dynamic_pointer_cast(factor)) { + if (auto hgf = std::dynamic_pointer_cast(factor)) { if (factor == nullptr) { std::cout << "nullptr" << "\n"; } else { - gmf->operator()(values.discrete())->print(ss.str(), keyFormatter); - std::cout << "error = " << gmf->error(values) << std::endl; + hgf->operator()(values.discrete())->print(ss.str(), keyFormatter); + std::cout << "error = " << factor->error(values) << std::endl; } } else if (auto hc = std::dynamic_pointer_cast(factor)) { if (factor == nullptr) { @@ -348,7 +348,7 @@ static std::shared_ptr createHybridGaussianFactor( const KeyVector &continuousSeparator, const DiscreteKeys &discreteSeparator) { // Correct for the normalization constant used up by the conditional - auto correct = [&](const Result &pair) -> GaussianFactor::shared_ptr { + auto correct = [&](const Result &pair) -> GaussianFactorValuePair { const auto &[conditional, factor] = pair; if (factor) { auto hf = std::dynamic_pointer_cast(factor); @@ -357,10 +357,10 @@ static std::shared_ptr createHybridGaussianFactor( // as per the Hessian definition hf->constantTerm() += 2.0 * conditional->logNormalizationConstant(); } - return factor; + return {factor, 0.0}; }; - DecisionTree newFactors(eliminationResults, - correct); + DecisionTree newFactors(eliminationResults, + correct); return std::make_shared(continuousSeparator, discreteSeparator, newFactors); @@ -597,10 +597,10 @@ GaussianFactorGraph HybridGaussianFactorGraph::operator()( gfg.push_back(gf); } else if (auto gc = std::dynamic_pointer_cast(f)) { gfg.push_back(gf); - } else if (auto gmf = std::dynamic_pointer_cast(f)) { - gfg.push_back((*gmf)(assignment)); - } else if (auto gm = dynamic_pointer_cast(f)) { - gfg.push_back((*gm)(assignment)); + } else if (auto hgf = std::dynamic_pointer_cast(f)) { + gfg.push_back((*hgf)(assignment)); + } else if (auto hgc = dynamic_pointer_cast(f)) { + gfg.push_back((*hgc)(assignment)); } else { continue; } diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index f5d9cd356a..1120fc9480 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -33,6 +33,9 @@ namespace gtsam { +/// Alias for a NonlinearFactor shared pointer and double scalar pair. +using NonlinearFactorValuePair = std::pair; + /** * @brief Implementation of a discrete conditional mixture factor. * @@ -53,9 +56,9 @@ class HybridNonlinearFactor : public HybridFactor { /** * @brief typedef for DecisionTree which has Keys as node labels and - * NonlinearFactor as leaf nodes. + * pairs of NonlinearFactor & an arbitrary scalar as leaf nodes. */ - using Factors = DecisionTree; + using Factors = DecisionTree; private: /// Decision tree of Gaussian factors indexed by discrete keys. @@ -89,32 +92,34 @@ class HybridNonlinearFactor : public HybridFactor { * @tparam FACTOR The type of the factor shared pointers being passed in. * Will be typecast to NonlinearFactor shared pointers. * @param keys Vector of keys for continuous factors. - * @param discreteKeys Vector of discrete keys. - * @param factors Vector of nonlinear factors. + * @param discreteKey The discrete key indexing each component factor. + * @param factors Vector of nonlinear factor and scalar pairs. + * Same size as the cardinality of discreteKey. * @param normalized Flag indicating if the factor error is already * normalized. */ template - HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, - const std::vector>& factors, - bool normalized = false) - : Base(keys, discreteKeys), normalized_(normalized) { - std::vector nonlinear_factors; + HybridNonlinearFactor( + const KeyVector& keys, const DiscreteKey& discreteKey, + const std::vector, double>>& factors, + bool normalized = false) + : Base(keys, {discreteKey}), normalized_(normalized) { + std::vector nonlinear_factors; KeySet continuous_keys_set(keys.begin(), keys.end()); KeySet factor_keys_set; - for (auto&& f : factors) { + for (auto&& [f, val] : factors) { // Insert all factor continuous keys in the continuous keys set. std::copy(f->keys().begin(), f->keys().end(), std::inserter(factor_keys_set, factor_keys_set.end())); if (auto nf = std::dynamic_pointer_cast(f)) { - nonlinear_factors.push_back(nf); + nonlinear_factors.emplace_back(nf, val); } else { throw std::runtime_error( "Factors passed into HybridNonlinearFactor need to be nonlinear!"); } } - factors_ = Factors(discreteKeys, nonlinear_factors); + factors_ = Factors({discreteKey}, nonlinear_factors); if (continuous_keys_set != factor_keys_set) { throw std::runtime_error( @@ -133,9 +138,11 @@ class HybridNonlinearFactor : public HybridFactor { */ AlgebraicDecisionTree errorTree(const Values& continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [continuousValues](const sharedFactor& factor) { - return factor->error(continuousValues); - }; + auto errorFunc = + [continuousValues](const std::pair& f) { + auto [factor, val] = f; + return factor->error(continuousValues) + (0.5 * val); + }; DecisionTree result(factors_, errorFunc); return result; } @@ -150,12 +157,10 @@ class HybridNonlinearFactor : public HybridFactor { double error(const Values& continuousValues, const DiscreteValues& discreteValues) const { // Retrieve the factor corresponding to the assignment in discreteValues. - auto factor = factors_(discreteValues); + auto [factor, val] = factors_(discreteValues); // Compute the error for the selected factor const double factorError = factor->error(continuousValues); - if (normalized_) return factorError; - return factorError + this->nonlinearFactorLogNormalizingConstant( - factor, continuousValues); + return factorError + (0.5 * val); } /** @@ -175,7 +180,7 @@ class HybridNonlinearFactor : public HybridFactor { */ size_t dim() const { const auto assignments = DiscreteValues::CartesianProduct(discreteKeys_); - auto factor = factors_(assignments.at(0)); + auto [factor, val] = factors_(assignments.at(0)); return factor->dim(); } @@ -189,9 +194,11 @@ class HybridNonlinearFactor : public HybridFactor { std::cout << (s.empty() ? "" : s + " "); Base::print("", keyFormatter); std::cout << "\nHybridNonlinearFactor\n"; - auto valueFormatter = [](const sharedFactor& v) { - if (v) { - return "Nonlinear factor on " + std::to_string(v->size()) + " keys"; + auto valueFormatter = [](const std::pair& v) { + auto [factor, val] = v; + if (factor) { + return "Nonlinear factor on " + std::to_string(factor->size()) + + " keys"; } else { return std::string("nullptr"); } @@ -211,8 +218,10 @@ class HybridNonlinearFactor : public HybridFactor { static_cast(other)); // Ensure that this HybridNonlinearFactor and `f` have the same `factors_`. - auto compare = [tol](const sharedFactor& a, const sharedFactor& b) { - return traits::Equals(*a, *b, tol); + auto compare = [tol](const std::pair& a, + const std::pair& b) { + return traits::Equals(*a.first, *b.first, tol) && + (a.second == b.second); }; if (!factors_.equals(f.factors_, compare)) return false; @@ -230,7 +239,7 @@ class HybridNonlinearFactor : public HybridFactor { GaussianFactor::shared_ptr linearize( const Values& continuousValues, const DiscreteValues& discreteValues) const { - auto factor = factors_(discreteValues); + auto factor = factors_(discreteValues).first; return factor->linearize(continuousValues); } @@ -238,12 +247,15 @@ class HybridNonlinearFactor : public HybridFactor { std::shared_ptr linearize( const Values& continuousValues) const { // functional to linearize each factor in the decision tree - auto linearizeDT = [continuousValues](const sharedFactor& factor) { - return factor->linearize(continuousValues); + auto linearizeDT = + [continuousValues](const std::pair& f) + -> GaussianFactorValuePair { + auto [factor, val] = f; + return {factor->linearize(continuousValues), val}; }; - DecisionTree linearized_factors( - factors_, linearizeDT); + DecisionTree> + linearized_factors(factors_, linearizeDT); return std::make_shared( continuousKeys_, discreteKeys_, linearized_factors); diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 686ce60ac2..37bcefe811 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -76,8 +76,9 @@ virtual class HybridConditional { class HybridGaussianFactor : gtsam::HybridFactor { HybridGaussianFactor( const gtsam::KeyVector& continuousKeys, - const gtsam::DiscreteKeys& discreteKeys, - const std::vector& factorsList); + const gtsam::DiscreteKey& discreteKey, + const std::vector>& + factorsList); void print(string s = "HybridGaussianFactor\n", const gtsam::KeyFormatter& keyFormatter = @@ -90,8 +91,12 @@ class HybridGaussianConditional : gtsam::HybridFactor { const gtsam::KeyVector& continuousFrontals, const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, - const std::vector& - conditionalsList); + const gtsam::HybridGaussianConditional::Conditionals& conditionals); + HybridGaussianConditional( + const gtsam::KeyVector& continuousFrontals, + const gtsam::KeyVector& continuousParents, + const gtsam::DiscreteKey& discreteParent, + const std::vector& conditionals); gtsam::HybridGaussianFactor* likelihood( const gtsam::VectorValues& frontals) const; @@ -242,14 +247,14 @@ class HybridNonlinearFactorGraph { class HybridNonlinearFactor : gtsam::HybridFactor { HybridNonlinearFactor( const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, - const gtsam::DecisionTree& factors, + const gtsam::DecisionTree< + gtsam::Key, std::pair>& factors, bool normalized = false); - template - HybridNonlinearFactor(const gtsam::KeyVector& keys, - const gtsam::DiscreteKeys& discreteKeys, - const std::vector& factors, - bool normalized = false); + HybridNonlinearFactor( + const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey, + const std::vector>& factors, + bool normalized = false); double error(const gtsam::Values& continuousValues, const gtsam::DiscreteValues& discreteValues) const; diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index f08d29fd5c..f18c195594 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -57,12 +57,16 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( // keyFunc(1) to keyFunc(n+1) for (size_t t = 1; t < n; t++) { - hfg.add(HybridGaussianFactor( - {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}}, - {std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), - I_3x3, Z_3x1), - std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), - I_3x3, Vector3::Ones())})); + DiscreteKeys dKeys{{dKeyFunc(t), 2}}; + HybridGaussianFactor::FactorValuePairs components( + dKeys, {{std::make_shared(keyFunc(t), I_3x3, + keyFunc(t + 1), I_3x3, Z_3x1), + 0.0}, + {std::make_shared( + keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Vector3::Ones()), + 0.0}}); + hfg.add( + HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)}, dKeys, components)); if (t > 1) { hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}}, @@ -159,12 +163,13 @@ struct Switching { for (size_t k = 0; k < K - 1; k++) { KeyVector keys = {X(k), X(k + 1)}; auto motion_models = motionModels(k, between_sigma); - std::vector components; + std::vector components; for (auto &&f : motion_models) { - components.push_back(std::dynamic_pointer_cast(f)); + components.push_back( + {std::dynamic_pointer_cast(f), 0.0}); } - nonlinearFactorGraph.emplace_shared( - keys, DiscreteKeys{modes[k]}, components); + nonlinearFactorGraph.emplace_shared(keys, modes[k], + components); } // Add measurement factors diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h index 00af283085..b3bb14c8f8 100644 --- a/gtsam/hybrid/tests/TinyHybridExample.h +++ b/gtsam/hybrid/tests/TinyHybridExample.h @@ -43,12 +43,11 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1, // Create Gaussian mixture z_i = x0 + noise for each measurement. for (size_t i = 0; i < num_measurements; i++) { const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode; + 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)}, DiscreteKeys{mode_i}, - std::vector{GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), - Z_1x1, 0.5), - GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), - Z_1x1, 3)}); + KeyVector{Z(i)}, KeyVector{X(0)}, mode_i, conditionals); } // Create prior on X(0). diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index b35468302d..bbe2d3df51 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -108,7 +108,7 @@ TEST(HybridBayesNet, evaluateHybrid) { HybridBayesNet bayesNet; bayesNet.push_back(continuousConditional); bayesNet.emplace_shared( - KeyVector{X(1)}, KeyVector{}, DiscreteKeys{Asia}, + KeyVector{X(1)}, KeyVector{}, Asia, std::vector{conditional0, conditional1}); bayesNet.emplace_shared(Asia, "99/1"); @@ -169,7 +169,7 @@ TEST(HybridBayesNet, Error) { X(1), Vector1::Constant(2), I_1x1, model1); auto gm = std::make_shared( - KeyVector{X(1)}, KeyVector{}, DiscreteKeys{Asia}, + KeyVector{X(1)}, KeyVector{}, Asia, std::vector{conditional0, conditional1}); // Create hybrid Bayes net. HybridBayesNet bayesNet; @@ -383,14 +383,16 @@ TEST(HybridBayesNet, Sampling) { HybridNonlinearFactorGraph nfg; auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0)); + nfg.emplace_shared>(X(0), 0.0, noise_model); + auto zero_motion = std::make_shared>(X(0), X(1), 0, noise_model); auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); - std::vector factors = {zero_motion, one_motion}; - nfg.emplace_shared>(X(0), 0.0, noise_model); nfg.emplace_shared( - KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); + KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2), + std::vector{{zero_motion, 0.0}, + {one_motion, 0.0}}); DiscreteKey mode(M(0), 2); nfg.emplace_shared(mode, "1/1"); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 308daca9aa..ee48ad5d8b 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -435,9 +435,10 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { std::make_shared>(X(0), X(1), 0, noise_model); const auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); - nfg.emplace_shared( - KeyVector{X(0), X(1)}, DiscreteKeys{m}, - std::vector{zero_motion, one_motion}); + std::vector components = {{zero_motion, 0.0}, + {one_motion, 0.0}}; + nfg.emplace_shared(KeyVector{X(0), X(1)}, m, + components); return nfg; } @@ -560,8 +561,13 @@ std::shared_ptr mixedVarianceFactor( } }; auto updated_components = gmf->factors().apply(func); + auto updated_pairs = HybridGaussianFactor::FactorValuePairs( + updated_components, + [](const GaussianFactor::shared_ptr& gf) -> GaussianFactorValuePair { + return {gf, 0.0}; + }); auto updated_gmf = std::make_shared( - gmf->continuousKeys(), gmf->discreteKeys(), updated_components); + gmf->continuousKeys(), gmf->discreteKeys(), updated_pairs); return updated_gmf; } @@ -577,9 +583,6 @@ TEST(HybridEstimation, ModeSelection) { graph.emplace_shared>(X(0), 0.0, measurement_model); graph.emplace_shared>(X(1), 0.0, measurement_model); - DiscreteKeys modes; - modes.emplace_back(M(0), 2); - // The size of the noise model size_t d = 1; double noise_tight = 0.5, noise_loose = 5.0; @@ -588,10 +591,11 @@ TEST(HybridEstimation, ModeSelection) { X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_loose)), model1 = std::make_shared( X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight)); - - std::vector components = {model0, model1}; + std::vector components = {{model0, 0.0}, + {model1, 0.0}}; KeyVector keys = {X(0), X(1)}; + DiscreteKey modes(M(0), 2); HybridNonlinearFactor mf(keys, modes, components); initial.insert(X(0), 0.0); @@ -610,18 +614,22 @@ TEST(HybridEstimation, ModeSelection) { /**************************************************************/ HybridBayesNet bn; - const DiscreteKey mode{M(0), 2}; + const DiscreteKey mode(M(0), 2); bn.push_back( GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(0), Z_1x1, 0.1)); bn.push_back( GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(1), Z_1x1, 0.1)); + + std::vector conditionals{ + GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1), + 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}, - std::vector{GaussianConditional::sharedMeanAndStddev( - Z(0), I_1x1, X(0), -I_1x1, X(1), Z_1x1, noise_loose), - GaussianConditional::sharedMeanAndStddev( - Z(0), I_1x1, X(0), -I_1x1, X(1), Z_1x1, noise_tight)}); + HybridGaussianConditional::Conditionals(DiscreteKeys{mode}, + conditionals)); VectorValues vv; vv.insert(Z(0), Z_1x1); @@ -641,18 +649,22 @@ TEST(HybridEstimation, ModeSelection2) { double noise_tight = 0.5, noise_loose = 5.0; HybridBayesNet bn; - const DiscreteKey mode{M(0), 2}; + const DiscreteKey mode(M(0), 2); bn.push_back( GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(0), Z_3x1, 0.1)); bn.push_back( GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(1), Z_3x1, 0.1)); + + std::vector conditionals{ + GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1), + 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}, - std::vector{GaussianConditional::sharedMeanAndStddev( - Z(0), I_3x3, X(0), -I_3x3, X(1), Z_3x1, noise_loose), - GaussianConditional::sharedMeanAndStddev( - Z(0), I_3x3, X(0), -I_3x3, X(1), Z_3x1, noise_tight)}); + HybridGaussianConditional::Conditionals(DiscreteKeys{mode}, + conditionals)); VectorValues vv; vv.insert(Z(0), Z_3x1); @@ -672,17 +684,15 @@ TEST(HybridEstimation, ModeSelection2) { graph.emplace_shared>(X(0), Z_3x1, measurement_model); graph.emplace_shared>(X(1), Z_3x1, measurement_model); - DiscreteKeys modes; - modes.emplace_back(M(0), 2); - auto model0 = std::make_shared>( X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_loose)), model1 = std::make_shared>( X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight)); - - std::vector components = {model0, model1}; + std::vector components = {{model0, 0.0}, + {model1, 0.0}}; KeyVector keys = {X(0), X(1)}; + DiscreteKey modes(M(0), 2); HybridNonlinearFactor mf(keys, modes, components); initial.insert(X(0), Z_3x1); diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp index bc0ef91d77..8a2e3be3c9 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp @@ -52,9 +52,9 @@ TEST(HybridFactorGraph, Keys) { // Add a gaussian mixture factor ϕ(x1, c1) DiscreteKey m1(M(1), 2); - DecisionTree dt( - M(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt)); KeySet expected_continuous{X(0), X(1)}; diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index 6156fee96d..d011584133 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -52,7 +52,9 @@ const std::vector conditionals{ commonSigma), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), commonSigma)}; -const HybridGaussianConditional mixture({Z(0)}, {X(0)}, {mode}, conditionals); +const HybridGaussianConditional mixture( + {Z(0)}, {X(0)}, {mode}, + HybridGaussianConditional::Conditionals({mode}, conditionals)); } // namespace equal_constants /* ************************************************************************* */ @@ -153,7 +155,9 @@ const std::vector conditionals{ 0.5), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), 3.0)}; -const HybridGaussianConditional mixture({Z(0)}, {X(0)}, {mode}, conditionals); +const HybridGaussianConditional mixture( + {Z(0)}, {X(0)}, {mode}, + HybridGaussianConditional::Conditionals({mode}, conditionals)); } // namespace mode_dependent_constants /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 0b40ecc724..850a1e4d1c 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -71,8 +71,9 @@ TEST(HybridGaussianFactor, Sum) { auto f20 = std::make_shared(X(1), A1, X(3), A3, b); auto f21 = std::make_shared(X(1), A1, X(3), A3, b); auto f22 = std::make_shared(X(1), A1, X(3), A3, b); - std::vector factorsA{f10, f11}; - std::vector factorsB{f20, f21, f22}; + std::vector factorsA{{f10, 0.0}, {f11, 0.0}}; + std::vector factorsB{ + {f20, 0.0}, {f21, 0.0}, {f22, 0.0}}; // 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? @@ -109,7 +110,7 @@ TEST(HybridGaussianFactor, Printing) { auto b = Matrix::Zero(2, 1); auto f10 = std::make_shared(X(1), A1, X(2), A2, b); auto f11 = std::make_shared(X(1), A1, X(2), A2, b); - std::vector factors{f10, f11}; + std::vector factors{{f10, 0.0}, {f11, 0.0}}; HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors); @@ -178,7 +179,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); - std::vector factors{f0, f1}; + std::vector factors{{f0, 0.0}, {f1, 0.0}}; HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors); @@ -232,8 +233,11 @@ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, c1 = make_shared(z, Vector1(mu1), I_1x1, model1); HybridBayesNet hbn; + DiscreteKeys discreteParents{m}; hbn.emplace_shared( - KeyVector{z}, KeyVector{}, DiscreteKeys{m}, std::vector{c0, c1}); + KeyVector{z}, KeyVector{}, discreteParents, + HybridGaussianConditional::Conditionals(discreteParents, + std::vector{c0, c1})); auto mixing = make_shared(m, "50/50"); hbn.push_back(mixing); @@ -407,8 +411,11 @@ static HybridGaussianConditional::shared_ptr CreateHybridMotionModel( -I_1x1, model0), c1 = make_shared(X(1), Vector1(mu1), I_1x1, X(0), -I_1x1, model1); + DiscreteKeys discreteParents{m1}; return std::make_shared( - KeyVector{X(1)}, KeyVector{X(0)}, DiscreteKeys{m1}, std::vector{c0, c1}); + KeyVector{X(1)}, KeyVector{X(0)}, discreteParents, + HybridGaussianConditional::Conditionals(discreteParents, + std::vector{c0, c1})); } /// Create two state Bayes network with 1 or two measurement models @@ -523,7 +530,7 @@ TEST(HybridGaussianFactor, TwoStateModel) { /** * Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1). * - * P(f01|x1,x0,m1) has different means and different covariances. + * P(x1|x0,m1) has different means and different covariances. * * Converting to a factor graph gives us * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1) @@ -613,13 +620,107 @@ TEST(HybridGaussianFactor, TwoStateModel2) { } } +/* ************************************************************************* */ +/** + * Test a model p(z0|x0)p(x1|x0,m1)p(z1|x1)p(m1). + * + * p(x1|x0,m1) has the same means but different covariances. + * + * Converting to a factor graph gives us + * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)p(m1) + * + * If we only have a measurement on z0, then + * the p(m1) should be 0.5/0.5. + * Getting a measurement on z1 gives use more information. + */ +TEST(HybridGaussianFactor, TwoStateModel3) { + using namespace test_two_state_estimation; + + double mu = 1.0; + double sigma0 = 0.5, sigma1 = 2.0; + auto hybridMotionModel = CreateHybridMotionModel(mu, mu, sigma0, sigma1); + + // Start with no measurement on x1, only on x0 + const Vector1 z0(0.5); + VectorValues given; + given.insert(Z(0), z0); + + { + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + + // Check that ratio of Bayes net and factor graph for different modes is + // equal for several values of {x0,x1}. + for (VectorValues vv : + {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, + VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { + vv.insert(given); // add measurements for HBN + HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); + EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), + gfg.error(hv1) / hbn.error(hv1), 1e-9); + } + + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Importance sampling run with 100k samples gives 50.095/49.905 + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); + + // Since no measurement on x1, we a 50/50 probability + auto p_m = bn->at(2)->asDiscrete(); + EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9); + EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9); + } + + { + // Now we add a measurement z1 on x1 + const Vector1 z1(4.0); // favors m==1 + given.insert(Z(1), z1); + + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + + // Check that ratio of Bayes net and factor graph for different modes is + // equal for several values of {x0,x1}. + for (VectorValues vv : + {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, + VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { + vv.insert(given); // add measurements for HBN + HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); + EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), + gfg.error(hv1) / hbn.error(hv1), 1e-9); + } + + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Values taken from an importance sampling run with 100k samples: + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); + DiscreteConditional expected(m1, "51.7762/48.2238"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); + } + + { + // Add a different measurement z1 on x1 that favors m==1 + const Vector1 z1(7.0); + given.insert_or_assign(Z(1), z1); + + HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Values taken from an importance sampling run with 100k samples: + // approximateDiscreteMarginal(hbn, hybridMotionModel, given); + DiscreteConditional expected(m1, "49.0762/50.9238"); + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.005)); + } +} + /* ************************************************************************* */ /** * Same model, P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1), but now with very informative * measurements and vastly different motion model: either stand still or move * far. This yields a very informative posterior. */ -TEST(HybridGaussianFactor, TwoStateModel3) { +TEST(HybridGaussianFactor, TwoStateModel4) { using namespace test_two_state_estimation; double mu0 = 0.0, mu1 = 10.0; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index acd7b280f1..dc88c8b7ee 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -127,9 +127,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { // Add a gaussian mixture factor ϕ(x1, c1) DiscreteKey m1(M(1), 2); - DecisionTree dt( - M(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(1)}, {m1}, dt)); auto result = hfg.eliminateSequential(); @@ -153,9 +153,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { // Add factor between x0 and x1 hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - std::vector factors = { - std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())}; + std::vector factors = { + {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}}; hfg.add(HybridGaussianFactor({X(1)}, {m1}, factors)); // Discrete probability table for c1 @@ -178,10 +178,10 @@ 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}}, - {std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())})); + std::vector factors = { + {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}}; + hfg.add(HybridGaussianFactor({X(1)}, {M(1), 2}, factors)); hfg.add(DecisionTreeFactor(m1, {2, 8})); // TODO(Varun) Adding extra discrete variable not connected to continuous @@ -208,9 +208,9 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); // Decision tree with different modes on x1 - DecisionTree dt( - M(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); // Hybrid factor P(x1|c1) hfg.add(HybridGaussianFactor({X(1)}, {m}, dt)); @@ -238,14 +238,14 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); { - hfg.add(HybridGaussianFactor( - {X(0)}, {{M(0), 2}}, - {std::make_shared(X(0), I_3x3, Z_3x1), - std::make_shared(X(0), I_3x3, Vector3::Ones())})); + std::vector factors = { + {std::make_shared(X(0), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(0), I_3x3, Vector3::Ones()), 0.0}}; + hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, factors)); - DecisionTree dt1( - M(1), std::make_shared(X(2), I_3x3, Z_3x1), - std::make_shared(X(2), I_3x3, Vector3::Ones())); + DecisionTree dt1( + M(1), {std::make_shared(X(2), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(2), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(2)}, {{M(1), 2}}, dt1)); } @@ -256,15 +256,15 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); { - DecisionTree dt( - M(3), std::make_shared(X(3), I_3x3, Z_3x1), - std::make_shared(X(3), I_3x3, Vector3::Ones())); + DecisionTree dt( + M(3), {std::make_shared(X(3), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(3), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(3)}, {{M(3), 2}}, dt)); - DecisionTree dt1( - M(2), std::make_shared(X(5), I_3x3, Z_3x1), - std::make_shared(X(5), I_3x3, Vector3::Ones())); + DecisionTree dt1( + M(2), {std::make_shared(X(5), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(5), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(5)}, {{M(2), 2}}, dt1)); } @@ -552,9 +552,9 @@ 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)); - DecisionTree dt( - C(1), std::make_shared(X(1), I_3x3, Z_3x1), - std::make_shared(X(1), I_3x3, Vector3::Ones())); + DecisionTree dt( + C(1), {std::make_shared(X(1), I_3x3, Z_3x1), 0.0}, + {std::make_shared(X(1), I_3x3, Vector3::Ones()), 0.0}); hfg.add(HybridGaussianFactor({X(1)}, {c1}, dt)); @@ -682,8 +682,11 @@ 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}, DiscreteKeys{m1}, std::vector{c0, c1}); + KeyVector{f01}, KeyVector{x0, x1}, discreteParents, + HybridGaussianConditional::Conditionals(discreteParents, + std::vector{c0, c1})); // Discrete uniform prior. hbn.emplace_shared(m1, "0.5/0.5"); @@ -806,9 +809,11 @@ 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{}, DiscreteKeys{mode}, - std::vector{conditional0, conditional1}); + KeyVector{X(0)}, KeyVector{}, discreteParents, + HybridGaussianConditional::Conditionals( + discreteParents, std::vector{conditional0, conditional1})); // Add prior on mode. expectedBayesNet.emplace_shared(mode, "74/26"); @@ -831,12 +836,13 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { HybridBayesNet bn; // Create Gaussian mixture z_0 = x0 + noise for each measurement. + 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}, - std::vector{ - GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3), - GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, - 0.5)}); + HybridGaussianConditional::Conditionals(DiscreteKeys{mode}, + conditionals)); bn.push_back(gm); // Create prior on X(0). @@ -865,7 +871,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { X(0), Vector1(14.1421), I_1x1 * 2.82843); expectedBayesNet.emplace_shared( KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, - std::vector{conditional0, conditional1}); + HybridGaussianConditional::Conditionals( + DiscreteKeys{mode}, std::vector{conditional0, conditional1})); // Add prior on mode. expectedBayesNet.emplace_shared(mode, "1/1"); @@ -902,7 +909,8 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) { X(0), Vector1(10.274), I_1x1 * 2.0548); expectedBayesNet.emplace_shared( KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, - std::vector{conditional0, conditional1}); + HybridGaussianConditional::Conditionals( + DiscreteKeys{mode}, std::vector{conditional0, conditional1})); // Add prior on mode. expectedBayesNet.emplace_shared(mode, "23/77"); @@ -947,12 +955,14 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { for (size_t t : {0, 1, 2}) { // Create Gaussian mixture on Z(t) conditioned on X(t) and mode N(t): const auto noise_mode_t = DiscreteKey{N(t), 2}; + std::vector conditionals{ + 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}, - std::vector{GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), - Z_1x1, 0.5), - GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), - Z_1x1, 3.0)}); + HybridGaussianConditional::Conditionals(DiscreteKeys{noise_mode_t}, + conditionals)); // Create prior on discrete mode N(t): bn.emplace_shared(noise_mode_t, "20/80"); @@ -962,12 +972,15 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { for (size_t t : {2, 1}) { // Create Gaussian mixture on X(t) conditioned on X(t-1) and mode M(t-1): const auto motion_model_t = DiscreteKey{M(t), 2}; + std::vector conditionals{ + GaussianConditional::sharedMeanAndStddev(X(t), I_1x1, X(t - 1), Z_1x1, + 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}, - std::vector{GaussianConditional::sharedMeanAndStddev( - X(t), I_1x1, X(t - 1), Z_1x1, 0.2), - GaussianConditional::sharedMeanAndStddev( - X(t), I_1x1, X(t - 1), I_1x1, 0.2)}); + HybridGaussianConditional::Conditionals(DiscreteKeys{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 d0a4a79af5..e2a10a783d 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -420,9 +420,10 @@ TEST(HybridGaussianISAM, NonTrivial) { noise_model), moving = std::make_shared(W(0), W(1), odometry, noise_model); - std::vector components = {moving, still}; + std::vector> components = { + {moving, 0.0}, {still, 0.0}}; auto mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + contKeys, gtsam::DiscreteKey(M(1), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor @@ -460,9 +461,9 @@ TEST(HybridGaussianISAM, NonTrivial) { noise_model); moving = std::make_shared(W(1), W(2), odometry, noise_model); - components = {moving, still}; + components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); + contKeys, gtsam::DiscreteKey(M(2), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor @@ -503,9 +504,9 @@ TEST(HybridGaussianISAM, NonTrivial) { noise_model); moving = std::make_shared(W(2), W(3), odometry, noise_model); - components = {moving, still}; + components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); + contKeys, gtsam::DiscreteKey(M(3), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp index 71d5108a05..a4c1b4cc7e 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp @@ -58,7 +58,7 @@ TEST(HybridNonlinearFactor, Printing) { std::make_shared>(X(1), X(2), between0, model); auto f1 = std::make_shared>(X(1), X(2), between1, model); - std::vector factors{f0, f1}; + std::vector factors{{f0, 0.0}, {f1, 0.0}}; HybridNonlinearFactor mixtureFactor({X(1), X(2)}, {m1}, factors); @@ -86,7 +86,7 @@ static HybridNonlinearFactor getHybridNonlinearFactor() { std::make_shared>(X(1), X(2), between0, model); auto f1 = std::make_shared>(X(1), X(2), between1, model); - std::vector factors{f0, f1}; + std::vector factors{{f0, 0.0}, {f1, 0.0}}; return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors); } diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index fd3494bea3..3631ac44ed 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -131,9 +132,10 @@ TEST(HybridGaussianFactorGraph, Resize) { auto still = std::make_shared(X(0), X(1), 0.0, noise_model), moving = std::make_shared(X(0), X(1), 1.0, noise_model); - std::vector components = {still, moving}; + std::vector> components = { + {still, 0.0}, {moving, 0.0}}; auto dcFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + contKeys, gtsam::DiscreteKey(M(1), 2), components); nhfg.push_back(dcFactor); Values linearizationPoint; @@ -162,17 +164,18 @@ TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) { auto still = std::make_shared(X(0), X(1), 0.0, noise_model), moving = std::make_shared(X(0), X(1), 1.0, noise_model); - std::vector components = {still, moving}; + std::vector> components = { + {still, 0.0}, {moving, 0.0}}; // Check for exception when number of continuous keys are under-specified. KeyVector contKeys = {X(0)}; THROWS_EXCEPTION(std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); + contKeys, gtsam::DiscreteKey(M(1), 2), components)); // Check for exception when number of continuous keys are too many. contKeys = {X(0), X(1), X(2)}; THROWS_EXCEPTION(std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); + contKeys, gtsam::DiscreteKey(M(1), 2), components)); } /***************************************************************************** @@ -440,7 +443,7 @@ TEST(HybridFactorGraph, Full_Elimination) { DiscreteFactorGraph discrete_fg; // TODO(Varun) Make this a function of HybridGaussianFactorGraph? - for (auto& factor : (*remainingFactorGraph_partial)) { + for (auto &factor : (*remainingFactorGraph_partial)) { auto df = dynamic_pointer_cast(factor); assert(df); discrete_fg.push_back(df); @@ -801,9 +804,10 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { noise_model), moving = std::make_shared(X(0), X(1), odometry, noise_model); - std::vector motion_models = {still, moving}; + std::vector> motion_models = + {{still, 0.0}, {moving, 0.0}}; fg.emplace_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); + contKeys, gtsam::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 @@ -838,9 +842,174 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { EXPECT_LONGS_EQUAL(1, remainingFactorGraph->size()); } +namespace test_relinearization { +/** + * @brief Create a Factor Graph by directly specifying all + * the factors instead of creating conditionals first. + * This way we can directly provide the likelihoods and + * then perform (re-)linearization. + * + * @param means The means of the GaussianMixtureFactor components. + * @param sigmas The covariances of the GaussianMixtureFactor components. + * @param m1 The discrete key. + * @param x0_measurement A measurement on X0 + * @return HybridGaussianFactorGraph + */ +static HybridNonlinearFactorGraph CreateFactorGraph( + const std::vector &means, const std::vector &sigmas, + DiscreteKey &m1, double x0_measurement) { + auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); + auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + + auto f0 = + std::make_shared>(X(0), X(1), means[0], model0); + auto f1 = + std::make_shared>(X(0), X(1), means[1], model1); + + // Create HybridNonlinearFactor + std::vector factors{ + {f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}}; + + HybridNonlinearFactor mixtureFactor({X(0), X(1)}, {m1}, factors); + + HybridNonlinearFactorGraph hfg; + hfg.push_back(mixtureFactor); + + hfg.push_back(PriorFactor(X(0), x0_measurement, prior_noise)); + + return hfg; +} +} // namespace test_relinearization + +/* ************************************************************************* */ +/** + * @brief Test components with differing means but the same covariances. + * The factor graph is + * *-X1-*-X2 + * | + * M1 + */ +TEST(HybridNonlinearFactorGraph, DifferentMeans) { + using namespace test_relinearization; + + DiscreteKey m1(M(1), 2); + + Values values; + double x0 = 0.0, x1 = 1.75; + values.insert(X(0), x0); + values.insert(X(1), x1); + + std::vector means = {0.0, 2.0}, sigmas = {1e-0, 1e-0}; + + HybridNonlinearFactorGraph hfg = CreateFactorGraph(means, sigmas, m1, x0); + + { + auto bn = hfg.linearize(values)->eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}}, + DiscreteValues{{M(1), 0}}); + + EXPECT(assert_equal(expected, actual)); + + DiscreteValues dv0{{M(1), 0}}; + VectorValues cont0 = bn->optimize(dv0); + double error0 = bn->error(HybridValues(cont0, dv0)); + + // TODO(Varun) Perform importance sampling to estimate error? + + // regression + EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9); + + DiscreteValues dv1{{M(1), 1}}; + VectorValues cont1 = bn->optimize(dv1); + double error1 = bn->error(HybridValues(cont1, dv1)); + EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9); + } + + { + // Add measurement on x1 + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + hfg.push_back(PriorFactor(X(1), means[1], prior_noise)); + + auto bn = hfg.linearize(values)->eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}}, + DiscreteValues{{M(1), 1}}); + + EXPECT(assert_equal(expected, actual)); + + { + DiscreteValues dv{{M(1), 0}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 1}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9); + } + } +} + /* ************************************************************************* */ +/** + * @brief Test components with differing covariances but the same means. + * The factor graph is + * *-X1-*-X2 + * | + * M1 + */ +TEST_DISABLED(HybridNonlinearFactorGraph, DifferentCovariances) { + using namespace test_relinearization; + + DiscreteKey m1(M(1), 2); + + Values values; + double x0 = 1.0, x1 = 1.0; + values.insert(X(0), x0); + values.insert(X(1), x1); + + std::vector means = {0.0, 0.0}, sigmas = {1e2, 1e-2}; + + // Create FG with HybridNonlinearFactor and prior on X1 + HybridNonlinearFactorGraph hfg = CreateFactorGraph(means, sigmas, m1, x0); + // Linearize and eliminate + auto hbn = hfg.linearize(values)->eliminateSequential(); + + VectorValues cv; + cv.insert(X(0), Vector1(0.0)); + cv.insert(X(1), Vector1(0.0)); + + // Check that the error values at the MLE point μ. + AlgebraicDecisionTree errorTree = hbn->errorTree(cv); + + DiscreteValues dv0{{M(1), 0}}; + DiscreteValues dv1{{M(1), 1}}; + + // regression + EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9); + EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9); + + DiscreteConditional expected_m1(m1, "0.5/0.5"); + DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); + + EXPECT(assert_equal(expected_m1, actual_m1)); +} + +/* ************************************************************************* + */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +/* ************************************************************************* + */ diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index f4054c11ab..6932508dfb 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -439,9 +439,10 @@ TEST(HybridNonlinearISAM, NonTrivial) { noise_model), moving = std::make_shared(W(0), W(1), odometry, noise_model); - std::vector components = {moving, still}; + std::vector> components = { + {moving, 0.0}, {still, 0.0}}; auto mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + contKeys, gtsam::DiscreteKey(M(1), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor @@ -479,9 +480,9 @@ TEST(HybridNonlinearISAM, NonTrivial) { noise_model); moving = std::make_shared(W(1), W(2), odometry, noise_model); - components = {moving, still}; + components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); + contKeys, gtsam::DiscreteKey(M(2), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor @@ -522,9 +523,9 @@ TEST(HybridNonlinearISAM, NonTrivial) { noise_model); moving = std::make_shared(W(2), W(3), odometry, noise_model); - components = {moving, still}; + components = {{moving, 0.0}, {still, 0.0}}; mixtureFactor = std::make_shared( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); + contKeys, gtsam::DiscreteKey(M(3), 2), components); fg.push_back(mixtureFactor); // Add equivalent of ImuFactor diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 365dfcc9fc..701b870f1c 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -76,16 +76,16 @@ BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet"); // Test HybridGaussianFactor serialization. TEST(HybridSerialization, HybridGaussianFactor) { KeyVector continuousKeys{X(0)}; - DiscreteKeys discreteKeys{{M(0), 2}}; + DiscreteKey discreteKey{M(0), 2}; auto A = Matrix::Zero(2, 1); auto b0 = Matrix::Zero(2, 1); auto b1 = Matrix::Ones(2, 1); auto f0 = std::make_shared(X(0), A, b0); auto f1 = std::make_shared(X(0), A, b1); - std::vector factors{f0, f1}; + std::vector factors{{f0, 0.0}, {f1, 0.0}}; - const HybridGaussianFactor factor(continuousKeys, discreteKeys, factors); + const HybridGaussianFactor factor(continuousKeys, discreteKey, factors); EXPECT(equalsObj(factor)); EXPECT(equalsXML(factor)); @@ -116,7 +116,8 @@ TEST(HybridSerialization, HybridGaussianConditional) { const auto conditional1 = std::make_shared( GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3)); const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode}, - {conditional0, conditional1}); + HybridGaussianConditional::Conditionals( + {mode}, {conditional0, conditional1})); EXPECT(equalsObj(gm)); EXPECT(equalsXML(gm)); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 7ea8e5a546..a586d119b7 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -707,6 +707,25 @@ const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ } /* ************************************************************************* */ - +} // namespace noiseModel + +/* *******************************************************************************/ +double ComputeLogNormalizer( + const noiseModel::Gaussian::shared_ptr& noise_model) { + // Since noise models are Gaussian, we can get the logDeterminant using + // the same trick as in GaussianConditional + // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1} + // log det(Sigma) = -log(det(R'R)) = -2*log(det(R)) + // Hence, log det(Sigma)) = -2.0 * logDetR() + double logDetR = noise_model->R() + .diagonal() + .unaryExpr([](double x) { return log(x); }) + .sum(); + double logDeterminantSigma = -2.0 * logDetR; + + size_t n = noise_model->dim(); + constexpr double log2pi = 1.8378770664093454835606594728112; + return n * log2pi + logDeterminantSigma; } + } // gtsam diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 489f11e4c2..ffc1a3ebcd 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -751,6 +751,18 @@ namespace gtsam { template<> struct traits : public Testable {}; template<> struct traits : public Testable {}; + /** + * @brief Helper function to compute the log(|2πΣ|) normalizer values + * for a Gaussian noise model. + * We compute this in the log-space for numerical accuracy. + * + * @param noise_model The Gaussian noise model + * whose normalizer we wish to compute. + * @return double + */ + GTSAM_EXPORT double ComputeLogNormalizer( + const noiseModel::Gaussian::shared_ptr& noise_model); + } //\ namespace gtsam diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 725680501d..49874f901c 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -807,6 +807,26 @@ TEST(NoiseModel, NonDiagonalGaussian) } } +TEST(NoiseModel, ComputeLogNormalizer) { + // Very simple 1D noise model, which we can compute by hand. + double sigma = 0.1; + auto noise_model = Isotropic::Sigma(1, sigma); + double actual_value = ComputeLogNormalizer(noise_model); + // Compute log(|2πΣ|) by hand. + // = log(2π) + log(Σ) (since it is 1D) + constexpr double log2pi = 1.8378770664093454835606594728112; + double expected_value = log2pi + log(sigma * sigma); + EXPECT_DOUBLES_EQUAL(expected_value, actual_value, 1e-9); + + // Similar situation in the 3D case + size_t n = 3; + auto noise_model2 = Isotropic::Sigma(n, sigma); + double actual_value2 = ComputeLogNormalizer(noise_model2); + // We multiply by 3 due to the determinant + double expected_value2 = n * (log2pi + log(sigma * sigma)); + EXPECT_DOUBLES_EQUAL(expected_value2, actual_value2, 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index 6e50c2bfe0..2f7dd4a987 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -43,14 +43,12 @@ def test_evaluate(self): # Create the conditionals conditional0 = GaussianConditional(X(1), [5], I_1x1, model0) conditional1 = GaussianConditional(X(1), [2], I_1x1, model1) - discrete_keys = DiscreteKeys() - discrete_keys.push_back(Asia) # Create hybrid Bayes net. bayesNet = HybridBayesNet() bayesNet.push_back(conditional) bayesNet.push_back( - HybridGaussianConditional([X(1)], [], discrete_keys, + HybridGaussianConditional([X(1)], [], Asia, [conditional0, conditional1])) bayesNet.push_back(DiscreteConditional(Asia, "99/1")) diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index f82665c495..5eafc05aed 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -20,7 +20,7 @@ from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional, HybridBayesNet, HybridGaussianConditional, HybridGaussianFactor, HybridGaussianFactorGraph, - HybridValues, JacobianFactor, Ordering, noiseModel) + HybridValues, JacobianFactor, noiseModel) DEBUG_MARGINALS = False @@ -31,13 +31,11 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): def test_create(self): """Test construction of hybrid factor graph.""" model = noiseModel.Unit.Create(3) - dk = DiscreteKeys() - dk.push_back((C(0), 2)) 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)], dk, [jf1, jf2]) + gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)]) hfg = HybridGaussianFactorGraph() hfg.push_back(jf1) @@ -58,13 +56,11 @@ def test_create(self): def test_optimize(self): """Test construction of hybrid factor graph.""" model = noiseModel.Unit.Create(3) - dk = DiscreteKeys() - dk.push_back((C(0), 2)) 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)], dk, [jf1, jf2]) + gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)]) hfg = HybridGaussianFactorGraph() hfg.push_back(jf1) @@ -96,8 +92,6 @@ def tiny(num_measurements: int = 1, # Create Gaussian mixture Z(0) = X(0) + noise for each measurement. I_1x1 = np.eye(1) - keys = DiscreteKeys() - keys.push_back(mode) for i in range(num_measurements): conditional0 = GaussianConditional.FromMeanAndStddev(Z(i), I_1x1, @@ -108,7 +102,7 @@ def tiny(num_measurements: int = 1, X(0), [0], sigma=3) bayesNet.push_back( - HybridGaussianConditional([Z(i)], [X(0)], keys, + HybridGaussianConditional([Z(i)], [X(0)], mode, [conditional0, conditional1])) # Create prior on X(0). diff --git a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py index 3a659c4e87..4a1abdcf39 100644 --- a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py +++ b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py @@ -27,21 +27,18 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): def test_nonlinear_hybrid(self): nlfg = gtsam.HybridNonlinearFactorGraph() - dk = gtsam.DiscreteKeys() - dk.push_back((10, 2)) nlfg.push_back( BetweenFactorPoint3(1, 2, Point3(1, 2, 3), noiseModel.Diagonal.Variances([1, 1, 1]))) nlfg.push_back( PriorFactorPoint3(2, Point3(1, 2, 3), noiseModel.Diagonal.Variances([0.5, 0.5, 0.5]))) - nlfg.push_back( - gtsam.HybridNonlinearFactor([1], dk, [ - PriorFactorPoint3(1, Point3(0, 0, 0), - noiseModel.Unit.Create(3)), - PriorFactorPoint3(1, Point3(1, 2, 1), - noiseModel.Unit.Create(3)) - ])) + + factors = [(PriorFactorPoint3(1, Point3(0, 0, 0), + 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")) values = gtsam.Values() values.insert_point3(1, Point3(0, 0, 0))