WIP

Signed-off-by: Max SCHMELLER <[email protected]>
tier4 · Feb 14, 2025 · cb77c96 · cb77c96
1 parent e18ae38
commit cb77c96
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 18 deletions.
diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/angles.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/angles.hpp
@@ -15,30 +15,57 @@
 #pragma once
 
 #include <cmath>
+#include <cstdint>
+
 namespace nebula::drivers
 {
 
-template <typename T>
+class Radians
+{
+  static constexpr double circle_modulus = 2 * M_PI;
+};
+
+template <uint32_t Subdivisions>
+class ScaledDegrees
+{
+  static constexpr double circle_modulus = 360 * Subdivisions;
+};
+
+using Degrees = ScaledDegrees<1>;
+using DeciDegrees = ScaledDegrees<10>;
+using CentiDegrees = ScaledDegrees<100>;
+using MilliDegrees = ScaledDegrees<1000>;
+
+template <typename T, typename AngleUnit>
 struct AnglePair
 {
   T azimuth;
   T elevation;
 };
 
-template <typename T>
+/// @brief A range defined by a start and end angle. Crossing the 0/circle_modulus boundary (end <
+/// start) is allowed.
+/// @tparam T The type of the angle.
+/// @tparam AngleUnit The unit of the angle.
+template <typename T, typename AngleUnit>
 struct AngleRange
 {
-  T min;
-  T max;
+  T start;
+  T end;
 
-  [[nodiscard]] T extent() const { return max - min; }
+  /// @brief The extent of the range, taking into account the 0/circle_modulus boundary. Will always
+  /// be positive.
+  [[nodiscard]] T extent() const
+  {
+    return (end < start) ? AngleUnit::circle_modulus - start + end : end - start;
+  }
 };
 
-template <typename T>
+template <typename T, typename AngleUnit>
 struct FieldOfView
 {
-  AngleRange<T> azimuth;
-  AngleRange<T> elevation;
+  AngleRange<T, AngleUnit> azimuth;
+  AngleRange<T, AngleUnit> elevation;
 };
 
 /**
@@ -63,11 +90,11 @@ bool angle_is_between(
  * `max_angle` is 360 for degrees, 2 * M_PI for radians, and the corresponding scaled value for
  * scaled units such as centi-degrees (36000).
  */
-template <typename T>
-T normalize_angle(T angle, T max_angle)
+template <typename T, typename AngleUnit>
+T normalize_angle(T angle)
 {
-  T factor = std::floor((1.0 * angle) / max_angle);
-  return angle - (factor * max_angle);
+  T factor = std::floor((1.0 * angle) / AngleUnit::circle_modulus);
+  return angle - (factor * AngleUnit::circle_modulus);
 }
 
 }  // namespace nebula::drivers
diff --git a/...decoders/include/nebula_decoders/nebula_decoders_common/point_filters/downsample_mask.hpp b/...decoders/include/nebula_decoders/nebula_decoders_common/point_filters/downsample_mask.hpp
@@ -35,7 +35,6 @@
 #include <sstream>
 #include <stdexcept>
 #include <string>
-#include <string_view>
 
 namespace nebula::drivers::point_filters
 {
@@ -87,10 +86,11 @@ class DownsampleMaskFilter
 
 public:
   DownsampleMaskFilter(
-    const std::string & filename, AngleRange<int32_t> azimuth_range_mdeg,
+    const std::string & filename, AngleRange<int32_t, MilliDegrees> 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.)}
+  : azimuth_range_{
+      deg2rad(azimuth_range_mdeg.start / 1000.), deg2rad(azimuth_range_mdeg.end / 1000.)}
   {
     if (azimuth_peak_resolution_mdeg == 0) {
       throw std::invalid_argument("azimuth_peak_resolution_mdeg must be positive");
@@ -134,7 +134,7 @@ class DownsampleMaskFilter
 
   bool excluded(const NebulaPoint & point)
   {
-    double azi_normalized = (point.azimuth - azimuth_range_.min) / azimuth_range_.extent();
+    double azi_normalized = (point.azimuth - azimuth_range_.start) / azimuth_range_.extent();
 
     auto x = static_cast<ssize_t>(std::round(azi_normalized * static_cast<double>(mask_.cols())));
     auto y = point.channel;
@@ -146,7 +146,7 @@ class DownsampleMaskFilter
   }
 
 private:
-  AngleRange<double> azimuth_range_;
+  AngleRange<double, Radians> azimuth_range_;
   Eigen::MatrixX<uint8_t> mask_;
 };
 

diff --git a/nebula_decoders/tests/point_filters/test_downsample_mask.cpp b/nebula_decoders/tests/point_filters/test_downsample_mask.cpp
@@ -85,7 +85,7 @@ TEST(TestDownsampleMask, TestFilter)
 
   auto logger = std::make_shared<ConsoleLogger>("TestFilter");
   auto image_path = downsample_mask_path() / ("q50.png");
-  AngleRange<int32_t> azi_range_mdeg{-5, 5};
+  AngleRange<int32_t, 360'000> azi_range_mdeg{-5, 5};
   uint16_t azi_step_mdeg = 1;
   uint8_t n_channels = 10;