From e48d3bafcdc6ed19b3aae959d23ac33bc258d4bc Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 9 Mar 2017 18:57:47 +0100 Subject: [PATCH] added kullback leibler for gaussian cases --- main.cpp | 2 +- math/Distributions.h | 1 + math/distribution/Normal.h | 9 + math/distribution/NormalN.h | 50 ++++ math/divergence/KullbackLeibler.h | 90 ++++++++ sensors/beacon/BeaconProbabilityFree.h | 4 +- sensors/radio/WiFiProbabilityFree.h | 7 +- sensors/radio/WiFiProbabilityGrid.h | 5 +- tests/math/divergence/TestKullbackLeibler.cpp | 214 ++++++++++++++++++ 9 files changed, 374 insertions(+), 8 deletions(-) create mode 100644 math/distribution/NormalN.h create mode 100644 math/divergence/KullbackLeibler.h create mode 100644 tests/math/divergence/TestKullbackLeibler.cpp diff --git a/main.cpp b/main.cpp index fbb4e08..b103b91 100755 --- a/main.cpp +++ b/main.cpp @@ -29,7 +29,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; - ::testing::GTEST_FLAG(filter) = "*MotionDetection*"; + ::testing::GTEST_FLAG(filter) = "*KullbackLeibler*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; diff --git a/math/Distributions.h b/math/Distributions.h index 1879357..93d35c8 100644 --- a/math/Distributions.h +++ b/math/Distributions.h @@ -8,5 +8,6 @@ #include "distribution/VonMises.h" #include "distribution/Region.h" #include "distribution/Triangle.h" +#include "distribution/NormalN.h" #endif // DISTRIBUTIONS_H diff --git a/math/distribution/Normal.h b/math/distribution/Normal.h index fc073f8..b40ce67 100644 --- a/math/distribution/Normal.h +++ b/math/distribution/Normal.h @@ -44,6 +44,15 @@ namespace Distribution { gen.seed(seed); } + /** get the mean value */ + const T getMu() { + return this->mu; + } + + /** get the standard deviation */ + const T getSigma() { + return this->sigma; + } /** get the probability for the given value */ static T getProbability(const T mu, const T sigma, const T val) { diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h new file mode 100644 index 0000000..c628828 --- /dev/null +++ b/math/distribution/NormalN.h @@ -0,0 +1,50 @@ +#ifndef NORMALN_H +#define NORMALN_H + +#include + +namespace Distribution { + + class NormalDistributionN { + + private: + + const Eigen::VectorXd mu; + const Eigen::MatrixXd sigma; + + const double _a; + const Eigen::MatrixXd _sigmaInv; + + public: + + /** ctor */ + NormalDistributionN(const Eigen::VectorXd mu, const Eigen::MatrixXd sigma) : + mu(mu), sigma(sigma), _a( 1.0 / std::sqrt( (sigma * 2.0 * M_PI).determinant() ) ), _sigmaInv(sigma.inverse()) { + + } + + + /** get probability for the given value */ + double getProbability(const Eigen::VectorXd val) const { + const double b = ((val-mu).transpose() * _sigmaInv * (val-mu)); + return _a * std::exp(-b/2.0); + } + + /** get the mean vector */ + const Eigen::VectorXd getMu(){ + return this->mu; + } + + /** get covariance matrix */ + const Eigen::MatrixXd getSigma(){ + return this->sigma; + } + + const Eigen::MatrixXd getSigmaInv(){ + return this->_sigmaInv; + } + + }; + +} +#endif // NORMALN_H diff --git a/math/divergence/KullbackLeibler.h b/math/divergence/KullbackLeibler.h new file mode 100644 index 0000000..357b1c3 --- /dev/null +++ b/math/divergence/KullbackLeibler.h @@ -0,0 +1,90 @@ +#ifndef KULLBACKLEIBLER_H +#define KULLBACKLEIBLER_H + +#include "../distribution/Normal.h" +#include "../distribution/NormalN.h" + +#include "../../Assertions.h" +#include + +namespace Divergence { + + template class KullbackLeibler { + + public: + + /** Calculate the Kullback Leibler Distance for a univariate Gaussian distribution + * Info: https://tgmstat.wordpress.com/2013/07/10/kullback-leibler-divergence/ + */ + static inline Scalar getUnivariateGauss(Distribution::Normal norm1, Distribution::Normal norm2){ + + auto sigma1Quad = norm1.getSigma() * norm1.getSigma(); + auto sigma2Quad = norm2.getSigma() * norm2.getSigma(); + auto mu12Quad = (norm1.getMu() - norm2.getMu()) * (norm1.getMu() - norm2.getMu()); + auto log1 = std::log(norm1.getSigma()); + auto log2 = std::log(norm2.getSigma()); + + // kl = log(sigma_2 / sigma_1) + ((sigma_1^2 + (mu_1 - mu_2)^2) / 2 * sigma_2^2) - 0.5 + double klb = (log2 - log1) + ((sigma1Quad + mu12Quad)/(2.0 * sigma2Quad)) - 0.5; + + //klb is always greater 0 + if(klb < 0.0){ + Assert::doThrow("The Kullback Leibler Distance is < 0! Thats not possible"); + } + return klb; + } + + /** Calculate the Kullback Leibler Distance for a univariate Gaussian distribution symmetric*/ + static inline Scalar getUnivariateGaussSymmetric(Distribution::Normal norm1, Distribution::Normal norm2){ + return getUnivariateGauss(norm1, norm2) + getUnivariateGauss(norm2, norm1); + } + + /** Calculate the Kullback Leibler Distance for a multivariate Gaussian distribution */ + static inline Scalar getMultivariateGauss(Distribution::NormalDistributionN norm1, Distribution::NormalDistributionN norm2){ + + //both gaussian have the same dimension. + Assert::equal(norm1.getMu().rows(), norm2.getMu().rows(), "mean vectors do not have the same dimension"); + Assert::equal(norm1.getSigma().rows(), norm2.getSigma().rows(), "cov matrices do not have the same dimension"); + Assert::equal(norm1.getSigma().cols(), norm2.getSigma().cols(), "cov matrices do not have the same dimension"); + + //log + auto det1 = norm1.getSigma().determinant(); + auto det2 = norm2.getSigma().determinant(); + auto log1 = std::log(det1); + auto log2 = std::log(det2); + + //trace + Eigen::MatrixXd toTrace(norm1.getSigma().rows(),norm1.getSigma().cols()); + toTrace = norm2.getSigmaInv() * norm1.getSigma(); + auto trace = toTrace.trace(); + + //transpose + Eigen::VectorXd toTranspose(norm1.getMu().rows()); + toTranspose = norm2.getMu() - norm1.getMu(); + auto transpose = toTranspose.transpose(); + + //rawdensity + auto rawDensity = transpose * norm2.getSigmaInv() * toTranspose; + auto dimension = norm1.getMu().rows(); + + //0.5 * ((log(det(cov_2)/det(cov_1)) + tr(cov_2^-1 cov_1) + (mu_2 - mu_1)^T * cov_2^-1 * (mu_2 - mu_1) - dimension) + double klb = 0.5 * ((log2 - log1) + trace + rawDensity - dimension); + + //klb is always greater 0 + if(klb < 0.0){ + Assert::doThrow("The Kullback Leibler Distance is < 0! Thats not possible"); + } + return klb; + } + + /** Calculate the Kullback Leibler Distance for a multivariate Gaussian distribution symmetric*/ + static inline Scalar getMultivariateGaussSymmetric(Distribution::NormalDistributionN norm1, Distribution::NormalDistributionN norm2){ + return getMultivariateGauss(norm1, norm2) + getMultivariateGauss(norm2, norm1); + } + + + }; + +} + +#endif // KULLBACKLEIBLER_H diff --git a/sensors/beacon/BeaconProbabilityFree.h b/sensors/beacon/BeaconProbabilityFree.h index 46f6475..a82de3e 100644 --- a/sensors/beacon/BeaconProbabilityFree.h +++ b/sensors/beacon/BeaconProbabilityFree.h @@ -57,7 +57,9 @@ public: const float modelRSSI = model.getRSSI(entry.getBeacon().getMAC(), pos_m); // NaN? -> AP not known to the model -> skip - if (modelRSSI != modelRSSI) {continue;} + if (modelRSSI != modelRSSI) { + continue; + } // the scan's RSSI diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index 0ffc045..e850101 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -58,7 +58,7 @@ public: const Timestamp age = curTime - entry.getTimestamp(); Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense"); - Assert::isTrue(age.ms() <= 40000, "found a 40 second old wifi measurement. maybe there is a coding error?"); + //Assert::isTrue(age.ms() <= 40000, "found a 40 second old wifi measurement. maybe there is a coding error?"); // sigma grows with measurement age const float sigma = this->sigma + this->sigmaPerSecond * age.sec(); @@ -72,14 +72,15 @@ public: } // sanity check - Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?"); + //Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?"); return prob; } template double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs, const int age_ms = 0) const { - throw Exception("todo??"); + + return this->getProbability(n.inMeter() + Point3(0,0,1.3), curTime, obs); } }; diff --git a/sensors/radio/WiFiProbabilityGrid.h b/sensors/radio/WiFiProbabilityGrid.h index 1ac2189..840d1dd 100644 --- a/sensors/radio/WiFiProbabilityGrid.h +++ b/sensors/radio/WiFiProbabilityGrid.h @@ -73,7 +73,7 @@ public: const Timestamp age = curTime - measurement.getTimestamp(); // sigma grows with measurement age - float sigma = this->sigma + this->sigmaPerSecond * age.sec(); + float sigma = this->sigma + this->sigmaPerSecond * age.sec(); // the RSSI from the scan const float measuredRSSI = measurement.getRSSI(); @@ -102,8 +102,7 @@ public: } // sanity check -// Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?"); -// if (numMatchingAPs == 0) {return 0;} + //Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?"); // as not every node has the same number of visible/matching APs // we MUST return something like the average probability diff --git a/tests/math/divergence/TestKullbackLeibler.cpp b/tests/math/divergence/TestKullbackLeibler.cpp new file mode 100644 index 0000000..0a14f92 --- /dev/null +++ b/tests/math/divergence/TestKullbackLeibler.cpp @@ -0,0 +1,214 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" +#include "../../../math/divergence/KullbackLeibler.h" +#include "../../../math/Distributions.h" + +#include + + +TEST(KullbackLeibler, univariateGaussEQ) { + //if the distributions are equal, kld is 0 + Distribution::Normal norm1(0,1); + Distribution::Normal norm2(0,1); + + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getUnivariateGauss(norm1, norm2)); + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm1, norm2)); +} + +TEST(KullbackLeibler, univariateGaussGEmu) { + //bigger mu means greater kld + Distribution::Normal norm1(0,1); + Distribution::Normal norm2(0,1); + Distribution::Normal norm3(0,1); + Distribution::Normal norm4(1,1); + + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGauss(norm3, norm4), Divergence::KullbackLeibler::getUnivariateGauss(norm1, norm2)); + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm3, norm4), Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm1, norm2)); +} + +TEST(KullbackLeibler, univariateGaussGEsigma) { + //bigger sigma means greater kld + Distribution::Normal norm1(0,1); + Distribution::Normal norm2(0,1); + Distribution::Normal norm5(0,1); + Distribution::Normal norm6(0,3); + + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGauss(norm5, norm6), Divergence::KullbackLeibler::getUnivariateGauss(norm1, norm2)); + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm5, norm6), Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm1, norm2)); +} + +TEST(KullbackLeibler, univariateGaussRAND) { + + for(int i = 0; i < 20; i++){ + auto randMu1 = rand() % 100; + auto randMu2 = rand() % 100 + 100; + + auto randMu3 = rand() % 100; + auto randMu4 = rand() % 100 + 200; + + Distribution::Normal norm7(randMu1,1); + Distribution::Normal norm8(randMu2,1); + + Distribution::Normal norm9(randMu3,1); + Distribution::Normal norm10(randMu4,1); + + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGauss(norm9, norm10), Divergence::KullbackLeibler::getUnivariateGauss(norm8, norm7)); + ASSERT_GE(Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm9, norm10), Divergence::KullbackLeibler::getUnivariateGaussSymmetric(norm8, norm7)); + } + +} + +TEST(KullbackLeibler, multivariateGaussEQ) { + + //eq + Eigen::VectorXd mu1(2); + mu1[0] = 1.0; + mu1[1] = 1.0; + + Eigen::VectorXd mu2(2); + mu2[0] = 1.0; + mu2[1] = 1.0; + + Eigen::MatrixXd cov1(2,2); + cov1(0,0) = 1.0; + cov1(0,1) = 0.0; + cov1(1,0) = 0.0; + cov1(1,1) = 1.0; + + Eigen::MatrixXd cov2(2,2); + cov2(0,0) = 1.0; + cov2(0,1) = 0.0; + cov2(1,0) = 0.0; + cov2(1,1) = 1.0; + + Distribution::NormalDistributionN norm1(mu1, cov1); + Distribution::NormalDistributionN norm2(mu2, cov2); + + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getMultivariateGauss(norm1, norm2)); + ASSERT_EQ(0.0f, Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm1, norm2)); + + +} + +TEST(KullbackLeibler, multivariateGaussGeMu) { + + //ge mu + Eigen::VectorXd mu1(2); + mu1[0] = 1.0; + mu1[1] = 1.0; + + Eigen::VectorXd mu2(2); + mu2[0] = 1.0; + mu2[1] = 1.0; + + Eigen::VectorXd mu3(2); + mu3[0] = 1.0; + mu3[1] = 1.0; + + Eigen::VectorXd mu4(2); + mu4[0] = 1.0; + mu4[1] = 3.0; + + Eigen::MatrixXd cov1(2,2); + cov1(0,0) = 1.0; + cov1(0,1) = 0.0; + cov1(1,0) = 0.0; + cov1(1,1) = 1.0; + + Eigen::MatrixXd cov2(2,2); + cov2(0,0) = 1.0; + cov2(0,1) = 0.0; + cov2(1,0) = 0.0; + cov2(1,1) = 1.0; + + Eigen::MatrixXd cov3(2,2); + cov3(0,0) = 1.0; + cov3(0,1) = 0.0; + cov3(1,0) = 0.0; + cov3(1,1) = 1.0; + + Eigen::MatrixXd cov4(2,2); + cov4(0,0) = 1.0; + cov4(0,1) = 0.0; + cov4(1,0) = 0.0; + cov4(1,1) = 1.0; + + Distribution::NormalDistributionN norm1(mu1, cov1); + Distribution::NormalDistributionN norm2(mu2, cov2); + Distribution::NormalDistributionN norm3(mu3, cov3); + Distribution::NormalDistributionN norm4(mu4, cov4); + + double kld12 = Divergence::KullbackLeibler::getMultivariateGauss(norm1, norm2); + double kld34 = Divergence::KullbackLeibler::getMultivariateGauss(norm3, norm4); + std::cout << kld34 << " > " << kld12 << std::endl; + + double kld12sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm1, norm2); + double kld34sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm3, norm4); + std::cout << kld34sym << " > " << kld12sym << std::endl; + + ASSERT_GE(kld34, kld12); + ASSERT_GE(kld34sym, kld12sym); +} + +TEST(KullbackLeibler, multivariateGaussGeCov) { + + //ge cov + Eigen::VectorXd mu1(2); + mu1[0] = 1.0; + mu1[1] = 1.0; + + Eigen::VectorXd mu2(2); + mu2[0] = 1.0; + mu2[1] = 1.0; + + Eigen::VectorXd mu3(2); + mu3[0] = 1.0; + mu3[1] = 1.0; + + Eigen::VectorXd mu4(2); + mu4[0] = 1.0; + mu4[1] = 1.0; + + Eigen::MatrixXd cov1(2,2); + cov1(0,0) = 1.0; + cov1(0,1) = 0.0; + cov1(1,0) = 0.0; + cov1(1,1) = 1.0; + + Eigen::MatrixXd cov2(2,2); + cov2(0,0) = 1.0; + cov2(0,1) = 0.0; + cov2(1,0) = 0.0; + cov2(1,1) = 1.0; + + Eigen::MatrixXd cov3(2,2); + cov3(0,0) = 1.0; + cov3(0,1) = 0.0; + cov3(1,0) = 0.0; + cov3(1,1) = 1.0; + + Eigen::MatrixXd cov4(2,2); + cov4(0,0) = 3.0; + cov4(0,1) = 0.0; + cov4(1,0) = 0.0; + cov4(1,1) = 1.0; + + Distribution::NormalDistributionN norm1(mu1, cov1); + Distribution::NormalDistributionN norm2(mu2, cov2); + Distribution::NormalDistributionN norm3(mu3, cov3); + Distribution::NormalDistributionN norm4(mu4, cov4); + + double kld12 = Divergence::KullbackLeibler::getMultivariateGauss(norm1, norm2); + double kld34 = Divergence::KullbackLeibler::getMultivariateGauss(norm3, norm4); + std::cout << kld34 << " >" << kld12 << std::endl; + + double kld12sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm1, norm2); + double kld34sym = Divergence::KullbackLeibler::getMultivariateGaussSymmetric(norm3, norm4); + std::cout << kld34sym << " > " << kld12sym << std::endl; + + ASSERT_GE(kld34, kld12); + ASSERT_GE(kld34sym, kld12sym); +} + +#endif