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

Declared and defined error method in Factor, getting rid of logDensity, as error is now unambiguously defined as -logDensity in conditional factors.

release/4.3a0
Frank Dellaert 2023-01-08 16:11:36 -08:00
parent aed576ca0a
commit 877e564744
13 changed files with 175 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/hybrid/GaussianMixture.h
View File

@ -188,9 +188,6 @@ class GTSAM_EXPORT GaussianMixture
// double operator()(const HybridValues &values) const { return
// evaluate(values); }
// /// Calculate log-density for given values `x`.
// double logDensity(const HybridValues &values) const;
/**
* @brief Prune the decision tree of Gaussian factors as per the discrete
* `decisionTree`.

8
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -260,18 +260,18 @@ double HybridBayesNet::evaluate(const HybridValues &values) const {
const DiscreteValues &discreteValues = values.discrete();
const VectorValues &continuousValues = values.continuous();
double logDensity = 0.0, probability = 1.0;
double error = 0.0, probability = 1.0;
// Iterate over each conditional.
for (auto &&conditional : *this) {
// TODO: should be delegated to derived classes.
if (auto gm = conditional->asMixture()) {
const auto component = (*gm)(discreteValues);
logDensity += component->logDensity(continuousValues);
error += component->error(continuousValues);
} else if (auto gc = conditional->asGaussian()) {
// If continuous only, evaluate the probability and multiply.
logDensity += gc->logDensity(continuousValues);
error += gc->error(continuousValues);
} else if (auto dc = conditional->asDiscrete()) {
// Conditional is discrete-only, so return its probability.
@ -279,7 +279,7 @@ double HybridBayesNet::evaluate(const HybridValues &values) const {
}
}
return probability * exp(logDensity);
return probability * exp(-error);
}
/* ************************************************************************* */

9
gtsam/hybrid/HybridFactor.h
View File

@ -143,15 +143,6 @@ class GTSAM_EXPORT HybridFactor : public Factor {
/// @name Standard Interface
/// @{
/**
* @brief Compute the error of this Gaussian Mixture given the continuous
* values and a discrete assignment.
*
* @param values Continuous values and discrete assignment.
* @return double
*/
virtual double error(const HybridValues &values) const = 0;
/// True if this is a factor of discrete variables only.
bool isDiscrete() const { return isDiscrete_; }

35
gtsam/inference/Factor.h
View File

@ -29,12 +29,16 @@
#include <gtsam/inference/Key.h>
namespace gtsam {
/// Define collection types:
typedef FastVector<FactorIndex> FactorIndices;
typedef FastSet<FactorIndex> FactorIndexSet;
/// Define collection types:
typedef FastVector<FactorIndex> FactorIndices;
typedef FastSet<FactorIndex> FactorIndexSet;
class HybridValues; // forward declaration of a Value type for error.
/**
* This is the base class for all factor types. This class does not store any
* This is the base class for all factor types, as well as conditionals,
* which are implemented as specialized factors. This class does not store any
* data other than its keys. Derived classes store data such as matrices and
* probability tables.
*
@ -61,9 +65,8 @@ typedef FastSet<FactorIndex> FactorIndexSet;
* \class SymbolicFactor and \class SymbolicConditional. They do not override the
* `error` method, and are used only for symbolic elimination etc.
*
* This class is \b not virtual for performance reasons - the derived class
* SymbolicFactor needs to be created and destroyed quickly during symbolic
* elimination. GaussianFactor and NonlinearFactor are virtual.
* Note that derived classes must also redefine the `This` and `shared_ptr`
* typedefs. See JacobianFactor, etc. for examples.
*
* \nosubgrouping
*/
@ -148,6 +151,15 @@ typedef FastSet<FactorIndex> FactorIndexSet;
/** Iterator at end of involved variable keys */
const_iterator end() const { return keys_.end(); }
/**
* All factor types need to implement an error function.
* In factor graphs, this is the negative log-likelihood.
* In Bayes nets, it is the negative log density, i.e., properly normalized.
*/
virtual double error(const HybridValues& c) const {
throw std::runtime_error("Factor::error is not implemented");
}
/**
* @return the number of variables involved in this factor
*/
@ -172,7 +184,6 @@ typedef FastSet<FactorIndex> FactorIndexSet;
bool equals(const This& other, double tol = 1e-9) const;
/// @}
/// @name Advanced Interface
/// @{
@ -185,7 +196,13 @@ typedef FastSet<FactorIndex> FactorIndexSet;
/** Iterator at end of involved variable keys */
iterator end() { return keys_.end(); }
/// @}
private:
/// @name Serialization
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
@ -197,4 +214,4 @@ typedef FastSet<FactorIndex> FactorIndexSet;
};
}
} // \namespace gtsam

19
gtsam/linear/GaussianBayesNet.cpp
View File

@ -107,6 +107,11 @@ namespace gtsam {
return GaussianFactorGraph(*this).error(x);
}
/* ************************************************************************* */
double GaussianBayesNet::evaluate(const VectorValues& x) const {
return exp(-error(x));
}
/* ************************************************************************* */
VectorValues GaussianBayesNet::backSubstitute(const VectorValues& rhs) const
{
@ -225,19 +230,5 @@ namespace gtsam {
}
/* ************************************************************************* */
double GaussianBayesNet::logDensity(const VectorValues& x) const {
double sum = 0.0;
for (const auto& conditional : *this) {
if (conditional) sum += conditional->logDensity(x);
}
return sum;
}
/* ************************************************************************* */
double GaussianBayesNet::evaluate(const VectorValues& x) const {
return exp(logDensity(x));
}
/* ************************************************************************* */
} // namespace gtsam

22
gtsam/linear/GaussianBayesNet.h
View File

@ -98,10 +98,16 @@ namespace gtsam {
/// @{
/**
* Calculate probability density for given values `x`:
* exp(-error(x)) / sqrt((2*pi)^n*det(Sigma))
* 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.
*/
double error(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.
* Note that error(x)=0.5*e'*e includes the 0.5 factor already.
*/
double evaluate(const VectorValues& x) const;
@ -110,13 +116,6 @@ namespace gtsam {
return evaluate(x);
}
/**
* Calculate log-density 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.
*/
double logDensity(const VectorValues& x) const;
/// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by
/// back-substitution
VectorValues optimize() const;
@ -216,9 +215,6 @@ namespace gtsam {
* allocateVectorValues */
VectorValues gradientAtZero() const;
/** 0.5 * sum of squared Mahalanobis distances. */
double error(const VectorValues& x) const;
/**
* Computes the determinant of a GassianBayesNet. A GaussianBayesNet is an upper triangular
* matrix and for an upper triangular matrix determinant is the product of the diagonal

8
gtsam/linear/GaussianConditional.cpp
View File

@ -193,14 +193,14 @@ double GaussianConditional::logNormalizationConstant() const {
/* ************************************************************************* */
// density = k exp(-error(x))
// log = log(k) -error(x)
double GaussianConditional::logDensity(const VectorValues& x) const {
return logNormalizationConstant() - error(x);
// -log(density) = error(x) - log(k)
double GaussianConditional::error(const VectorValues& x) const {
return JacobianFactor::error(x) - logNormalizationConstant();
}
/* ************************************************************************* */
double GaussianConditional::evaluate(const VectorValues& x) const {
return exp(logDensity(x));
return exp(-error(x));
}
/* ************************************************************************* */

120
gtsam/linear/GaussianConditional.h
View File

@ -132,64 +132,6 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/**
* Calculate probability density for given values `x`:
* exp(-error(x)) / sqrt((2*pi)^n*det(Sigma))
* where x is the vector of values, and Sigma is the covariance matrix.
* Note that error(x)=0.5*e'*e includes the 0.5 factor already.
*/
double evaluate(const VectorValues& x) const;
/// Evaluate probability density, sugar.
double operator()(const VectorValues& x) const {
return evaluate(x);
}
/**
* Calculate log-density 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.
*/
double logDensity(const VectorValues& x) const;
/** Return a view of the upper-triangular R block of the conditional */
constABlock R() const { return Ab_.range(0, nrFrontals()); }
/** Get a view of the parent blocks. */
constABlock S() const { return Ab_.range(nrFrontals(), size()); }
/** Get a view of the S matrix for the variable pointed to by the given key iterator */
constABlock S(const_iterator it) const { return BaseFactor::getA(it); }
/** Get a view of the r.h.s. vector d */
const constBVector d() const { return BaseFactor::getb(); }
/**
* @brief Compute the determinant of the R matrix.
*
* The determinant is computed in log form using logDeterminant for
* numerical stability and then exponentiated.
*
* Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
* \f$ \det(\Sigma) = 1 / \det(R^T R) = 1 / determinant()^ 2 \f$.
*
* @return double
*/
inline double determinant() const { return exp(logDeterminant()); }
/**
* @brief Compute the log determinant of the R matrix.
*
* For numerical stability, the determinant is computed in log
* form, so it is a summation rather than a multiplication.
*
* Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
* \f$ \log \det(\Sigma) = - \log \det(R^T R) = - 2 logDeterminant() \f$.
*
* @return double
*/
double logDeterminant() const;
/**
* normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
* log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
@ -203,6 +145,26 @@ namespace gtsam {
return exp(logNormalizationConstant());
}
/**
* Calculate error(x) == -log(evaluate()) for given values `x`:
* - GaussianFactor::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.
*/
double error(const VectorValues& x) const override;
/**
* Calculate probability density for given values `x`:
* exp(-error(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;
/// Evaluate probability density, sugar.
double operator()(const VectorValues& x) const {
return evaluate(x);
}
/**
* Solves a conditional Gaussian and writes the solution into the entries of
* \c x for each frontal variable of the conditional. The parents are
@ -255,6 +217,48 @@ namespace gtsam {
VectorValues sample(const VectorValues& parentsValues) const;
/// @}
/// @name Linear algebra.
/// @{
/** Return a view of the upper-triangular R block of the conditional */
constABlock R() const { return Ab_.range(0, nrFrontals()); }
/** Get a view of the parent blocks. */
constABlock S() const { return Ab_.range(nrFrontals(), size()); }
/** Get a view of the S matrix for the variable pointed to by the given key iterator */
constABlock S(const_iterator it) const { return BaseFactor::getA(it); }
/** Get a view of the r.h.s. vector d */
const constBVector d() const { return BaseFactor::getb(); }
/**
* @brief Compute the determinant of the R matrix.
*
* The determinant is computed in log form using logDeterminant for
* numerical stability and then exponentiated.
*
* Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
* \f$ \det(\Sigma) = 1 / \det(R^T R) = 1 / determinant()^ 2 \f$.
*
* @return double
*/
inline double determinant() const { return exp(logDeterminant()); }
/**
* @brief Compute the log determinant of the R matrix.
*
* For numerical stability, the determinant is computed in log
* form, so it is a summation rather than a multiplication.
*
* Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
* \f$ \log \det(\Sigma) = - \log \det(R^T R) = - 2 logDeterminant() \f$.
*
* @return double
*/
double logDeterminant() const;
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @name Deprecated

18
gtsam/linear/GaussianFactor.cpp
View File

@ -18,16 +18,22 @@
// \callgraph
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
/* ************************************************************************* */
VectorValues GaussianFactor::hessianDiagonal() const {
VectorValues d;
hessianDiagonalAdd(d);
return d;
}
const VectorValues& GetVectorValues(const HybridValues& c) {
return c.continuous();
}
/* ************************************************************************* */
VectorValues GaussianFactor::hessianDiagonal() const {
VectorValues d;
hessianDiagonalAdd(d);
return d;
}
} // namespace gtsam

23
gtsam/linear/GaussianFactor.h
View File

@ -31,6 +31,9 @@ namespace gtsam {
class Scatter;
class SymmetricBlockMatrix;
// Forward declaration of function to extract VectorValues from HybridValues.
const VectorValues& GetVectorValues(const HybridValues& c);
/**
* An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a
* quadratic error function. GaussianFactor is non-mutable (all methods const!). The factor value
@ -63,8 +66,24 @@ namespace gtsam {
/** Equals for testable */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0;
/** Print for testable */
virtual double error(const VectorValues& c) const = 0; /** 0.5*(A*x-b)'*D*(A*x-b) */
/**
* In Gaussian factors, the error function returns either the negative log-likelihood, e.g.,
* 0.5*(A*x-b)'*D*(A*x-b)
* for a \class JacobianFactor, or the negative log-density, e.g.,
* 0.5*(A*x-b)'*D*(A*x-b) - log(k)
* for a \class GaussianConditional, where k is the normalization constant.
*/
virtual double error(const VectorValues& c) const {
throw std::runtime_error("GaussianFactor::error::error is not implemented");
}
/**
* The factor::error simply extracts the \class VectorValues from the
* \class HybridValues and calculates the error.
*/
double error(const HybridValues& c) const override {
return GaussianFactor::error(GetVectorValues(c));
}
/** Return the dimension of the variable pointed to by the given key iterator */
virtual DenseIndex getDim(const_iterator variable) const = 0;

4
gtsam/linear/linear.i
View File

@ -498,7 +498,7 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
// Standard Interface
double evaluate(const gtsam::VectorValues& x) const;
double logDensity(const gtsam::VectorValues& x) const;
double error(const gtsam::VectorValues& x) const;
gtsam::Key firstFrontalKey() const;
gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
gtsam::JacobianFactor* likelihood(
@ -559,7 +559,7 @@ virtual class GaussianBayesNet {
// Standard interface
double evaluate(const gtsam::VectorValues& x) const;
double logDensity(const gtsam::VectorValues& x) const;
double error(const gtsam::VectorValues& x) const;
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(const gtsam::VectorValues& given) const;

6
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -16,12 +16,18 @@
* @author Richard Roberts
*/
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/make_shared.hpp>
#include <boost/format.hpp>
namespace gtsam {
/* ************************************************************************* */
const Values& GetValues(const HybridValues& c) {
return c.nonlinear();
}
/* ************************************************************************* */
void NonlinearFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const {

28
gtsam/nonlinear/NonlinearFactor.h
View File

@ -34,6 +34,9 @@ namespace gtsam {
/* ************************************************************************* */
// Forward declaration of function to extract Values from HybridValues.
const Values& GetValues(const HybridValues& c);
/**
* Nonlinear factor base class
*
@ -74,6 +77,7 @@ public:
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
@ -81,13 +85,29 @@ public:
/** Destructor */
virtual ~NonlinearFactor() {}
/**
* In nonlinear factors, the error function returns the negative log-likelihood
* as a non-linear function of the values in a \class Values object.
*
* The idea is that Gaussian factors have a quadratic error function that locally
* approximates the negative log-likelihood, and are obtained by \b linearizing
* the nonlinear error function at a given linearization.
*
* The derived class, \class NoiseModelFactor, adds a noise model to the factor,
* and calculates the error by asking the user to implement the method
* \code double evaluateError(const Values& c) const \endcode.
*/
virtual double error(const Values& c) const {
throw std::runtime_error("NonlinearFactor::error::error is not implemented");
}
/**
* Calculate the error of the factor
* This is typically equal to log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/sigma^2 \f$ in case of Gaussian.
* You can override this for systems with unusual noise models.
* The factor::error simply extracts the \class Values from the
* \class HybridValues and calculates the error.
*/
virtual double error(const Values& c) const = 0;
double error(const HybridValues& c) const override {
return NonlinearFactor::error(GetValues(c));
}
/** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const = 0;