diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 0b1dc53377..fb943366cb 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -323,40 +323,6 @@ AlgebraicDecisionTree HybridGaussianConditional::logProbability( return DecisionTree(conditionals_, probFunc); } -/* ************************************************************************* */ -double HybridGaussianConditional::conditionalError( - const GaussianConditional::shared_ptr &conditional, - const VectorValues &continuousValues) const { - // Check if valid pointer - if (conditional) { - return conditional->error(continuousValues) + // - -logConstant_ - conditional->logNormalizationConstant(); - } else { - // If not valid, pointer, it means this conditional was pruned, - // so we return maximum error. - // This way the negative exponential will give - // a probability value close to 0.0. - return std::numeric_limits::max(); - } -} - -/* *******************************************************************************/ -AlgebraicDecisionTree HybridGaussianConditional::errorTree( - const VectorValues &continuousValues) const { - auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) { - return conditionalError(conditional, continuousValues); - }; - DecisionTree error_tree(conditionals_, errorFunc); - return error_tree; -} - -/* *******************************************************************************/ -double HybridGaussianConditional::error(const HybridValues &values) const { - // Directly index to get the conditional, no need to build the whole tree. - auto conditional = conditionals_(values.discrete()); - return conditionalError(conditional, values.continuous()); -} - /* *******************************************************************************/ double HybridGaussianConditional::logProbability( const HybridValues &values) const { diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 5e585acefe..4a5fdcc89e 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -174,43 +174,6 @@ class GTSAM_EXPORT HybridGaussianConditional AlgebraicDecisionTree logProbability( const VectorValues &continuousValues) const; - /** - * @brief Compute the error of this hybrid Gaussian conditional. - * - * This requires some care, as different components may have - * different normalization constants. Let's consider p(x|y,m), where m is - * discrete. We need the error to satisfy the invariant: - * - * error(x;y,m) = K - log(probability(x;y,m)) - * - * For all x,y,m. But note that K, the (log) normalization constant defined - * in Conditional.h, should not depend on x, y, or m, only on the parameters - * of the density. Hence, we delegate to the underlying Gaussian - * conditionals, indexed by m, which do satisfy: - * - * log(probability_m(x;y)) = K_m - error_m(x;y) - * - * We resolve by having K == max(K_m) and - * - * error(x;y,m) = error_m(x;y) + K - K_m - * - * which also makes error(x;y,m) >= 0 for all x,y,m. - * - * @param values Continuous values and discrete assignment. - * @return double - */ - double error(const HybridValues &values) const override; - - /** - * @brief Compute error of the HybridGaussianConditional as a tree. - * - * @param continuousValues The continuous VectorValues. - * @return AlgebraicDecisionTree A decision tree on the discrete keys - * only, with the leaf values as the error for each assignment. - */ - virtual AlgebraicDecisionTree errorTree( - const VectorValues &continuousValues) const override; - /** * @brief Compute the logProbability of this hybrid Gaussian conditional. * @@ -241,10 +204,6 @@ class GTSAM_EXPORT HybridGaussianConditional /// Check whether `given` has values for all frontal keys. bool allFrontalsGiven(const VectorValues &given) const; - /// Helper method to compute the error of a conditional. - double conditionalError(const GaussianConditional::shared_ptr &conditional, - const VectorValues &continuousValues) const; - #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access;