diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 79f5f8fa7..a8010e17c 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -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`. diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 628ac5fb1..1b12255ac 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -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); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index bab38aa07..5d287c238 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -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_; } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 2d5887309..2fa5e3f88 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -29,12 +29,16 @@ #include namespace gtsam { -/// Define collection types: -typedef FastVector FactorIndices; -typedef FastSet FactorIndexSet; + + /// Define collection types: + typedef FastVector FactorIndices; + typedef FastSet 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 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 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 FactorIndexSet; bool equals(const This& other, double tol = 1e-9) const; /// @} - /// @name Advanced Interface /// @{ @@ -185,7 +196,13 @@ typedef FastSet FactorIndexSet; /** Iterator at end of involved variable keys */ iterator end() { return keys_.end(); } + /// @} + private: + + /// @name Serialization + /// @{ + /** Serialization function */ friend class boost::serialization::access; template @@ -197,4 +214,4 @@ typedef FastSet FactorIndexSet; }; -} +} // \namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 52dc49347..0a34a25e3 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -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 diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 62366b602..8c40d9728 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -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 diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 9fd217abf..db822ffba 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -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)); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 05b8b86b8..c838051cf 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -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 diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp index 0802deff4..74582ecdc 100644 --- a/gtsam/linear/GaussianFactor.cpp +++ b/gtsam/linear/GaussianFactor.cpp @@ -18,16 +18,22 @@ // \callgraph +#include #include #include 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 diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 672f5aa0d..327147b66 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -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; diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index d733340c9..41bce61d1 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -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; diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index debff54ac..ec943e9b1 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -16,12 +16,18 @@ * @author Richard Roberts */ +#include #include #include #include namespace gtsam { +/* ************************************************************************* */ +const Values& GetValues(const HybridValues& c) { + return c.nonlinear(); +} + /* ************************************************************************* */ void NonlinearFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 622d4de37..c33e2b531 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -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;