Add likelihood field pre-computation

beluga/include/beluga/algorithm.h

@@ -14,6 +14,7 @@
 
 #pragma once
 
+#include <beluga/algorithm/distance_map.h>
 #include <beluga/algorithm/exponential_filter.h>
 #include <beluga/algorithm/particle_filter.h>
 #include <beluga/algorithm/sampling.h>

beluga/include/beluga/algorithm/distance_map.h

@@ -0,0 +1,62 @@
+// Copyright 2022 Ekumen, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <queue>
+#include <vector>
+
+template <class Range, class DistanceFunction, class NeighborsFunction>
+auto nearest_obstacle_distance_map(
+    Range&& obstacle_map,
+    DistanceFunction&& distance_function,
+    NeighborsFunction&& neighbors_function) {
+  struct IndexPair {
+    std::size_t nearest_obstacle_index;
+    std::size_t index;
+  };
+
+  using DistanceType = std::invoke_result_t<DistanceFunction, std::size_t, std::size_t>;
+  auto distance_map = std::vector<DistanceType>(obstacle_map.size());
+  auto visited = std::vector<bool>(obstacle_map.size(), false);
+
+  auto compare = [&distance_map](const IndexPair& first, const IndexPair& second) {
+    return distance_map[first.index] > distance_map[second.index];
+  };
+  auto queue = std::priority_queue<IndexPair, std::vector<IndexPair>, decltype(compare)>{compare};
+
+  auto begin = obstacle_map.begin();
+  for (std::size_t index = 0; index < obstacle_map.size(); ++index) {
+    bool is_obstacle = *(begin + index);
+    if (is_obstacle) {
+      visited[index] = true;
+      distance_map[index] = 0;
+      queue.push(IndexPair{index, index});  // The nearest obstacle is itself
+    }
+  }
+
+  while (!queue.empty()) {
+    auto parent = queue.top();
+    queue.pop();
+    for (std::size_t index : neighbors_function(parent.index)) {
+      if (!visited[index]) {
+        visited[index] = true;
+        distance_map[index] = distance_function(parent.nearest_obstacle_index, index);
+        queue.push(IndexPair{parent.nearest_obstacle_index, index});
+      }
+    }
+  }
+
+  return distance_map;
+}

beluga/test/beluga/CMakeLists.txt

@@ -1,6 +1,7 @@
 find_package(ament_cmake_gmock REQUIRED)
 
 ament_add_gmock(test_beluga
+  algorithm/test_distance_map.cpp
   algorithm/test_exponential_filter.cpp
   algorithm/test_particle_filter.cpp
   algorithm/test_sampling.cpp

beluga/test/beluga/algorithm/test_distance_map.cpp

@@ -0,0 +1,74 @@
+// Copyright 2022 Ekumen, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gmock/gmock.h>
+
+#include <beluga/algorithm/distance_map.h>
+
+namespace {
+
+auto array_distance(std::size_t first, std::size_t second) {
+  return std::abs(static_cast<intmax_t>(first) - static_cast<intmax_t>(second));
+}
+
+auto make_neighbors_function(std::size_t size) {
+  return [=](std::size_t index) {
+    auto result = std::vector<std::size_t>{};
+    if (index > 0) {
+      result.push_back(index - 1);
+    }
+    if (index < size - 1) {
+      result.push_back(index + 1);
+    }
+    return result;
+  };
+}
+
+TEST(DistanceMap, None) {
+  auto map = std::vector<bool>{};
+  auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
+  ASSERT_EQ(distance_map.size(), 0);
+}
+
+TEST(DistanceMap, Empty) {
+  auto map = std::array<bool, 6>{false, false, false, false, false, false};
+  auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
+  ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 0, 0, 0, 0));
+}
+
+TEST(DistanceMap, Full) {
+  auto map = std::array<bool, 6>{true, true, true, true, true, true};
+  auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
+  ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 0, 0, 0, 0));
+}
+
+TEST(DistanceMap, Case1) {
+  auto map = std::array<bool, 6>{false, true, false, false, false, true};
+  auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
+  ASSERT_THAT(distance_map, testing::ElementsAre(1, 0, 1, 2, 1, 0));
+}
+
+TEST(DistanceMap, Case2) {
+  auto map = std::array<bool, 6>{true, true, false, false, false, false};
+  auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
+  ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 1, 2, 3, 4));
+}
+
+TEST(DistanceMap, Case3) {
+  auto map = std::array<bool, 6>{false, false, false, false, false, true};
+  auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6));
+  ASSERT_THAT(distance_map, testing::ElementsAre(5, 4, 3, 2, 1, 0));
+}
+
+}  // namespace

beluga_amcl/include/beluga_amcl/amcl_node.hpp

@@ -53,6 +53,8 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::ParticleCloud>::SharedPtr
     particle_cloud_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr
+    likelihood_field_pub_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
 };
 

beluga_amcl/include/beluga_amcl/models.hpp

@@ -20,16 +20,20 @@
 #ifndef BELUGA_AMCL__MODELS_HPP_
 #define BELUGA_AMCL__MODELS_HPP_
 
+#include <beluga/algorithm/distance_map.h>
 #include <beluga/spatial_hash.h>
 
+#include <algorithm>
 #include <cmath>
+#include <queue>
 #include <random>
 #include <tuple>
 #include <utility>
 #include <vector>
 
 #include <geometry_msgs/msg/pose.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
+#include <range/v3/view.hpp>
 
 namespace beluga_amcl
 {
@@ -68,28 +72,32 @@ class MockMotionModel : public Mixin
   [[nodiscard]] auto apply_motion(const Pose & state) {return state;}
 };
 
+struct LikelihoodSensorModelParam
+{
+  double max_obstacle_distance;
+  double max_laser_distance;
+  double z_hit;
+  double z_random;
+  double sigma_hit;
+};
+
 // TODO(nahuel): Move this to the core package once it doesn't depend on
 // ROS message types, or if we decide to add that dependecy to the core.
 template<class Mixin>
 class LikelihoodSensorModel : public Mixin
 {
 public:
   template<class ... Args>
-  explicit LikelihoodSensorModel(const nav_msgs::msg::OccupancyGrid & map, Args && ... rest)
-  : Mixin(std::forward<Args>(rest)...)
-  {
-    const std::size_t size = map.info.width * map.info.height;
-
-    free_cells_.reserve(size);
-    for (std::size_t index = 0; index < size; ++index) {
-      if (map.data[index] == 0) {
-        free_cells_.push_back(index);
-      }
-    }
-
-    map_metadata_ = map.info;
-    index_distribution_ = std::uniform_int_distribution<std::size_t>{0, free_cells_.size() - 1};
-  }
+  explicit LikelihoodSensorModel(
+    const LikelihoodSensorModelParam & params,
+    const nav_msgs::msg::OccupancyGrid & map, Args && ... rest)
+  : Mixin(std::forward<Args>(rest)...),
+    params_{params},
+    map_metadata_{map.info},
+    free_cells_{make_free_cells_list(map)},
+    index_distribution_{0, free_cells_.size() - 1},
+    likelihood_field_{make_likelihood_field(params, map)}
+  {}
 
   [[nodiscard]] double importance_weight(const Pose &)
   {
@@ -100,29 +108,109 @@ class LikelihoodSensorModel : public Mixin
   template<class Generator>
   [[nodiscard]] auto generate_random_state(Generator & generator)
   {
-    return pose_from_index_and_angle(
-      map_metadata_,
-      free_cells_[index_distribution_(generator)],
-      theta_distribution_(generator));
+    auto pose = Pose{};
+    auto [x, y] = xy_from_index(map_metadata_, free_cells_[index_distribution_(generator)]);
+    pose.x = x;
+    pose.y = y;
+    pose.theta = theta_distribution_(generator);
+    return pose;
+  }
+
+  auto get_likelihood_field_as_gridmap() const
+  {
+    auto message = nav_msgs::msg::OccupancyGrid{};
+    message.info = map_metadata_;
+    const std::size_t size = map_metadata_.width * map_metadata_.height;
+    message.data.resize(size);
+
+    for (std::size_t index = 0; index < size; ++index) {
+      message.data[index] = static_cast<std::int8_t>(likelihood_field_[index] * 100);
+    }
+    return message;
   }
 
 private:
+  LikelihoodSensorModelParam params_;
   nav_msgs::msg::MapMetaData map_metadata_;
   std::vector<std::size_t> free_cells_;
   std::uniform_int_distribution<std::size_t> index_distribution_;
   std::uniform_real_distribution<double> theta_distribution_{-M_PI, M_PI};
 
-  static auto pose_from_index_and_angle(
-    const nav_msgs::msg::MapMetaData & metadata,
-    std::size_t index, double angle)
+  std::vector<double> likelihood_field_;
+
+  static std::vector<std::size_t> make_free_cells_list(const nav_msgs::msg::OccupancyGrid & map)
   {
-    double delta_x = (static_cast<double>(index % metadata.height) + 0.5) * metadata.resolution;
-    double delta_y = (static_cast<double>(index / metadata.width) + 0.5) * metadata.resolution;
-    auto pose = Pose{};
-    pose.x = delta_x + metadata.origin.position.x;
-    pose.y = delta_y + metadata.origin.position.y;
-    pose.theta = angle;
-    return pose;
+    const std::size_t size = map.info.width * map.info.height;
+    auto free_cells = std::vector<std::size_t>{};
+    free_cells.reserve(size);
+    for (std::size_t index = 0; index < size; ++index) {
+      if (map.data[index] == 0) {
+        free_cells.push_back(index);
+      }
+    }
+    return free_cells;
+  }
+
+  static std::vector<double> make_likelihood_field(
+    const LikelihoodSensorModelParam & params,
+    const nav_msgs::msg::OccupancyGrid & map)
+  {
+    auto obstacle_map = map.data | ranges::views::transform(
+      [](std::int8_t value) -> bool {
+        // Map server occupied output value
+        // https://wiki.ros.org/map_server#Value_Interpretation
+        return value == 100;
+      });
+
+    auto squared_distance =
+      [width = map.info.width,
+        squared_resolution = map.info.resolution * map.info.resolution,
+        squared_max_distance = params.max_obstacle_distance * params.max_obstacle_distance
+      ](std::size_t first, std::size_t second) {
+        auto delta_x =
+          static_cast<std::intmax_t>(first % width) - static_cast<std::intmax_t>(second % width);
+        auto delta_y =
+          static_cast<std::intmax_t>(first / width) - static_cast<std::intmax_t>(second / width);
+        return std::min(
+          static_cast<double>(delta_x * delta_x + delta_y * delta_y) * squared_resolution,
+          squared_max_distance);
+      };
+
+    auto neighbors = [&map](std::size_t index) {
+        auto result = std::vector<std::size_t>{};
+        if (index / map.info.width < (map.info.height - 1)) {
+          result.push_back(index + map.info.width);
+        }
+        if (index / map.info.width > 0) {
+          result.push_back(index - map.info.width);
+        }
+        if (index % map.info.width < (map.info.width - 1)) {
+          result.push_back(index + 1);
+        }
+        if (index % map.info.width > 0) {
+          result.push_back(index - 1);
+        }
+        return result;
+      };
+
+    auto to_likelihood =
+      [amplitude = params.z_hit / (params.sigma_hit * std::sqrt(2 * M_PI)),
+        two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit,
+        offset = params.z_random / params.max_laser_distance](double squared_distance) {
+        return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset;
+      };
+
+    auto distance_map = nearest_obstacle_distance_map(obstacle_map, squared_distance, neighbors);
+    return distance_map | ranges::views::transform(to_likelihood) | ranges::to<std::vector>;
+  }
+
+  static auto xy_from_index(
+    const nav_msgs::msg::MapMetaData & info,
+    std::size_t index)
+  {
+    return std::make_pair(
+      (static_cast<double>(index % info.width) + 0.5) * info.resolution + info.origin.position.x,
+      (static_cast<double>(index / info.width) + 0.5) * info.resolution + info.origin.position.y);
   }
 };