From bbf2e52cc7c5da3619749b71e10ad96731ba7b99 Mon Sep 17 00:00:00 2001
From: f-fl0 <66578286+f-fl0@users.noreply.github.com>
Date: Tue, 14 Nov 2023 19:46:20 +0900
Subject: [PATCH] Fix reported entropy (#408)

* Add entropy as a class member of ParticleFilter
* Compute entropy in the measure method
* Use getEntropy from the particle filter object
* Check if the particle probability is greater than 0
* Add simple tests
---
 include/mcl_3dl/pf.h | 11 ++++++++
 src/mcl_3dl.cpp      | 21 ++-------------
 test/src/test_pf.cpp | 63 ++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 76 insertions(+), 19 deletions(-)

diff --git a/include/mcl_3dl/pf.h b/include/mcl_3dl/pf.h
index 0fd9b99c..bb40c60a 100644
--- a/include/mcl_3dl/pf.h
+++ b/include/mcl_3dl/pf.h
@@ -256,10 +256,16 @@ class ParticleFilter
     }
     if (sum > 0.0)
     {
+      entropy_ = 0;
       for (auto& p : particles_)
       {
         p.probability_ /= sum;
+        if (p.probability_ > 0)
+        {
+          entropy_ += p.probability_ * std::log(p.probability_);
+        }
       }
+      entropy_ *= -1;
     }
     else
     {
@@ -437,12 +443,17 @@ class ParticleFilter
   {
     return particles_.end();
   }
+  FLT_TYPE getEntropy() const
+  {
+    return entropy_;
+  }
 
 protected:
   std::vector<Particle<T, FLT_TYPE>> particles_;
   std::vector<Particle<T, FLT_TYPE>> particles_dup_;
   RANDOM_ENGINE engine_;
   T ie_;
+  FLT_TYPE entropy_;
 };
 
 }  // namespace pf
diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp
index 5582d478..6a899bd6 100644
--- a/src/mcl_3dl.cpp
+++ b/src/mcl_3dl.cpp
@@ -1119,25 +1119,8 @@ class MCL3dlNode
       pm.orientation.w = p.rot_.w_;
       pa.poses.push_back(pm);
     }
-    pub_particle_.publish(pa);
-  }
-
-  float getEntropy()
-  {
-    float sum = 0.0f;
-    for (auto& particle : *pf_)
-    {
-      sum += particle.probability_;
-    }
-
-    float entropy = 0.0f;
-    for (auto& particle : *pf_)
-    {
-      if (particle.probability_ / sum > 0.0)
-        entropy += particle.probability_ / sum * std::log(particle.probability_ / sum);
-    }
 
-    return -entropy;
+    pub_particle_.publish(pa);
   }
 
   void diagnoseStatus(diagnostic_updater::DiagnosticStatusWrapper& stat)
@@ -1159,7 +1142,7 @@ class MCL3dlNode
     stat.add("Odometry Availability", has_odom_ ? "true" : "false");
     stat.add("IMU Availability", has_imu_ ? "true" : "false");
 
-    status_.entropy = getEntropy();
+    status_.entropy = pf_->getEntropy();
     pub_status_.publish(status_);
   }
 
diff --git a/test/src/test_pf.cpp b/test/src/test_pf.cpp
index de8e6554..d977333f 100644
--- a/test/src/test_pf.cpp
+++ b/test/src/test_pf.cpp
@@ -327,6 +327,69 @@ TEST(Pf, AppendParticles)
     ASSERT_EQ(pf.getParticle(i)[0], val1);
 }
 
+TEST(Pf, Entropy)
+{
+  mcl_3dl::pf::ParticleFilter<State, float> pf(10);
+
+  // no uncertainty
+  {
+    unsigned int idx = 0;
+    auto likelihood = [&idx](const State& s) -> float
+    {
+      return idx++ == 0 ? 1.0 : 0.0;
+    };
+    pf.init(State(0), State(0.1));
+    pf.measure(likelihood);
+    ASSERT_EQ(pf.getEntropy(), 0);
+  }
+
+  // uniform distribution
+  {
+    auto likelihood = [](const State& s) -> float
+    {
+      return 0.1;
+    };
+    pf.init(State(0), State(0.1));
+    pf.measure(likelihood);
+    ASSERT_NEAR(pf.getEntropy(), 2.303, 1e-3);
+  }
+
+  // compare the entropy of two distributions with first one having a narrower peak
+  // i.e. less uncertainty that the other
+  {
+    unsigned int idx = 0;
+    auto likelihood1 = [&idx](const State& s) -> float
+    {
+      float lk = 0.025;
+      if (idx >= 4 && idx < 6)
+      {
+        lk = 0.4;
+      }
+      idx++;
+      return lk;
+    };
+    pf.init(State(0), State(0.1));
+    pf.measure(likelihood1);
+    const float entropy1 = pf.getEntropy();
+
+    idx = 0;
+    auto likelihood2 = [&idx](const State& s) -> float
+    {
+      float lk = 0.025;
+      if (idx >= 2 && idx < 8)
+      {
+        lk = 0.15;
+      }
+      idx++;
+      return lk;
+    };
+    pf.init(State(0), State(0.1));
+    pf.measure(likelihood2);
+    const float entropy2 = pf.getEntropy();
+    ASSERT_GT(entropy2, entropy1);
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);