Parallelize sensor model update

beluga/CMakeLists.txt

@@ -17,13 +17,14 @@ find_package(ament_cmake REQUIRED)
 find_package(Eigen3 REQUIRED NO_MODULE)
 find_package(range-v3 REQUIRED)
 find_package(Sophus REQUIRED)
+find_package(TBB REQUIRED)
 
 add_library(${PROJECT_NAME} INTERFACE)
 target_include_directories(${PROJECT_NAME} INTERFACE
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 ament_target_dependencies(${PROJECT_NAME} range-v3)
-target_link_libraries(${PROJECT_NAME} INTERFACE Eigen3::Eigen Sophus::Sophus)
+target_link_libraries(${PROJECT_NAME} INTERFACE Eigen3::Eigen Sophus::Sophus TBB::tbb)
 target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17)
 target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG SOPHUS_USE_BASIC_LOGGING)
 
@@ -65,7 +66,8 @@ ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
 ament_export_dependencies(
   Eigen3
   range-v3
-  Sophus)
+  Sophus
+  TBB)
 
 ament_package()
 

beluga/include/beluga/algorithm/particle_filter.hpp

@@ -15,6 +15,8 @@
 #ifndef BELUGA_ALGORITHM_PARTICLE_FILTER_HPP
 #define BELUGA_ALGORITHM_PARTICLE_FILTER_HPP
 
+#include <execution>
+
 #include <beluga/algorithm/estimation.hpp>
 #include <beluga/algorithm/sampling.hpp>
 #include <beluga/tuple_vector.hpp>
@@ -49,33 +51,34 @@ struct BootstrapParticleFilter : public Mixin {
   auto weights() const { return views::weights(particles_) | ranges::views::const_; }
 
   void update() {
-    sampling();
-    importance_sampling();
-    resampling();
+    sample();
+    importance_sample();
+    resample();
   }
 
- private:
-  Container particles_;
-
-  void sampling() {
-    auto&& states = views::states(particles_);
+  void sample() {
+    auto states = views::states(particles_);
     ranges::transform(
         states, std::begin(states), [this](const auto& state) { return this->self().apply_motion(state); });
   }
 
-  void importance_sampling() {
-    ranges::transform(views::states(particles_), std::begin(views::weights(particles_)), [this](const auto& state) {
-      return this->self().importance_weight(state);
-    });
+  void importance_sample() {
+    const auto states = views::states(particles_);
+    std::transform(
+        std::execution::par_unseq, states.begin(), states.end(), views::weights(particles_).begin(),
+        [this](const auto& state) { return this->self().importance_weight(state); });
   }
 
-  void resampling() {
+  void resample() {
     auto new_particles = Container{this->self().max_samples()};
     auto first = std::begin(views::all(new_particles));
     auto last = ranges::copy(this->self().generate_samples_from(particles_) | this->self().take_samples(), first).out;
     new_particles.resize(std::distance(first, last));
     particles_ = std::move(new_particles);
   }
+
+ private:
+  Container particles_;
 };
 
 template <

beluga/package.xml

@@ -16,6 +16,7 @@
   <depend>eigen</depend>
   <depend>range-v3</depend>
   <depend>sophus</depend>
+  <depend>tbb</depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_clang_tidy</test_depend>

beluga_amcl/include/beluga_amcl/amcl_node.hpp

@@ -81,6 +81,8 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
   std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
     rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
   message_filters::Connection laser_scan_connection_;
+
+  rclcpp::Time last_particle_filter_log_{now()};
 };
 
 }  // namespace beluga_amcl

beluga_amcl/src/amcl_node.cpp

@@ -442,39 +442,53 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_
   }
 
   try {
-    auto base_to_laser_transform = tf2::Transform{};
-    tf2::convert(
-      tf_buffer_->lookupTransform(
-        get_parameter("base_frame_id").as_string(),
-        laser_scan->header.frame_id,
-        laser_scan->header.stamp,
-        std::chrono::seconds(1)).transform, base_to_laser_transform);
-    const float range_min =
-      std::max(
-      laser_scan->range_min,
-      static_cast<float>(get_parameter("laser_min_range").as_double()));
-    const float range_max =
-      std::min(
-      laser_scan->range_max,
-      static_cast<float>(get_parameter("laser_max_range").as_double()));
-    auto points = std::vector<std::pair<double, double>>{};
-    points.reserve(laser_scan->ranges.size());
-    for (std::size_t index = 0; index < laser_scan->ranges.size(); ++index) {
-      const float range = laser_scan->ranges[index];
-      if (std::isnan(range) || range < range_min || range > range_max) {
-        continue;
+    {
+      auto base_to_laser_transform = tf2::Transform{};
+      tf2::convert(
+        tf_buffer_->lookupTransform(
+          get_parameter("base_frame_id").as_string(),
+          laser_scan->header.frame_id,
+          laser_scan->header.stamp,
+          std::chrono::seconds(1)).transform, base_to_laser_transform);
+      const float range_min =
+        std::max(
+        laser_scan->range_min,
+        static_cast<float>(get_parameter("laser_min_range").as_double()));
+      const float range_max =
+        std::min(
+        laser_scan->range_max,
+        static_cast<float>(get_parameter("laser_max_range").as_double()));
+      auto points = std::vector<std::pair<double, double>>{};
+      points.reserve(laser_scan->ranges.size());
+      for (std::size_t index = 0; index < laser_scan->ranges.size(); ++index) {
+        const float range = laser_scan->ranges[index];
+        if (std::isnan(range) || range < range_min || range > range_max) {
+          continue;
+        }
+        // Store points in the robot's reference frame
+        const float angle = laser_scan->angle_min +
+          static_cast<float>(index) * laser_scan->angle_increment;
+        const auto point = base_to_laser_transform * tf2::Vector3{
+          range * std::cos(angle),
+          range * std::sin(angle),
+          0.0};
+        points.emplace_back(point.x(), point.y());
       }
-      // Store points in the robot's reference frame
-      const float angle = laser_scan->angle_min +
-        static_cast<float>(index) * laser_scan->angle_increment;
-      const auto point = base_to_laser_transform * tf2::Vector3{
-        range * std::cos(angle),
-        range * std::sin(angle),
-        0.0};
-      points.emplace_back(point.x(), point.y());
+      particle_filter_->update_sensor(std::move(points));
     }
-    particle_filter_->update_sensor(std::move(points));
-    particle_filter_->update();
+
+    const bool do_log = now() > last_particle_filter_log_ + tf2::durationFromSec(2.0);
+    if (do_log) {
+      last_particle_filter_log_ = now();
+    }
+
+    RCLCPP_INFO_EXPRESSION(get_logger(), do_log, "Sampling from particle set (motion update)");
+    particle_filter_->sample();
+    RCLCPP_INFO_EXPRESSION(get_logger(), do_log, "Computing particle weights (sensor update)");
+    particle_filter_->importance_sample();
+    RCLCPP_INFO_EXPRESSION(get_logger(), do_log, "Resampling from particle set");
+    particle_filter_->resample();
+    RCLCPP_INFO_EXPRESSION(get_logger(), do_log, "Particle filter update complete");
   } catch (const tf2::TransformException & error) {
     RCLCPP_ERROR(get_logger(), "Could not transform laser: %s", error.what());
   }