WIP

Signed-off-by: Max SCHMELLER <[email protected]>

nebula_decoders/CMakeLists.txt

@@ -25,6 +25,8 @@ find_package(robosense_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(velodyne_msgs REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(PNG REQUIRED)
+find_package(png++ REQUIRED)
 
 include_directories(PUBLIC
     include
@@ -52,10 +54,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

nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/polar_mask.hpp

@@ -0,0 +1,63 @@
+// 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_common/point_types.hpp>
+#include <png++/gray_pixel.hpp>
+#include <png++/image.hpp>
+
+#include <algorithm>
+#include <cstddef>
+#include <string_view>
+
+namespace nebula::drivers::point_filters
+{
+
+class PolarMaskFilter
+{
+public:
+  PolarMaskFilter(
+    const std::string_view & filename, double azimuth_min, double azimuth_max, double elevation_min,
+    double elevation_max)
+  : azimuth_min_(azimuth_min),
+    azimuth_max_(azimuth_max),
+    elevation_min_(elevation_min),
+    elevation_max_(elevation_max),
+    mask_{filename.data()}
+  {
+  }
+
+  bool excluded(const NebulaPoint & point)
+  {
+    double azi_normalized = (point.azimuth - azimuth_min_) / (azimuth_max_ - azimuth_min_);
+    double ele_normalized = (point.elevation - elevation_min_) / (elevation_max_ - elevation_min_);
+
+    size_t x = std::clamp<ssize_t>(
+      static_cast<ssize_t>(azi_normalized * mask_.get_width()), 0, mask_.get_width() - 1);
+    size_t y = std::clamp<ssize_t>(
+      static_cast<ssize_t>(ele_normalized * mask_.get_height()), 0, mask_.get_height() - 1);
+
+    return !static_cast<bool>(mask_.get_pixel(x, y));
+  }
+
+private:
+  double azimuth_min_;
+  double azimuth_max_;
+  double elevation_min_;
+  double elevation_max_;
+  png::image<png::gray_pixel> mask_;
+};
+
+}  // namespace nebula::drivers::point_filters

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp

@@ -15,6 +15,7 @@
 #pragma once
 
 #include "nebula_decoders/nebula_decoders_common/angles.hpp"
+#include "nebula_decoders/nebula_decoders_common/point_filters/polar_mask.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp"
@@ -28,7 +29,9 @@
 #include <algorithm>
 #include <array>
 #include <cmath>
+#include <cstdint>
 #include <memory>
+#include <optional>
 #include <tuple>
 #include <utility>
 #include <vector>
@@ -83,6 +86,8 @@ class HesaiDecoder : public HesaiScanDecoder
   std::array<std::array<int, SensorT::packet_t::n_blocks>, SensorT::packet_t::max_returns>
     block_firing_offset_ns_;
 
+  point_filters::PolarMaskFilter mask_filter_;
+
   /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc.
   /// @param packet The incoming PandarPacket
   /// @return Whether the packet was parsed successfully
@@ -128,18 +133,18 @@ class HesaiDecoder : public HesaiScanDecoder
       for (size_t block_offset = 0; block_offset < n_blocks; ++block_offset) {
         auto & unit = *return_units[block_offset];
 
-        if (unit.distance == 0) {
-          continue;
-        }
+        // if (unit.distance == 0) {
+        //   continue;
+        // }
 
-        float distance = get_distance(unit);
+        float distance = 10;  // get_distance(unit);
 
-        if (
-          distance < SensorT::min_range || SensorT::max_range < distance ||
-          distance < sensor_configuration_->min_range ||
-          sensor_configuration_->max_range < distance) {
-          continue;
-        }
+        // if (
+        //   distance < SensorT::min_range || SensorT::max_range < distance ||
+        //   distance < sensor_configuration_->min_range ||
+        //   sensor_configuration_->max_range < distance) {
+        //   continue;
+        // }
 
         auto return_type = sensor_.get_return_type(
           static_cast<hesai_packet::return_mode::ReturnMode>(packet_.tail.return_mode),
@@ -195,7 +200,7 @@ class HesaiDecoder : public HesaiScanDecoder
         uint64_t scan_timestamp_ns =
           in_current_scan ? decode_scan_timestamp_ns_ : output_scan_timestamp_ns_;
 
-        NebulaPoint & point = pc->emplace_back();
+        NebulaPoint point;
         point.distance = distance;
         point.intensity = unit.reflectivity;
         point.time_stamp = get_point_time_relative(
@@ -207,13 +212,17 @@ class HesaiDecoder : public HesaiScanDecoder
         // The raw_azimuth and channel are only used as indices, sin/cos functions use the precise
         // corrected angles
         float xy_distance = distance * corrected_angle_data.cos_elevation;
-        point.x = xy_distance * corrected_angle_data.sin_azimuth;
-        point.y = xy_distance * corrected_angle_data.cos_azimuth;
-        point.z = distance * corrected_angle_data.sin_elevation;
+        point.x = corrected_angle_data.azimuth_rad;
+        point.y = point.channel / 128.0 * 6.28;
+        point.z = 0;
 
         // The driver wrapper converts to degrees, expects radians
         point.azimuth = corrected_angle_data.azimuth_rad;
         point.elevation = corrected_angle_data.elevation_rad;
+
+        if (!mask_filter_.excluded(point)) {
+          pc->emplace_back(point);
+        }
       }
     }
   }
@@ -251,7 +260,10 @@ class HesaiDecoder : public HesaiScanDecoder
     angle_corrector_(
       correction_data, sensor_configuration_->cloud_min_angle,
       sensor_configuration_->cloud_max_angle, sensor_configuration_->cut_angle),
-    logger_(rclcpp::get_logger("HesaiDecoder"))
+    logger_(rclcpp::get_logger("HesaiDecoder")),
+    mask_filter_(
+      "/home/maxschmeller/nebula/nebula_ros/config/lidar/hesai/roi-grad2_mask.png", 0, 2 * M_PI,
+      deg2rad(-25), deg2rad(15))
   {
     logger_.set_level(rclcpp::Logger::Level::Debug);
 

scripts/make_downsample_mask.py

@@ -0,0 +1,53 @@
+#!/usr/bin/python3
+
+from argparse import ArgumentParser
+from fractions import Fraction
+from pathlib import Path
+
+from PIL import Image
+import numpy as np
+
+
+def create_dithered_mask(keep_ratio: np.ndarray):
+    mask = np.zeros_like(keep_ratio, dtype=bool)
+
+    h, w = mask.shape
+
+    for y in range(h):
+        for x in range(w):
+            factor = keep_ratio[y, x]
+            frac = Fraction(round(factor * 10), 10)
+            positions = [round(1 / frac * i) for i in range(frac.numerator)]
+
+            mask[y, x] = (x + y) % frac.denominator in positions
+
+    return mask
+
+
+def convert_to_mask(in_path: Path, out_path: Path):
+    image = Image.open(in_path).convert("L")
+    keep_ratio = np.asarray(image) / 255.0
+
+    mask = create_dithered_mask(keep_ratio)
+
+    luminance = mask * np.uint8(255)
+    pil_image = Image.fromarray(luminance, mode="L")
+
+    pil_image.save(out_path, format="PNG", optimize=True, compress_level=9)
+
+    return mask
+
+
+if __name__ == "__main__":
+    parser = ArgumentParser()
+    parser.add_argument("input_file", type=Path)
+    parser.add_argument("-o", "--output-file", type=Path)
+    args = parser.parse_args()
+
+    in_path: Path = args.input_file
+    if "output_file" in args and args.output_file is not None:
+        out_path = args.output_file
+    else:
+        out_path = in_path.with_stem(in_path.stem + "_mask").with_suffix(".png")
+
+    mask = convert_to_mask(in_path, out_path)

scripts/requirements.txt

@@ -2,3 +2,4 @@ pandas
 tabulate
 matplotlib
 rich
+pillow