diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 0a34a25e3..2887e6d3a 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -104,12 +104,25 @@ namespace gtsam { /* ************************************************************************* */ double GaussianBayesNet::error(const VectorValues& x) const { - return GaussianFactorGraph(*this).error(x); + double sum = 0.; + for (const auto& gc : *this) { + if (gc) sum += gc->error(x); + } + return sum; + } + + /* ************************************************************************* */ + double GaussianBayesNet::logProbability(const VectorValues& x) const { + double sum = 0.; + for (const auto& gc : *this) { + if (gc) sum += gc->logProbability(x); + } + return sum; } /* ************************************************************************* */ double GaussianBayesNet::evaluate(const VectorValues& x) const { - return exp(-error(x)); + return exp(-logProbability(x)); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 8c40d9728..28397a88a 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -97,17 +97,16 @@ namespace gtsam { /// @name Standard Interface /// @{ - /** - * The error is the negative log-density for given values `x`: - * neg_log_likelihood(x) + 0.5 * n*log(2*pi) + 0.5 * log det(Sigma) - * where x is the vector of values, and Sigma is the covariance matrix. - */ + /// Sum error over all variables. double error(const VectorValues& x) const; + /// Sum logProbability over all variables. + double logProbability(const VectorValues& x) const; + /** * Calculate probability density for given values `x`: - * exp(-error) == exp(-neg_log_likelihood(x)) / sqrt((2*pi)^n*det(Sigma)) - * where x is the vector of values, and Sigma is the covariance matrix. + * exp(logProbability) + * where x is the vector of values. */ double evaluate(const VectorValues& x) const; @@ -247,6 +246,14 @@ namespace gtsam { VectorValues backSubstituteTranspose(const VectorValues& gx) const; /// @} + /// @name HybridValues methods. + /// @{ + + using Base::evaluate; // Expose evaluate(const HybridValues&) method.. + using Base::logProbability; // Expose logProbability(const HybridValues&) method.. + using Base::error; // Expose error(const HybridValues&) method.. + + /// @} private: /** Serialization function */ diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index db822ffba..10f4eabbb 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #ifdef __GNUC__ @@ -34,6 +35,7 @@ #include #include #include +#include // In Wrappers we have no access to this so have a default ready static std::mt19937_64 kRandomNumberGenerator(42); @@ -170,39 +172,42 @@ namespace gtsam { } } -/* ************************************************************************* */ -double GaussianConditional::logDeterminant() const { - if (get_model()) { - Vector diag = R().diagonal(); - get_model()->whitenInPlace(diag); - return diag.unaryExpr([](double x) { return log(x); }).sum(); - } else { - return R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); + /* ************************************************************************* */ + double GaussianConditional::logDeterminant() const { + if (get_model()) { + Vector diag = R().diagonal(); + get_model()->whitenInPlace(diag); + return diag.unaryExpr([](double x) { return log(x); }).sum(); + } else { + return R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); + } } -} -/* ************************************************************************* */ -// normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) -// log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) -double GaussianConditional::logNormalizationConstant() const { - constexpr double log2pi = 1.8378770664093454835606594728112; - size_t n = d().size(); - // log det(Sigma)) = - 2.0 * logDeterminant() - return - 0.5 * n * log2pi + logDeterminant(); -} + /* ************************************************************************* */ + // normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) + // log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) + double GaussianConditional::logNormalizationConstant() const { + constexpr double log2pi = 1.8378770664093454835606594728112; + size_t n = d().size(); + // log det(Sigma)) = - 2.0 * logDeterminant() + return - 0.5 * n * log2pi + logDeterminant(); + } -/* ************************************************************************* */ -// density = k exp(-error(x)) -// -log(density) = error(x) - log(k) -double GaussianConditional::error(const VectorValues& x) const { - return JacobianFactor::error(x) - logNormalizationConstant(); -} + /* ************************************************************************* */ + // density = k exp(-error(x)) + // log = log(k) - error(x) + double GaussianConditional::logProbability(const VectorValues& x) const { + return logNormalizationConstant() - error(x); + } -/* ************************************************************************* */ -double GaussianConditional::evaluate(const VectorValues& x) const { - return exp(-error(x)); -} + double GaussianConditional::logProbability(const HybridValues& x) const { + return logProbability(x.continuous()); + } + /* ************************************************************************* */ + double GaussianConditional::evaluate(const VectorValues& c) const { + return exp(logProbability(c)); + } /* ************************************************************************* */ VectorValues GaussianConditional::solve(const VectorValues& x) const { // Concatenate all vector values that correspond to parent variables diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index b41e1a394..880d13064 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -145,19 +145,18 @@ namespace gtsam { return exp(logNormalizationConstant()); } - using Base::error; // Expose error(const HybridValues&) method.. - /** - * Calculate error(x) == -log(evaluate()) for given values `x`: - * - GaussianFactor::error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) + * Calculate log-probability log(evaluate(x)) for given values `x`: + * -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) * where x is the vector of values, and Sigma is the covariance matrix. - * Note that GaussianFactor:: error(x)=0.5*e'*e includes the 0.5 factor already. + * This differs from error as it is log, not negative log, and it + * includes the normalization constant. */ - double error(const VectorValues& x) const override; + double logProbability(const VectorValues& x) const; /** * Calculate probability density for given values `x`: - * exp(-error(x)) == exp(-GaussianFactor::error(x)) / sqrt((2*pi)^n*det(Sigma)) + * exp(logProbability(x)) == exp(-GaussianFactor::error(x)) / sqrt((2*pi)^n*det(Sigma)) * where x is the vector of values, and Sigma is the covariance matrix. */ double evaluate(const VectorValues& x) const; @@ -261,6 +260,21 @@ namespace gtsam { double logDeterminant() const; /// @} + /// @name HybridValues methods. + /// @{ + + /** + * Calculate log-probability log(evaluate(x)) for HybridValues `x`. + * Simply dispatches to VectorValues version. + */ + double logProbability(const HybridValues& x) const override; + + using Conditional::evaluate; // Expose evaluate(const HybridValues&) method.. + using Conditional::operator(); // Expose evaluate(const HybridValues&) method.. + using Base::error; // Expose error(const HybridValues&) method.. + + /// @} + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @name Deprecated diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 72eb107d0..0f1163982 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -67,6 +67,22 @@ namespace gtsam { return spec; } + /* ************************************************************************* */ + double GaussianFactorGraph::error(const VectorValues& x) const { + double total_error = 0.; + for(const sharedFactor& factor: *this){ + if(factor) + total_error += factor->error(x); + } + return total_error; + } + + /* ************************************************************************* */ + double GaussianFactorGraph::probPrime(const VectorValues& c) const { + // NOTE the 0.5 constant is handled by the factor error. + return exp(-error(c)); + } + /* ************************************************************************* */ GaussianFactorGraph::shared_ptr GaussianFactorGraph::cloneToPtr() const { gtsam::GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index ce475e100..fb5e1ea36 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -167,20 +167,10 @@ namespace gtsam { std::map getKeyDimMap() const; /** unnormalized error */ - double error(const VectorValues& x) const { - double total_error = 0.; - for(const sharedFactor& factor: *this){ - if(factor) - total_error += factor->error(x); - } - return total_error; - } + double error(const VectorValues& x) const; /** Unnormalized probability. O(n) */ - double probPrime(const VectorValues& c) const { - // NOTE the 0.5 constant is handled by the factor error. - return exp(-error(c)); - } + double probPrime(const VectorValues& c) const; /** * Clone() performs a deep-copy of the graph, including all of the factors. diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 973485484..eb90f8aab 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -381,14 +381,20 @@ TEST(GaussianConditional, FromMeanAndStddev) { auto conditional1 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma); Vector2 e1 = (x0 - (A1 * x1 + b)) / sigma; - double expected1 = 0.5 * e1.dot(e1) - conditional1.logNormalizationConstant(); + double expected1 = 0.5 * e1.dot(e1); EXPECT_DOUBLES_EQUAL(expected1, conditional1.error(values), 1e-9); + double expected2 = conditional1.logNormalizationConstant() - 0.5 * e1.dot(e1); + EXPECT_DOUBLES_EQUAL(expected2, conditional1.logProbability(values), 1e-9); + auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), A2, X(2), b, sigma); Vector2 e2 = (x0 - (A1 * x1 + A2 * x2 + b)) / sigma; - double expected2 = 0.5 * e2.dot(e2) - conditional2.logNormalizationConstant(); - EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9); + double expected3 = 0.5 * e2.dot(e2); + EXPECT_DOUBLES_EQUAL(expected3, conditional2.error(values), 1e-9); + + double expected4 = conditional2.logNormalizationConstant() - 0.5 * e2.dot(e2); + EXPECT_DOUBLES_EQUAL(expected4, conditional2.logProbability(values), 1e-9); } /* ************************************************************************* */ @@ -454,12 +460,13 @@ TEST(GaussianConditional, Error) { GaussianConditional::FromMeanAndStddev(X(0), Vector1::Zero(), 1.0); VectorValues values; values.insert(X(0), Vector1::Zero()); - double error = stdGaussian.error(values); + double logProbability = stdGaussian.logProbability(values); // Regression. // These values were computed by hand for a univariate standard gaussian. - EXPECT_DOUBLES_EQUAL(0.9189385332046727, error, 1e-9); - EXPECT_DOUBLES_EQUAL(0.3989422804014327, exp(-error), 1e-9); + EXPECT_DOUBLES_EQUAL(-0.9189385332046727, logProbability, 1e-9); + EXPECT_DOUBLES_EQUAL(0.3989422804014327, exp(logProbability), 1e-9); + EXPECT_DOUBLES_EQUAL(stdGaussian(values), exp(logProbability), 1e-9); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index 3e73bce13..97eb0de70 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -52,8 +52,11 @@ TEST(GaussianDensity, FromMeanAndStddev) { auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma); Vector2 e = (x0 - b) / sigma; - double expected = 0.5 * e.dot(e) - density.logNormalizationConstant(); - EXPECT_DOUBLES_EQUAL(expected, density.error(values), 1e-9); + double expected1 = 0.5 * e.dot(e); + EXPECT_DOUBLES_EQUAL(expected1, density.error(values), 1e-9); + + double expected2 = density.logNormalizationConstant()- 0.5 * e.dot(e); + EXPECT_DOUBLES_EQUAL(expected2, density.logProbability(values), 1e-9); } /* ************************************************************************* */