diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 888872f81..d9c654570 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace gtsam { @@ -318,8 +319,15 @@ AlgebraicDecisionTree GaussianMixture::logProbability( AlgebraicDecisionTree GaussianMixture::errorTree( const VectorValues &continuousValues) const { auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) { - return conditional->error(continuousValues) + // - logConstant_ - conditional->logNormalizationConstant(); + // Check if valid pointer + if (conditional) { + return conditional->error(continuousValues) + // + logConstant_ - conditional->logNormalizationConstant(); + } else { + // If not valid, pointer, it means this conditional was pruned, + // so we return maximum error. + return std::numeric_limits::max(); + } }; DecisionTree error_tree(conditionals_, errorFunc); return error_tree; @@ -327,10 +335,33 @@ AlgebraicDecisionTree GaussianMixture::errorTree( /* *******************************************************************************/ double GaussianMixture::error(const HybridValues &values) const { + // Check if discrete keys in discrete assignment are + // present in the GaussianMixture + KeyVector dKeys = this->discreteKeys_.indices(); + bool valid_assignment = false; + for (auto &&kv : values.discrete()) { + if (std::find(dKeys.begin(), dKeys.end(), kv.first) != dKeys.end()) { + valid_assignment = true; + break; + } + } + + // The discrete assignment is not valid so we throw an error. + if (!valid_assignment) { + throw std::runtime_error( + "Invalid discrete values in values. Not all discrete keys specified."); + } + // Directly index to get the conditional, no need to build the whole tree. auto conditional = conditionals_(values.discrete()); - return conditional->error(values.continuous()) + // - logConstant_ - conditional->logNormalizationConstant(); + if (conditional) { + return conditional->error(values.continuous()) + // + logConstant_ - conditional->logNormalizationConstant(); + } else { + // If not valid, pointer, it means this conditional was pruned, + // so we return maximum error. + return std::numeric_limits::max(); + } } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index c1ef504f8..41692bb1c 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -67,7 +67,7 @@ class GTSAM_EXPORT GaussianMixture double logConstant_; ///< log of the normalization constant. /** - * @brief Convert a DecisionTree of factors into + * @brief Convert a GaussianMixture of conditionals into * a DecisionTree of Gaussian factor graphs. */ GaussianFactorGraphTree asGaussianFactorGraphTree() const; diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 65ec23ffd..94bc09407 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -54,7 +54,9 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { /* *******************************************************************************/ void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { - HybridFactor::print(s, formatter); + std::cout << (s.empty() ? "" : s + "\n"); + std::cout << "GaussianMixtureFactor" << std::endl; + HybridFactor::print("", formatter); std::cout << "{\n"; if (factors_.empty()) { std::cout << " empty" << std::endl; @@ -117,6 +119,5 @@ double GaussianMixtureFactor::error(const HybridValues &values) const { const sharedFactor gf = factors_(values.discrete()); return gf->error(values.continuous()); } -/* *******************************************************************************/ } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 67d12ddb0..2459af259 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -80,8 +80,8 @@ class GTSAM_EXPORT GaussianMixtureFactor : 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 stored + * as the mixture density. */ GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, @@ -107,9 +107,8 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { bool equals(const HybridFactor &lf, double tol = 1e-9) const override; - void print( - const std::string &s = "GaussianMixtureFactor\n", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; + void print(const std::string &s = "", const KeyFormatter &formatter = + DefaultKeyFormatter) const override; /// @} /// @name Standard API diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index b02967555..1d01baed2 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -220,15 +220,16 @@ GaussianBayesNet HybridBayesNet::choose( /* ************************************************************************* */ HybridValues HybridBayesNet::optimize() const { // Collect all the discrete factors to compute MPE - DiscreteBayesNet discrete_bn; + DiscreteFactorGraph discrete_fg; + for (auto &&conditional : *this) { if (conditional->isDiscrete()) { - discrete_bn.push_back(conditional->asDiscrete()); + discrete_fg.push_back(conditional->asDiscrete()); } } // Solve for the MPE - DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize(); + DiscreteValues mpe = discrete_fg.optimize(); // Given the MPE, compute the optimal continuous values. return HybridValues(optimize(mpe), mpe); diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index afd1c8032..a9c0e53d2 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -13,6 +13,7 @@ * @file HybridFactor.h * @date Mar 11, 2022 * @author Fan Jiang + * @author Varun Agrawal */ #pragma once diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 168c6c15e..5cb985bb8 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -242,6 +242,18 @@ discreteElimination(const HybridGaussianFactorGraph &factors, for (auto &f : factors) { if (auto df = dynamic_pointer_cast(f)) { dfg.push_back(df); + } else if (auto gmf = dynamic_pointer_cast(f)) { + // Case where we have a GaussianMixtureFactor with no continuous keys. + // In this case, compute discrete probabilities. + auto probability = + [&](const GaussianFactor::shared_ptr &factor) -> double { + if (!factor) return 0.0; + return exp(-factor->error(VectorValues())); + }; + dfg.emplace_shared( + gmf->discreteKeys(), + DecisionTree(gmf->factors(), probability)); + } else if (auto orphan = dynamic_pointer_cast(f)) { // Ignore orphaned clique. // TODO(dellaert): is this correct? If so explain here. @@ -279,21 +291,37 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) { using Result = std::pair, GaussianMixtureFactor::sharedFactor>; -// Integrate the probability mass in the last continuous conditional using -// the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean. -// discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) +/** + * Compute the probability q(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m) + * from the residual error at the mean μ. + * The residual error contains no keys, and only + * depends on the discrete separator if present. + */ static std::shared_ptr createDiscreteFactor( const DecisionTree &eliminationResults, const DiscreteKeys &discreteSeparator) { - auto probability = [&](const Result &pair) -> double { + auto logProbability = [&](const Result &pair) -> double { const auto &[conditional, factor] = pair; static const VectorValues kEmpty; // If the factor is not null, it has no keys, just contains the residual. if (!factor) return 1.0; // TODO(dellaert): not loving this. - return exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); + + // Logspace version of: + // exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); + // We take negative of the logNormalizationConstant `log(1/k)` + // to get `log(k)`. + return -factor->error(kEmpty) + (-conditional->logNormalizationConstant()); }; - DecisionTree probabilities(eliminationResults, probability); + AlgebraicDecisionTree logProbabilities( + DecisionTree(eliminationResults, logProbability)); + + // Perform normalization + double max_log = logProbabilities.max(); + AlgebraicDecisionTree probabilities = DecisionTree( + logProbabilities, + [&max_log](const double x) { return exp(x - max_log); }); + probabilities = probabilities.normalize(probabilities.sum()); return std::make_shared(discreteSeparator, probabilities); } diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 9cc7e6bfd..0910d2f40 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -22,9 +22,13 @@ #include #include #include +#include +#include #include #include #include +#include +#include // Include for test suite #include @@ -34,6 +38,7 @@ using namespace gtsam; using noiseModel::Isotropic; using symbol_shorthand::M; using symbol_shorthand::X; +using symbol_shorthand::Z; /* ************************************************************************* */ // Check iterators of empty mixture. @@ -56,7 +61,6 @@ TEST(GaussianMixtureFactor, Sum) { auto b = Matrix::Zero(2, 1); Vector2 sigmas; sigmas << 1, 2; - auto model = noiseModel::Diagonal::Sigmas(sigmas, true); auto f10 = std::make_shared(X(1), A1, X(2), A2, b); auto f11 = std::make_shared(X(1), A1, X(2), A2, b); @@ -106,7 +110,8 @@ TEST(GaussianMixtureFactor, Printing) { GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); std::string expected = - R"(Hybrid [x1 x2; 1]{ + R"(GaussianMixtureFactor +Hybrid [x1 x2; 1]{ Choice(1) 0 Leaf : A[x1] = [ @@ -178,7 +183,8 @@ TEST(GaussianMixtureFactor, Error) { continuousValues.insert(X(2), Vector2(1, 1)); // error should return a tree of errors, with nodes for each discrete value. - AlgebraicDecisionTree error_tree = mixtureFactor.errorTree(continuousValues); + AlgebraicDecisionTree error_tree = + mixtureFactor.errorTree(continuousValues); std::vector discrete_keys = {m1}; // Error values for regression test @@ -191,8 +197,422 @@ TEST(GaussianMixtureFactor, Error) { DiscreteValues discreteValues; discreteValues[m1.first] = 1; EXPECT_DOUBLES_EQUAL( - 4.0, mixtureFactor.error({continuousValues, discreteValues}), - 1e-9); + 4.0, mixtureFactor.error({continuousValues, discreteValues}), 1e-9); +} + +/* ************************************************************************* */ +/** + * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) + * where m is a discrete variable and z is a continuous variable. + * m is binary and depending on m, we have 2 different means + * μ1 and μ2 for the Gaussian distribution around which we sample z. + * + * The resulting factor graph should eliminate to a Bayes net + * which represents a sigmoid function. + */ +TEST(GaussianMixtureFactor, GaussianMixtureModel) { + double mu0 = 1.0, mu1 = 3.0; + double sigma = 2.0; + auto model = noiseModel::Isotropic::Sigma(1, sigma); + + DiscreteKey m(M(0), 2); + Key z = Z(0); + + auto c0 = make_shared(z, Vector1(mu0), I_1x1, model), + c1 = make_shared(z, Vector1(mu1), I_1x1, model); + + auto gm = new GaussianMixture({z}, {}, {m}, {c0, c1}); + auto mixing = new DiscreteConditional(m, "0.5/0.5"); + + HybridBayesNet hbn; + hbn.emplace_back(gm); + hbn.emplace_back(mixing); + + // The result should be a sigmoid. + // So should be m = 0.5 at z=3.0 - 1.0=2.0 + VectorValues given; + given.insert(z, Vector1(mu1 - mu0)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + HybridBayesNet expected; + expected.emplace_back(new DiscreteConditional(m, "0.5/0.5")); + + EXPECT(assert_equal(expected, *bn)); +} + +/* ************************************************************************* */ +/** + * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) + * where m is a discrete variable and z is a continuous variable. + * m is binary and depending on m, we have 2 different means + * and covariances each for the + * Gaussian distribution around which we sample z. + * + * The resulting factor graph should eliminate to a Bayes net + * which represents a sigmoid function leaning towards + * the tighter covariance Gaussian. + */ +TEST(GaussianMixtureFactor, GaussianMixtureModel2) { + double mu0 = 1.0, mu1 = 3.0; + auto model0 = noiseModel::Isotropic::Sigma(1, 8.0); + auto model1 = noiseModel::Isotropic::Sigma(1, 4.0); + + DiscreteKey m(M(0), 2); + Key z = Z(0); + + auto c0 = make_shared(z, Vector1(mu0), I_1x1, model0), + c1 = make_shared(z, Vector1(mu1), I_1x1, model1); + + auto gm = new GaussianMixture({z}, {}, {m}, {c0, c1}); + auto mixing = new DiscreteConditional(m, "0.5/0.5"); + + HybridBayesNet hbn; + hbn.emplace_back(gm); + hbn.emplace_back(mixing); + + // The result should be a sigmoid leaning towards model1 + // since it has the tighter covariance. + // So should be m = 0.34/0.66 at z=3.0 - 1.0=2.0 + VectorValues given; + given.insert(z, Vector1(mu1 - mu0)); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + HybridBayesNet expected; + expected.emplace_back( + new DiscreteConditional(m, "0.338561851224/0.661438148776")); + + EXPECT(assert_equal(expected, *bn)); +} + +/* ************************************************************************* */ +/** + * Test a model P(x0)P(z0|x0)p(x1|m1)p(z1|x1)p(m1). + * + * p(x1|m1) has different means and same covariance. + * + * Converting to a factor graph gives us + * P(x0)ϕ(x0)P(x1|m1)ϕ(x1)P(m1) + * + * If we only have a measurement on z0, then + * the probability of x1 should be 0.5/0.5. + * Getting a measurement on z1 gives use more information. + */ +TEST(GaussianMixtureFactor, TwoStateModel) { + double mu0 = 1.0, mu1 = 3.0; + auto model = noiseModel::Isotropic::Sigma(1, 2.0); + + DiscreteKey m1(M(1), 2); + Key z0 = Z(0), z1 = Z(1), x0 = X(0), x1 = X(1); + + auto c0 = make_shared(x1, Vector1(mu0), I_1x1, model), + c1 = make_shared(x1, Vector1(mu1), I_1x1, model); + + auto p_x0 = new GaussianConditional(x0, Vector1(0.0), I_1x1, + noiseModel::Isotropic::Sigma(1, 1.0)); + auto p_z0x0 = new GaussianConditional(z0, Vector1(0.0), I_1x1, x0, -I_1x1, + noiseModel::Isotropic::Sigma(1, 1.0)); + auto p_x1m1 = new GaussianMixture({x1}, {}, {m1}, {c0, c1}); + auto p_z1x1 = new GaussianConditional(z1, Vector1(0.0), I_1x1, x1, -I_1x1, + noiseModel::Isotropic::Sigma(1, 3.0)); + auto p_m1 = new DiscreteConditional(m1, "0.5/0.5"); + + HybridBayesNet hbn; + hbn.emplace_back(p_x0); + hbn.emplace_back(p_z0x0); + hbn.emplace_back(p_x1m1); + hbn.emplace_back(p_m1); + + VectorValues given; + given.insert(z0, Vector1(0.5)); + + { + // Start with no measurement on x1, only on x0 + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Since no measurement on x1, we hedge our bets + DiscreteConditional expected(m1, "0.5/0.5"); + + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()))); + } + + { + // Now we add a measurement z1 on x1 + hbn.emplace_back(p_z1x1); + + given.insert(z1, Vector1(2.2)); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Since we have a measurement on z2, we get a definite result + DiscreteConditional expected(m1, "0.4923083/0.5076917"); + + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 1e-6)); + } +} + +/* ************************************************************************* */ +/** + * Test a model P(x0)P(z0|x0)p(x1|m1)p(z1|x1)p(m1). + * + * p(x1|m1) has different means and different covariances. + * + * Converting to a factor graph gives us + * P(x0)ϕ(x0)P(x1|m1)ϕ(x1)P(m1) + * + * If we only have a measurement on z0, then + * the probability of x1 should be the ratio of covariances. + * Getting a measurement on z1 gives use more information. + */ +TEST(GaussianMixtureFactor, TwoStateModel2) { + double mu0 = 1.0, mu1 = 3.0; + auto model0 = noiseModel::Isotropic::Sigma(1, 6.0); + auto model1 = noiseModel::Isotropic::Sigma(1, 4.0); + + DiscreteKey m1(M(1), 2); + Key z0 = Z(0), z1 = Z(1), x0 = X(0), x1 = X(1); + + auto c0 = make_shared(x1, Vector1(mu0), I_1x1, model0), + c1 = make_shared(x1, Vector1(mu1), I_1x1, model1); + + auto p_x0 = new GaussianConditional(x0, Vector1(0.0), I_1x1, + noiseModel::Isotropic::Sigma(1, 1.0)); + auto p_z0x0 = new GaussianConditional(z0, Vector1(0.0), I_1x1, x0, -I_1x1, + noiseModel::Isotropic::Sigma(1, 1.0)); + auto p_x1m1 = new GaussianMixture({x1}, {}, {m1}, {c0, c1}); + auto p_z1x1 = new GaussianConditional(z1, Vector1(0.0), I_1x1, x1, -I_1x1, + noiseModel::Isotropic::Sigma(1, 3.0)); + auto p_m1 = new DiscreteConditional(m1, "0.5/0.5"); + + HybridBayesNet hbn; + hbn.emplace_back(p_x0); + hbn.emplace_back(p_z0x0); + hbn.emplace_back(p_x1m1); + hbn.emplace_back(p_m1); + + VectorValues given; + given.insert(z0, Vector1(0.5)); + + { + // Start with no measurement on x1, only on x0 + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Since no measurement on x1, we get the ratio of covariances. + DiscreteConditional expected(m1, "0.6/0.4"); + + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()))); + } + + { + // Now we add a measurement z1 on x1 + hbn.emplace_back(p_z1x1); + + given.insert(z1, Vector1(2.2)); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + // Since we have a measurement on z2, we get a definite result + DiscreteConditional expected(m1, "0.52706646/0.47293354"); + + EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 1e-6)); + } +} + +/** + * @brief Helper function to specify a Hybrid Bayes Net + * {P(X1) P(Z1 | X1, X2, M1)} and convert it to a Hybrid Factor Graph + * {P(X1)L(X1, X2, M1; Z1)} by converting to likelihoods given Z1. + * + * We can specify either different means or different sigmas, + * or both for each hybrid factor component. + * + * @param values Initial values for linearization. + * @param means The mean values for the conditional components. + * @param sigmas Noise model sigma values (standard deviation). + * @param m1 The discrete mode key. + * @param z1 The measurement value. + * @return HybridGaussianFactorGraph + */ +HybridGaussianFactorGraph GetFactorGraphFromBayesNet( + const gtsam::Values &values, const std::vector &means, + const std::vector &sigmas, DiscreteKey &m1, double z1 = 0.0) { + // Noise models + 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); + + // GaussianMixtureFactor component factors + auto f0 = + std::make_shared>(X(1), X(2), means[0], model0); + auto f1 = + std::make_shared>(X(1), X(2), means[1], model1); + std::vector factors{f0, f1}; + + /// Get terms for each p^m(z1 | x1, x2) + Matrix H0_1, H0_2, H1_1, H1_2; + double x1 = values.at(X(1)), x2 = values.at(X(2)); + Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); + std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H0_1 /*Sp1*/}, + {X(2), H0_2 /*Tp2*/}}; + + Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); + std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H1_1 /*Sp1*/}, + {X(2), H1_2 /*Tp2*/}}; + // Create conditional P(Z1 | X1, X2, M1) + auto gm = new gtsam::GaussianMixture( + {Z(1)}, {X(1), X(2)}, {m1}, + {std::make_shared(terms0, 1, -d0, model0), + std::make_shared(terms1, 1, -d1, model1)}); + gtsam::HybridBayesNet bn; + bn.emplace_back(gm); + // bn.print(); + + // Create FG via toFactorGraph + gtsam::VectorValues measurements; + measurements.insert(Z(1), gtsam::I_1x1 * z1); // Set Z1 = 0 + HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); + + // Linearized prior factor on X1 + auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); + mixture_fg.push_back(prior); + + return mixture_fg; +} + +/* ************************************************************************* */ +/** + * @brief Test components with differing means. + * + * We specify a hybrid Bayes network P(Z | X, M) =p(X1)p(Z1 | X1, X2, M1), + * which is then converted to a factor graph by specifying Z1. + * This is a different case since now we have a hybrid factor + * with 2 continuous variables ϕ(x1, x2, m1). + * p(Z1 | X1, X2, M1) has 2 factors each for the binary + * mode m1, with only the means being different. + */ +TEST(GaussianMixtureFactor, DifferentMeans) { + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 0.0, x2 = 1.75; + values.insert(X(1), x1); + values.insert(X(2), x2); + + // Different means, same sigma + std::vector means{0.0, 2.0}, sigmas{1e-0, 1e-0}; + + HybridGaussianFactorGraph hfg = + GetFactorGraphFromBayesNet(values, means, sigmas, m1, 0.0); + + { + // With no measurement on X2, each mode should be equally likely + auto bn = hfg.eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(1), Vector1(0.0)}, {X(2), Vector1(-1.75)}}, + DiscreteValues{{M(1), 0}}); + + 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(0.69314718056, 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.69314718056, error, 1e-9); + } + } + { + // If we add a measurement on X2, we have more information to work with. + // Add a measurement on X2 + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + GaussianConditional meas_z2(Z(2), Vector1(2.0), I_1x1, X(2), I_1x1, + prior_noise); + auto prior_x2 = meas_z2.likelihood(Vector1(x2)); + + hfg.push_back(prior_x2); + + auto bn = hfg.eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(1), Vector1(0.0)}, {X(2), 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 with a Bayes net P(Z|X, M) converted to a FG. + * Same as the DifferentMeans example but in this case, + * we keep the means the same and vary the covariances. + */ +TEST(GaussianMixtureFactor, DifferentCovariances) { + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 1.0, x2 = 1.0; + values.insert(X(1), x1); + values.insert(X(2), x2); + + std::vector means{0.0, 0.0}, sigmas{1e2, 1e-2}; + HybridGaussianFactorGraph mixture_fg = + GetFactorGraphFromBayesNet(values, means, sigmas, m1); + + auto hbn = mixture_fg.eliminateSequential(); + + VectorValues cv; + cv.insert(X(1), Vector1(0.0)); + cv.insert(X(2), 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)); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 1dcd0bbb1..2d851b0ff 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -510,6 +510,7 @@ factor 0: b = [ -10 ] No noise model factor 1: +GaussianMixtureFactor Hybrid [x0 x1; m0]{ Choice(m0) 0 Leaf : @@ -534,6 +535,7 @@ Hybrid [x0 x1; m0]{ } factor 2: +GaussianMixtureFactor Hybrid [x1 x2; m1]{ Choice(m1) 0 Leaf : diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 0b2564403..48b193eeb 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -18,6 +18,9 @@ #include #include +#include +#include +#include #include #include #include @@ -115,6 +118,156 @@ TEST(MixtureFactor, Dim) { EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); } +/* ************************************************************************* */ +// Test components with differing means +TEST(MixtureFactor, DifferentMeans) { + DiscreteKey m1(M(1), 2), m2(M(2), 2); + + Values values; + double x1 = 0.0, x2 = 1.75, x3 = 2.60; + values.insert(X(1), x1); + values.insert(X(2), x2); + values.insert(X(3), x3); + + auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0); + auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0); + + auto f0 = std::make_shared>(X(1), X(2), 0.0, model0); + auto f1 = std::make_shared>(X(1), X(2), 2.0, model1); + std::vector factors{f0, f1}; + + MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); + HybridNonlinearFactorGraph hnfg; + hnfg.push_back(mixtureFactor); + + f0 = std::make_shared>(X(2), X(3), 0.0, model0); + f1 = std::make_shared>(X(2), X(3), 2.0, model1); + std::vector factors23{f0, f1}; + hnfg.push_back(MixtureFactor({X(2), X(3)}, {m2}, factors23)); + + auto prior = PriorFactor(X(1), x1, prior_noise); + hnfg.push_back(prior); + + hnfg.emplace_shared>(X(2), 2.0, prior_noise); + + auto hgfg = hnfg.linearize(values); + auto bn = hgfg->eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{ + {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}}, + DiscreteValues{{M(1), 1}, {M(2), 0}}); + + EXPECT(assert_equal(expected, actual)); + + { + DiscreteValues dv{{M(1), 0}, {M(2), 0}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 0}, {M(2), 1}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 1}, {M(2), 0}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 1}, {M(2), 1}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); + } +} + +/* ************************************************************************* */ +// Test components with differing covariances +TEST(MixtureFactor, DifferentCovariances) { + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 1.0, x2 = 1.0; + values.insert(X(1), x1); + values.insert(X(2), x2); + + double between = 0.0; + + auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); + auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + + auto f0 = + std::make_shared>(X(1), X(2), between, model0); + auto f1 = + std::make_shared>(X(1), X(2), between, model1); + std::vector factors{f0, f1}; + + // Create via toFactorGraph + using symbol_shorthand::Z; + Matrix H0_1, H0_2, H1_1, H1_2; + Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); + std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H0_1 /*Sp1*/}, + {X(2), H0_2 /*Tp2*/}}; + + Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); + std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H1_1 /*Sp1*/}, + {X(2), H1_2 /*Tp2*/}}; + auto gm = new gtsam::GaussianMixture( + {Z(1)}, {X(1), X(2)}, {m1}, + {std::make_shared(terms0, 1, -d0, model0), + std::make_shared(terms1, 1, -d1, model1)}); + gtsam::HybridBayesNet bn; + bn.emplace_back(gm); + + gtsam::VectorValues measurements; + measurements.insert(Z(1), gtsam::Z_1x1); + // Create FG with single GaussianMixtureFactor + HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); + + // Linearized prior factor on X1 + auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); + mixture_fg.push_back(prior); + + auto hbn = mixture_fg.eliminateSequential(); + + VectorValues cv; + cv.insert(X(1), Vector1(0.0)); + cv.insert(X(2), Vector1(0.0)); + // P(m1) = [0.5, 0.5], so we should pick 0 + DiscreteValues dv; + dv.insert({M(1), 0}); + HybridValues expected_values(cv, dv); + + HybridValues actual_values = hbn->optimize(); + EXPECT(assert_equal(expected_values, actual_values)); + + // Check that we get different error values at the MLE point μ. + AlgebraicDecisionTree errorTree = hbn->errorTree(cv); + + HybridValues hv0(cv, DiscreteValues{{M(1), 0}}); + HybridValues hv1(cv, DiscreteValues{{M(1), 1}}); + + AlgebraicDecisionTree expectedErrorTree(m1, 9.90348755254, + 0.69314718056); + EXPECT(assert_equal(expectedErrorTree, errorTree)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 2fa50b7f6..7fbd43ffc 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -263,11 +263,6 @@ namespace gtsam { /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const; - /// Check equality. - friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) { - return lhs.equals(rhs); - } - /// @{ /// @name Advanced Interface /// @{