12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Consistency test in testGaussianConditional

release/4.3a0
Frank Dellaert 2023-01-13 12:29:11 -08:00
parent ebb5ae6f18
commit b99d464049
3 changed files with 28 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/inference/Conditional-inst.h
View File

@ -63,20 +63,4 @@ double Conditional<FACTOR, DERIVEDCONDITIONAL>::normalizationConstant() const {
return std::exp(logNormalizationConstant());
}
/* ************************************************************************* */
template <class FACTOR, class DERIVEDCONDITIONAL>
bool Conditional<FACTOR, DERIVEDCONDITIONAL>::checkInvariants(
const HybridValues& values) const {
const double probability = evaluate(values);
if (probability < 0.0 || probability > 1.0)
return false; // probability is not in [0,1]
const double logProb = logProbability(values);
if (std::abs(probability - std::exp(logProb)) > 1e-9)
return false; // logProb is not consistent with probability
const double expected =
this->logNormalizationConstant() - this->error(values);
if (std::abs(logProb - expected) > 1e-9)
return false; // logProb is not consistent with error
}
} // namespace gtsam

5
gtsam/inference/Conditional.h
View File

@ -145,7 +145,7 @@ namespace gtsam {
* By default, log normalization constant = 0.0.
* Override if this depends on the parameters.
*/
virtual double logNormalizationConstant() const;
virtual double logNormalizationConstant() const { return 0.0; }
/** Non-virtual, exponentiate logNormalizationConstant. */
double normalizationConstant() const;
@ -181,9 +181,6 @@ namespace gtsam {
/** Mutable iterator pointing past the last parent key. */
typename FACTOR::iterator endParents() { return asFactor().end(); }
/** Check that the invariants hold for derived class at a given point. */
bool checkInvariants(const HybridValues& values) const;
/// @}
private:

40
gtsam/linear/tests/testGaussianConditional.cpp
View File

@ -25,6 +25,7 @@
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianDensity.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/hybrid/HybridValues.h>
#include <boost/make_shared.hpp>
@ -135,18 +136,20 @@ static const auto unitPrior =
} // namespace density
/* ************************************************************************* */
bool checkInvariants(const GaussianConditional* self,
const HybridValues& values) {
const double probability = self->evaluate(values);
template <class VALUES>
bool checkInvariants(const GaussianConditional& conditional,
const VALUES& values) {
const double probability = conditional.evaluate(values);
if (probability < 0.0 || probability > 1.0)
return false; // probability is not in [0,1]
const double logProb = self->logProbability(values);
const double logProb = conditional.logProbability(values);
if (std::abs(probability - std::exp(logProb)) > 1e-9)
return false; // logProb is not consistent with probability
const double expected =
self->logNormalizationConstant() - self->error(values);
conditional.logNormalizationConstant() - conditional.error(values);
if (std::abs(logProb - expected) > 1e-9)
return false; // logProb is not consistent with error
return true;
}
/* ************************************************************************* */
@ -169,6 +172,12 @@ TEST(GaussianConditional, Evaluate1) {
using density::key;
using density::sigma;
// Check Invariants at the mean and a different value
for (auto vv : {mean, VectorValues{{key, Vector1(4)}}}) {
EXPECT(checkInvariants(density::unitPrior, vv));
EXPECT(checkInvariants(density::unitPrior, HybridValues{vv, {}, {}}));
}
// Let's numerically integrate and see that we integrate to 1.0.
double integral = 0.0;
// Loop from -5*sigma to 5*sigma in 0.1*sigma steps:
@ -179,7 +188,6 @@ TEST(GaussianConditional, Evaluate1) {
integral += 0.1 * sigma * density;
}
EXPECT_DOUBLES_EQUAL(1.0, integral, 1e-9);
EXPECT(checkInvariants(&density::unitPrior, mean));
}
/* ************************************************************************* */
@ -196,6 +204,12 @@ TEST(GaussianConditional, Evaluate2) {
using density::key;
using density::sigma;
// Check Invariants at the mean and a different value
for (auto vv : {mean, VectorValues{{key, Vector1(4)}}}) {
EXPECT(checkInvariants(density::widerPrior, vv));
EXPECT(checkInvariants(density::widerPrior, HybridValues{vv, {}, {}}));
}
// Let's numerically integrate and see that we integrate to 1.0.
double integral = 0.0;
// Loop from -5*sigma to 5*sigma in 0.1*sigma steps:
@ -400,17 +414,17 @@ TEST(GaussianConditional, FromMeanAndStddev) {
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 expected3 = 0.5 * e2.dot(e2);
EXPECT_DOUBLES_EQUAL(expected3, conditional2.error(values), 1e-9);
double expected2 = 0.5 * e2.dot(e2);
EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9);
double expected4 = conditional2.logNormalizationConstant() - 0.5 * e2.dot(e2);
EXPECT_DOUBLES_EQUAL(expected4, conditional2.logProbability(values), 1e-9);
// Check Invariants for both conditionals
for (auto conditional : {conditional1, conditional2}) {
EXPECT(checkInvariants(conditional, values));
EXPECT(checkInvariants(conditional, HybridValues{values, {}, {}}));
}
}
/* ************************************************************************* */