diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 5373e6727..ee975da98 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include #include @@ -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{}(s.translation().x()); + std::size_t h2 = std::hash{}(s.translation().y()); + std::size_t h3 = std::hash{}(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 + 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); } diff --git a/beluga_amcl/src/amcl_nodelet.cpp b/beluga_amcl/src/amcl_nodelet.cpp index 7a38187be..96031f887 100644 --- a/beluga_amcl/src/amcl_nodelet.cpp +++ b/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); } diff --git a/beluga_example/rviz/rviz.ros2.rviz b/beluga_example/rviz/rviz.ros2.rviz index 2f2eb8a37..de1a0d2db 100644 --- a/beluga_example/rviz/rviz.ros2.rviz +++ b/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,10 +272,10 @@ 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: @@ -287,7 +283,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false - Width: 3696 - X: 144 - Y: 54 + collapsed: true + Width: 1850 + X: 70 + Y: 27