feat(hesai): implement mask-based pruning filter for Hesai

.cspell.json

@@ -25,6 +25,7 @@
     "Idat",
     "ipaddr",
     "manc",
+    "mdeg",
     "memcpy",
     "mkdoxy",
     "Msop",

docs/filters.md

@@ -0,0 +1,97 @@
+# Point Filters
+
+Point filters run for every decoded point, reducing pointcloud size right at the decode stage.
+This can speed up the later parts of pointcloud processing pipelines by reducing the number of points copied between and processed by modules.
+
+## Configuration
+
+Filters are configured via the ROS parameter namespace `point_filters`.
+
+```yaml
+{
+  point_filters:
+    filter_type_a:
+      parameter_1: 5
+      parameter_2: "abc"
+    filter_type_b:
+      # ...
+}
+```
+
+Where each `filter_type` can be specified at most once.
+The configuration options available depend on the respective filter type.
+
+Filters can also be set during runtime, e.g. via:
+
+```shell
+# replace <vendor> with the name of a supported vendor
+ros2 param set /<vendor>_ros_wrapper_node point_filters.filter_type_a ...'
+```
+
+## Supported Filters
+
+The following filter types are supported:
+
+| Filter Name            | Filter Type       | Hesai | Robosense | Velodyne |
+| ---------------------- | ----------------- | :---: | :-------: | :------: |
+| Downsample Mask Filter | `downsample_mask` |  ✅   |    ❌     |    ❌    |
+
+Compatibility:  
+✅: compatible  
+❌: incompatible
+
+Below, each filter type is documented in detail.
+
+### Downsample Mask Filter
+
+This filter takes a greyscale PNG image that represents polar coordinates (x=azimuth, y=elevation)
+and downsamples the pointcloud according to the lightness values of the image's pixels.
+
+<!-- prettier-ignore-start -->
+!!! note
+    For ring-based sensors, `y` represents the `channel` as a proxy for `elevation`.
+    The image height has to be equal to the sensor's number of channels.
+<!-- prettier-ignore-end -->
+
+The input image is dithered to a boolean mask:
+
+| Stage                                      |                         Image                         |
+| :----------------------------------------- | :---------------------------------------------------: |
+| Input greyscale mask                       |     ![Greyscale mask](filters/at128_test_roi.png)     |
+| Internal dithered mask generated by Nebula | ![Dithered mask](filters/at128_test_roi_dithered.png) |
+
+The decoded points are then kept/discarded based on that mask:
+
+| ![Pointcloud density](filters/at128_test_roi_cloud.png)                                                                                               | ![Pointcloud closeup](filters/at128_test_roi_cloud_closeup.png)                                       |
+| ----------------------------------------------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------- |
+| Pointcloud output. The original pointcloud had uniform density (all white). 2D azimuth-elevation view, points are blurred to better visualize density | Close-up view of a region with multiple different downsampling levels (bottom left of the pointcloud) |
+
+#### Configuration Options
+
+Configuration is done in the following format:
+
+```yaml
+downsample_mask:
+  path: /path/to/mask.png
+```
+
+Or, during runtime, by setting:
+
+```shell
+ros2 param set /<vendor>_ros_wrapper_node point_filters.downsample_mask.path /path/to/mask.png'
+```
+
+The filter can be disabled by omitting the `downsample_mask` config item, or by setting `path` to an empty string:
+
+```shell
+ros2 param set /<vendor>_ros_wrapper_node point_filters.downsample_mask.path ""'
+```
+
+#### Behavior
+
+- Greyscale values are quantized to the nearest 10th (yielding 11 quantization levels in total)
+- Mask resolution is dictated by the sensor's maximum FoV, the number of channels (for rotational LiDARs) and the peak angular resolution:
+  - For a 40-channel LiDAR with `360 deg` FoV and `0.1 deg` peak azimuth resolution, the mask has to be `(360 / 0.1, 40) = (3600, 40)` pixels
+  - Currently, non-rotational LiDARs are not yet supported
+- Image editors like GIMP use perceptual color profiles, which can lead to unexpected results (more/less downsampling than expected). Check the generated `_dithered.png` mask to see if you are affected.
+- Dithering performed by Nebula is spatial only, meaning that it stays constant over time. Decoded points are checked against the nearest pixel in the dithered mask

docs/index.md

@@ -25,6 +25,7 @@ Nebula works with ROS 2 and is the recommended sensor driver for the [Autoware](
 - [Design](design.md)
 - [Parameters](parameters.md)
 - [Point cloud types](point_types.md)
+- [Point filters](filters.md)
 
 ## Supported sensors
 

docs/parameters.md

@@ -94,3 +94,7 @@ This parameter is used to set this preference.
 ### `dual_return_distance_threshold`
 
 For multiple returns that are close together, the points will be fused into one if they are below this threshold (in meters).
+
+### `point_filters`
+
+Filters that are applied while decoding the pointcloud. For the full reference, see [Point filters](filters.md).

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -50,6 +50,7 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase
   PtpTransportType ptp_transport_type;
   PtpSwitchType ptp_switch_type;
   uint8_t ptp_lock_threshold;
+  std::string downsample_mask_path;
 };
 /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator)
 /// @param os
@@ -73,7 +74,8 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
   os << "PTP Domain: " << std::to_string(arg.ptp_domain) << '\n';
   os << "PTP Transport Type: " << arg.ptp_transport_type << '\n';
   os << "PTP Switch Type: " << arg.ptp_switch_type << '\n';
-  os << "PTP Lock Threshold: " << std::to_string(arg.ptp_lock_threshold);
+  os << "PTP Lock Threshold: " << std::to_string(arg.ptp_lock_threshold) << '\n';
+  os << "Downsample Mask Path: " << arg.downsample_mask_path;
   return os;
 }
 

nebula_decoders/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(robosense_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(velodyne_msgs REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(PNG REQUIRED)
 
 include_directories(PUBLIC
     include
@@ -52,10 +53,12 @@ add_library(nebula_decoders_hesai SHARED
 )
 target_link_libraries(nebula_decoders_hesai PUBLIC
     ${pandar_msgs_TARGETS}
+    ${PNG_LIBRARIES}
 )
 
 target_include_directories(nebula_decoders_hesai PUBLIC
     ${pandar_msgs_INCLUDE_DIRS}
+    ${PNG_INCLUDE_DIRS}
 )
 
 # Velodyne
@@ -125,6 +128,17 @@ install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
 if(BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
     ament_lint_auto_find_test_dependencies()
+
+    find_package(ament_cmake_gtest REQUIRED)
+
+    add_definitions(-D_TEST_RESOURCES_PATH="${PROJECT_SOURCE_DIR}/test_resources/")
+
+    ament_add_gtest(test_downsample_mask tests/point_filters/test_downsample_mask.cpp)
+    target_link_libraries(test_downsample_mask
+        ${PNG_LIBRARIES})
+    target_include_directories(test_downsample_mask PUBLIC
+        include
+        ${PNG_INCLUDE_DIRS})
 endif()
 
 ament_export_include_directories("include/${PROJECT_NAME}")

nebula_decoders/include/nebula_decoders/nebula_decoders_common/angles.hpp

@@ -18,6 +18,29 @@
 namespace nebula::drivers
 {
 
+template <typename T>
+struct AnglePair
+{
+  T azimuth;
+  T elevation;
+};
+
+template <typename T>
+struct AngleRange
+{
+  T min;
+  T max;
+
+  [[nodiscard]] T extent() const { return max - min; }
+};
+
+template <typename T>
+struct FieldOfView
+{
+  AngleRange<T> azimuth;
+  AngleRange<T> elevation;
+};
+
 /**
  * @brief Tests if `angle` is in the region of the circle defined by `start_angle` and `end_angle`.
  * Notably, `end_angle` can be smaller than `start_angle`, in which case the region passes over the

nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/downsample_mask.hpp

@@ -0,0 +1,143 @@
+// Copyright 2024 TIER IV, 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 "nebula_decoders/nebula_decoders_common/angles.hpp"
+
+#include <nebula_common/loggers/logger.hpp>
+#include <nebula_common/nebula_common.hpp>
+#include <nebula_common/point_types.hpp>
+#include <nebula_common/util/string_conversions.hpp>
+#include <png++/error.hpp>
+#include <png++/gray_pixel.hpp>
+#include <png++/image.hpp>
+
+#include <Eigen/src/Core/Matrix.h>
+#include <sys/types.h>
+
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
+#include <filesystem>
+#include <memory>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <string_view>
+
+namespace nebula::drivers::point_filters
+{
+
+namespace impl
+{
+
+inline void dither(
+  const png::image<png::gray_pixel> & in, png::image<png::gray_pixel> & out,
+  uint8_t quantization_levels)
+{
+  if (in.get_width() != out.get_width() || in.get_height() != out.get_height()) {
+    std::stringstream ss;
+    ss << "Expected downsample mask of size "
+       << "(" << out.get_width() << ", " << out.get_height() << ")";
+    ss << ", got "
+       << "(" << in.get_width() << ", " << in.get_height() << ")";
+
+    throw std::runtime_error(ss.str());
+  }
+
+  uint32_t denominator = quantization_levels;
+
+  auto should_keep = [denominator](uint32_t numerator, uint32_t pos) {
+    for (uint32_t i = 0; i < numerator; ++i) {
+      auto dithered_pos =
+        static_cast<size_t>(std::round(denominator / static_cast<double>(numerator) * i));
+      if (dithered_pos == pos) return true;
+    }
+    return false;
+  };
+
+  for (size_t y = 0; y < out.get_height(); ++y) {
+    for (size_t x = 0; x < out.get_width(); ++x) {
+      const auto & pixel = in.get_pixel(x, y);
+      uint32_t numerator = static_cast<uint32_t>(pixel) * denominator / 255;
+      size_t pos = (x + y) % denominator;
+      bool keep = should_keep(numerator, pos);
+      out.set_pixel(x, y, keep * 255);
+    }
+  }
+}
+
+}  // namespace impl
+
+class DownsampleMaskFilter
+{
+  static const uint8_t g_quantization_levels = 10;
+
+public:
+  DownsampleMaskFilter(
+    const std::string & filename, AngleRange<int32_t> azimuth_range_mdeg,
+    uint32_t azimuth_peak_resolution_mdeg, size_t n_channels,
+    const std::shared_ptr<loggers::Logger> & logger, bool export_dithered_mask = false)
+  : azimuth_range_{deg2rad(azimuth_range_mdeg.min / 1000.), deg2rad(azimuth_range_mdeg.max / 1000.)}
+  {
+    png::image<png::gray_pixel> factors(filename);
+
+    size_t mask_cols = azimuth_range_mdeg.extent() / azimuth_peak_resolution_mdeg;
+    size_t mask_rows = n_channels;
+
+    png::image<png::gray_pixel> dithered(mask_cols, mask_rows);
+    impl::dither(factors, dithered, g_quantization_levels);
+
+    mask_ = Eigen::MatrixX<uint8_t>(mask_rows, mask_cols);
+
+    for (size_t y = 0; y < dithered.get_height(); ++y) {
+      for (size_t x = 0; x < dithered.get_width(); ++x) {
+        mask_.coeffRef(static_cast<int32_t>(y), static_cast<int32_t>(x)) = dithered.get_pixel(x, y);
+      }
+    }
+
+    if (export_dithered_mask) {
+      std::filesystem::path out_path{filename};
+      out_path = out_path.replace_filename(
+        out_path.stem().string() + "_dithered" + out_path.extension().string());
+
+      try {
+        dithered.write(out_path);
+        logger->info("Wrote dithered mask to " + out_path.native());
+      } catch (const png::std_error & e) {
+        logger->warn("Could not write " + out_path.native() + ": " + e.what());
+      }
+    }
+  }
+
+  bool excluded(const NebulaPoint & point)
+  {
+    double azi_normalized = (point.azimuth - azimuth_range_.min) / azimuth_range_.extent();
+
+    auto x = static_cast<ssize_t>(std::round(azi_normalized * static_cast<double>(mask_.cols())));
+    auto y = point.channel;
+
+    bool x_out_of_bounds = x < 0 || x >= mask_.cols();
+    bool y_out_of_bounds = y >= mask_.rows();
+
+    return x_out_of_bounds || y_out_of_bounds || !mask_.coeff(y, x);
+  }
+
+private:
+  AngleRange<double> azimuth_range_;
+  Eigen::MatrixX<uint8_t> mask_;
+};
+
+}  // namespace nebula::drivers::point_filters