diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 36a34226b..531a9f2e5 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include namespace gtsam { @@ -93,35 +92,6 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { return {conditionals_, wrap}; } -/* -*******************************************************************************/ -GaussianBayesNetTree GaussianMixture::add( - const GaussianBayesNetTree &sum) const { - using Y = GaussianBayesNet; - auto add = [](const Y &graph1, const Y &graph2) { - auto result = graph1; - if (graph2.size() == 0) { - return GaussianBayesNet(); - } - result.push_back(graph2); - return result; - }; - const auto tree = asGaussianBayesNetTree(); - return sum.empty() ? tree : sum.apply(tree, add); -} - -/* *******************************************************************************/ -GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const { - auto wrap = [](const GaussianConditional::shared_ptr &gc) { - if (gc) { - return GaussianBayesNet{gc}; - } else { - return GaussianBayesNet(); - } - }; - return {conditionals_, wrap}; -} - /* *******************************************************************************/ size_t GaussianMixture::nrComponents() const { size_t total = 0; @@ -348,15 +318,8 @@ AlgebraicDecisionTree GaussianMixture::logProbability( AlgebraicDecisionTree GaussianMixture::errorTree( const VectorValues &continuousValues) const { auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) { - // 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(); - } + return conditional->error(continuousValues) + // + logConstant_ - conditional->logNormalizationConstant(); }; DecisionTree error_tree(conditionals_, errorFunc); return error_tree; @@ -364,32 +327,10 @@ 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 return 0.0 erorr. - if (!valid_assignment) { - return 0.0; - } - // Directly index to get the conditional, no need to build the whole tree. auto conditional = conditionals_(values.discrete()); - 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(); - } + return conditional->error(values.continuous()) + // + logConstant_ - conditional->logNormalizationConstant(); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index cefc14241..c1ef504f8 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -72,12 +72,6 @@ class GTSAM_EXPORT GaussianMixture */ GaussianFactorGraphTree asGaussianFactorGraphTree() const; - /** - * @brief Convert a DecisionTree of conditionals into - * a DecisionTree of Gaussian Bayes nets. - */ - GaussianBayesNetTree asGaussianBayesNetTree() const; - /** * @brief Helper function to get the pruner functor. * @@ -221,7 +215,8 @@ class GTSAM_EXPORT GaussianMixture * @return AlgebraicDecisionTree A decision tree on the discrete keys * only, with the leaf values as the error for each assignment. */ - AlgebraicDecisionTree errorTree(const VectorValues &continuousValues) const; + AlgebraicDecisionTree errorTree( + const VectorValues &continuousValues) const; /** * @brief Compute the logProbability of this Gaussian Mixture. @@ -255,15 +250,6 @@ class GTSAM_EXPORT GaussianMixture * @return GaussianFactorGraphTree */ GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const; - - /** - * @brief Merge the Gaussian Bayes Nets in `this` and `sum` while - * maintaining the decision tree structure. - * - * @param sum Decision Tree of Gaussian Bayes Nets - * @return GaussianBayesNetTree - */ - GaussianBayesNetTree add(const GaussianBayesNetTree &sum) const; /// @} private: diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index e519cefe6..a3db16d04 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -28,86 +28,11 @@ namespace gtsam { -/** - * @brief Helper function to correct 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 GaussianFactor shared pointers. - * @param varyingNormalizers Flag indicating the normalizers are different for - * each component. - * @return GaussianMixtureFactor::Factors - */ -GaussianMixtureFactor::Factors correct( - const GaussianMixtureFactor::Factors &factors, bool varyingNormalizers) { - if (!varyingNormalizers) { - return factors; - } - - // First compute all the sqrt(|2 pi Sigma|) terms - auto computeNormalizers = [](const GaussianMixtureFactor::sharedFactor &gf) { - auto jf = std::dynamic_pointer_cast(gf); - // If we have, say, a Hessian factor, then no need to do anything - if (!jf) return 0.0; - - auto model = jf->get_model(); - // If there is no noise model, there is nothing to do. - if (!model) { - return 0.0; - } - // Since noise models are Gaussian, we can get the logDeterminant using the - // same trick as in GaussianConditional - double logDetR = - model->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); - double logDeterminantSigma = -2.0 * logDetR; - - size_t n = model->dim(); - constexpr double log2pi = 1.8378770664093454835606594728112; - return n * log2pi + logDeterminantSigma; - }; - - AlgebraicDecisionTree log_normalizers = - DecisionTree(factors, computeNormalizers); - - // Find the minimum value so we can "proselytize" to positive values. - // Done because we can't have sqrt of negative numbers. - double min_log_normalizer = log_normalizers.min(); - log_normalizers = log_normalizers.apply( - [&min_log_normalizer](double n) { return n - min_log_normalizer; }); - - // Finally, update the [A|b] matrices. - auto update = [&log_normalizers]( - const Assignment &assignment, - const GaussianMixtureFactor::sharedFactor &gf) { - auto jf = std::dynamic_pointer_cast(gf); - if (!jf) return gf; - // If there is no noise model, there is nothing to do. - if (!jf->get_model()) return gf; - // If the log_normalizer is 0, do nothing - if (log_normalizers(assignment) == 0.0) return gf; - - GaussianFactorGraph gfg; - gfg.push_back(jf); - - Vector c(1); - c << std::sqrt(log_normalizers(assignment)); - auto constantFactor = std::make_shared(c); - - gfg.push_back(constantFactor); - return std::dynamic_pointer_cast( - std::make_shared(gfg)); - }; - return factors.apply(update); -} - /* *******************************************************************************/ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const Factors &factors, - bool varyingNormalizers) - : Base(continuousKeys, discreteKeys), - factors_(correct(factors, varyingNormalizers)) {} + const Factors &factors) + : Base(continuousKeys, discreteKeys), factors_(factors) {} /* *******************************************************************************/ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { @@ -129,9 +54,7 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { /* *******************************************************************************/ void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { - std::cout << (s.empty() ? "" : s + "\n"); - std::cout << "GaussianMixtureFactor" << std::endl; - HybridFactor::print("", formatter); + HybridFactor::print(s, formatter); std::cout << "{\n"; if (factors_.empty()) { std::cout << " empty" << std::endl; diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 588501bbe..67d12ddb0 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -82,13 +82,10 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * their cardinalities. * @param factors The decision tree of Gaussian factors stored as the mixture * density. - * @param varyingNormalizers Flag indicating factor components have varying - * normalizer values. */ GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const Factors &factors, - bool varyingNormalizers = false); + const Factors &factors); /** * @brief Construct a new GaussianMixtureFactor object using a vector of @@ -97,16 +94,12 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * @param continuousKeys Vector of keys for continuous factors. * @param discreteKeys Vector of discrete keys. * @param factors Vector of gaussian factor shared pointers. - * @param varyingNormalizers Flag indicating factor components have varying - * normalizer values. */ GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const std::vector &factors, - bool varyingNormalizers = false) + const std::vector &factors) : GaussianMixtureFactor(continuousKeys, discreteKeys, - Factors(discreteKeys, factors), - varyingNormalizers) {} + Factors(discreteKeys, factors)) {} /// @} /// @name Testable @@ -114,8 +107,9 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { bool equals(const HybridFactor &lf, double tol = 1e-9) const override; - void print(const std::string &s = "", const KeyFormatter &formatter = - DefaultKeyFormatter) const override; + void print( + const std::string &s = "GaussianMixtureFactor\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override; /// @} /// @name Standard API diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 1d01baed2..bb0f23686 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -220,16 +220,16 @@ GaussianBayesNet HybridBayesNet::choose( /* ************************************************************************* */ HybridValues HybridBayesNet::optimize() const { // Collect all the discrete factors to compute MPE - DiscreteFactorGraph discrete_fg; + DiscreteBayesNet discrete_bn; for (auto &&conditional : *this) { if (conditional->isDiscrete()) { - discrete_fg.push_back(conditional->asDiscrete()); + discrete_bn.push_back(conditional->asDiscrete()); } } // Solve for the MPE - DiscreteValues mpe = discrete_fg.optimize(); + DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).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 418489d66..afd1c8032 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -13,7 +13,6 @@ * @file HybridFactor.h * @date Mar 11, 2022 * @author Fan Jiang - * @author Varun Agrawal */ #pragma once @@ -34,8 +33,6 @@ class HybridValues; /// Alias for DecisionTree of GaussianFactorGraphs using GaussianFactorGraphTree = DecisionTree; -/// Alias for DecisionTree of GaussianBayesNets -using GaussianBayesNetTree = DecisionTree; KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index f05dfd423..b764dc9e0 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -279,37 +279,21 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) { using Result = std::pair, GaussianMixtureFactor::sharedFactor>; -/** - * 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. - */ +// 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)) static std::shared_ptr createDiscreteFactor( const DecisionTree &eliminationResults, const DiscreteKeys &discreteSeparator) { - auto logProbability = [&](const Result &pair) -> double { + auto probability = [&](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. - - // 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()); + return exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); }; - 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()); + DecisionTree probabilities(eliminationResults, probability); return std::make_shared(discreteSeparator, probabilities); } @@ -370,12 +354,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // Perform elimination! DecisionTree eliminationResults(factorGraphTree, eliminate); - // Create the GaussianMixture from the conditionals - GaussianMixture::Conditionals conditionals( - eliminationResults, [](const Result &pair) { return pair.first; }); - auto gaussianMixture = std::make_shared( - frontalKeys, continuousSeparator, discreteSeparator, conditionals); - // If there are no more continuous parents we create a DiscreteFactor with the // error for each discrete choice. Otherwise, create a GaussianMixtureFactor // on the separator, taking care to correct for conditional constants. @@ -385,6 +363,12 @@ hybridElimination(const HybridGaussianFactorGraph &factors, : createGaussianMixtureFactor(eliminationResults, continuousSeparator, discreteSeparator); + // Create the GaussianMixture from the conditionals + GaussianMixture::Conditionals conditionals( + eliminationResults, [](const Result &pair) { return pair.first; }); + auto gaussianMixture = std::make_shared( + frontalKeys, continuousSeparator, discreteSeparator, conditionals); + return {std::make_shared(gaussianMixture), newFactor}; } diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 47b9ddc99..9cc7e6bfd 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -22,13 +22,9 @@ #include #include #include -#include -#include #include #include #include -#include -#include // Include for test suite #include @@ -60,6 +56,7 @@ 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); @@ -109,8 +106,7 @@ TEST(GaussianMixtureFactor, Printing) { GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); std::string expected = - R"(GaussianMixtureFactor -Hybrid [x1 x2; 1]{ + R"(Hybrid [x1 x2; 1]{ Choice(1) 0 Leaf : A[x1] = [ @@ -182,8 +178,7 @@ 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 @@ -196,240 +191,8 @@ TEST(GaussianMixtureFactor, Error) { DiscreteValues discreteValues; discreteValues[m1.first] = 1; EXPECT_DOUBLES_EQUAL( - 4.0, mixtureFactor.error({continuousValues, discreteValues}), 1e-9); -} - -/* ************************************************************************* */ -// Test components with differing means -TEST(GaussianMixtureFactor, 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) - ->linearize(values); - auto f1 = std::make_shared>(X(1), X(2), 2.0, model1) - ->linearize(values); - std::vector factors{f0, f1}; - - GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors, true); - HybridGaussianFactorGraph hfg; - hfg.push_back(mixtureFactor); - - f0 = std::make_shared>(X(2), X(3), 0.0, model0) - ->linearize(values); - f1 = std::make_shared>(X(2), X(3), 2.0, model1) - ->linearize(values); - std::vector factors23{f0, f1}; - hfg.push_back(GaussianMixtureFactor({X(2), X(3)}, {m2}, factors23, true)); - - auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); - hfg.push_back(prior); - - hfg.push_back(PriorFactor(X(2), 2.0, prior_noise).linearize(values)); - - auto bn = hfg.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); - } -} - -/* ************************************************************************* */ -/** - * @brief Test components with differing covariances. - * The factor graph is - * *-X1-*-X2 - * | - * M1 - */ -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); - - 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*/}}; - gtsam::GaussianMixtureFactor gmf( - {X(1), X(2)}, {m1}, - {std::make_shared(X(1), H0_1, X(2), H0_2, -d0, model0), - std::make_shared(X(1), H1_1, X(2), H1_2, -d1, model1)}, - true); - - // Create FG with single GaussianMixtureFactor - HybridGaussianFactorGraph mixture_fg; - mixture_fg.add(gmf); - - // 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(); - // hbn->print(); - - 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)); -} - -/* ************************************************************************* */ -/** - * @brief Test components with differing covariances - * but with a Bayes net P(Z|X, M) converted to a FG. - */ -TEST(GaussianMixtureFactor, DifferentCovariances2) { - 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)); - - // 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)); + 4.0, mixtureFactor.error({continuousValues, discreteValues}), + 1e-9); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 2d851b0ff..cf56e35d6 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -510,7 +510,6 @@ factor 0: b = [ -10 ] No noise model factor 1: -GaussianMixtureFactor Hybrid [x0 x1; m0]{ Choice(m0) 0 Leaf : @@ -535,7 +534,6 @@ Hybrid [x0 x1; m0]{ } factor 2: -GaussianMixtureFactor Hybrid [x1 x2; m1]{ Choice(m1) 0 Leaf : @@ -677,26 +675,22 @@ factor 6: P( m1 | m0 ): size: 3 conditional 0: Hybrid P( x0 | x1 m0) Discrete Keys = (m0, 2), - logNormalizationConstant: 1.38862 Choice(m0) 0 Leaf p(x0 | x1) R = [ 10.0499 ] S[x1] = [ -0.0995037 ] d = [ -9.85087 ] - logNormalizationConstant: 1.38862 No noise model 1 Leaf p(x0 | x1) R = [ 10.0499 ] S[x1] = [ -0.0995037 ] d = [ -9.95037 ] - logNormalizationConstant: 1.38862 No noise model conditional 1: Hybrid P( x1 | x2 m0 m1) Discrete Keys = (m0, 2), (m1, 2), - logNormalizationConstant: 1.3935 Choice(m1) 0 Choice(m0) @@ -704,14 +698,12 @@ conditional 1: Hybrid P( x1 | x2 m0 m1) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -9.99901 ] - logNormalizationConstant: 1.3935 No noise model 0 1 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -9.90098 ] - logNormalizationConstant: 1.3935 No noise model 1 Choice(m0) @@ -719,19 +711,16 @@ conditional 1: Hybrid P( x1 | x2 m0 m1) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -10.098 ] - logNormalizationConstant: 1.3935 No noise model 1 1 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -10 ] - logNormalizationConstant: 1.3935 No noise model conditional 2: Hybrid P( x2 | m0 m1) Discrete Keys = (m0, 2), (m1, 2), - logNormalizationConstant: 1.38857 Choice(m1) 0 Choice(m0) @@ -740,7 +729,6 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.1489 ] mean: 1 elements x2: -1.0099 - logNormalizationConstant: 1.38857 No noise model 0 1 Leaf p(x2) @@ -748,7 +736,6 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.1479 ] mean: 1 elements x2: -1.0098 - logNormalizationConstant: 1.38857 No noise model 1 Choice(m0) @@ -757,7 +744,6 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.0504 ] mean: 1 elements x2: -1.0001 - logNormalizationConstant: 1.38857 No noise model 1 1 Leaf p(x2) @@ -765,7 +751,6 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.0494 ] mean: 1 elements x2: -1 - logNormalizationConstant: 1.38857 No noise model )"; diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 58a12b3d8..0b2564403 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -18,9 +18,6 @@ #include #include -#include -#include -#include #include #include #include @@ -118,152 +115,6 @@ 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)); - - // 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}}); - - auto cond0 = hbn->at(0)->asMixture(); - auto cond1 = hbn->at(1)->asMixture(); - auto discrete_cond = hbn->at(2)->asDiscrete(); - AlgebraicDecisionTree expectedErrorTree(m1, 9.90348755254, - 0.69314718056); - EXPECT(assert_equal(expectedErrorTree, errorTree)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 861e19cc9..b04878ac5 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -243,25 +243,5 @@ namespace gtsam { } /* ************************************************************************* */ - double GaussianBayesNet::logNormalizationConstant() const { - /* - normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) - logConstant = -0.5 * n*log(2*pi) - 0.5 * log det(Sigma) - - log det(Sigma)) = -2.0 * logDeterminant() - thus, logConstant = -0.5*n*log(2*pi) + logDeterminant() - - BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i()) - = sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i()) - = sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant() - */ - double logNormConst = 0.0; - for (const sharedConditional& cg : *this) { - logNormConst += cg->logNormalizationConstant(); - } - return logNormConst; - } - - /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index ea1cb8603..19781d1e6 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -82,12 +82,6 @@ namespace gtsam { /** Check equality */ bool equals(const This& bn, double tol = 1e-9) const; - /// Check exact equality. - friend bool operator==(const GaussianBayesNet& lhs, - const GaussianBayesNet& rhs) { - return lhs.isEqual(rhs); - } - /// print graph void print( const std::string& s = "", @@ -234,14 +228,6 @@ namespace gtsam { * @return The determinant */ double logDeterminant() const; - /** - * @brief Get the log of the normalization constant corresponding to the - * joint Gaussian density represented by this Bayes net. - * - * @return double - */ - double logNormalizationConstant() const; - /** * Backsubstitute with a different RHS vector than the one stored in this BayesNet. * gy=inv(R*inv(Sigma))*gx diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index f986eed02..0112835aa 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -121,7 +121,6 @@ namespace gtsam { const auto mean = solve({}); // solve for mean. mean.print(" mean", formatter); } - cout << " logNormalizationConstant: " << logNormalizationConstant() << std::endl; if (model_) model_->print(" Noise model: "); else @@ -185,13 +184,8 @@ namespace gtsam { double GaussianConditional::logNormalizationConstant() const { constexpr double log2pi = 1.8378770664093454835606594728112; size_t n = d().size(); - // 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 * logDeterminant() - // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant()) - // = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant()) - // = -0.5*n*log(2*pi) + logDeterminant() - return -0.5 * n * log2pi + logDeterminant(); + // log det(Sigma)) = - 2.0 * logDeterminant() + return - 0.5 * n * log2pi + logDeterminant(); } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 99ee4eb83..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 exact equality. - friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) { - return lhs.equals(rhs); - } - /// @{ /// @name Advanced Interface /// @{ diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 3a629f349..4b0963b8d 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -510,17 +510,12 @@ virtual class GaussianConditional : gtsam::JacobianFactor { GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S, size_t name2, gtsam::Matrix T, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(const vector> terms, - size_t nrFrontals, gtsam::Vector d, - const gtsam::noiseModel::Diagonal* sigmas); // Constructors with no noise model GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R); GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S); GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S, size_t name2, gtsam::Matrix T); - GaussianConditional(const gtsam::KeyVector& keys, size_t nrFrontals, - const gtsam::VerticalBlockMatrix& augmentedMatrix); // Named constructors static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index dcd821889..a4a722012 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -516,7 +516,6 @@ TEST(GaussianConditional, Print) { " d = [ 20 40 ]\n" " mean: 1 elements\n" " x0: 20 40\n" - " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected, conditional, "GaussianConditional")); @@ -531,7 +530,6 @@ TEST(GaussianConditional, Print) { " S[x1] = [ -1 -2 ]\n" " [ -3 -4 ]\n" " d = [ 20 40 ]\n" - " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional")); @@ -547,7 +545,6 @@ TEST(GaussianConditional, Print) { " S[y1] = [ -5 -6 ]\n" " [ -7 -8 ]\n" " d = [ 20 40 ]\n" - " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional")); }