Decouple ROS message types from the observation model (#27)

This patch decouples ROS message types from the likelihood field sensor model, allowing it to be moved to the main package. It also replaces the custom state class with SE(2) group elements using the sophus library. The only functional change with respect to the previous implementation is that we now support different orientations for the map origin.

beluga/CMakeLists.txt

@@ -14,14 +14,18 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(Eigen3 REQUIRED NO_MODULE)
 find_package(range-v3 REQUIRED)
+find_package(Sophus 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_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17)
+target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG SOPHUS_USE_BASIC_LOGGING)
 
 add_executable(clang_tidy_findable)
 target_sources(clang_tidy_findable PRIVATE src/clang_tidy_findable.cpp)
@@ -58,7 +62,10 @@ install(
 ament_export_include_directories("include/${PROJECT_NAME}")
 
 ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_dependencies(range-v3)
+ament_export_dependencies(
+  Eigen3
+  range-v3
+  Sophus)
 
 ament_package()
 

beluga/include/beluga/beluga.h

@@ -15,6 +15,8 @@
 #pragma once
 
 #include <beluga/algorithm.h>
+#include <beluga/motion.h>
+#include <beluga/sensor.h>
 #include <beluga/spatial_hash.h>
 #include <beluga/tuple_vector.h>
 #include <beluga/type_traits.h>

beluga/include/beluga/motion.h

@@ -0,0 +1,17 @@
+// 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 <beluga/motion/stationary_model.h>

beluga/include/beluga/motion/stationary_model.h

@@ -0,0 +1,39 @@
+// 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 <random>
+
+#include <sophus/se2.hpp>
+#include <sophus/so2.hpp>
+
+namespace beluga {
+
+template <class Mixin>
+class StationaryModel : public Mixin {
+ public:
+  template <class... Args>
+  explicit StationaryModel(Args&&... args) : Mixin(std::forward<Args>(args)...) {}
+
+  [[nodiscard]] Sophus::SE2d apply_motion(const Sophus::SE2d& state) const {
+    static thread_local std::mt19937 generator{std::random_device()()};
+    auto distribution = std::normal_distribution<>{0, 0.02};
+    return state * Sophus::SE2d{
+                       Sophus::SO2d{distribution(generator)},
+                       Eigen::Vector2d{distribution(generator), distribution(generator)}};
+  }
+};
+
+}  // namespace beluga

beluga/include/beluga/sensor.h

@@ -0,0 +1,17 @@
+// 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 <beluga/sensor/likelihood_field_model.h>

beluga/include/beluga/sensor/likelihood_field_model.h

@@ -0,0 +1,125 @@
+// 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 <beluga/algorithm/distance_map.h>
+
+#include <algorithm>
+#include <cmath>
+#include <random>
+#include <shared_mutex>
+#include <vector>
+
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/all.hpp>
+#include <range/v3/view/transform.hpp>
+#include <sophus/se2.hpp>
+#include <sophus/so2.hpp>
+
+namespace beluga {
+
+struct LikelihoodFieldModelParam {
+  double max_obstacle_distance;
+  double min_laser_distance;
+  double max_laser_distance;
+  double z_hit;
+  double z_random;
+  double sigma_hit;
+};
+
+template <class Mixin, class OccupancyGrid>
+class LikelihoodFieldModel : public Mixin {
+ public:
+  template <class... Args>
+  explicit LikelihoodFieldModel(const LikelihoodFieldModelParam& params, const OccupancyGrid& grid, Args&&... rest)
+      : Mixin(std::forward<Args>(rest)...),
+        grid_{grid},
+        free_cells_{make_free_cells_vector(grid)},
+        likelihood_field_{make_likelihood_field(params, grid)} {}
+
+  const auto& likelihood_field() const { return likelihood_field_; }
+
+  template <class Generator>
+  [[nodiscard]] Sophus::SE2d generate_random_state(Generator& generator) const {
+    auto index_distribution = std::uniform_int_distribution<std::size_t>{0, free_cells_.size() - 1};
+    return Sophus::SE2d{
+        Sophus::SO2d::sampleUniform(generator),
+        grid_.origin() * grid_.point(free_cells_[index_distribution(generator)])};
+  }
+
+  [[nodiscard]] double importance_weight(const Sophus::SE2d& state) const {
+    const auto transform = grid_.origin().inverse() * state;
+    const auto lock = std::shared_lock<std::shared_mutex>{points_mutex_};
+    return std::transform_reduce(
+        points_.cbegin(), points_.cend(), 0.0, std::plus{},
+        [this, x_offset = transform.translation().x(), y_offset = transform.translation().y(),
+         cos_theta = transform.so2().unit_complex().x(),
+         sin_theta = transform.so2().unit_complex().y()](const auto& point) {
+          // Transform the end point of the laser to the global coordinate system.
+          // Not using Eigen/Sophus because they make the routine x10 slower.
+          // See `benchmark_likelihood_field_model.cpp` for reference.
+          const auto x = point.first * cos_theta - point.second * sin_theta + x_offset;
+          const auto y = point.first * sin_theta + point.second * cos_theta + y_offset;
+          const auto index = grid_.index(x, y);
+          return index < likelihood_field_.size() ? likelihood_field_[index] : 0.0;
+        });
+  }
+
+  void update_sensor(std::vector<std::pair<double, double>> points) {
+    const auto lock = std::lock_guard<std::shared_mutex>{points_mutex_};
+    points_ = std::move(points);
+  }
+
+ private:
+  OccupancyGrid grid_;
+  std::vector<std::size_t> free_cells_;
+  std::vector<double> likelihood_field_;
+  std::vector<std::pair<double, double>> points_;
+  mutable std::shared_mutex points_mutex_;
+
+  static std::vector<std::size_t> make_free_cells_vector(const OccupancyGrid& grid) {
+    auto free_cells = std::vector<std::size_t>{};
+    free_cells.reserve(grid.size());
+    for (std::size_t index = 0; index < grid.size(); ++index) {
+      if (OccupancyGrid::Traits::is_free(grid.data()[index])) {
+        free_cells.push_back(index);
+      }
+    }
+    return free_cells;
+  }
+
+  static std::vector<double> make_likelihood_field(const LikelihoodFieldModelParam& params, const OccupancyGrid& grid) {
+    const auto obstacle_map = grid.data() | ranges::views::transform(&OccupancyGrid::Traits::is_occupied);
+
+    auto squared_distance = [&grid, squared_max_distance = params.max_obstacle_distance * params.max_obstacle_distance](
+                                std::size_t first, std::size_t second) {
+      return std::min((grid.point(first) - grid.point(second)).squaredNorm(), squared_max_distance);
+    };
+
+    const auto distance_map = nearest_obstacle_distance_map(
+        obstacle_map, squared_distance, std::bind(&OccupancyGrid::neighbors, &grid, std::placeholders::_1));
+
+    auto to_likelihood = [amplitude =
+                              params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants<double>::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;
+    };
+
+    return distance_map | ranges::views::transform(to_likelihood) | ranges::to<std::vector>;
+  }
+};
+
+}  // namespace beluga

beluga/include/beluga/spatial_hash.h

@@ -22,6 +22,8 @@
 #include <type_traits>
 #include <utility>
 
+#include <sophus/se2.hpp>
+
 namespace beluga {
 
 namespace detail {
@@ -64,4 +66,13 @@ struct spatial_hash<Tuple<Types...>, std::enable_if_t<(std::is_arithmetic_v<Type
   }
 };
 
+template <>
+struct spatial_hash<Sophus::SE2d, void> {
+ public:
+  std::size_t operator()(const Sophus::SE2d& state, double resolution = 1.) const {
+    const auto& position = state.translation();
+    return spatial_hash<std::tuple<double, double>>{}(std::make_tuple(position.x(), position.y()), resolution);
+  }
+};
+
 }  // namespace beluga

beluga/package.xml

@@ -13,7 +13,9 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>eigen</depend>
   <depend>range-v3</depend>
+  <depend>sophus</depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_clang_tidy</test_depend>

beluga/test/beluga/CMakeLists.txt

@@ -5,6 +5,7 @@ ament_add_gmock(test_beluga
   algorithm/test_exponential_filter.cpp
   algorithm/test_particle_filter.cpp
   algorithm/test_sampling.cpp
+  sensor/test_likelihood_field_model.cpp
   test_tuple_vector.cpp
   test_spatial_hash.cpp)
 target_link_libraries(test_beluga ${PROJECT_NAME})