From e0a40b306db8c30a30e959ba90ff02bcde724a6a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 16:09:20 -0800 Subject: [PATCH 01/21] Added nonlinear Values --- gtsam/hybrid/HybridValues.h | 64 ++++++++++++++++++++++++++----------- 1 file changed, 45 insertions(+), 19 deletions(-) diff --git a/gtsam/hybrid/HybridValues.h b/gtsam/hybrid/HybridValues.h index 4c4f5fa1e4..3a0ad516a6 100644 --- a/gtsam/hybrid/HybridValues.h +++ b/gtsam/hybrid/HybridValues.h @@ -37,12 +37,15 @@ namespace gtsam { */ class GTSAM_EXPORT HybridValues { private: - // VectorValue stored the continuous components of the HybridValues. + /// Continuous multi-dimensional vectors for \class GaussianFactor. VectorValues continuous_; - // DiscreteValue stored the discrete components of the HybridValues. + /// Discrete values for \class DiscreteFactor. DiscreteValues discrete_; + /// Continuous, differentiable manifold values for \class NonlinearFactor. + Values nonlinear_; + public: /// @name Standard Constructors /// @{ @@ -54,6 +57,11 @@ class GTSAM_EXPORT HybridValues { HybridValues(const VectorValues& cv, const DiscreteValues& dv) : continuous_(cv), discrete_(dv){}; + /// Construct from all values types. + HybridValues(const VectorValues& cv, const DiscreteValues& dv, + const Values& v) + : continuous_(cv), discrete_(dv), nonlinear_(v){}; + /// @} /// @name Testable /// @{ @@ -77,26 +85,30 @@ class GTSAM_EXPORT HybridValues { /// @name Interface /// @{ - /// Return the discrete MPE assignment + /// Return the multi-dimensional vector values. + const VectorValues& continuous() const { return continuous_; } + + /// Return the discrete values. const DiscreteValues& discrete() const { return discrete_; } - /// Return the delta update for the continuous vectors - const VectorValues& continuous() const { return continuous_; } + /// Return the nonlinear values. + const Values& nonlinear() const { return nonlinear_; } - /// Check whether a variable with key \c j exists in DiscreteValue. + /// Check whether a variable with key \c j exists in VectorValues. + bool existsVector(Key j) { return continuous_.exists(j); }; + + /// Check whether a variable with key \c j exists in DiscreteValues. bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); }; - /// Check whether a variable with key \c j exists in VectorValue. - bool existsVector(Key j) { return continuous_.exists(j); }; + /// Check whether a variable with key \c j exists in values. + bool existsNonlinear(Key j) { + return (nonlinear_.find(j) != nonlinear_.end()); + }; /// Check whether a variable with key \c j exists. - bool exists(Key j) { return existsDiscrete(j) || existsVector(j); }; - - /** Insert a discrete \c value with key \c j. Replaces the existing value if - * the key \c j is already used. - * @param value The vector to be inserted. - * @param j The index with which the value will be associated. */ - void insert(Key j, size_t value) { discrete_[j] = value; }; + bool exists(Key j) { + return existsVector(j) || existsDiscrete(j) || existsNonlinear(j); + }; /** Insert a vector \c value with key \c j. Throws an invalid_argument * exception if the key \c j is already used. @@ -104,6 +116,12 @@ class GTSAM_EXPORT HybridValues { * @param j The index with which the value will be associated. */ void insert(Key j, const Vector& value) { continuous_.insert(j, value); } + /** Insert a discrete \c value with key \c j. Replaces the existing value if + * the key \c j is already used. + * @param value The vector to be inserted. + * @param j The index with which the value will be associated. */ + void insert(Key j, size_t value) { discrete_[j] = value; }; + /** Insert all continuous values from \c values. Throws an invalid_argument * exception if any keys to be inserted are already used. */ HybridValues& insert(const VectorValues& values) { @@ -118,27 +136,35 @@ class GTSAM_EXPORT HybridValues { return *this; } + /** Insert all values from \c values. Throws an invalid_argument + * exception if any keys to be inserted are already used. */ + HybridValues& insert(const Values& values) { + nonlinear_.insert(values); + return *this; + } + /** Insert all values from \c values. Throws an invalid_argument exception if * any keys to be inserted are already used. */ HybridValues& insert(const HybridValues& values) { continuous_.insert(values.continuous()); discrete_.insert(values.discrete()); + nonlinear_.insert(values.nonlinear()); return *this; } // TODO(Shangjie)- insert_or_assign() , similar to Values.h /** - * Read/write access to the discrete value with key \c j, throws + * Read/write access to the vector value with key \c j, throws * std::out_of_range if \c j does not exist. */ - size_t& atDiscrete(Key j) { return discrete_.at(j); }; + Vector& at(Key j) { return continuous_.at(j); }; /** - * Read/write access to the vector value with key \c j, throws + * Read/write access to the discrete value with key \c j, throws * std::out_of_range if \c j does not exist. */ - Vector& at(Key j) { return continuous_.at(j); }; + size_t& atDiscrete(Key j) { return discrete_.at(j); }; /** For all key/value pairs in \c values, replace continuous values with * corresponding keys in this object with those in \c values. Throws From aed576ca0a91baf287f77e0aa54c3c093cf4118a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 16:11:23 -0800 Subject: [PATCH 02/21] Added better docs --- gtsam/inference/Factor.h | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 1fb2b8df1a..2d5887309d 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -38,8 +38,28 @@ typedef FastSet FactorIndexSet; * data other than its keys. Derived classes store data such as matrices and * probability tables. * - * Note that derived classes *must* redefine the `This` and `shared_ptr` - * typedefs. See JacobianFactor, etc. for examples. + * The `error` method is used to evaluate the factor, and is the only method + * that is required to be implemented in derived classes, although it has a + * default implementation that throws an exception. The meaning of the error + * is slightly different for factors and conditionals: in the former it is the + * negative log-likelihood, and in the latter it is the negative log of the + * properly normalized conditional distribution or density. + * + * There are five broad classes of factors that derive from Factor: + * + * - \b Nonlinear factors, such as \class NonlinearFactor and \class NoiseModelFactor, which + * represent a nonlinear likelihood function over a set of variables. + * - \b Gaussian factors, such as \class JacobianFactor and \class HessianFactor, which + * represent a Gaussian likelihood over a set of variables. + * A \class GaussianConditional, which represent a Gaussian density over a set of + * variables conditioned on another set of variables. + * - \b Discrete factors, such as \class DiscreteFactor and \class DiscreteConditional, which + * represent a discrete distribution over a set of variables. + * - \b Hybrid factors, such as \class HybridFactor, which represent a mixture of + * Gaussian and discrete distributions over a set of variables. + * - \b Symbolic factors, used to represent a graph structure, such as + * \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 From 877e564744114a2718e64a14ac59437c02cb713a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 16:11:36 -0800 Subject: [PATCH 03/21] Declared and defined error method in Factor, getting rid of logDensity, as error is now unambiguously defined as -logDensity in conditional factors. --- gtsam/hybrid/GaussianMixture.h | 3 - gtsam/hybrid/HybridBayesNet.cpp | 8 +- gtsam/hybrid/HybridFactor.h | 9 -- gtsam/inference/Factor.h | 35 ++++++-- gtsam/linear/GaussianBayesNet.cpp | 19 ++--- gtsam/linear/GaussianBayesNet.h | 20 ++--- gtsam/linear/GaussianConditional.cpp | 8 +- gtsam/linear/GaussianConditional.h | 118 ++++++++++++++------------- gtsam/linear/GaussianFactor.cpp | 16 ++-- gtsam/linear/GaussianFactor.h | 23 +++++- gtsam/linear/linear.i | 4 +- gtsam/nonlinear/NonlinearFactor.cpp | 6 ++ gtsam/nonlinear/NonlinearFactor.h | 28 ++++++- 13 files changed, 172 insertions(+), 125 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 79f5f8fa7a..a8010e17cc 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 628ac5fb1b..1b12255ac2 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 bab38aa079..5d287c2381 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 2d5887309d..2fa5e3f889 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 52dc49347a..0a34a25e3a 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 62366b602f..8c40d97287 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -97,11 +97,17 @@ 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. + */ + double error(const VectorValues& x) const; + /** * Calculate probability density for given values `x`: - * exp(-error(x)) / sqrt((2*pi)^n*det(Sigma)) + * 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 9fd217abf8..db822ffba0 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 05b8b86b88..c838051cf7 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -133,74 +133,36 @@ namespace gtsam { /// @{ /** - * 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. + * normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) + * log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) */ - 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(); } + double logNormalizationConstant() const; /** - * @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 + * normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) */ - inline double determinant() const { return exp(logDeterminant()); } + inline double normalizationConstant() const { + return exp(logNormalizationConstant()); + } /** - * @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 + * 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 logDeterminant() const; + double error(const VectorValues& x) const override; /** - * normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) - * log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) + * 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 logNormalizationConstant() const; + double evaluate(const VectorValues& x) const; - /** - * normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) - */ - inline double normalizationConstant() const { - return exp(logNormalizationConstant()); + /// Evaluate probability density, sugar. + double operator()(const VectorValues& x) const { + return evaluate(x); } /** @@ -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 0802deff4e..74582ecdc9 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 672f5aa0d9..327147b66a 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 d733340c90..41bce61d18 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 debff54acd..ec943e9b1f 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 622d4de376..c33e2b531a 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 @@ class GTSAM_EXPORT NonlinearFactor: public Factor { /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; + /// @} /// @name Standard Interface /// @{ @@ -81,13 +85,29 @@ class GTSAM_EXPORT NonlinearFactor: public Factor { /** 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; From 4858e39ecf1df0ff057c50230f9a2c9420a40309 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 16:12:00 -0800 Subject: [PATCH 04/21] Fixed tests to work with new definition of error. --- gtsam/hybrid/tests/testGaussianMixture.cpp | 12 +++++----- gtsam/hybrid/tests/testHybridBayesNet.cpp | 9 ++++---- .../linear/tests/testGaussianConditional.cpp | 22 +++++++++++-------- gtsam/linear/tests/testGaussianDensity.cpp | 2 +- 4 files changed, 25 insertions(+), 20 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index ff8edd46e7..0b28669217 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -118,9 +118,10 @@ TEST(GaussianMixture, Error) { values.insert(X(2), Vector2::Zero()); auto error_tree = mixture.error(values); - // regression + // Check result. std::vector discrete_keys = {m1}; - std::vector leaves = {0.5, 4.3252595}; + std::vector leaves = {conditional0->error(values), + conditional1->error(values)}; AlgebraicDecisionTree expected_error(discrete_keys, leaves); EXPECT(assert_equal(expected_error, error_tree, 1e-6)); @@ -128,10 +129,11 @@ TEST(GaussianMixture, Error) { // Regression for non-tree version. DiscreteValues assignment; assignment[M(1)] = 0; - EXPECT_DOUBLES_EQUAL(0.5, mixture.error({values, assignment}), 1e-8); + EXPECT_DOUBLES_EQUAL(conditional0->error(values), + mixture.error({values, assignment}), 1e-8); assignment[M(1)] = 1; - EXPECT_DOUBLES_EQUAL(4.3252595155709335, mixture.error({values, assignment}), - 1e-8); + EXPECT_DOUBLES_EQUAL(conditional1->error(values), + mixture.error({values, assignment}), 1e-8); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index bdf858b22f..d62626ea60 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -217,23 +217,22 @@ TEST(HybridBayesNet, Error) { auto error_tree = hybridBayesNet->error(delta.continuous()); std::vector discrete_keys = {{M(0), 2}, {M(1), 2}}; - std::vector leaves = {0.0097568009, 3.3973404e-31, 0.029126214, - 0.0097568009}; + std::vector leaves = {-4.1609374, -4.1706942, -4.141568, -4.1609374}; AlgebraicDecisionTree expected_error(discrete_keys, leaves); // regression - EXPECT(assert_equal(expected_error, error_tree, 1e-9)); + EXPECT(assert_equal(expected_error, error_tree, 1e-6)); // Error on pruned Bayes net auto prunedBayesNet = hybridBayesNet->prune(2); auto pruned_error_tree = prunedBayesNet.error(delta.continuous()); - std::vector pruned_leaves = {2e50, 3.3973404e-31, 2e50, 0.0097568009}; + std::vector pruned_leaves = {2e50, -4.1706942, 2e50, -4.1609374}; AlgebraicDecisionTree expected_pruned_error(discrete_keys, pruned_leaves); // regression - EXPECT(assert_equal(expected_pruned_error, pruned_error_tree, 1e-9)); + EXPECT(assert_equal(expected_pruned_error, pruned_error_tree, 1e-6)); // Verify error computation and check for specific error value DiscreteValues discrete_values{{M(0), 1}, {M(1), 1}}; diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index e4bdbdffc4..9734854845 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -381,13 +381,13 @@ 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); + double expected1 = 0.5 * e1.dot(e1) - conditional1.logNormalizationConstant(); EXPECT_DOUBLES_EQUAL(expected1, conditional1.error(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); + double expected2 = 0.5 * e2.dot(e2) - conditional2.logNormalizationConstant(); EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9); } @@ -448,20 +448,23 @@ TEST(GaussianConditional, sample) { } /* ************************************************************************* */ -TEST(GaussianConditional, LogNormalizationConstant) { +TEST(GaussianConditional, Error) { // Create univariate standard gaussian conditional - auto std_gaussian = + auto stdGaussian = GaussianConditional::FromMeanAndStddev(X(0), Vector1::Zero(), 1.0); VectorValues values; values.insert(X(0), Vector1::Zero()); - double logDensity = std_gaussian.logDensity(values); + double error = stdGaussian.error(values); // Regression. // These values were computed by hand for a univariate standard gaussian. - EXPECT_DOUBLES_EQUAL(-0.9189385332046727, logDensity, 1e-9); - EXPECT_DOUBLES_EQUAL(0.3989422804014327, exp(logDensity), 1e-9); + EXPECT_DOUBLES_EQUAL(0.9189385332046727, error, 1e-9); + EXPECT_DOUBLES_EQUAL(0.3989422804014327, exp(-error), 1e-9); +} - // Similar test for multivariate gaussian but with sigma 2.0 +/* ************************************************************************* */ +// Similar test for multivariate gaussian but with sigma 2.0 +TEST(GaussianConditional, LogNormalizationConstant) { double sigma = 2.0; auto conditional = GaussianConditional::FromMeanAndStddev(X(0), Vector3::Zero(), sigma); VectorValues x; @@ -469,7 +472,8 @@ TEST(GaussianConditional, LogNormalizationConstant) { Matrix3 Sigma = I_3x3 * sigma * sigma; double expectedLogNormalizingConstant = log(1 / sqrt((2 * M_PI * Sigma).determinant())); - EXPECT_DOUBLES_EQUAL(expectedLogNormalizingConstant, conditional.logNormalizationConstant(), 1e-9); + EXPECT_DOUBLES_EQUAL(expectedLogNormalizingConstant, + conditional.logNormalizationConstant(), 1e-9); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index 14608e7949..3e73bce132 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -52,7 +52,7 @@ TEST(GaussianDensity, FromMeanAndStddev) { auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma); Vector2 e = (x0 - b) / sigma; - double expected = 0.5 * e.dot(e); + double expected = 0.5 * e.dot(e) - density.logNormalizationConstant(); EXPECT_DOUBLES_EQUAL(expected, density.error(values), 1e-9); } From f6d7be97da10a58b1e2005429bb6dfcb69e763d1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 16:35:01 -0800 Subject: [PATCH 05/21] small doc edits --- gtsam/linear/GaussianFactor.h | 2 +- gtsam/nonlinear/NonlinearFactor.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 327147b66a..8a50541f56 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -78,7 +78,7 @@ namespace gtsam { } /** - * The factor::error simply extracts the \class VectorValues from the + * The Factor::error simply extracts the \class VectorValues from the * \class HybridValues and calculates the error. */ double error(const HybridValues& c) const override { diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index c33e2b531a..d046b91cec 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -98,11 +98,11 @@ class GTSAM_EXPORT NonlinearFactor: public Factor { * \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"); + throw std::runtime_error("NonlinearFactor::error is not implemented"); } /** - * The factor::error simply extracts the \class Values from the + * The Factor::error simply extracts the \class Values from the * \class HybridValues and calculates the error. */ double error(const HybridValues& c) const override { From 58bc4b6863cf04e9c68978e5fce83f07013bb78e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 16:37:39 -0800 Subject: [PATCH 06/21] Fixed discrete classes. --- gtsam/discrete/DecisionTreeFactor.h | 2 +- gtsam/discrete/DiscreteFactor.cpp | 16 ++++++++++++++++ gtsam/discrete/DiscreteFactor.h | 13 +++++++++++++ gtsam/discrete/tests/testDecisionTreeFactor.cpp | 3 +++ 4 files changed, 33 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index f1df7ae038..f759c10f33 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -97,7 +97,7 @@ namespace gtsam { /// @name Standard Interface /// @{ - /// Value is just look up in AlgebraicDecisonTree + /// Value is just look up in AlgebraicDecisionTree. double operator()(const DiscreteValues& values) const override { return ADT::operator()(values); } diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index 08309e2e17..c60594f57a 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -27,6 +28,21 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +const DiscreteValues& GetDiscreteValues(const HybridValues& c) { + return c.discrete(); +} + +/* ************************************************************************* */ +double DiscreteFactor::error(const DiscreteValues& values) const { + return -std::log((*this)(values)); +} + +/* ************************************************************************* */ +double DiscreteFactor::error(const HybridValues& c) const { + return DiscreteFactor::error(GetDiscreteValues(c)); +} + /* ************************************************************************* */ std::vector expNormalize(const std::vector& logProbs) { double maxLogProb = -std::numeric_limits::infinity(); diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index c6bb079fb5..dc0adbab4a 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -27,6 +27,10 @@ namespace gtsam { class DecisionTreeFactor; class DiscreteConditional; +class HybridValues; + +// Forward declaration of function to extract Values from HybridValues. +const DiscreteValues& GetDiscreteValues(const HybridValues& c); /** * Base class for discrete probabilistic factors @@ -83,6 +87,15 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { /// Find value for given assignment of values to variables virtual double operator()(const DiscreteValues&) const = 0; + /// Error is just -log(value) + double error(const DiscreteValues& values) const; + + /** + * The Factor::error simply extracts the \class DiscreteValues from the + * \class HybridValues and calculates the error. + */ + double error(const HybridValues& c) const override; + /// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index 869b3c6303..3dbb3e64f3 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -48,6 +48,9 @@ TEST( DecisionTreeFactor, constructors) EXPECT_DOUBLES_EQUAL(8, f1(values), 1e-9); EXPECT_DOUBLES_EQUAL(7, f2(values), 1e-9); EXPECT_DOUBLES_EQUAL(75, f3(values), 1e-9); + + // Assert that error = -log(value) + EXPECT_DOUBLES_EQUAL(-log(f1(values)), f1.error(values), 1e-9); } /* ************************************************************************* */ From b4706bec851022b222a2885afed4455d1a950e0c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 17:13:13 -0800 Subject: [PATCH 07/21] Easier scheme for error(HybridValues) --- gtsam/discrete/DiscreteFactor.cpp | 7 +------ gtsam/discrete/DiscreteFactor.h | 3 --- gtsam/linear/GaussianFactor.cpp | 10 ++++++---- gtsam/linear/GaussianFactor.h | 11 ++--------- gtsam/nonlinear/NonlinearFactor.cpp | 9 +++++++-- gtsam/nonlinear/NonlinearFactor.h | 11 ++--------- 6 files changed, 18 insertions(+), 33 deletions(-) diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index c60594f57a..2b1bc36a3a 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -28,11 +28,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -const DiscreteValues& GetDiscreteValues(const HybridValues& c) { - return c.discrete(); -} - /* ************************************************************************* */ double DiscreteFactor::error(const DiscreteValues& values) const { return -std::log((*this)(values)); @@ -40,7 +35,7 @@ double DiscreteFactor::error(const DiscreteValues& values) const { /* ************************************************************************* */ double DiscreteFactor::error(const HybridValues& c) const { - return DiscreteFactor::error(GetDiscreteValues(c)); + return this->error(c.discrete()); } /* ************************************************************************* */ diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index dc0adbab4a..712e1ff75b 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -29,9 +29,6 @@ class DecisionTreeFactor; class DiscreteConditional; class HybridValues; -// Forward declaration of function to extract Values from HybridValues. -const DiscreteValues& GetDiscreteValues(const HybridValues& c); - /** * Base class for discrete probabilistic factors * The most general one is the derived DecisionTreeFactor diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp index 74582ecdc9..e60e626a1d 100644 --- a/gtsam/linear/GaussianFactor.cpp +++ b/gtsam/linear/GaussianFactor.cpp @@ -24,12 +24,14 @@ namespace gtsam { -/* ************************************************************************* */ -const VectorValues& GetVectorValues(const HybridValues& c) { - return c.continuous(); +double GaussianFactor::error(const VectorValues& c) const { + throw std::runtime_error("GaussianFactor::error is not implemented"); +} + +double GaussianFactor::error(const HybridValues& c) const { + return this->error(c.continuous()); } -/* ************************************************************************* */ VectorValues GaussianFactor::hessianDiagonal() const { VectorValues d; hessianDiagonalAdd(d); diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 8a50541f56..eb4ea7e06d 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -31,9 +31,6 @@ 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 @@ -73,17 +70,13 @@ namespace gtsam { * 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"); - } + virtual double error(const VectorValues& c) const; /** * 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)); - } + double error(const HybridValues& c) const override; /** 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/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index ec943e9b1f..b669be4723 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -24,8 +24,13 @@ namespace gtsam { /* ************************************************************************* */ -const Values& GetValues(const HybridValues& c) { - return c.nonlinear(); +double NonlinearFactor::error(const Values& c) const { + throw std::runtime_error("NonlinearFactor::error is not implemented"); +} + +/* ************************************************************************* */ +double NonlinearFactor::error(const HybridValues& c) const { + return this->error(c.nonlinear()); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index d046b91cec..9ccd0246f7 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -34,9 +34,6 @@ namespace gtsam { /* ************************************************************************* */ -// Forward declaration of function to extract Values from HybridValues. -const Values& GetValues(const HybridValues& c); - /** * Nonlinear factor base class * @@ -97,17 +94,13 @@ class GTSAM_EXPORT NonlinearFactor: public 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 is not implemented"); - } + virtual double error(const Values& c) const; /** * The Factor::error simply extracts the \class Values from the * \class HybridValues and calculates the error. */ - double error(const HybridValues& c) const override { - return NonlinearFactor::error(GetValues(c)); - } + double error(const HybridValues& c) const override; /** get the dimension of the factor (number of rows on linearization) */ virtual size_t dim() const = 0; From 83bae7d70158038166b6f912ef71620f6a653154 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 18:19:12 -0800 Subject: [PATCH 08/21] Moved factor graph error(HybridValues) to FactorGraph base class. --- gtsam/hybrid/HybridBayesNet.cpp | 40 ++----------------- gtsam/hybrid/HybridBayesNet.h | 9 +---- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 18 --------- gtsam/hybrid/HybridGaussianFactorGraph.h | 10 +---- gtsam/hybrid/HybridNonlinearFactorGraph.h | 7 ++++ gtsam/hybrid/tests/testHybridBayesNet.cpp | 34 +++++++--------- .../tests/testHybridNonlinearFactorGraph.cpp | 10 +++-- gtsam/inference/FactorGraph-inst.h | 10 +++++ gtsam/inference/FactorGraph.h | 5 +++ gtsam/linear/GaussianConditional.h | 2 + 10 files changed, 52 insertions(+), 93 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 1b12255ac2..e662b9c819 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -257,29 +257,7 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const { /* ************************************************************************* */ double HybridBayesNet::evaluate(const HybridValues &values) const { - const DiscreteValues &discreteValues = values.discrete(); - const VectorValues &continuousValues = values.continuous(); - - 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); - error += component->error(continuousValues); - - } else if (auto gc = conditional->asGaussian()) { - // If continuous only, evaluate the probability and multiply. - error += gc->error(continuousValues); - - } else if (auto dc = conditional->asDiscrete()) { - // Conditional is discrete-only, so return its probability. - probability *= dc->operator()(discreteValues); - } - } - - return probability * exp(-error); + return exp(-error(values)); } /* ************************************************************************* */ @@ -317,12 +295,6 @@ HybridValues HybridBayesNet::sample() const { return sample(&kRandomNumberGenerator); } -/* ************************************************************************* */ -double HybridBayesNet::error(const HybridValues &values) const { - GaussianBayesNet gbn = choose(values.discrete()); - return gbn.error(values.continuous()); -} - /* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::error( const VectorValues &continuousValues) const { @@ -332,19 +304,15 @@ AlgebraicDecisionTree HybridBayesNet::error( for (auto &&conditional : *this) { if (auto gm = conditional->asMixture()) { // If conditional is hybrid, select based on assignment and compute error. - AlgebraicDecisionTree conditional_error = - gm->error(continuousValues); - - error_tree = error_tree + conditional_error; + error_tree = error_tree + gm->error(continuousValues); } else if (auto gc = conditional->asGaussian()) { - // If continuous only, get the (double) error - // and add it to the error_tree + // If continuous, get the (double) error and add it to the error_tree double error = gc->error(continuousValues); // Add the computed error to every leaf of the error tree. error_tree = error_tree.apply( [error](double leaf_value) { return leaf_value + error; }); } else if (auto dc = conditional->asDiscrete()) { - // Conditional is discrete-only, we skip. + // TODO(dellaert): if discrete, we need to add error in the right branch? continue; } } diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index dd8d38a4cb..c5a16f9ddd 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -187,14 +187,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { /// Prune the Hybrid Bayes Net such that we have at most maxNrLeaves leaves. HybridBayesNet prune(size_t maxNrLeaves); - /** - * @brief 0.5 * sum of squared Mahalanobis distances - * for a specific discrete assignment. - * - * @param values Continuous values and discrete assignment. - * @return double - */ - double error(const HybridValues &values) const; + using Base::error; // Expose error(const HybridValues&) method.. /** * @brief Compute conditional error for each discrete assignment, diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index d0351afbc9..c59187f4eb 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -463,24 +463,6 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( return error_tree; } -/* ************************************************************************ */ -double HybridGaussianFactorGraph::error(const HybridValues &values) const { - double error = 0.0; - for (auto &f : factors_) { - if (auto hf = dynamic_pointer_cast(f)) { - error += hf->error(values.continuous()); - } else if (auto hf = dynamic_pointer_cast(f)) { - // TODO(dellaert): needs to change when we discard other wrappers. - error += hf->error(values); - } else if (auto dtf = dynamic_pointer_cast(f)) { - error -= log((*dtf)(values.discrete())); - } else { - throwRuntimeError("HybridGaussianFactorGraph::error(HV)", f); - } - } - return error; -} - /* ************************************************************************ */ double HybridGaussianFactorGraph::probPrime(const HybridValues &values) const { double error = this->error(values); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 0dc7372500..0db4f734b8 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -145,6 +145,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// @name Standard Interface /// @{ + using Base::error; // Expose error(const HybridValues&) method.. + /** * @brief Compute error for each discrete assignment, * and return as a tree. @@ -156,14 +158,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph */ AlgebraicDecisionTree error(const VectorValues& continuousValues) const; - /** - * @brief Compute error given a continuous vector values - * and a discrete assignment. - * - * @return double - */ - double error(const HybridValues& values) const; - /** * @brief Compute unnormalized probability \f$ P(X | M, Z) \f$ * for each discrete assignment, and return as a tree. diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 60aee431b2..ebefb52cb6 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -55,12 +55,18 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { : Base(graph) {} /// @} + /// @name Constructors + /// @{ /// Print the factor graph. void print( const std::string& s = "HybridNonlinearFactorGraph", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + /// @} + /// @name Standard Interface + /// @{ + /** * @brief Linearize all the continuous factors in the * HybridNonlinearFactorGraph. @@ -70,6 +76,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { */ HybridGaussianFactorGraph::shared_ptr linearize( const Values& continuousValues) const; + /// @} }; template <> diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index d62626ea60..2c114a3350 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -212,6 +212,7 @@ TEST(HybridBayesNet, Error) { HybridBayesNet::shared_ptr hybridBayesNet = s.linearizedFactorGraph.eliminateSequential(); + EXPECT_LONGS_EQUAL(5, hybridBayesNet->size()); HybridValues delta = hybridBayesNet->optimize(); auto error_tree = hybridBayesNet->error(delta.continuous()); @@ -235,26 +236,21 @@ TEST(HybridBayesNet, Error) { EXPECT(assert_equal(expected_pruned_error, pruned_error_tree, 1e-6)); // Verify error computation and check for specific error value - DiscreteValues discrete_values{{M(0), 1}, {M(1), 1}}; - - double total_error = 0; - for (size_t idx = 0; idx < hybridBayesNet->size(); idx++) { - if (hybridBayesNet->at(idx)->isHybrid()) { - double error = hybridBayesNet->at(idx)->asMixture()->error( - {delta.continuous(), discrete_values}); - total_error += error; - } else if (hybridBayesNet->at(idx)->isContinuous()) { - double error = - hybridBayesNet->at(idx)->asGaussian()->error(delta.continuous()); - total_error += error; - } - } + const DiscreteValues discrete_values{{M(0), 1}, {M(1), 1}}; + const HybridValues hybridValues{delta.continuous(), discrete_values}; + double error = 0; + error += hybridBayesNet->at(0)->asMixture()->error(hybridValues); + error += hybridBayesNet->at(1)->asMixture()->error(hybridValues); + error += hybridBayesNet->at(2)->asMixture()->error(hybridValues); + + // TODO(dellaert): the discrete errors are not added in error tree! + EXPECT_DOUBLES_EQUAL(error, error_tree(discrete_values), 1e-9); + EXPECT_DOUBLES_EQUAL(error, pruned_error_tree(discrete_values), 1e-9); + + error += hybridBayesNet->at(3)->asDiscrete()->error(discrete_values); + error += hybridBayesNet->at(4)->asDiscrete()->error(discrete_values); + EXPECT_DOUBLES_EQUAL(error, hybridBayesNet->error(hybridValues), 1e-9); - EXPECT_DOUBLES_EQUAL( - total_error, hybridBayesNet->error({delta.continuous(), discrete_values}), - 1e-9); - EXPECT_DOUBLES_EQUAL(total_error, error_tree(discrete_values), 1e-9); - EXPECT_DOUBLES_EQUAL(total_error, pruned_error_tree(discrete_values), 1e-9); } /* ****************************************************************************/ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 94a611a9ee..99be3ed1c5 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -60,12 +60,14 @@ TEST(HybridFactorGraph, GaussianFactorGraph) { Values linearizationPoint; linearizationPoint.insert(X(0), 0); + // Linearize the factor graph. HybridGaussianFactorGraph ghfg = *fg.linearize(linearizationPoint); + EXPECT_LONGS_EQUAL(1, ghfg.size()); - // Add a factor to the GaussianFactorGraph - ghfg.add(JacobianFactor(X(0), I_1x1, Vector1(5))); - - EXPECT_LONGS_EQUAL(2, ghfg.size()); + // Check that the error is the same for the nonlinear values. + const VectorValues zero{{X(0), Vector1(0)}}; + const HybridValues hybridValues{zero, {}, linearizationPoint}; + EXPECT_DOUBLES_EQUAL(fg.error(hybridValues), ghfg.error(hybridValues), 1e-9); } /*************************************************************************** diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index d6c1d5d8ae..355fdf87ec 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -61,6 +61,16 @@ bool FactorGraph::equals(const This& fg, double tol) const { return true; } +/* ************************************************************************ */ +template +double FactorGraph::error(const HybridValues &values) const { + double error = 0.0; + for (auto &f : factors_) { + error += f->error(values); + } + return error; +} + /* ************************************************************************* */ template size_t FactorGraph::nrFactors() const { diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 68dc79d3fd..2e9dd3d532 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -47,6 +47,8 @@ typedef FastVector FactorIndices; template class BayesTree; +class HybridValues; + /** Helper */ template class CRefCallPushBack { @@ -359,6 +361,9 @@ class FactorGraph { /** Get the last factor */ sharedFactor back() const { return factors_.back(); } + /** Add error for all factors. */ + double error(const HybridValues &values) const; + /// @} /// @name Modifying Factor Graphs (imperative, discouraged) /// @{ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index c838051cf7..b41e1a3945 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -145,6 +145,8 @@ 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) From 69398d0e60248f6174ef9c92767f761155fdb34d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 21:53:26 -0800 Subject: [PATCH 09/21] Fix dogleg unit test (gbn error != linearized error anymore) --- tests/testDoglegOptimizer.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index e9ae2454b8..61276e89b9 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -126,17 +126,22 @@ TEST(DoglegOptimizer, Iterate) { double Delta = 1.0; for(size_t it=0; it<10; ++it) { - GaussianBayesNet gbn = *fg.linearize(config)->eliminateSequential(); + auto linearized = fg.linearize(config); + // Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true double nonlinearError = fg.error(config); - double linearError = GaussianFactorGraph(gbn).error(config.zeroVectors()); + double linearError = linearized->error(config.zeroVectors()); DOUBLES_EQUAL(nonlinearError, linearError, 1e-5); -// cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl; - VectorValues dx_u = gbn.optimizeGradientSearch(); - VectorValues dx_n = gbn.optimize(); - DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, gbn, fg, config, fg.error(config)); + + auto gbn = linearized->eliminateSequential(); + VectorValues dx_u = gbn->optimizeGradientSearch(); + VectorValues dx_n = gbn->optimize(); + DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate( + Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, *gbn, fg, + config, fg.error(config)); Delta = result.delta; EXPECT(result.f_error < fg.error(config)); // Check that error decreases + Values newConfig(config.retract(result.dx_d)); config = newConfig; DOUBLES_EQUAL(fg.error(config), result.f_error, 1e-5); // Check that error is correctly filled in From 8e29140ff7cd4e8981d8ff2c142da342b75ee2e6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 13:12:17 -0800 Subject: [PATCH 10/21] Added logProbability/evaluate in conditionals/Bayes net --- gtsam/inference/BayesNet-inst.h | 16 +++++++++ gtsam/inference/BayesNet.h | 10 +++++- gtsam/inference/Conditional-inst.h | 54 +++++++++++++++++------------ gtsam/inference/Conditional.h | 55 ++++++++++++++++++++++++++---- gtsam/inference/Factor.cpp | 6 ++++ gtsam/inference/Factor.h | 17 +++------ 6 files changed, 117 insertions(+), 41 deletions(-) diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h index f43b4025ec..f06008c880 100644 --- a/gtsam/inference/BayesNet-inst.h +++ b/gtsam/inference/BayesNet-inst.h @@ -88,6 +88,22 @@ void BayesNet::saveGraph(const std::string& filename, of.close(); } +/* ************************************************************************* */ +template +double BayesNet::logProbability(const HybridValues& x) const { + double sum = 0.; + for (const auto& gc : *this) { + if (gc) sum += gc->logProbability(x); + } + return sum; +} + +/* ************************************************************************* */ +template +double BayesNet::evaluate(const HybridValues& x) const { + return exp(-logProbability(x)); +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 4704d28738..4d266df46c 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -25,6 +25,8 @@ namespace gtsam { +class HybridValues; + /** * A BayesNet is a tree of conditionals, stored in elimination order. * @ingroup inference @@ -68,7 +70,6 @@ class BayesNet : public FactorGraph { const KeyFormatter& formatter = DefaultKeyFormatter) const override; /// @} - /// @name Graph Display /// @{ @@ -86,6 +87,13 @@ class BayesNet : public FactorGraph { const KeyFormatter& keyFormatter = DefaultKeyFormatter, const DotWriter& writer = DotWriter()) const; + /// @} + /// @name HybridValues methods + /// @{ + + double logProbability(const HybridValues& x) const; + double evaluate(const HybridValues& c) const; + /// @} }; diff --git a/gtsam/inference/Conditional-inst.h b/gtsam/inference/Conditional-inst.h index 9879a582c5..30433263c0 100644 --- a/gtsam/inference/Conditional-inst.h +++ b/gtsam/inference/Conditional-inst.h @@ -18,30 +18,42 @@ // \callgraph #pragma once -#include - #include +#include +#include + namespace gtsam { - /* ************************************************************************* */ - template - void Conditional::print(const std::string& s, const KeyFormatter& formatter) const { - std::cout << s << " P("; - for(Key key: frontals()) - std::cout << " " << formatter(key); - if (nrParents() > 0) - std::cout << " |"; - for(Key parent: parents()) - std::cout << " " << formatter(parent); - std::cout << ")" << std::endl; - } - - /* ************************************************************************* */ - template - bool Conditional::equals(const This& c, double tol) const - { - return nrFrontals_ == c.nrFrontals_; - } +/* ************************************************************************* */ +template +void Conditional::print( + const std::string& s, const KeyFormatter& formatter) const { + std::cout << s << " P("; + for (Key key : frontals()) std::cout << " " << formatter(key); + if (nrParents() > 0) std::cout << " |"; + for (Key parent : parents()) std::cout << " " << formatter(parent); + std::cout << ")" << std::endl; +} + +/* ************************************************************************* */ +template +bool Conditional::equals(const This& c, + double tol) const { + return nrFrontals_ == c.nrFrontals_; +} + +/* ************************************************************************* */ +template +double Conditional::logProbability( + const HybridValues& c) const { + throw std::runtime_error("Conditional::logProbability is not implemented"); +} +/* ************************************************************************* */ +template +double Conditional::evaluate( + const HybridValues& c) const { + return exp(static_cast(this)->logProbability(c)); } +} // namespace gtsam diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 7594da78d0..9083c5c1a8 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -24,13 +24,37 @@ namespace gtsam { + class HybridValues; // forward declaration. + /** - * Base class for conditional densities. This class iterators and - * access to the frontal and separator keys. + * This is the base class for all conditional distributions/densities, + * 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. + * + * The `evaluate` method is used to evaluate the factor, and together with + * `logProbability` is the main methods that need to be implemented in derived + * classes. These two methods relate to the `error` method in the factor by: + * probability(x) = k exp(-error(x)) + * where k is a normalization constant making \int probability(x) == 1.0, and + * logProbability(x) = K - error(x) + * i.e., K = log(K). + * + * There are four broad classes of conditionals that derive from Conditional: + * + * - \b Gaussian conditionals, implemented in \class GaussianConditional, a + * Gaussian density over a set of continuous variables. + * - \b Discrete conditionals, implemented in \class DiscreteConditional, which + * represent a discrete conditional distribution over discrete variables. + * - \b Hybrid conditional densities, such as \class GaussianMixture, which is + * a density over continuous variables given discrete/continuous parents. + * - \b Symbolic factors, used to represent a graph structure, implemented in + * \class SymbolicConditional. Only used for symbolic elimination etc. * * Derived classes *must* redefine the Factor and shared_ptr typedefs to refer * to the associated factor type and shared_ptr type of the derived class. See * SymbolicConditional and GaussianConditional for examples. + * * \nosubgrouping */ template @@ -78,6 +102,8 @@ namespace gtsam { /// @name Standard Interface /// @{ + virtual ~Conditional() {} + /** return the number of frontals */ size_t nrFrontals() const { return nrFrontals_; } @@ -98,6 +124,27 @@ namespace gtsam { /** return a view of the parent keys */ Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); } + /** + * All conditional types need to implement a `logProbability` function, for which + * exp(logProbability(x)) = evaluate(x). + */ + virtual double logProbability(const HybridValues& c) const; + + /** + * All conditional types need to implement an `evaluate` function, that yields + * a true probability. The default implementation just exponentiates logProbability. + */ + virtual double evaluate(const HybridValues& c) const; + + /// Evaluate probability density, sugar. + double operator()(const HybridValues& x) const { + return evaluate(x); + } + + /// @} + /// @name Advanced Interface + /// @{ + /** Iterator pointing to first frontal key. */ typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); } @@ -110,10 +157,6 @@ namespace gtsam { /** Iterator pointing past the last parent key. */ typename FACTOR::const_iterator endParents() const { return asFactor().end(); } - /// @} - /// @name Advanced Interface - /// @{ - /** Mutable version of nrFrontals */ size_t& nrFrontals() { return nrFrontals_; } diff --git a/gtsam/inference/Factor.cpp b/gtsam/inference/Factor.cpp index 6fe96c7771..2590d7b59a 100644 --- a/gtsam/inference/Factor.cpp +++ b/gtsam/inference/Factor.cpp @@ -43,4 +43,10 @@ namespace gtsam { return keys_ == other.keys_; } + /* ************************************************************************* */ + double Factor::error(const HybridValues& c) const { + throw std::runtime_error("Factor::error is not implemented"); + } + + } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 2fa5e3f889..f59a5972d1 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -44,10 +44,7 @@ namespace gtsam { * * The `error` method is used to evaluate the factor, and is the only method * that is required to be implemented in derived classes, although it has a - * default implementation that throws an exception. The meaning of the error - * is slightly different for factors and conditionals: in the former it is the - * negative log-likelihood, and in the latter it is the negative log of the - * properly normalized conditional distribution or density. + * default implementation that throws an exception. * * There are five broad classes of factors that derive from Factor: * @@ -55,15 +52,12 @@ namespace gtsam { * represent a nonlinear likelihood function over a set of variables. * - \b Gaussian factors, such as \class JacobianFactor and \class HessianFactor, which * represent a Gaussian likelihood over a set of variables. - * A \class GaussianConditional, which represent a Gaussian density over a set of - * variables conditioned on another set of variables. - * - \b Discrete factors, such as \class DiscreteFactor and \class DiscreteConditional, which + * - \b Discrete factors, such as \class DiscreteFactor and \class DecisionTreeFactor, which * represent a discrete distribution over a set of variables. * - \b Hybrid factors, such as \class HybridFactor, which represent a mixture of * Gaussian and discrete distributions over a set of variables. * - \b Symbolic factors, used to represent a graph structure, such as - * \class SymbolicFactor and \class SymbolicConditional. They do not override the - * `error` method, and are used only for symbolic elimination etc. + * \class SymbolicFactor, only used for symbolic elimination etc. * * Note that derived classes must also redefine the `This` and `shared_ptr` * typedefs. See JacobianFactor, etc. for examples. @@ -154,11 +148,8 @@ namespace gtsam { /** * 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"); - } + virtual double error(const HybridValues& c) const; /** * @return the number of variables involved in this factor From ecb0be494e742634d08629706a1e60685ed102bc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 13:12:36 -0800 Subject: [PATCH 11/21] Implemented all Gaussian versions of logProbability --- gtsam/linear/GaussianBayesNet.cpp | 17 ++++- gtsam/linear/GaussianBayesNet.h | 21 ++++-- gtsam/linear/GaussianConditional.cpp | 67 ++++++++++--------- gtsam/linear/GaussianConditional.h | 28 ++++++-- gtsam/linear/GaussianFactorGraph.cpp | 16 +++++ gtsam/linear/GaussianFactorGraph.h | 14 +--- .../linear/tests/testGaussianConditional.cpp | 19 ++++-- gtsam/linear/tests/testGaussianDensity.cpp | 7 +- 8 files changed, 122 insertions(+), 67 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 0a34a25e3a..2887e6d3a2 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 8c40d97287..28397a88a7 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 db822ffba0..10f4eabbbf 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(); -} - -/* ************************************************************************* */ -// density = k exp(-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(-error(x)); -} + /* ************************************************************************* */ + // 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 = log(k) - error(x) + double GaussianConditional::logProbability(const VectorValues& x) const { + return logNormalizationConstant() - 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 b41e1a3945..880d13064e 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 72eb107d09..0f11639822 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 ce475e1009..fb5e1ea362 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 9734854845..eb90f8aabe 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 3e73bce132..97eb0de704 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); } /* ************************************************************************* */ From b7fbe3f6a7d320f0974f8d3de513616da900f3f0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 13:12:57 -0800 Subject: [PATCH 12/21] Symbolic logProbability just throws --- gtsam/symbolic/SymbolicConditional.cpp | 6 ++++++ gtsam/symbolic/SymbolicConditional.h | 16 +++++++++++++--- gtsam/symbolic/SymbolicFactor.cpp | 5 +++++ gtsam/symbolic/SymbolicFactor.h | 9 ++++++--- 4 files changed, 30 insertions(+), 6 deletions(-) diff --git a/gtsam/symbolic/SymbolicConditional.cpp b/gtsam/symbolic/SymbolicConditional.cpp index f0d9944b22..eb8a95cff1 100644 --- a/gtsam/symbolic/SymbolicConditional.cpp +++ b/gtsam/symbolic/SymbolicConditional.cpp @@ -33,4 +33,10 @@ bool SymbolicConditional::equals(const This& c, double tol) const { return BaseFactor::equals(c) && BaseConditional::equals(c); } +/* ************************************************************************* */ +double SymbolicConditional::logProbability(const HybridValues& c) const { + throw std::runtime_error("SymbolicConditional::logProbability is not implemented"); +} + + } // namespace gtsam diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 4f49179502..8970902d72 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -95,13 +95,10 @@ namespace gtsam { return FromIteratorsShared(keys.begin(), keys.end(), nrFrontals); } - ~SymbolicConditional() override {} - /// Copy this object as its actual derived type. SymbolicFactor::shared_ptr clone() const { return boost::make_shared(*this); } /// @} - /// @name Testable /// @{ @@ -114,6 +111,19 @@ namespace gtsam { bool equals(const This& c, double tol = 1e-9) const; /// @} + /// @name HybridValues methods. + /// @{ + + /** + * logProbability throws exception, symbolic. + */ + double logProbability(const HybridValues& x) const override; + + using Conditional::evaluate; // Expose evaluate(const HybridValues&) method.. + using Conditional::operator(); // Expose evaluate(const HybridValues&) method.. + using SymbolicFactor::error; // Expose error(const HybridValues&) method.. + + /// @} private: /** Serialization function */ diff --git a/gtsam/symbolic/SymbolicFactor.cpp b/gtsam/symbolic/SymbolicFactor.cpp index 327de0c107..671b71f932 100644 --- a/gtsam/symbolic/SymbolicFactor.cpp +++ b/gtsam/symbolic/SymbolicFactor.cpp @@ -26,6 +26,11 @@ using namespace std; namespace gtsam { + /* ************************************************************************* */ + double SymbolicFactor::error(const HybridValues& c) const { + throw std::runtime_error("SymbolicFactor::error is not implemented"); + } + /* ************************************************************************* */ std::pair, boost::shared_ptr > EliminateSymbolic(const SymbolicFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 208efafb87..12b3374c82 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -30,6 +30,7 @@ namespace gtsam { // Forward declarations class SymbolicConditional; + class HybridValues; class Ordering; /** SymbolicFactor represents a symbolic factor that specifies graph topology but is not @@ -46,7 +47,7 @@ namespace gtsam { /** Overriding the shared_ptr typedef */ typedef boost::shared_ptr shared_ptr; - /// @name Standard Interface + /// @name Standard Constructors /// @{ /** Default constructor for I/O */ @@ -106,10 +107,9 @@ namespace gtsam { } /// @} - /// @name Advanced Constructors /// @{ - public: + /** Constructor from a collection of keys */ template static SymbolicFactor FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey) { @@ -143,6 +143,9 @@ namespace gtsam { /// @name Standard Interface /// @{ + /// The `error` method throws an exception. + double error(const HybridValues& c) const override; + /** Eliminate the variables in \c keys, in the order specified in \c keys, returning a * conditional and marginal. */ std::pair, boost::shared_ptr > From 28658490e87c1e565dd143fd0f26fcaff3f71610 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 14:36:58 -0800 Subject: [PATCH 13/21] Fix sign issue and added test --- gtsam/linear/GaussianBayesNet.cpp | 2 +- gtsam/linear/tests/testGaussianBayesNet.cpp | 12 ++++++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 2887e6d3a2..2e62d2d42d 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -122,7 +122,7 @@ namespace gtsam { /* ************************************************************************* */ double GaussianBayesNet::evaluate(const VectorValues& x) const { - return exp(-logProbability(x)); + return exp(logProbability(x)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 7b55d00c38..7f0641dbf8 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -78,9 +78,13 @@ TEST(GaussianBayesNet, Evaluate1) { // which at the mean is 1.0! So, the only thing we need to calculate is // the normalization constant 1.0/sqrt((2*pi*Sigma).det()). // The covariance matrix inv(Sigma) = R'*R, so the determinant is - const double expected = sqrt((invSigma / (2 * M_PI)).determinant()); + const double constant = sqrt((invSigma / (2 * M_PI)).determinant()); + EXPECT_DOUBLES_EQUAL(log(constant), + smallBayesNet.at(0)->logNormalizationConstant() + + smallBayesNet.at(1)->logNormalizationConstant(), + 1e-9); const double actual = smallBayesNet.evaluate(mean); - EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9); + EXPECT_DOUBLES_EQUAL(constant, actual, 1e-9); } // Check the evaluate with non-unit noise. @@ -89,9 +93,9 @@ TEST(GaussianBayesNet, Evaluate2) { const VectorValues mean = noisyBayesNet.optimize(); const Matrix R = noisyBayesNet.matrix().first; const Matrix invSigma = R.transpose() * R; - const double expected = sqrt((invSigma / (2 * M_PI)).determinant()); + const double constant = sqrt((invSigma / (2 * M_PI)).determinant()); const double actual = noisyBayesNet.evaluate(mean); - EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9); + EXPECT_DOUBLES_EQUAL(constant, actual, 1e-9); } /* ************************************************************************* */ From f89ef731a5f653201058ad6331ba4041c7de1b47 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 14:48:25 -0800 Subject: [PATCH 14/21] Fix linking error --- gtsam/inference/Conditional-inst.h | 2 +- gtsam/symbolic/SymbolicConditional.cpp | 6 +++++- gtsam/symbolic/SymbolicConditional.h | 12 ++++++------ 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/gtsam/inference/Conditional-inst.h b/gtsam/inference/Conditional-inst.h index 30433263c0..ee13946d9c 100644 --- a/gtsam/inference/Conditional-inst.h +++ b/gtsam/inference/Conditional-inst.h @@ -54,6 +54,6 @@ double Conditional::logProbability( template double Conditional::evaluate( const HybridValues& c) const { - return exp(static_cast(this)->logProbability(c)); + throw std::runtime_error("Conditional::evaluate is not implemented"); } } // namespace gtsam diff --git a/gtsam/symbolic/SymbolicConditional.cpp b/gtsam/symbolic/SymbolicConditional.cpp index eb8a95cff1..fd9cc453d4 100644 --- a/gtsam/symbolic/SymbolicConditional.cpp +++ b/gtsam/symbolic/SymbolicConditional.cpp @@ -15,7 +15,6 @@ * @date Oct 17, 2010 */ -#include #include namespace gtsam { @@ -38,5 +37,10 @@ double SymbolicConditional::logProbability(const HybridValues& c) const { throw std::runtime_error("SymbolicConditional::logProbability is not implemented"); } +/* ************************************************************************* */ +double SymbolicConditional::evaluate(const HybridValues& c) const { + throw std::runtime_error("SymbolicConditional::evaluate is not implemented"); +} + } // namespace gtsam diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 8970902d72..6bd966450e 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -17,10 +17,10 @@ #pragma once -#include -#include #include #include +#include +#include namespace gtsam { @@ -114,12 +114,12 @@ namespace gtsam { /// @name HybridValues methods. /// @{ - /** - * logProbability throws exception, symbolic. - */ + /// logProbability throws exception, symbolic. double logProbability(const HybridValues& x) const override; - using Conditional::evaluate; // Expose evaluate(const HybridValues&) method.. + /// evaluate throws exception, symbolic. + double evaluate(const HybridValues& x) const override; + using Conditional::operator(); // Expose evaluate(const HybridValues&) method.. using SymbolicFactor::error; // Expose error(const HybridValues&) method.. From 11ef99b3f098343941b840fc5ce92f2aa241cb4b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 15:16:55 -0800 Subject: [PATCH 15/21] discrete now has all logProbability and evaluate versions needed. --- gtsam/discrete/DecisionTreeFactor.cpp | 11 +++++++++ gtsam/discrete/DecisionTreeFactor.h | 24 +++++++++++++++++-- gtsam/discrete/DiscreteConditional.h | 22 ++++++++++++++++- .../tests/testDiscreteConditional.cpp | 23 ++++++++++++++++++ 4 files changed, 77 insertions(+), 3 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 7f604086c1..14a24b6e62 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -18,6 +18,7 @@ */ #include +#include #include #include @@ -56,6 +57,16 @@ namespace gtsam { } } + /* ************************************************************************ */ + double DecisionTreeFactor::error(const DiscreteValues& values) const { + return -std::log(evaluate(values)); + } + + /* ************************************************************************ */ + double DecisionTreeFactor::error(const HybridValues& values) const { + return error(values.discrete()); + } + /* ************************************************************************ */ double DecisionTreeFactor::safe_div(const double& a, const double& b) { // The use for safe_div is when we divide the product factor by the sum diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index f759c10f33..dd292cae8a 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -34,6 +34,7 @@ namespace gtsam { class DiscreteConditional; + class HybridValues; /** * A discrete probabilistic factor. @@ -97,11 +98,20 @@ namespace gtsam { /// @name Standard Interface /// @{ - /// Value is just look up in AlgebraicDecisionTree. + /// Calculate probability for given values `x`, + /// is just look up in AlgebraicDecisionTree. + double evaluate(const DiscreteValues& values) const { + return ADT::operator()(values); + } + + /// Evaluate probability density, sugar. double operator()(const DiscreteValues& values) const override { return ADT::operator()(values); } + /// Calculate error for DiscreteValues `x`, is -log(probability). + double error(const DiscreteValues& values) const; + /// multiply two factors DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { return apply(f, ADT::Ring::mul); @@ -230,7 +240,17 @@ namespace gtsam { std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, const Names& names = {}) const override; - /// @} + /// @} + /// @name HybridValues methods. + /// @{ + + /** + * Calculate error for HybridValues `x`, is -log(probability) + * Simply dispatches to DiscreteValues version. + */ + double error(const HybridValues& values) const override; + + /// @} private: /** Serialization function */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index b68953eb58..94451d407c 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -18,9 +18,9 @@ #pragma once +#include #include #include -#include #include #include @@ -147,6 +147,11 @@ class GTSAM_EXPORT DiscreteConditional /// @name Standard Interface /// @{ + /// Log-probability is just -error(x). + double logProbability(const DiscreteValues& x) const { + return -error(x); + } + /// print index signature only void printSignature( const std::string& s = "Discrete Conditional: ", @@ -225,6 +230,21 @@ class GTSAM_EXPORT DiscreteConditional std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, const Names& names = {}) const override; + + /// @} + /// @name HybridValues methods. + /// @{ + + /** + * Calculate log-probability log(evaluate(x)) for HybridValues `x`. + * This is actually just -error(x). + */ + double logProbability(const HybridValues& x) const override { + return -error(x); + } + + using DecisionTreeFactor::evaluate; + /// @} #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index f4a2e30ea4..fdfe4a145b 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -88,6 +88,29 @@ TEST(DiscreteConditional, constructors3) { EXPECT(assert_equal(expected, static_cast(actual))); } +/* ****************************************************************************/ +// Test evaluate for a discrete Prior P(Asia). +TEST(DiscreteConditional, PriorProbability) { + constexpr Key asiaKey = 0; + const DiscreteKey Asia(asiaKey, 2); + DiscreteConditional dc(Asia, "4/6"); + DiscreteValues values{{asiaKey, 0}}; + EXPECT_DOUBLES_EQUAL(0.4, dc.evaluate(values), 1e-9); +} + +/* ************************************************************************* */ +// Check that error, logProbability, evaluate all work as expected. +TEST(DiscreteConditional, probability) { + DiscreteKey C(2, 2), D(4, 2), E(3, 2); + DiscreteConditional C_given_DE((C | D, E) = "4/1 1/1 1/1 1/4"); + + DiscreteValues given {{C.first, 1}, {D.first, 0}, {E.first, 0}}; + EXPECT_DOUBLES_EQUAL(0.2, C_given_DE.evaluate(given), 1e-9); + EXPECT_DOUBLES_EQUAL(0.2, C_given_DE(given), 1e-9); + EXPECT_DOUBLES_EQUAL(log(0.2), C_given_DE.logProbability(given), 1e-9); + EXPECT_DOUBLES_EQUAL(-log(0.2), C_given_DE.error(given), 1e-9); +} + /* ************************************************************************* */ // Check calculation of joint P(A,B) TEST(DiscreteConditional, Multiply) { From 426a49dc72c563164f18698faa9eb91daa5ac249 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 21:55:18 -0800 Subject: [PATCH 16/21] Endowed hybrid with logProbability --- gtsam/hybrid/GaussianMixture.cpp | 13 +++--- gtsam/hybrid/GaussianMixture.h | 13 +++--- gtsam/hybrid/HybridBayesNet.cpp | 39 +++++++++-------- gtsam/hybrid/HybridBayesNet.h | 9 ++-- gtsam/hybrid/HybridConditional.cpp | 14 +++--- gtsam/hybrid/HybridConditional.h | 4 +- gtsam/hybrid/tests/testGaussianMixture.cpp | 14 +++--- gtsam/hybrid/tests/testHybridBayesNet.cpp | 51 ++++++++++++---------- 8 files changed, 86 insertions(+), 71 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index cabfd28b8e..c5ffed27b5 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -271,15 +271,16 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) { } /* *******************************************************************************/ -AlgebraicDecisionTree GaussianMixture::error( +AlgebraicDecisionTree GaussianMixture::logProbability( const VectorValues &continuousValues) const { - // functor to calculate to double error value from GaussianConditional. + // functor to calculate to double logProbability value from + // GaussianConditional. auto errorFunc = [continuousValues](const GaussianConditional::shared_ptr &conditional) { if (conditional) { - return conditional->error(continuousValues); + return conditional->logProbability(continuousValues); } else { - // Return arbitrarily large error if conditional is null + // Return arbitrarily large logProbability if conditional is null // Conditional is null if it is pruned out. return 1e50; } @@ -289,10 +290,10 @@ AlgebraicDecisionTree GaussianMixture::error( } /* *******************************************************************************/ -double GaussianMixture::error(const HybridValues &values) const { +double GaussianMixture::logProbability(const HybridValues &values) const { // Directly index to get the conditional, no need to build the whole tree. auto conditional = conditionals_(values.discrete()); - return conditional->error(values.continuous()); + return conditional->logProbability(values.continuous()); } } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index a8010e17cc..a9f82d5559 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -164,22 +164,23 @@ class GTSAM_EXPORT GaussianMixture const Conditionals &conditionals() const; /** - * @brief Compute error of the GaussianMixture as a tree. + * @brief Compute logProbability of the GaussianMixture as a tree. * * @param continuousValues The continuous VectorValues. * @return AlgebraicDecisionTree A decision tree with the same keys - * as the conditionals, and leaf values as the error. + * as the conditionals, and leaf values as the logProbability. */ - AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + AlgebraicDecisionTree logProbability( + const VectorValues &continuousValues) const; /** - * @brief Compute the error of this Gaussian Mixture given the continuous - * values and a discrete assignment. + * @brief Compute the logProbability of this Gaussian Mixture given the + * continuous values and a discrete assignment. * * @param values Continuous values and discrete assignment. * @return double */ - double error(const HybridValues &values) const override; + double logProbability(const HybridValues &values) const override; // /// Calculate probability density for given values `x`. // double evaluate(const HybridValues &values) const; diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index e662b9c819..bc0d8e95e1 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -255,11 +255,6 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const { return gbn.optimize(); } -/* ************************************************************************* */ -double HybridBayesNet::evaluate(const HybridValues &values) const { - return exp(-error(values)); -} - /* ************************************************************************* */ HybridValues HybridBayesNet::sample(const HybridValues &given, std::mt19937_64 *rng) const { @@ -296,23 +291,28 @@ HybridValues HybridBayesNet::sample() const { } /* ************************************************************************* */ -AlgebraicDecisionTree HybridBayesNet::error( +AlgebraicDecisionTree HybridBayesNet::logProbability( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree(0.0); // Iterate over each conditional. for (auto &&conditional : *this) { if (auto gm = conditional->asMixture()) { - // If conditional is hybrid, select based on assignment and compute error. - error_tree = error_tree + gm->error(continuousValues); + // If conditional is hybrid, select based on assignment and compute + // logProbability. + error_tree = error_tree + gm->logProbability(continuousValues); } else if (auto gc = conditional->asGaussian()) { - // If continuous, get the (double) error and add it to the error_tree - double error = gc->error(continuousValues); - // Add the computed error to every leaf of the error tree. - error_tree = error_tree.apply( - [error](double leaf_value) { return leaf_value + error; }); + // If continuous, get the (double) logProbability and add it to the + // error_tree + double logProbability = gc->logProbability(continuousValues); + // Add the computed logProbability to every leaf of the logProbability + // tree. + error_tree = error_tree.apply([logProbability](double leaf_value) { + return leaf_value + logProbability; + }); } else if (auto dc = conditional->asDiscrete()) { - // TODO(dellaert): if discrete, we need to add error in the right branch? + // TODO(dellaert): if discrete, we need to add logProbability in the right + // branch? continue; } } @@ -321,10 +321,15 @@ AlgebraicDecisionTree HybridBayesNet::error( } /* ************************************************************************* */ -AlgebraicDecisionTree HybridBayesNet::probPrime( +AlgebraicDecisionTree HybridBayesNet::evaluate( const VectorValues &continuousValues) const { - AlgebraicDecisionTree error_tree = this->error(continuousValues); - return error_tree.apply([](double error) { return exp(-error); }); + AlgebraicDecisionTree tree = this->logProbability(continuousValues); + return tree.apply([](double log) { return exp(log); }); +} + +/* ************************************************************************* */ +double HybridBayesNet::evaluate(const HybridValues &values) const { + return exp(logProbability(values)); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index c5a16f9ddd..46a2b4f771 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -187,8 +187,6 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { /// Prune the Hybrid Bayes Net such that we have at most maxNrLeaves leaves. HybridBayesNet prune(size_t maxNrLeaves); - using Base::error; // Expose error(const HybridValues&) method.. - /** * @brief Compute conditional error for each discrete assignment, * and return as a tree. @@ -196,7 +194,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @param continuousValues Continuous values at which to compute the error. * @return AlgebraicDecisionTree */ - AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + AlgebraicDecisionTree logProbability( + const VectorValues &continuousValues) const; + + using BayesNet::logProbability; // expose HybridValues version /** * @brief Compute unnormalized probability q(μ|M), @@ -208,7 +209,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * probability. * @return AlgebraicDecisionTree */ - AlgebraicDecisionTree probPrime( + AlgebraicDecisionTree evaluate( const VectorValues &continuousValues) const; /** diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index f10a692aff..df92ffcb8d 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -122,18 +122,18 @@ bool HybridConditional::equals(const HybridFactor &other, double tol) const { } /* ************************************************************************ */ -double HybridConditional::error(const HybridValues &values) const { - if (auto gm = asMixture()) { - return gm->error(values); - } +double HybridConditional::logProbability(const HybridValues &values) const { if (auto gc = asGaussian()) { - return gc->error(values.continuous()); + return gc->logProbability(values.continuous()); + } + if (auto gm = asMixture()) { + return gm->logProbability(values); } if (auto dc = asDiscrete()) { - return -log((*dc)(values.discrete())); + return dc->logProbability(values.discrete()); } throw std::runtime_error( - "HybridConditional::error: conditional type not handled"); + "HybridConditional::logProbability: conditional type not handled"); } } // namespace gtsam diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 6199fe7b0b..030e6c8354 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -176,8 +176,8 @@ class GTSAM_EXPORT HybridConditional /// Get the type-erased pointer to the inner type boost::shared_ptr inner() const { return inner_; } - /// Return the error of the underlying conditional. - double error(const HybridValues& values) const override; + /// Return the logProbability of the underlying conditional. + double logProbability(const HybridValues& values) const override; /// Check if VectorValues `measurements` contains all frontal keys. bool frontalsIn(const VectorValues& measurements) const { diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 0b28669217..a2ee2c21f4 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -116,12 +116,12 @@ TEST(GaussianMixture, Error) { VectorValues values; values.insert(X(1), Vector2::Ones()); values.insert(X(2), Vector2::Zero()); - auto error_tree = mixture.error(values); + auto error_tree = mixture.logProbability(values); // Check result. std::vector discrete_keys = {m1}; - std::vector leaves = {conditional0->error(values), - conditional1->error(values)}; + std::vector leaves = {conditional0->logProbability(values), + conditional1->logProbability(values)}; AlgebraicDecisionTree expected_error(discrete_keys, leaves); EXPECT(assert_equal(expected_error, error_tree, 1e-6)); @@ -129,11 +129,11 @@ TEST(GaussianMixture, Error) { // Regression for non-tree version. DiscreteValues assignment; assignment[M(1)] = 0; - EXPECT_DOUBLES_EQUAL(conditional0->error(values), - mixture.error({values, assignment}), 1e-8); + EXPECT_DOUBLES_EQUAL(conditional0->logProbability(values), + mixture.logProbability({values, assignment}), 1e-8); assignment[M(1)] = 1; - EXPECT_DOUBLES_EQUAL(conditional1->error(values), - mixture.error({values, assignment}), 1e-8); + EXPECT_DOUBLES_EQUAL(conditional1->logProbability(values), + mixture.logProbability({values, assignment}), 1e-8); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 2c114a3350..3af131f09c 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -64,10 +64,10 @@ TEST(HybridBayesNet, Add) { // Test evaluate for a pure discrete Bayes net P(Asia). TEST(HybridBayesNet, EvaluatePureDiscrete) { HybridBayesNet bayesNet; - bayesNet.emplace_back(new DiscreteConditional(Asia, "99/1")); + bayesNet.emplace_back(new DiscreteConditional(Asia, "4/6")); HybridValues values; values.insert(asiaKey, 0); - EXPECT_DOUBLES_EQUAL(0.99, bayesNet.evaluate(values), 1e-9); + EXPECT_DOUBLES_EQUAL(0.4, bayesNet.evaluate(values), 1e-9); } /* ****************************************************************************/ @@ -207,7 +207,7 @@ TEST(HybridBayesNet, Optimize) { /* ****************************************************************************/ // Test Bayes net error -TEST(HybridBayesNet, Error) { +TEST(HybridBayesNet, logProbability) { Switching s(3); HybridBayesNet::shared_ptr hybridBayesNet = @@ -215,42 +215,49 @@ TEST(HybridBayesNet, Error) { EXPECT_LONGS_EQUAL(5, hybridBayesNet->size()); HybridValues delta = hybridBayesNet->optimize(); - auto error_tree = hybridBayesNet->error(delta.continuous()); + auto error_tree = hybridBayesNet->logProbability(delta.continuous()); std::vector discrete_keys = {{M(0), 2}, {M(1), 2}}; - std::vector leaves = {-4.1609374, -4.1706942, -4.141568, -4.1609374}; + std::vector leaves = {4.1609374, 4.1706942, 4.141568, 4.1609374}; AlgebraicDecisionTree expected_error(discrete_keys, leaves); // regression EXPECT(assert_equal(expected_error, error_tree, 1e-6)); - // Error on pruned Bayes net + // logProbability on pruned Bayes net auto prunedBayesNet = hybridBayesNet->prune(2); - auto pruned_error_tree = prunedBayesNet.error(delta.continuous()); + auto pruned_error_tree = prunedBayesNet.logProbability(delta.continuous()); - std::vector pruned_leaves = {2e50, -4.1706942, 2e50, -4.1609374}; + std::vector pruned_leaves = {2e50, 4.1706942, 2e50, 4.1609374}; AlgebraicDecisionTree expected_pruned_error(discrete_keys, pruned_leaves); // regression EXPECT(assert_equal(expected_pruned_error, pruned_error_tree, 1e-6)); - // Verify error computation and check for specific error value + // Verify logProbability computation and check for specific logProbability + // value const DiscreteValues discrete_values{{M(0), 1}, {M(1), 1}}; const HybridValues hybridValues{delta.continuous(), discrete_values}; - double error = 0; - error += hybridBayesNet->at(0)->asMixture()->error(hybridValues); - error += hybridBayesNet->at(1)->asMixture()->error(hybridValues); - error += hybridBayesNet->at(2)->asMixture()->error(hybridValues); - - // TODO(dellaert): the discrete errors are not added in error tree! - EXPECT_DOUBLES_EQUAL(error, error_tree(discrete_values), 1e-9); - EXPECT_DOUBLES_EQUAL(error, pruned_error_tree(discrete_values), 1e-9); - - error += hybridBayesNet->at(3)->asDiscrete()->error(discrete_values); - error += hybridBayesNet->at(4)->asDiscrete()->error(discrete_values); - EXPECT_DOUBLES_EQUAL(error, hybridBayesNet->error(hybridValues), 1e-9); - + double logProbability = 0; + logProbability += + hybridBayesNet->at(0)->asMixture()->logProbability(hybridValues); + logProbability += + hybridBayesNet->at(1)->asMixture()->logProbability(hybridValues); + logProbability += + hybridBayesNet->at(2)->asMixture()->logProbability(hybridValues); + + // TODO(dellaert): the discrete errors are not added in logProbability tree! + EXPECT_DOUBLES_EQUAL(logProbability, error_tree(discrete_values), 1e-9); + EXPECT_DOUBLES_EQUAL(logProbability, pruned_error_tree(discrete_values), + 1e-9); + + logProbability += + hybridBayesNet->at(3)->asDiscrete()->logProbability(discrete_values); + logProbability += + hybridBayesNet->at(4)->asDiscrete()->logProbability(discrete_values); + EXPECT_DOUBLES_EQUAL(logProbability, + hybridBayesNet->logProbability(hybridValues), 1e-9); } /* ****************************************************************************/ From eb37d080d423cdc49fa80a8e2a91a35f85964c1d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 23:13:27 -0800 Subject: [PATCH 17/21] Added missing methods --- gtsam/discrete/DiscreteBayesNet.cpp | 9 +++++++++ gtsam/discrete/DiscreteBayesNet.h | 13 ++++++++++++- gtsam/discrete/DiscreteFactorGraph.h | 6 ++++++ gtsam/discrete/tests/testDiscreteBayesNet.cpp | 6 +++++- 4 files changed, 32 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index ccc52585e6..7cd190cc26 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -33,6 +33,15 @@ bool DiscreteBayesNet::equals(const This& bn, double tol) const { return Base::equals(bn, tol); } +/* ************************************************************************* */ +double DiscreteBayesNet::logProbability(const DiscreteValues& values) const { + // evaluate all conditionals and add + double result = 0.0; + for (const DiscreteConditional::shared_ptr& conditional : *this) + result += conditional->logProbability(values); + return result; +} + /* ************************************************************************* */ double DiscreteBayesNet::evaluate(const DiscreteValues& values) const { // evaluate all conditionals and multiply diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index 8ded827ceb..700f81d7e5 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -103,6 +103,9 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet { return evaluate(values); } + //** log(evaluate(values)) for given DiscreteValues */ + double logProbability(const DiscreteValues & values) const; + /** * @brief do ancestral sampling * @@ -136,7 +139,15 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet { std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, const DiscreteFactor::Names& names = {}) const; - ///@} + /// @} + /// @name HybridValues methods. + /// @{ + + using Base::error; // Expose error(const HybridValues&) method.. + using Base::evaluate; // Expose evaluate(const HybridValues&) method.. + using Base::logProbability; // Expose logProbability(const HybridValues&) + + /// @} #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @name Deprecated functionality diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index e665ea88b1..3fbb64d506 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -222,6 +222,12 @@ class GTSAM_EXPORT DiscreteFactorGraph std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, const DiscreteFactor::Names& names = {}) const; + /// @} + /// @name HybridValues methods. + /// @{ + + using Base::error; // Expose error(const HybridValues&) method.. + /// @} }; // \ DiscreteFactorGraph diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 6303a5487f..38c64b13b2 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -25,7 +25,6 @@ #include - #include #include #include @@ -101,6 +100,11 @@ TEST(DiscreteBayesNet, Asia) { DiscreteConditional expected2(Bronchitis % "11/9"); EXPECT(assert_equal(expected2, *chordal->back())); + // Check evaluate and logProbability + auto result = chordal->optimize(); + EXPECT_DOUBLES_EQUAL(asia.logProbability(result), + std::log(asia.evaluate(result)), 1e-9); + // add evidence, we were in Asia and we have dyspnea fg.add(Asia, "0 1"); fg.add(Dyspnea, "0 1"); From 87ff4af32dcbeaa97f536da9de2931ad43483289 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 23:13:40 -0800 Subject: [PATCH 18/21] Wrapper and tests for logProbability --- gtsam/discrete/discrete.i | 8 ++++++++ gtsam/hybrid/hybrid.i | 8 +++++++- gtsam/linear/linear.i | 3 +++ python/gtsam/tests/test_DiscreteBayesNet.py | 18 +++++++++++++----- python/gtsam/tests/test_GaussianBayesNet.py | 18 ++++++++++++++++-- python/gtsam/tests/test_HybridBayesNet.py | 17 +++++++++++------ 6 files changed, 58 insertions(+), 14 deletions(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index fa98f36fa5..a25897ffa9 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -95,6 +95,9 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor { DiscreteConditional(const gtsam::DecisionTreeFactor& joint, const gtsam::DecisionTreeFactor& marginal, const gtsam::Ordering& orderedKeys); + double logProbability(const gtsam::DiscreteValues& values) const; + double evaluate(const gtsam::DiscreteValues& values) const; + double operator()(const gtsam::DiscreteValues& values) const; gtsam::DiscreteConditional operator*( const gtsam::DiscreteConditional& other) const; gtsam::DiscreteConditional marginal(gtsam::Key key) const; @@ -157,7 +160,12 @@ class DiscreteBayesNet { const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::DiscreteBayesNet& other, double tol = 1e-9) const; + + // Standard interface. + double logProbability(const gtsam::DiscreteValues& values) const; + double evaluate(const gtsam::DiscreteValues& values) const; double operator()(const gtsam::DiscreteValues& values) const; + gtsam::DiscreteValues sample() const; gtsam::DiscreteValues sample(gtsam::DiscreteValues given) const; diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 012f707e4c..aad1cca9bd 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -61,6 +61,9 @@ virtual class HybridConditional { size_t nrParents() const; // Standard interface: + double logProbability(const gtsam::HybridValues& values) const; + double evaluate(const gtsam::HybridValues& values) const; + double operator()(const gtsam::HybridValues& values) const; gtsam::GaussianMixture* asMixture() const; gtsam::GaussianConditional* asGaussian() const; gtsam::DiscreteConditional* asDiscrete() const; @@ -133,7 +136,10 @@ class HybridBayesNet { gtsam::KeySet keys() const; const gtsam::HybridConditional* at(size_t i) const; - double evaluate(const gtsam::HybridValues& x) const; + // Standard interface: + double logProbability(const gtsam::HybridValues& values) const; + double evaluate(const gtsam::HybridValues& values) const; + gtsam::HybridValues optimize() const; gtsam::HybridValues sample(const gtsam::HybridValues &given) const; gtsam::HybridValues sample() const; diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 41bce61d18..2d88c5f938 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -497,6 +497,7 @@ virtual class GaussianConditional : gtsam::JacobianFactor { bool equals(const gtsam::GaussianConditional& cg, double tol) const; // Standard Interface + double logProbability(const gtsam::VectorValues& x) const; double evaluate(const gtsam::VectorValues& x) const; double error(const gtsam::VectorValues& x) const; gtsam::Key firstFrontalKey() const; @@ -558,6 +559,8 @@ virtual class GaussianBayesNet { gtsam::GaussianConditional* back() const; // Standard interface + // Standard Interface + double logProbability(const gtsam::VectorValues& x) const; double evaluate(const gtsam::VectorValues& x) const; double error(const gtsam::VectorValues& x) const; diff --git a/python/gtsam/tests/test_DiscreteBayesNet.py b/python/gtsam/tests/test_DiscreteBayesNet.py index ff2ba99d15..d597effa88 100644 --- a/python/gtsam/tests/test_DiscreteBayesNet.py +++ b/python/gtsam/tests/test_DiscreteBayesNet.py @@ -11,13 +11,15 @@ # pylint: disable=no-name-in-module, invalid-name +import math import textwrap import unittest +from gtsam.utils.test_case import GtsamTestCase + import gtsam from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution, DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering) -from gtsam.utils.test_case import GtsamTestCase # Some keys: Asia = (0, 2) @@ -111,7 +113,7 @@ def test_Asia(self): self.assertEqual(len(actualSample), 8) def test_fragment(self): - """Test sampling and optimizing for Asia fragment.""" + """Test evaluate/sampling/optimizing for Asia fragment.""" # Create a reverse-topologically sorted fragment: fragment = DiscreteBayesNet() @@ -125,8 +127,14 @@ def test_fragment(self): given[key[0]] = 0 # Now sample from fragment: - actual = fragment.sample(given) - self.assertEqual(len(actual), 5) + values = fragment.sample(given) + self.assertEqual(len(values), 5) + + for i in [0, 1, 2]: + self.assertAlmostEqual(fragment.at(i).logProbability(values), + math.log(fragment.at(i).evaluate(values))) + self.assertAlmostEqual(fragment.logProbability(values), + math.log(fragment.evaluate(values))) def test_dot(self): """Check that dot works with position hints.""" @@ -139,7 +147,7 @@ def test_dot(self): # Make sure we can *update* position hints writer = gtsam.DotWriter() ph: dict = writer.positionHints - ph['a'] = 2 # hint at symbol position + ph['a'] = 2 # hint at symbol position writer.positionHints = ph # Check the output of dot diff --git a/python/gtsam/tests/test_GaussianBayesNet.py b/python/gtsam/tests/test_GaussianBayesNet.py index 022de8c3fd..9065c7beec 100644 --- a/python/gtsam/tests/test_GaussianBayesNet.py +++ b/python/gtsam/tests/test_GaussianBayesNet.py @@ -12,13 +12,15 @@ from __future__ import print_function +import math import unittest -import gtsam import numpy as np -from gtsam import GaussianBayesNet, GaussianConditional from gtsam.utils.test_case import GtsamTestCase +import gtsam +from gtsam import GaussianBayesNet, GaussianConditional + # some keys _x_ = 11 _y_ = 22 @@ -45,6 +47,18 @@ def test_matrix(self): np.testing.assert_equal(R, R1) np.testing.assert_equal(d, d1) + def test_evaluate(self): + """Test evaluate method""" + bayesNet = smallBayesNet() + values = gtsam.VectorValues() + values.insert(_x_, np.array([9.0])) + values.insert(_y_, np.array([5.0])) + for i in [0, 1]: + self.assertAlmostEqual(bayesNet.at(i).logProbability(values), + math.log(bayesNet.at(i).evaluate(values))) + self.assertAlmostEqual(bayesNet.logProbability(values), + math.log(bayesNet.evaluate(values))) + def test_sample(self): """Test sample method""" bayesNet = smallBayesNet() diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index 75a2e9f8b6..c949551c4d 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -10,14 +10,15 @@ """ # pylint: disable=invalid-name, no-name-in-module, no-member +import math import unittest import numpy as np from gtsam.symbol_shorthand import A, X from gtsam.utils.test_case import GtsamTestCase -from gtsam import (DiscreteKeys, GaussianMixture, DiscreteConditional, GaussianConditional, GaussianMixture, - HybridBayesNet, HybridValues, noiseModel) +from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional, + GaussianMixture, HybridBayesNet, HybridValues, noiseModel) class TestHybridBayesNet(GtsamTestCase): @@ -30,8 +31,8 @@ def test_evaluate(self): # Create the continuous conditional I_1x1 = np.eye(1) - gc = GaussianConditional.FromMeanAndStddev(X(0), 2 * I_1x1, X(1), [-4], - 5.0) + conditional = GaussianConditional.FromMeanAndStddev(X(0), 2 * I_1x1, X(1), [-4], + 5.0) # Create the noise models model0 = noiseModel.Diagonal.Sigmas([2.0]) @@ -45,7 +46,7 @@ def test_evaluate(self): # Create hybrid Bayes net. bayesNet = HybridBayesNet() - bayesNet.push_back(gc) + bayesNet.push_back(conditional) bayesNet.push_back(GaussianMixture( [X(1)], [], discrete_keys, [conditional0, conditional1])) bayesNet.push_back(DiscreteConditional(Asia, "99/1")) @@ -56,13 +57,17 @@ def test_evaluate(self): values.insert(X(0), [-6]) values.insert(X(1), [1]) - conditionalProbability = gc.evaluate(values.continuous()) + conditionalProbability = conditional.evaluate(values.continuous()) mixtureProbability = conditional0.evaluate(values.continuous()) self.assertAlmostEqual(conditionalProbability * mixtureProbability * 0.99, bayesNet.evaluate(values), places=5) + # Check logProbability + self.assertAlmostEqual(bayesNet.logProbability(values), + math.log(bayesNet.evaluate(values))) + if __name__ == "__main__": unittest.main() From 4340b468288bc6cfda1acac74aef8ad9220ac7c4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 Jan 2023 23:56:49 -0800 Subject: [PATCH 19/21] Don't use deprecated method. --- gtsam/discrete/tests/testDiscreteBayesNet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 38c64b13b2..0f1fb23cdd 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -101,7 +101,7 @@ TEST(DiscreteBayesNet, Asia) { EXPECT(assert_equal(expected2, *chordal->back())); // Check evaluate and logProbability - auto result = chordal->optimize(); + auto result = fg.optimize(); EXPECT_DOUBLES_EQUAL(asia.logProbability(result), std::log(asia.evaluate(result)), 1e-9); From 28f440a62339deedb6d547029c306b2909c35cca Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 Jan 2023 18:11:28 -0800 Subject: [PATCH 20/21] Resolved review comments. --- gtsam/hybrid/HybridBayesNet.cpp | 10 +++++----- gtsam/inference/BayesNet.h | 9 +++++++-- python/gtsam/tests/test_GaussianBayesNet.py | 7 ++----- 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index bc0d8e95e1..be9cdba85b 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -293,21 +293,21 @@ HybridValues HybridBayesNet::sample() const { /* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::logProbability( const VectorValues &continuousValues) const { - AlgebraicDecisionTree error_tree(0.0); + AlgebraicDecisionTree result(0.0); // Iterate over each conditional. for (auto &&conditional : *this) { if (auto gm = conditional->asMixture()) { // If conditional is hybrid, select based on assignment and compute // logProbability. - error_tree = error_tree + gm->logProbability(continuousValues); + result = result + gm->logProbability(continuousValues); } else if (auto gc = conditional->asGaussian()) { // If continuous, get the (double) logProbability and add it to the - // error_tree + // result double logProbability = gc->logProbability(continuousValues); // Add the computed logProbability to every leaf of the logProbability // tree. - error_tree = error_tree.apply([logProbability](double leaf_value) { + result = result.apply([logProbability](double leaf_value) { return leaf_value + logProbability; }); } else if (auto dc = conditional->asDiscrete()) { @@ -317,7 +317,7 @@ AlgebraicDecisionTree HybridBayesNet::logProbability( } } - return error_tree; + return result; } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 4d266df46c..3e6d552810 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -54,9 +54,11 @@ class BayesNet : public FactorGraph { /** * Constructor that takes an initializer list of shared pointers. - * BayesNet bn = {make_shared(), ...}; + * BayesNet bn = {make_shared(), + * ...}; */ - BayesNet(std::initializer_list conditionals): Base(conditionals) {} + BayesNet(std::initializer_list conditionals) + : Base(conditionals) {} /// @} @@ -91,7 +93,10 @@ class BayesNet : public FactorGraph { /// @name HybridValues methods /// @{ + // Expose HybridValues version of logProbability. double logProbability(const HybridValues& x) const; + + // Expose HybridValues version of evaluate. double evaluate(const HybridValues& c) const; /// @} diff --git a/python/gtsam/tests/test_GaussianBayesNet.py b/python/gtsam/tests/test_GaussianBayesNet.py index 9065c7beec..05522441b4 100644 --- a/python/gtsam/tests/test_GaussianBayesNet.py +++ b/python/gtsam/tests/test_GaussianBayesNet.py @@ -10,9 +10,6 @@ """ # pylint: disable=invalid-name, no-name-in-module, no-member -from __future__ import print_function - -import math import unittest import numpy as np @@ -55,9 +52,9 @@ def test_evaluate(self): values.insert(_y_, np.array([5.0])) for i in [0, 1]: self.assertAlmostEqual(bayesNet.at(i).logProbability(values), - math.log(bayesNet.at(i).evaluate(values))) + np.log(bayesNet.at(i).evaluate(values))) self.assertAlmostEqual(bayesNet.logProbability(values), - math.log(bayesNet.evaluate(values))) + np.log(bayesNet.evaluate(values))) def test_sample(self): """Test sample method""" From f4c3131ee209ccaaf15f73c1195fcc40b91906b7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 Jan 2023 23:50:17 -0800 Subject: [PATCH 21/21] Return a set of DiscreteKey, Fixes issue #1384 --- gtsam/hybrid/HybridFactorGraph.cpp | 8 ++++---- gtsam/hybrid/HybridFactorGraph.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 4238925d6e..098942f106 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -25,17 +25,17 @@ namespace gtsam { /* ************************************************************************* */ -DiscreteKeys HybridFactorGraph::discreteKeys() const { - DiscreteKeys keys; +std::set HybridFactorGraph::discreteKeys() const { + std::set keys; for (auto& factor : factors_) { if (auto p = boost::dynamic_pointer_cast(factor)) { for (const DiscreteKey& key : p->discreteKeys()) { - keys.push_back(key); + keys.insert(key); } } if (auto p = boost::dynamic_pointer_cast(factor)) { for (const DiscreteKey& key : p->discreteKeys()) { - keys.push_back(key); + keys.insert(key); } } } diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 7d30663a3c..a02d4a2129 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -65,7 +65,7 @@ class HybridFactorGraph : public FactorGraph { /// @{ /// Get all the discrete keys in the factor graph. - DiscreteKeys discreteKeys() const; + std::set discreteKeys() const; /// Get all the discrete keys in the factor graph, as a set. KeySet discreteKeySet() const;