add errorConstant method and use it for logNormalizationConstant in Conditional

release/4.3a0
Varun Agrawal 2024-09-22 22:11:23 -04:00
parent 796d85d7fa
commit 2d2213e880
15 changed files with 84 additions and 44 deletions

View File

@ -465,6 +465,12 @@ string DiscreteConditional::html(const KeyFormatter& keyFormatter,
double DiscreteConditional::evaluate(const HybridValues& x) const {
return this->evaluate(x.discrete());
}
/* ************************************************************************* */
double DiscreteConditional::errorConstant() const {
return 0.0;
}
/* ************************************************************************* */
} // namespace gtsam

View File

@ -264,11 +264,12 @@ class GTSAM_EXPORT DiscreteConditional
}
/**
* logNormalizationConstant K is just zero, such that
* errorConstant is just zero, such that
* logProbability(x) = log(evaluate(x)) = - error(x)
* and hence error(x) = - log(evaluate(x)) > 0 for all x.
* Thus -log(K) for the normalization constant k is 0.
*/
double logNormalizationConstant() const override { return 0.0; }
double errorConstant() const override;
/// @}

View File

@ -161,18 +161,18 @@ double HybridConditional::logProbability(const HybridValues &values) const {
}
/* ************************************************************************ */
double HybridConditional::logNormalizationConstant() const {
double HybridConditional::errorConstant() const {
if (auto gc = asGaussian()) {
return gc->logNormalizationConstant();
return gc->errorConstant();
}
if (auto gm = asHybrid()) {
return gm->logNormalizationConstant(); // 0.0!
return gm->errorConstant(); // 0.0!
}
if (auto dc = asDiscrete()) {
return dc->logNormalizationConstant(); // 0.0!
return dc->errorConstant(); // 0.0!
}
throw std::runtime_error(
"HybridConditional::logProbability: conditional type not handled");
"HybridConditional::errorConstant: conditional type not handled");
}
/* ************************************************************************ */

View File

@ -193,11 +193,12 @@ class GTSAM_EXPORT HybridConditional
double logProbability(const HybridValues& values) const override;
/**
* Return the log normalization constant.
* Return the negative log of the normalization constant.
* This shows up in the error as -(error(x) + errorConstant)
* Note this is 0.0 for discrete and hybrid conditionals, but depends
* on the continuous parameters for Gaussian conditionals.
*/
double logNormalizationConstant() const override;
double errorConstant() const override;
/// Return the probability (or density) of the underlying conditional.
double evaluate(const HybridValues& values) const override;

View File

@ -36,7 +36,7 @@ HybridGaussianFactor::FactorValuePairs GetFactorValuePairs(
// Check if conditional is pruned
if (conditional) {
// Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|))
value = conditional->logNormalizationConstant();
value = conditional->errorConstant();
}
return {std::dynamic_pointer_cast<GaussianFactor>(conditional), value};
};
@ -57,8 +57,8 @@ HybridGaussianConditional::HybridGaussianConditional(
conditionals_.visit(
[this](const GaussianConditional::shared_ptr &conditional) {
if (conditional) {
this->logConstant_ = std::min(
this->logConstant_, conditional->logNormalizationConstant());
this->logConstant_ =
std::min(this->logConstant_, conditional->errorConstant());
}
});
}
@ -84,8 +84,7 @@ GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree()
auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
// First check if conditional has not been pruned
if (gc) {
const double Cgm_Kgcm =
gc->logNormalizationConstant() - this->logConstant_;
const double Cgm_Kgcm = gc->errorConstant() - this->logConstant_;
// If there is a difference in the covariances, we need to account for
// that since the error is dependent on the mode.
if (Cgm_Kgcm > 0.0) {
@ -215,8 +214,7 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
[&](const GaussianConditional::shared_ptr &conditional)
-> GaussianFactorValuePair {
const auto likelihood_m = conditional->likelihood(given);
const double Cgm_Kgcm =
conditional->logNormalizationConstant() - logConstant_;
const double Cgm_Kgcm = conditional->errorConstant() - logConstant_;
if (Cgm_Kgcm == 0.0) {
return {likelihood_m, 0.0};
} else {

View File

@ -150,9 +150,15 @@ class GTSAM_EXPORT HybridGaussianConditional
/// Returns the continuous keys among the parents.
KeyVector continuousParents() const;
/// The log normalization constant is max of the the individual
/// log-normalization constants.
double logNormalizationConstant() const override { return logConstant_; }
/**
* @brief Return log normalization constant in negative log space.
*
* The log normalization constant is the max of the individual
* log-normalization constants.
*
* @return double
*/
inline double errorConstant() const override { return logConstant_; }
/**
* Create a likelihood factor for a hybrid Gaussian conditional,

View File

@ -329,9 +329,9 @@ static std::shared_ptr<Factor> createDiscreteFactor(
// Logspace version of:
// exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
// logNormalizationConstant gives `-log(k)`
// errorConstant gives `-log(k)`
// which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`.
return -factor->error(kEmpty) + conditional->logNormalizationConstant();
return -factor->error(kEmpty) + conditional->errorConstant();
};
AlgebraicDecisionTree<Key> logProbabilities(
@ -357,7 +357,7 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
// Add 2.0 term since the constant term will be premultiplied by 0.5
// as per the Hessian definition,
// and negative since we want log(k)
hf->constantTerm() += -2.0 * conditional->logNormalizationConstant();
hf->constantTerm() += -2.0 * conditional->errorConstant();
}
return {factor, 0.0};
};

View File

@ -59,10 +59,17 @@ double Conditional<FACTOR, DERIVEDCONDITIONAL>::evaluate(
/* ************************************************************************* */
template <class FACTOR, class DERIVEDCONDITIONAL>
double Conditional<FACTOR, DERIVEDCONDITIONAL>::logNormalizationConstant()
double Conditional<FACTOR, DERIVEDCONDITIONAL>::errorConstant()
const {
throw std::runtime_error(
"Conditional::logNormalizationConstant is not implemented");
"Conditional::errorConstant is not implemented");
}
/* ************************************************************************* */
template <class FACTOR, class DERIVEDCONDITIONAL>
double Conditional<FACTOR, DERIVEDCONDITIONAL>::logNormalizationConstant()
const {
return -errorConstant();
}
/* ************************************************************************* */
@ -89,7 +96,7 @@ bool Conditional<FACTOR, DERIVEDCONDITIONAL>::CheckInvariants(
// normalization constant
const double error = conditional.error(values);
if (error < 0.0) return false; // prob_or_density is negative.
const double expected = -(conditional.logNormalizationConstant() + error);
const double expected = -(conditional.errorConstant() + error);
if (std::abs(logProb - expected) > 1e-9)
return false; // logProb is not consistent with error
return true;

View File

@ -164,9 +164,16 @@ namespace gtsam {
}
/**
* All conditional types need to implement a
* (negative) log normalization constant
* to make it such that error>=0.
* @brief All conditional types need to implement this as the negative log
* of the normalization constant.
*
* @return double
*/
virtual double errorConstant() const;
/**
* All conditional types need to implement a log normalization constant to
* make it such that error>=0.
*/
virtual double logNormalizationConstant() const;

View File

@ -247,14 +247,15 @@ namespace gtsam {
/*
normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
logConstant = -log(normalizationConstant)
= 0.5 * n*log(2*pi) + 0.5 * log(det(Sigma))
log(det(Sigma)) = -2.0 * logDeterminant()
thus, logConstant = 0.5*n*log(2*pi) - logDeterminant()
= -0.5 * n*log(2*pi) - 0.5 * log(det(Sigma))
BayesNet logConstant = sum(0.5*n_i*log(2*pi) - logDeterminant_i())
= sum(0.5*n_i*log(2*pi)) - sum(logDeterminant_i())
= sum(0.5*n_i*log(2*pi)) - bn->logDeterminant()
log(det(Sigma)) = -2.0 * logDeterminant()
thus, logConstant = -0.5*n*log(2*pi) + logDeterminant()
BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i())
= sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i())
= sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant()
= sum(logNormalizationConstant_i)
*/
double logNormConst = 0.0;
for (const sharedConditional& cg : *this) {

View File

@ -182,7 +182,7 @@ namespace gtsam {
/* ************************************************************************* */
// normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
// neg-log = 0.5 * n*log(2*pi) + 0.5 * log det(Sigma)
double GaussianConditional::logNormalizationConstant() const {
double GaussianConditional::errorConstant() const {
constexpr double log2pi = 1.8378770664093454835606594728112;
size_t n = d().size();
// Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
@ -195,10 +195,10 @@ namespace gtsam {
}
/* ************************************************************************* */
// density = 1/k exp(-error(x))
// log = -log(k) - error(x)
// density = k exp(-error(x))
// log = log(k) - error(x)
double GaussianConditional::logProbability(const VectorValues& x) const {
return -logNormalizationConstant() - error(x);
return logNormalizationConstant() - error(x);
}
double GaussianConditional::logProbability(const HybridValues& x) const {

View File

@ -133,12 +133,14 @@ namespace gtsam {
/// @{
/**
* Return normalization constant in negative log space.
* @brief Return the negative log of the normalization constant.
*
* normalization constant k = 1.0 / sqrt((2*pi)^n*det(Sigma))
* -log(k) = 0.5 * n*log(2*pi) + 0.5 * log det(Sigma)
*
* @return double
*/
double logNormalizationConstant() const override;
double errorConstant() const override;
/**
* Calculate log-probability log(evaluate(x)) for given values `x`:

View File

@ -255,7 +255,7 @@ double Gaussian::logDeterminant() const {
}
/* *******************************************************************************/
double Gaussian::logNormalizationConstant() const {
double Gaussian::errorConstant() const {
// log(det(Sigma)) = -2.0 * logDetR
// which gives neg-log = 0.5*n*log(2*pi) + 0.5*(-2.0 * logDetR())
// = 0.5*n*log(2*pi) - (0.5*2.0 * logDetR())
@ -266,6 +266,10 @@ double Gaussian::logNormalizationConstant() const {
return 0.5 * n * log2pi - logDetR();
}
/* *******************************************************************************/
double Gaussian::logNormalizationConstant() const {
return -errorConstant();
}
/* ************************************************************************* */
// Diagonal

View File

@ -271,11 +271,17 @@ namespace gtsam {
/// Compute the log of |Σ|
double logDeterminant() const;
/**
* @brief Compute the negative log of the normalization constant
* for a Gaussian noise model k = \sqrt(1/|2πΣ|).
*
* @return double
*/
double errorConstant() const;
/**
* @brief Method to compute the normalization constant
* for a Gaussian noise model k = \sqrt(1/|2πΣ|).
* We compute this in the negative log-space for numerical accuracy,
* thus returning -log(k).
*
* @return double
*/

View File

@ -548,6 +548,7 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
bool equals(const gtsam::GaussianConditional& cg, double tol) const;
// Standard Interface
double errorConstant() const;
double logNormalizationConstant() const;
double logProbability(const gtsam::VectorValues& x) const;
double evaluate(const gtsam::VectorValues& x) const;