From a4aebb548ae5b2231d4b41c0a9771f8bcc7f22c1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 12 Jan 2023 13:29:28 -0800 Subject: [PATCH] Expose correct error versions --- gtsam/discrete/DiscreteConditional.h | 3 ++- gtsam/hybrid/GaussianMixture.cpp | 7 +++++++ gtsam/hybrid/GaussianMixture.h | 13 +++++++++++-- gtsam/hybrid/HybridConditional.cpp | 15 +++++++++++++++ gtsam/hybrid/HybridConditional.h | 3 +++ gtsam/linear/GaussianConditional.h | 2 +- gtsam/linear/HessianFactor.h | 3 +++ gtsam/linear/JacobianFactor.h | 7 ++++++- 8 files changed, 48 insertions(+), 5 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 94451d407..2760ea538 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -243,7 +243,8 @@ class GTSAM_EXPORT DiscreteConditional return -error(x); } - using DecisionTreeFactor::evaluate; + using DecisionTreeFactor::error; ///< HybridValues version + using DecisionTreeFactor::evaluate; ///< HybridValues version /// @} diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index c5ffed27b..f61b280cb 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -289,6 +289,13 @@ AlgebraicDecisionTree GaussianMixture::logProbability( return errorTree; } +/* *******************************************************************************/ +double GaussianMixture::error(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()); +} + /* *******************************************************************************/ double GaussianMixture::logProbability(const HybridValues &values) const { // Directly index to get the conditional, no need to build the whole tree. diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index a9f82d555..a8d07cbc8 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -174,8 +174,17 @@ class GTSAM_EXPORT GaussianMixture const VectorValues &continuousValues) const; /** - * @brief Compute the logProbability of this Gaussian Mixture given the - * continuous values and a discrete assignment. + * @brief Compute the error of this Gaussian Mixture. + * + * log(probability(x)) = K - error(x) + * + * @param values Continuous values and discrete assignment. + * @return double + */ + double error(const HybridValues &values) const override; + + /** + * @brief Compute the logProbability of this Gaussian Mixture. * * @param values Continuous values and discrete assignment. * @return double diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index df92ffcb8..55fd5d5d4 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -121,6 +121,21 @@ bool HybridConditional::equals(const HybridFactor &other, double tol) const { : !(e->inner_); } +/* ************************************************************************ */ +double HybridConditional::error(const HybridValues &values) const { + if (auto gc = asGaussian()) { + return gc->error(values.continuous()); + } + if (auto gm = asMixture()) { + return gm->error(values); + } + if (auto dc = asDiscrete()) { + return dc->error(values.discrete()); + } + throw std::runtime_error( + "HybridConditional::error: conditional type not handled"); +} + /* ************************************************************************ */ double HybridConditional::logProbability(const HybridValues &values) const { if (auto gc = asGaussian()) { diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 030e6c835..19c070974 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -176,6 +176,9 @@ 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; diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 69e2ef2d3..18f0257cb 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -264,7 +264,7 @@ namespace gtsam { using Conditional::evaluate; // Expose evaluate(const HybridValues&) method.. using Conditional::operator(); // Expose evaluate(const HybridValues&) method.. - using Base::error; // Expose error(const HybridValues&) method.. + using JacobianFactor::error; // Expose error(const HybridValues&) method.. /// @} diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 3eefe1228..492df138f 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -196,6 +196,9 @@ namespace gtsam { /** Compare to another factor for testing (implementing Testable) */ bool equals(const GaussianFactor& lf, double tol = 1e-9) const override; + /// HybridValues simply extracts the \class VectorValues and calls error. + using GaussianFactor::error; + /** * Evaluate the factor error f(x). * returns 0.5*[x -1]'*H*[x -1] (also see constructor documentation) diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 8bcf18268..ae661c642 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -198,7 +198,12 @@ namespace gtsam { Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */ - double error(const VectorValues& c) const override; /** 0.5*(A*x-b)'*D*(A*x-b) */ + + /// HybridValues simply extracts the \class VectorValues and calls error. + using GaussianFactor::error; + + //// 0.5*(A*x-b)'*D*(A*x-b). + double error(const VectorValues& c) const override; /** Return the augmented information matrix represented by this GaussianFactor. * The augmented information matrix contains the information matrix with an