diff --git a/floorplan/v2/FloorplanCeilings.h b/floorplan/v2/FloorplanCeilings.h index b86e22e..5a8699f 100644 --- a/floorplan/v2/FloorplanCeilings.h +++ b/floorplan/v2/FloorplanCeilings.h @@ -114,8 +114,9 @@ namespace Floorplan { #ifdef WITH_ASSERTIONS - static size_t numNear = 0; - static size_t numFar = 0; + static uint64_t numNear = 0; + static uint64_t numFar = 0; + for (const float z : ceilingsAtHeight_m) { const float diff = std::min( std::abs(z-zMin), std::abs(z-zMax) ); if (diff < 0.1) {++numNear;} else {++numFar;} diff --git a/main.cpp b/main.cpp index 2808d16..46f1192 100755 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Offline.readWrite*"; - ::testing::GTEST_FLAG(filter) = "*Angle*"; + ::testing::GTEST_FLAG(filter) = "*Jensen*"; //::testing::GTEST_FLAG(filter) = "*Barometer*"; diff --git a/math/Random.h b/math/Random.h index 6dd8f8f..ae8d8c5 100644 --- a/math/Random.h +++ b/math/Random.h @@ -18,10 +18,10 @@ class RandomGenerator : public std::minstd_rand { public: /** ctor with default seed */ - RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;} + RandomGenerator() : std::minstd_rand(RANDOM_SEED) {;} /** ctor with custom seed */ - RandomGenerator(result_type seed) : std::minstd_rand(seed) {;} + RandomGenerator(result_type seed) : std::minstd_rand(seed) {;} }; diff --git a/math/distribution/NormalN.h b/math/distribution/NormalN.h index bb332c4..5075dea 100644 --- a/math/distribution/NormalN.h +++ b/math/distribution/NormalN.h @@ -91,7 +91,7 @@ namespace Distribution { Assert::notEqual(numElements, 0, "data is empty, thats not enough for getting the distribution!"); const Eigen::MatrixXd centered = data.rowwise() - mean.transpose(); - const Eigen::MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1); + Eigen::MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1); return NormalDistributionN(mean, cov); } diff --git a/math/divergence/JensenShannon.h b/math/divergence/JensenShannon.h index 5c9bbb3..df64088 100644 --- a/math/divergence/JensenShannon.h +++ b/math/divergence/JensenShannon.h @@ -18,6 +18,13 @@ public: * @param Q is a vector containg the densities of the same samples set then P */ static inline Scalar getGeneralFromSamples(Eigen::VectorXd P, Eigen::VectorXd Q, LOGMODE mode){ + // normalize + P /= P.sum(); + Q /= Q.sum(); + + Assert::isNear((double)P.sum(), 1.0, 0.01,"Normalization failed.. this shouldn't happen"); + Assert::isNear((double)Q.sum(), 1.0, 0.01, "Normalization failed.. this shouldn't happen"); + Eigen::VectorXd M = 0.5 * (P + Q); return (0.5 * KullbackLeibler::getGeneralFromSamples(P, M, mode)) + (0.5 * KullbackLeibler::getGeneralFromSamples(Q, M, mode)); diff --git a/sensors/radio/VAPGrouper.h b/sensors/radio/VAPGrouper.h index 89906a2..d12cd7a 100644 --- a/sensors/radio/VAPGrouper.h +++ b/sensors/radio/VAPGrouper.h @@ -109,7 +109,6 @@ public: true ); - // done return result; diff --git a/tests/math/divergence/TestJensenShannon.cpp b/tests/math/divergence/TestJensenShannon.cpp new file mode 100644 index 0000000..4bf8cf3 --- /dev/null +++ b/tests/math/divergence/TestJensenShannon.cpp @@ -0,0 +1,94 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" +#include "../../../math/divergence/JensenShannon.h" +#include "../../../math/Distributions.h" + +#include + + +TEST(JensenShannon, generalFromSamples) { + //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] = 9.0; + mu4[1] = 9.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) = 13.0; + + Distribution::NormalDistributionN norm1(mu1, cov1); + Distribution::NormalDistributionN norm2(mu2, cov2); + Distribution::NormalDistributionN norm3(mu3, cov3); + Distribution::NormalDistributionN norm4(mu4, cov4); + + int size = 10000; + Eigen::VectorXd samples1(size); + Eigen::VectorXd samples2(size); + Eigen::VectorXd samples3(size); + Eigen::VectorXd samples4(size); + + //random numbers + std::mt19937_64 rng; + // initialize the random number generator with time-dependent seed + uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count(); + std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed>>32)}; + rng.seed(ss); + // initialize a uniform distribution between 0 and 1 + std::uniform_real_distribution unif(-9, 10); + + //generate samples + for(int i = 0; i < size; ++i){ + + double r1 = unif(rng); + double r2 = unif(rng); + Eigen::VectorXd v(2); + v << r1, r2; + + samples1[i] = norm1.getProbability(v); + samples2[i] = norm2.getProbability(v); + samples3[i] = norm3.getProbability(v); + samples4[i] = norm4.getProbability(v); + } + + double kld12 = Divergence::JensenShannon::getGeneralFromSamples(samples1, samples2, Divergence::LOGMODE::NATURALIS); + double kld34 = Divergence::JensenShannon::getGeneralFromSamples(samples3, samples4, Divergence::LOGMODE::NATURALIS); + std::cout << kld34 << " > " << kld12 << std::endl; + + ASSERT_GE(kld34, kld12); + +} + +#endif diff --git a/tests/math/divergence/TestJensenShannon.h b/tests/math/divergence/TestJensenShannon.h new file mode 100644 index 0000000..e69de29