replace errorConstant with negLogConstant
parent
aae5f9e040
commit
e09344c6ba
|
@ -467,7 +467,7 @@ double DiscreteConditional::evaluate(const HybridValues& x) const {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
double DiscreteConditional::errorConstant() const {
|
double DiscreteConditional::negLogConstant() const {
|
||||||
return 0.0;
|
return 0.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -264,12 +264,12 @@ class GTSAM_EXPORT DiscreteConditional
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* errorConstant is just zero, such that
|
* negLogConstant is just zero, such that
|
||||||
* logProbability(x) = log(evaluate(x)) = - error(x)
|
* -logProbability(x) = -log(evaluate(x)) = error(x)
|
||||||
* and hence error(x) = - log(evaluate(x)) > 0 for all x.
|
* and hence error(x) > 0 for all x.
|
||||||
* Thus -log(K) for the normalization constant k is 0.
|
* Thus -log(K) for the normalization constant k is 0.
|
||||||
*/
|
*/
|
||||||
double errorConstant() const override;
|
double negLogConstant() const override;
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
||||||
|
|
|
@ -112,7 +112,7 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor {
|
||||||
const std::vector<double>& table);
|
const std::vector<double>& table);
|
||||||
|
|
||||||
// Standard interface
|
// Standard interface
|
||||||
double errorConstant() const;
|
double negLogConstant() const;
|
||||||
double logNormalizationConstant() const;
|
double logNormalizationConstant() const;
|
||||||
double logProbability(const gtsam::DiscreteValues& values) const;
|
double logProbability(const gtsam::DiscreteValues& values) const;
|
||||||
double evaluate(const gtsam::DiscreteValues& values) const;
|
double evaluate(const gtsam::DiscreteValues& values) const;
|
||||||
|
|
|
@ -161,18 +161,18 @@ double HybridConditional::logProbability(const HybridValues &values) const {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************ */
|
/* ************************************************************************ */
|
||||||
double HybridConditional::errorConstant() const {
|
double HybridConditional::negLogConstant() const {
|
||||||
if (auto gc = asGaussian()) {
|
if (auto gc = asGaussian()) {
|
||||||
return gc->errorConstant();
|
return gc->negLogConstant();
|
||||||
}
|
}
|
||||||
if (auto gm = asHybrid()) {
|
if (auto gm = asHybrid()) {
|
||||||
return gm->errorConstant(); // 0.0!
|
return gm->negLogConstant(); // 0.0!
|
||||||
}
|
}
|
||||||
if (auto dc = asDiscrete()) {
|
if (auto dc = asDiscrete()) {
|
||||||
return dc->errorConstant(); // 0.0!
|
return dc->negLogConstant(); // 0.0!
|
||||||
}
|
}
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
"HybridConditional::errorConstant: conditional type not handled");
|
"HybridConditional::negLogConstant: conditional type not handled");
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************ */
|
/* ************************************************************************ */
|
||||||
|
|
|
@ -194,11 +194,11 @@ class GTSAM_EXPORT HybridConditional
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Return the negative log of the normalization constant.
|
* Return the negative log of the normalization constant.
|
||||||
* This shows up in the error as -(error(x) + errorConstant)
|
* This shows up in the error as -(error(x) + negLogConstant)
|
||||||
* Note this is 0.0 for discrete and hybrid conditionals, but depends
|
* Note this is 0.0 for discrete and hybrid conditionals, but depends
|
||||||
* on the continuous parameters for Gaussian conditionals.
|
* on the continuous parameters for Gaussian conditionals.
|
||||||
*/
|
*/
|
||||||
double errorConstant() const override;
|
double negLogConstant() const override;
|
||||||
|
|
||||||
/// Return the probability (or density) of the underlying conditional.
|
/// Return the probability (or density) of the underlying conditional.
|
||||||
double evaluate(const HybridValues& values) const override;
|
double evaluate(const HybridValues& values) const override;
|
||||||
|
|
|
@ -36,7 +36,7 @@ HybridGaussianFactor::FactorValuePairs GetFactorValuePairs(
|
||||||
// Check if conditional is pruned
|
// Check if conditional is pruned
|
||||||
if (conditional) {
|
if (conditional) {
|
||||||
// Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|))
|
// Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|))
|
||||||
value = conditional->errorConstant();
|
value = conditional->negLogConstant();
|
||||||
}
|
}
|
||||||
return {std::dynamic_pointer_cast<GaussianFactor>(conditional), value};
|
return {std::dynamic_pointer_cast<GaussianFactor>(conditional), value};
|
||||||
};
|
};
|
||||||
|
@ -58,7 +58,7 @@ HybridGaussianConditional::HybridGaussianConditional(
|
||||||
[this](const GaussianConditional::shared_ptr &conditional) {
|
[this](const GaussianConditional::shared_ptr &conditional) {
|
||||||
if (conditional) {
|
if (conditional) {
|
||||||
this->logConstant_ =
|
this->logConstant_ =
|
||||||
std::min(this->logConstant_, conditional->errorConstant());
|
std::min(this->logConstant_, conditional->negLogConstant());
|
||||||
}
|
}
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
@ -84,7 +84,7 @@ GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree()
|
||||||
auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
|
auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
|
||||||
// First check if conditional has not been pruned
|
// First check if conditional has not been pruned
|
||||||
if (gc) {
|
if (gc) {
|
||||||
const double Cgm_Kgcm = gc->errorConstant() - this->logConstant_;
|
const double Cgm_Kgcm = gc->negLogConstant() - this->logConstant_;
|
||||||
// If there is a difference in the covariances, we need to account for
|
// If there is a difference in the covariances, we need to account for
|
||||||
// that since the error is dependent on the mode.
|
// that since the error is dependent on the mode.
|
||||||
if (Cgm_Kgcm > 0.0) {
|
if (Cgm_Kgcm > 0.0) {
|
||||||
|
@ -214,7 +214,7 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
|
||||||
[&](const GaussianConditional::shared_ptr &conditional)
|
[&](const GaussianConditional::shared_ptr &conditional)
|
||||||
-> GaussianFactorValuePair {
|
-> GaussianFactorValuePair {
|
||||||
const auto likelihood_m = conditional->likelihood(given);
|
const auto likelihood_m = conditional->likelihood(given);
|
||||||
const double Cgm_Kgcm = conditional->errorConstant() - logConstant_;
|
const double Cgm_Kgcm = conditional->negLogConstant() - logConstant_;
|
||||||
if (Cgm_Kgcm == 0.0) {
|
if (Cgm_Kgcm == 0.0) {
|
||||||
return {likelihood_m, 0.0};
|
return {likelihood_m, 0.0};
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -158,7 +158,7 @@ class GTSAM_EXPORT HybridGaussianConditional
|
||||||
*
|
*
|
||||||
* @return double
|
* @return double
|
||||||
*/
|
*/
|
||||||
inline double errorConstant() const override { return logConstant_; }
|
inline double negLogConstant() const override { return logConstant_; }
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Create a likelihood factor for a hybrid Gaussian conditional,
|
* Create a likelihood factor for a hybrid Gaussian conditional,
|
||||||
|
|
|
@ -329,9 +329,9 @@ static std::shared_ptr<Factor> createDiscreteFactor(
|
||||||
|
|
||||||
// Logspace version of:
|
// Logspace version of:
|
||||||
// exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
|
// exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
|
||||||
// errorConstant gives `-log(k)`
|
// negLogConstant gives `-log(k)`
|
||||||
// which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`.
|
// which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`.
|
||||||
return -factor->error(kEmpty) + conditional->errorConstant();
|
return -factor->error(kEmpty) + conditional->negLogConstant();
|
||||||
};
|
};
|
||||||
|
|
||||||
AlgebraicDecisionTree<Key> logProbabilities(
|
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
|
// Add 2.0 term since the constant term will be premultiplied by 0.5
|
||||||
// as per the Hessian definition,
|
// as per the Hessian definition,
|
||||||
// and negative since we want log(k)
|
// and negative since we want log(k)
|
||||||
hf->constantTerm() += -2.0 * conditional->errorConstant();
|
hf->constantTerm() += -2.0 * conditional->negLogConstant();
|
||||||
}
|
}
|
||||||
return {factor, 0.0};
|
return {factor, 0.0};
|
||||||
};
|
};
|
||||||
|
|
|
@ -61,7 +61,7 @@ virtual class HybridConditional {
|
||||||
size_t nrParents() const;
|
size_t nrParents() const;
|
||||||
|
|
||||||
// Standard interface:
|
// Standard interface:
|
||||||
double errorConstant() const;
|
double negLogConstant() const;
|
||||||
double logNormalizationConstant() const;
|
double logNormalizationConstant() const;
|
||||||
double logProbability(const gtsam::HybridValues& values) const;
|
double logProbability(const gtsam::HybridValues& values) const;
|
||||||
double evaluate(const gtsam::HybridValues& values) const;
|
double evaluate(const gtsam::HybridValues& values) const;
|
||||||
|
|
|
@ -180,16 +180,16 @@ TEST(HybridGaussianConditional, Error2) {
|
||||||
|
|
||||||
// Check result.
|
// Check result.
|
||||||
DiscreteKeys discrete_keys{mode};
|
DiscreteKeys discrete_keys{mode};
|
||||||
double errorConstant0 = conditionals[0]->errorConstant();
|
double negLogConstant0 = conditionals[0]->negLogConstant();
|
||||||
double errorConstant1 = conditionals[1]->errorConstant();
|
double negLogConstant1 = conditionals[1]->negLogConstant();
|
||||||
double minErrorConstant = std::min(errorConstant0, errorConstant1);
|
double minErrorConstant = std::min(negLogConstant0, negLogConstant1);
|
||||||
|
|
||||||
// Expected error is e(X) + log(sqrt(|2πΣ|)).
|
// Expected error is e(X) + log(sqrt(|2πΣ|)).
|
||||||
// We normalize log(sqrt(|2πΣ|)) with min(errorConstant)
|
// We normalize log(sqrt(|2πΣ|)) with min(negLogConstant)
|
||||||
// so it is non-negative.
|
// so it is non-negative.
|
||||||
std::vector<double> leaves = {
|
std::vector<double> leaves = {
|
||||||
conditionals[0]->error(vv) + errorConstant0 - minErrorConstant,
|
conditionals[0]->error(vv) + negLogConstant0 - minErrorConstant,
|
||||||
conditionals[1]->error(vv) + errorConstant1 - minErrorConstant};
|
conditionals[1]->error(vv) + negLogConstant1 - minErrorConstant};
|
||||||
AlgebraicDecisionTree<Key> expected(discrete_keys, leaves);
|
AlgebraicDecisionTree<Key> expected(discrete_keys, leaves);
|
||||||
|
|
||||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||||
|
@ -198,7 +198,7 @@ TEST(HybridGaussianConditional, Error2) {
|
||||||
for (size_t mode : {0, 1}) {
|
for (size_t mode : {0, 1}) {
|
||||||
const HybridValues hv{vv, {{M(0), mode}}};
|
const HybridValues hv{vv, {{M(0), mode}}};
|
||||||
EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv) +
|
EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv) +
|
||||||
conditionals[mode]->errorConstant() -
|
conditionals[mode]->negLogConstant() -
|
||||||
minErrorConstant,
|
minErrorConstant,
|
||||||
hybrid_conditional.error(hv), 1e-8);
|
hybrid_conditional.error(hv), 1e-8);
|
||||||
}
|
}
|
||||||
|
|
|
@ -780,8 +780,8 @@ static HybridGaussianFactorGraph CreateFactorGraph(
|
||||||
// Create HybridGaussianFactor
|
// Create HybridGaussianFactor
|
||||||
// We take negative since we want
|
// We take negative since we want
|
||||||
// the underlying scalar to be log(\sqrt(|2πΣ|))
|
// the underlying scalar to be log(\sqrt(|2πΣ|))
|
||||||
std::vector<GaussianFactorValuePair> factors{{f0, model0->errorConstant()},
|
std::vector<GaussianFactorValuePair> factors{{f0, model0->negLogConstant()},
|
||||||
{f1, model1->errorConstant()}};
|
{f1, model1->negLogConstant()}};
|
||||||
HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors);
|
HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors);
|
||||||
|
|
||||||
HybridGaussianFactorGraph hfg;
|
HybridGaussianFactorGraph hfg;
|
||||||
|
|
|
@ -902,8 +902,8 @@ static HybridNonlinearFactorGraph CreateFactorGraph(
|
||||||
// Create HybridNonlinearFactor
|
// Create HybridNonlinearFactor
|
||||||
// We take negative since we want
|
// We take negative since we want
|
||||||
// the underlying scalar to be log(\sqrt(|2πΣ|))
|
// the underlying scalar to be log(\sqrt(|2πΣ|))
|
||||||
std::vector<NonlinearFactorValuePair> factors{{f0, model0->errorConstant()},
|
std::vector<NonlinearFactorValuePair> factors{{f0, model0->negLogConstant()},
|
||||||
{f1, model1->errorConstant()}};
|
{f1, model1->negLogConstant()}};
|
||||||
|
|
||||||
HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors);
|
HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors);
|
||||||
|
|
||||||
|
|
|
@ -59,17 +59,17 @@ double Conditional<FACTOR, DERIVEDCONDITIONAL>::evaluate(
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template <class FACTOR, class DERIVEDCONDITIONAL>
|
template <class FACTOR, class DERIVEDCONDITIONAL>
|
||||||
double Conditional<FACTOR, DERIVEDCONDITIONAL>::errorConstant()
|
double Conditional<FACTOR, DERIVEDCONDITIONAL>::negLogConstant()
|
||||||
const {
|
const {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
"Conditional::errorConstant is not implemented");
|
"Conditional::negLogConstant is not implemented");
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
template <class FACTOR, class DERIVEDCONDITIONAL>
|
template <class FACTOR, class DERIVEDCONDITIONAL>
|
||||||
double Conditional<FACTOR, DERIVEDCONDITIONAL>::logNormalizationConstant()
|
double Conditional<FACTOR, DERIVEDCONDITIONAL>::logNormalizationConstant()
|
||||||
const {
|
const {
|
||||||
return -errorConstant();
|
return -negLogConstant();
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -96,7 +96,7 @@ bool Conditional<FACTOR, DERIVEDCONDITIONAL>::CheckInvariants(
|
||||||
// normalization constant
|
// normalization constant
|
||||||
const double error = conditional.error(values);
|
const double error = conditional.error(values);
|
||||||
if (error < 0.0) return false; // prob_or_density is negative.
|
if (error < 0.0) return false; // prob_or_density is negative.
|
||||||
const double expected = -(conditional.errorConstant() + error);
|
const double expected = -(conditional.negLogConstant() + error);
|
||||||
if (std::abs(logProb - expected) > 1e-9)
|
if (std::abs(logProb - expected) > 1e-9)
|
||||||
return false; // logProb is not consistent with error
|
return false; // logProb is not consistent with error
|
||||||
return true;
|
return true;
|
||||||
|
|
|
@ -169,7 +169,7 @@ namespace gtsam {
|
||||||
*
|
*
|
||||||
* @return double
|
* @return double
|
||||||
*/
|
*/
|
||||||
virtual double errorConstant() const;
|
virtual double negLogConstant() const;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* All conditional types need to implement a log normalization constant to
|
* All conditional types need to implement a log normalization constant to
|
||||||
|
|
|
@ -182,7 +182,7 @@ namespace gtsam {
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
|
// normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
|
||||||
// neg-log = 0.5 * n*log(2*pi) + 0.5 * log det(Sigma)
|
// neg-log = 0.5 * n*log(2*pi) + 0.5 * log det(Sigma)
|
||||||
double GaussianConditional::errorConstant() const {
|
double GaussianConditional::negLogConstant() const {
|
||||||
constexpr double log2pi = 1.8378770664093454835606594728112;
|
constexpr double log2pi = 1.8378770664093454835606594728112;
|
||||||
size_t n = d().size();
|
size_t n = d().size();
|
||||||
// Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
|
// Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
|
||||||
|
|
|
@ -140,7 +140,7 @@ namespace gtsam {
|
||||||
*
|
*
|
||||||
* @return double
|
* @return double
|
||||||
*/
|
*/
|
||||||
double errorConstant() const override;
|
double negLogConstant() const override;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Calculate log-probability log(evaluate(x)) for given values `x`:
|
* Calculate log-probability log(evaluate(x)) for given values `x`:
|
||||||
|
|
|
@ -255,7 +255,7 @@ double Gaussian::logDeterminant() const {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* *******************************************************************************/
|
/* *******************************************************************************/
|
||||||
double Gaussian::errorConstant() const {
|
double Gaussian::negLogConstant() const {
|
||||||
// log(det(Sigma)) = -2.0 * logDetR
|
// log(det(Sigma)) = -2.0 * logDetR
|
||||||
// which gives neg-log = 0.5*n*log(2*pi) + 0.5*(-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())
|
// = 0.5*n*log(2*pi) - (0.5*2.0 * logDetR())
|
||||||
|
@ -268,7 +268,7 @@ double Gaussian::errorConstant() const {
|
||||||
|
|
||||||
/* *******************************************************************************/
|
/* *******************************************************************************/
|
||||||
double Gaussian::logNormalizationConstant() const {
|
double Gaussian::logNormalizationConstant() const {
|
||||||
return -errorConstant();
|
return -negLogConstant();
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -277,7 +277,7 @@ namespace gtsam {
|
||||||
*
|
*
|
||||||
* @return double
|
* @return double
|
||||||
*/
|
*/
|
||||||
double errorConstant() const;
|
double negLogConstant() const;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Method to compute the normalization constant
|
* @brief Method to compute the normalization constant
|
||||||
|
|
|
@ -548,7 +548,7 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
|
||||||
bool equals(const gtsam::GaussianConditional& cg, double tol) const;
|
bool equals(const gtsam::GaussianConditional& cg, double tol) const;
|
||||||
|
|
||||||
// Standard Interface
|
// Standard Interface
|
||||||
double errorConstant() const;
|
double negLogConstant() const;
|
||||||
double logNormalizationConstant() const;
|
double logNormalizationConstant() const;
|
||||||
double logProbability(const gtsam::VectorValues& x) const;
|
double logProbability(const gtsam::VectorValues& x) const;
|
||||||
double evaluate(const gtsam::VectorValues& x) const;
|
double evaluate(const gtsam::VectorValues& x) const;
|
||||||
|
|
|
@ -55,7 +55,7 @@ TEST(GaussianDensity, FromMeanAndStddev) {
|
||||||
double expected1 = 0.5 * e.dot(e);
|
double expected1 = 0.5 * e.dot(e);
|
||||||
EXPECT_DOUBLES_EQUAL(expected1, density.error(values), 1e-9);
|
EXPECT_DOUBLES_EQUAL(expected1, density.error(values), 1e-9);
|
||||||
|
|
||||||
double expected2 = -(density.errorConstant() + 0.5 * e.dot(e));
|
double expected2 = -(density.negLogConstant() + 0.5 * e.dot(e));
|
||||||
EXPECT_DOUBLES_EQUAL(expected2, density.logProbability(values), 1e-9);
|
EXPECT_DOUBLES_EQUAL(expected2, density.logProbability(values), 1e-9);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -90,7 +90,7 @@ class TestHybridBayesNet(GtsamTestCase):
|
||||||
self.assertTrue(probability >= 0.0)
|
self.assertTrue(probability >= 0.0)
|
||||||
logProb = conditional.logProbability(values)
|
logProb = conditional.logProbability(values)
|
||||||
self.assertAlmostEqual(probability, np.exp(logProb))
|
self.assertAlmostEqual(probability, np.exp(logProb))
|
||||||
expected = -(conditional.errorConstant() + conditional.error(values))
|
expected = -(conditional.negLogConstant() + conditional.error(values))
|
||||||
self.assertAlmostEqual(logProb, expected)
|
self.assertAlmostEqual(logProb, expected)
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue