12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Implemented all Gaussian versions of logProbability

release/4.3a0
Frank Dellaert 2023-01-10 13:12:36 -08:00
parent 8e29140ff7
commit ecb0be494e
8 changed files with 119 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/linear/GaussianBayesNet.cpp
View File

@ -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));
}
/* ************************************************************************* */

21
gtsam/linear/GaussianBayesNet.h
View File

@ -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 */

61
gtsam/linear/GaussianConditional.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/linear/Sampler.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/hybrid/HybridValues.h>
#include <boost/format.hpp>
#ifdef __GNUC__
@ -34,6 +35,7 @@
#include <functional>
#include <list>
#include <string>
#include <cmath>
// 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

28
gtsam/linear/GaussianConditional.h
View File

@ -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

16
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -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());

14
gtsam/linear/GaussianFactorGraph.h
View File

@ -167,20 +167,10 @@ namespace gtsam {
std::map<Key, size_t> 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.

19
gtsam/linear/tests/testGaussianConditional.cpp
View File

@ -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);
}
/* ************************************************************************* */

7
gtsam/linear/tests/testGaussianDensity.cpp
View File

@ -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);
}
/* ************************************************************************* */