Make particle cloud message weights represent pdf (#260)

This change makes the weights in the particle cloud message be
representative of the estimated probability distribution.

Signed-off-by: Gerardo Puga <glpuga@gmail.com>

beluga_amcl/src/amcl_node.cpp

@@ -24,6 +24,8 @@
 #include <stdexcept>
 #include <string>
 #include <string_view>
+#include <tuple>
+#include <unordered_map>
 #include <utility>
 
 #include <beluga/mixin.hpp>
@@ -816,19 +818,67 @@ void AmclNode::timer_callback() {
     return;
   }
 
+  // Particle weights from the filter may or may not be representative of the
+  // true distribution. If we resampled, they are not, and there will be multiple copies
+  // of the most likely candidates, all with unit weight. In this case the number of copies
+  // is a proxy for the prob density at each candidate. If we did not resample before updating
+  // the estimation and publishing this message (which can happen if the resample interval
+  // is set to something other than 1), then all particles are expected to be different
+  // and their weights are proportional to the prob density at each candidate.
+  //
+  // Only the combination of both the state distribution and the candidate weights together
+  // provide information about the probability density at each candidate.
+  // To handle both cases, we group repeated candidates and compute the accumulated weight
+
+  struct RepresentativeData {
+    Sophus::SE2d state;
+    double weight{0.};
+  };
+
+  struct RepresentativeBinHash {
+    std::size_t operator()(const Sophus::SE2d& s) const noexcept {
+      std::size_t h1 = std::hash<double>{}(s.translation().x());
+      std::size_t h2 = std::hash<double>{}(s.translation().y());
+      std::size_t h3 = std::hash<double>{}(s.so2().log());
+      return h1 ^ (h2 << 1) ^ (h3 << 2);
+    }
+  };
+
+  struct RepresentativeBinEqual {
+    bool operator()(const Sophus::SE2d& lhs, const Sophus::SE2d& rhs) const noexcept {
+      // good enough, since copies of the same candidate are expected to be identical copies
+      return lhs.translation().x() == rhs.translation().x() &&  //
+             lhs.translation().y() == rhs.translation().y() &&  //
+             lhs.so2().log() == rhs.so2().log();
+    }
+  };
+
+  std::unordered_map<Sophus::SE2d, RepresentativeData, RepresentativeBinHash, RepresentativeBinEqual>
+      representatives_map;
+  representatives_map.reserve(particle_filter_->particle_count());
+  double max_weight = 1e-5;  // never risk dividing by zero
+
+  for (const auto& [state, weight] :
+       ranges::views::zip(particle_filter_->states_view(), particle_filter_->weights_view())) {
+    auto& representative = representatives_map[state];  // if the element does not exist, create it
+    representative.state = state;
+    representative.weight += weight;
+    if (representative.weight > max_weight) {
+      max_weight = representative.weight;
+    }
+  }
+
   auto message = nav2_msgs::msg::ParticleCloud{};
   message.header.stamp = now();
   message.header.frame_id = get_parameter("global_frame_id").as_string();
-  message.particles.resize(particle_filter_->particle_count());
-  ranges::transform(
-      ranges::views::zip(particle_filter_->states_view(), particle_filter_->weights_view()),
-      std::begin(message.particles), [](const auto& particle) {
-        const auto& [state, weight] = particle;
-        auto message = nav2_msgs::msg::Particle{};
-        tf2::toMsg(state, message.pose);
-        message.weight = weight;
-        return message;
-      });
+  message.particles.reserve(particle_filter_->particle_count());
+
+  for (const auto& [key, representative] : representatives_map) {
+    auto& particle = message.particles.emplace_back();
+    tf2::toMsg(representative.state, particle.pose);
+    particle.weight = representative.weight / max_weight;
+  }
+
   particle_cloud_pub_->publish(message);
 }
 

beluga_amcl/src/amcl_nodelet.cpp

@@ -372,14 +372,11 @@ void AmclNodelet::particle_cloud_timer_callback(const ros::TimerEvent& ev) {
   message.header.stamp = ev.current_real;
   message.header.frame_id = config_.global_frame_id;
   message.poses.resize(particle_filter_->particle_count());
-  ranges::transform(
-      ranges::views::zip(particle_filter_->states_view(), particle_filter_->weights_view()), std::begin(message.poses),
-      [](const auto& particle) {
-        const auto& [state, _] = particle;
-        auto message = geometry_msgs::Pose{};
-        tf2::toMsg(state, message);
-        return message;
-      });
+  ranges::transform(particle_filter_->states_view(), std::begin(message.poses), [](const auto& state) {
+    auto message = geometry_msgs::Pose{};
+    tf2::toMsg(state, message);
+    return message;
+  });
   particle_cloud_pub_.publish(message);
 }
 

beluga_example/rviz/rviz.ros2.rviz

@@ -6,15 +6,11 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
-        - /ParticleCloud1
-        - /Map1
-        - /Map2
-        - /PoseWithCovariance1
         - /PoseWithCovariance1/Covariance1
         - /PoseWithCovariance1/Covariance1/Position1
         - /PoseWithCovariance1/Covariance1/Orientation1
       Splitter Ratio: 0.5
-    Tree Height: 1555
+    Tree Height: 646
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -58,10 +54,10 @@ Visualization Manager:
       Class: nav2_rviz_plugins/ParticleCloud
       Color: 255; 25; 0
       Enabled: true
-      Max Arrow Length: 0.05000000074505806
-      Min Arrow Length: 0.05000000074505806
+      Max Arrow Length: 0.5
+      Min Arrow Length: 0.019999999552965164
       Name: ParticleCloud
-      Shape: Arrow (3D)
+      Shape: Arrow (Flat)
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -196,8 +192,8 @@ Visualization Manager:
       Head Length: 0.10000000149011612
       Head Radius: 0.10000000149011612
       Name: PoseWithCovariance
-      Shaft Length: 0.10000000149011612
-      Shaft Radius: 0.019999999552965164
+      Shaft Length: 0.4000000059604645
+      Shaft Radius: 0.029999999329447746
       Shape: Arrow
       Topic:
         Depth: 5
@@ -224,7 +220,7 @@ Visualization Manager:
     - Class: rviz_default_plugins/SetInitialPose
       Covariance x: 0.25
       Covariance y: 0.25
-      Covariance yaw: 1.57
+      Covariance yaw: 1.5700000524520874
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -276,18 +272,18 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 2032
+  Height: 1016
   Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000701fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000007010000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f00000701fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000007010000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7000000051fc0100000002fb0000000800540069006d0065010000000000000e700000046000fffffffb0000000800540069006d0065010000000000000450000000000000000000000b2d0000070100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001a00000034bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000034b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f0000034bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000034b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a00000051fc0100000002fb0000000800540069006d006501000000000000073a0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005940000034b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
-  Width: 3696
-  X: 144
-  Y: 54
+    collapsed: true
+  Width: 1850
+  X: 70
+  Y: 27