feat(azimuth_bin_index): implement azimuth bin indexing for improved tracker association

- Introduced a new AzimuthBinIndex class to manage tracker indices across azimuth bins, enhancing the efficiency of the association process.
- Updated PolarAssociation to utilize the AzimuthBinIndex for registering and querying tracker indices, streamlining measurement processing.
- Removed legacy azimuth bin helper functions to simplify the codebase and improve maintainability.

Author: technolojin

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/azimuth_bin_index.hpp

@@ -0,0 +1,117 @@
+// Copyright 2026 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.
+
+#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__AZIMUTH_BIN_INDEX_HPP_
+#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__AZIMUTH_BIN_INDEX_HPP_
+
+#include "autoware/multi_object_tracker/association/scoring/polar_assignment_scoring.hpp"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <vector>
+
+namespace autoware::multi_object_tracker
+{
+
+/// Per-bin tracker record: index + range of the near face.
+struct AzimuthBinEntry
+{
+  size_t tracker_idx;
+  float r_min;  // near-face range [m]; float precision is sufficient (< 1 cm error at 300 m)
+};
+
+/// 1-D azimuth bin index with soft radial pre-filtering.
+
+class AzimuthBinIndex
+{
+public:
+  static constexpr int kNumBins = 24;                           // 15° per bin, full 360°
+  static constexpr double kBinWidth = 2.0 * M_PI / kNumBins;  // [rad]
+
+  void clear()
+  {
+    for (auto & bin : bins_) bin.clear();
+  }
+
+  /// Register @p tracker_idx to every bin its @p interval covers.
+  /// The +1 guard ensures boundary trackers are always findable by find().
+  void add(
+    const polar_scoring::AzimuthInterval & interval, size_t tracker_idx, double r_min)
+  {
+    const int n = std::min(
+      static_cast<int>(std::ceil(2.0 * interval.half_span / kBinWidth)) + 1, kNumBins);
+    const int start = angleToBin(interval.center - interval.half_span);
+    const AzimuthBinEntry entry{tracker_idx, static_cast<float>(r_min)};
+    for (int i = 0; i < n; ++i) {
+      bins_[(start + i) % kNumBins].push_back(entry);
+    }
+  }
+
+  /// Return unique tracker indices whose bins overlap @p interval and whose
+  /// stored r_min passes the soft radial pre-filter for @p r_min_query.
+  std::vector<size_t> find(
+    const polar_scoring::AzimuthInterval & interval, double r_min_query) const
+  {
+    // Exact bin count — no +1, relying on add()'s guard for boundary coverage.
+    const int n =
+      std::max(1, static_cast<int>(std::ceil(2.0 * interval.half_span / kBinWidth)));
+    const int start = angleToBin(interval.center - interval.half_span);
+
+    std::vector<size_t> candidates;
+    for (int i = 0; i < n; ++i) {
+      for (const auto & entry : bins_[(start + i) % kNumBins]) {
+        if (radialPrePass(r_min_query, static_cast<double>(entry.r_min))) {
+          candidates.push_back(entry.tracker_idx);
+        }
+      }
+    }
+    std::sort(candidates.begin(), candidates.end());
+    candidates.erase(std::unique(candidates.begin(), candidates.end()), candidates.end());
+    return candidates;
+  }
+
+private:
+  // Sigma formula mirrors radialCompatibility() in polar_assignment_scoring.cpp.
+  static constexpr double kSigmaBase = 2.0;   // [m]
+  static constexpr double kSigmaRate = 0.03;  // [m/m]
+  // Gate factor: -ln(0.05) ≈ 3.0.  Skip when exp(-gap/sigma) < 0.05,
+  // i.e., gap > 3σ.  Always above the 2 m hard gate, so no true match is lost.
+  static constexpr double kGateFactor = 3.0;
+
+  std::array<std::vector<AzimuthBinEntry>, kNumBins> bins_;
+
+  /// Returns true when the radial pre-filter passes (candidate should be kept).
+  /// Equivalent to exp(-r_gap/sigma) >= 0.05, computed without std::exp.
+  static bool radialPrePass(double r_query, double r_cand)
+  {
+    const double r_gap = std::abs(r_query - r_cand);
+    const double sigma = kSigmaBase + kSigmaRate * std::max(r_query, r_cand);
+    return r_gap <= kGateFactor * sigma;
+  }
+
+  /// Map any angle [rad] to a bin index in [0, kNumBins).
+  /// Shifts [-π, π) → [0, 2π) by adding π before bucketing.
+  static int angleToBin(double angle)
+  {
+    double a = std::fmod(angle + M_PI, 2.0 * M_PI);
+    if (a < 0.0) a += 2.0 * M_PI;
+    const int bin = static_cast<int>(a / kBinWidth);
+    return std::min(bin, kNumBins - 1);
+  }
+};
+
+}  // namespace autoware::multi_object_tracker
+
+#endif  // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__AZIMUTH_BIN_INDEX_HPP_

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/polar_association.hpp

@@ -16,6 +16,7 @@
 #define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__POLAR_ASSOCIATION_HPP_
 
 #include "autoware/multi_object_tracker/association/association_base.hpp"
+#include "autoware/multi_object_tracker/association/azimuth_bin_index.hpp"
 #include "autoware/multi_object_tracker/association/scoring/polar_assignment_scoring.hpp"
 #include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp"
 #include "autoware/multi_object_tracker/configurations.hpp"
@@ -26,7 +27,6 @@
 
 #include <geometry_msgs/msg/pose.hpp>
 
-#include <array>
 #include <list>
 #include <memory>
 #include <optional>
@@ -45,8 +45,6 @@ namespace autoware::multi_object_tracker
 class PolarAssociation : public AssociationBase
 {
 public:
-  // Number of azimuth bins (15° each, full circle)
-  static constexpr int kNumAzimuthBins = 24;
 
   explicit PolarAssociation(const AssociatorConfig & config);
   ~PolarAssociation() override = default;
@@ -77,8 +75,7 @@ class PolarAssociation : public AssociationBase
   std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper_;
   std::optional<geometry_msgs::msg::Pose> ego_pose_;
 
-  // Azimuth bin index: kNumAzimuthBins bins × 15° each, each bin holds tracker indices
-  std::array<std::vector<size_t>, kNumAzimuthBins> azimuth_bins_;
+  AzimuthBinIndex azimuth_bin_index_;
 
   PolarPreparationData prepareAssociationData(
     const types::DynamicObjectList & measurements,

perception/autoware_multi_object_tracker/lib/association/polar_association.cpp

@@ -23,7 +23,6 @@
 
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
-#include <algorithm>
 #include <cmath>
 #include <iterator>
 #include <list>
@@ -43,56 +42,6 @@ constexpr double INVALID_SCORE = 0.0;
 // surface of an object, not from its interior or far side.
 constexpr double NEAR_FACE_GAP_THRESHOLD = 2.0;
 
-// Azimuth bin helpers
-// 24 bins × 15° (π/12 rad) each, covering the full [0, 2π) circle.
-constexpr double kAzimuthBinWidth =
-  2.0 * M_PI / static_cast<double>(PolarAssociation::kNumAzimuthBins);
-
-/// Map any angle to a bin index in [0, kNumAzimuthBins).
-int azimuthToBin(double angle)
-{
-  // Shift from [-π, π) to [0, 2π) then divide by bin width.
-  double a = std::fmod(angle + M_PI, 2.0 * M_PI);
-  if (a < 0.0) a += 2.0 * M_PI;
-  const int bin = static_cast<int>(a / kAzimuthBinWidth);
-  return std::min(bin, PolarAssociation::kNumAzimuthBins - 1);
-}
-
-/// Register tracker_idx to every bin that the azimuth interval covers.
-void registerToAzimuthBins(
-  std::array<std::vector<size_t>, PolarAssociation::kNumAzimuthBins> & bins,
-  const polar_scoring::AzimuthInterval & azimuth, const size_t tracker_idx)
-{
-  // Number of bins the interval spans (always at least 1, capped at all bins).
-  const int n = std::min(
-    static_cast<int>(std::ceil(2.0 * azimuth.half_span / kAzimuthBinWidth)) + 1,
-    PolarAssociation::kNumAzimuthBins);
-  const int start_bin = azimuthToBin(azimuth.center - azimuth.half_span);
-  for (int i = 0; i < n; ++i) {
-    bins[(start_bin + i) % PolarAssociation::kNumAzimuthBins].push_back(tracker_idx);
-  }
-}
-
-/// Collect unique tracker indices from all bins covered by the azimuth interval.
-std::vector<size_t> queryAzimuthBins(
-  const std::array<std::vector<size_t>, PolarAssociation::kNumAzimuthBins> & bins,
-  const polar_scoring::AzimuthInterval & azimuth)
-{
-  const int n = std::min(
-    static_cast<int>(std::ceil(2.0 * azimuth.half_span / kAzimuthBinWidth)) + 1,
-    PolarAssociation::kNumAzimuthBins);
-  const int start_bin = azimuthToBin(azimuth.center - azimuth.half_span);
-
-  std::vector<size_t> candidates;
-  for (int i = 0; i < n; ++i) {
-    const auto & bin_vec = bins[(start_bin + i) % PolarAssociation::kNumAzimuthBins];
-    candidates.insert(candidates.end(), bin_vec.begin(), bin_vec.end());
-  }
-  std::sort(candidates.begin(), candidates.end());
-  candidates.erase(std::unique(candidates.begin(), candidates.end()), candidates.end());
-  return candidates;
-}
-
 }  // namespace
 
 using autoware_utils_debug::ScopedTimeTrack;
@@ -146,7 +95,7 @@ PolarAssociation::PolarPreparationData PolarAssociation::prepareAssociationData(
   // Extract tracked objects and build azimuth bin index
   {
     size_t tracker_idx = 0;
-    for (auto & bin : azimuth_bins_) bin.clear();
+    azimuth_bin_index_.clear();
 
     for (const auto & tracker : trackers) {
       types::DynamicObject tracked_object;
@@ -155,8 +104,8 @@ PolarAssociation::PolarPreparationData PolarAssociation::prepareAssociationData(
       // Compute polar footprint and register to all azimuth bins it covers
       prep_data.tracker_footprints.emplace_back(
         polar_scoring::computePolarFootprint(tracked_object, ego_x, ego_y, ego_yaw));
-      registerToAzimuthBins(
-        azimuth_bins_, prep_data.tracker_footprints.back().azimuth, tracker_idx);
+      const auto & fp = prep_data.tracker_footprints.back();
+      azimuth_bin_index_.add(fp.azimuth, tracker_idx, fp.r_min);
 
       prep_data.tracked_objects.emplace_back(std::move(tracked_object));
       prep_data.tracker_labels.emplace_back(tracker->getHighestProbLabel());
@@ -184,7 +133,7 @@ void PolarAssociation::processMeasurement(
   // Compute measurement polar footprint and query azimuth bins for candidate trackers
   const auto meas_fp =
     polar_scoring::computePolarFootprint(measurement_object, ego_x, ego_y, ego_yaw);
-  const auto candidate_indices = queryAzimuthBins(azimuth_bins_, meas_fp.azimuth);
+  const auto candidate_indices = azimuth_bin_index_.find(meas_fp.azimuth, meas_fp.r_min);
 
   for (const size_t tracker_idx : candidate_indices) {
     const auto tracker_type = prep_data.tracker_types[tracker_idx];

perception/autoware_multi_object_tracker/lib/association/scoring/polar_assignment_scoring.cpp

@@ -53,14 +53,18 @@ PolarFootprint computePolarFootprint(
     return {{azimuth, 0.0}, range, range, z_min, z_max};
   }
 
-  // Compute centroid-based azimuth as the reference center
+  // Centroid direction used as the reference from which corner offsets are measured.
+  // For partial observations (I-shape, L-shape) the centroid may not be the angular
+  // midpoint of the visible polygon, so we recompute the true angular midpoint below.
   const double cx = object.pose.position.x - ego_x;
   const double cy = object.pose.position.y - ego_y;
   const double center_azimuth = normalizeAngle(std::atan2(cy, cx) - ego_yaw);
 
   double r_min = std::numeric_limits<double>::max();
   double r_max = 0.0;
-  double max_offset = 0.0;
+  // Signed angular offsets from center_azimuth tracked independently.
+  double az_lo = std::numeric_limits<double>::max();
+  double az_hi = std::numeric_limits<double>::lowest();
 
   for (const auto & pt : points) {
     const double dx = pt.x() - ego_x;
@@ -69,17 +73,27 @@ PolarFootprint computePolarFootprint(
     r_min = std::min(r_min, range);
     r_max = std::max(r_max, range);
 
-    // Skip corners too close to ego for stable azimuth computation
-    if (range < MIN_RANGE) continue;
+    // Skip only truly degenerate corners (corner at ego position).
+    if (range < 1e-6) continue;
 
     const double azimuth = normalizeAngle(std::atan2(dy, dx) - ego_yaw);
-    const double offset = std::abs(normalizeAngle(azimuth - center_azimuth));
-    max_offset = std::max(max_offset, offset);
+    const double offset = normalizeAngle(azimuth - center_azimuth);  // signed
+    az_lo = std::min(az_lo, offset);
+    az_hi = std::max(az_hi, offset);
   }
 
   r_min = std::max(r_min, MIN_RANGE);
 
-  return {{center_azimuth, max_offset}, r_min, r_max, z_min, z_max};
+  if (az_lo > az_hi) {
+    // All corners were at the ego position — degenerate polygon.
+    return {{center_azimuth, 0.0}, r_min, r_max, z_min, z_max};
+  }
+
+  // Angular midpoint of the visible span, corrected from the centroid's azimuth.
+  const double corrected_center = normalizeAngle(center_azimuth + (az_lo + az_hi) / 2.0);
+  const double half_span = (az_hi - az_lo) / 2.0;
+
+  return {{corrected_center, half_span}, r_min, r_max, z_min, z_max};
 }
 
 double azimuthIoU(const AzimuthInterval & a, const AzimuthInterval & b)