fix(autoware_multi_object_tracker): tracker update path, shape gating, and association data structure

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

@@ -50,13 +50,19 @@ typedef bg::model::point<double, 2, bg::cs::cartesian> Point;
 typedef bg::model::box<Point> Box;
 typedef std::pair<Point, size_t> ValueType;  // (position, tracker index)
 
+// Per-tracker entry bundling all precomputed data for one tracker
+struct TrackerBevEntry
+{
+  types::DynamicObject object;
+  classes::Label label;
+  types::TrackerType type;
+  InverseCovariance2D inv_cov;
+};
+
 // Per-tracker precomputed data for a single association round
 struct PreparationData
 {
-  std::vector<types::DynamicObject> tracked_objects;
-  std::vector<classes::Label> tracker_labels;
-  std::vector<types::TrackerType> tracker_types;
-  std::vector<InverseCovariance2D> tracker_inverse_covariances;
+  std::vector<TrackerBevEntry> trackers;
 };
 
 class BevAssociation : public AssociationBase

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/scoring/bev_assignment_scoring.hpp

@@ -15,6 +15,7 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SCORING__BEV_ASSIGNMENT_SCORING_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SCORING__BEV_ASSIGNMENT_SCORING_HPP_
 
+#include "autoware/multi_object_tracker/association/scoring/scoring_types.hpp"
 #include "autoware/multi_object_tracker/configurations.hpp"
 #include "autoware/multi_object_tracker/types.hpp"
 
@@ -35,15 +36,13 @@ constexpr double CHECK_GIOU_THRESHOLD = 0.7;
 constexpr double AREA_RATIO_THRESHOLD = 1.3;
 
 /// Computes a [0, 1] assignment score between a tracker and a measurement for D2T GNN assignment.
-/// Returns 0.0 (INVALID_SCORE) when the pair fails any gate and should be rejected.
-/// Sets has_significant_shape_change=true when matched vehicle objects differ noticeably in size.
-double calculateBevAssignmentScore(
-  const types::DynamicObject & tracked_object, const classes::Label tracker_label,
-  const types::TrackerType tracker_type,
+/// Returns score=0.0 when the pair fails any gate and should be rejected.
+ScoringResult calculateBevAssignmentScore(
+  const types::DynamicObject & tracked_object, classes::Label tracker_label,
+  types::TrackerType tracker_type,
   const AssociatorConfig::TrackerAssociationParameters & association_params,
-  const types::DynamicObject & measurement_object, const classes::Label measurement_label,
-  const InverseCovariance2D & inv_cov, double unknown_association_giou_threshold,
-  bool & has_significant_shape_change);
+  const types::DynamicObject & measurement_object, classes::Label measurement_label,
+  const InverseCovariance2D & inv_cov, double unknown_association_giou_threshold);
 
 }  // namespace autoware::multi_object_tracker
 

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/scoring/scoring_types.hpp

@@ -0,0 +1,30 @@
+// 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__SCORING__SCORING_TYPES_HPP_
+#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SCORING__SCORING_TYPES_HPP_
+
+namespace autoware::multi_object_tracker
+{
+
+/// Result of a pairwise assignment score computation.
+struct ScoringResult
+{
+  double score;
+  bool has_significant_shape_change;
+};
+
+}  // namespace autoware::multi_object_tracker
+
+#endif  // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SCORING__SCORING_TYPES_HPP_

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp

@@ -53,6 +53,14 @@ class MultipleVehicleTracker : public Tracker
   void setOrientationAvailability(
     const types::OrientationAvailability & orientation_availability) override;
   virtual ~MultipleVehicleTracker() {}
+
+  // Same policy as VehicleTracker: bicycle model owns shape; clusters use conditioned update.
+  UpdatePath selectUpdatePath(
+    bool trust_extension, bool has_significant_shape_change) const override
+  {
+    if (!trust_extension) return UpdatePath::CONDITIONED;
+    return has_significant_shape_change ? UpdatePath::TRY_EXTENSION : UpdatePath::NORMAL;
+  }
 };
 
 }  // namespace autoware::multi_object_tracker

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp

@@ -42,6 +42,8 @@
 namespace autoware::multi_object_tracker
 {
 
+enum class UpdatePath { NORMAL, TRY_EXTENSION, CONDITIONED };
+
 class Tracker
 {
 private:
@@ -193,6 +195,17 @@ class Tracker
     const autoware_perception_msgs::msg::Shape & tracker_shape,
     const rclcpp::Time & measurement_time, const types::InputChannel & channel_info);
 
+  // Selects the update path for a given measurement.
+  // NORMAL      — standard Kalman update (with optional shape-filter history accumulation)
+  // TRY_EXTENSION — attempt extension update via shape filter; fall back to CONDITIONED if unstable
+  // CONDITIONED — edge-aligned / weak conditioned update; shape management is bypassed entirely
+  virtual UpdatePath selectUpdatePath(bool trust_extension, bool has_significant_shape_change) const
+  {
+    (void)trust_extension;
+    (void)has_significant_shape_change;
+    return UpdatePath::NORMAL;
+  }
+
 public:
   virtual bool getTrackedObject(
     const rclcpp::Time & time, types::DynamicObject & object,

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp

@@ -20,6 +20,8 @@
 #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
 #include "autoware/multi_object_tracker/types.hpp"
 
+#include <optional>
+
 namespace autoware::multi_object_tracker
 {
 
@@ -71,7 +73,16 @@ class VehicleTracker : public Tracker
 
   void setObjectShape(const autoware_perception_msgs::msg::Shape & shape) override;
 
-  const double ALIGNMENT_RATIO_THRESHOLD = 0.09;  // 9% of length as alignment tolerance
+  // Clusters (trust_extension=false) have unreliable bbox orientation — always use conditioned.
+  UpdatePath selectUpdatePath(
+    bool trust_extension, bool has_significant_shape_change) const override
+  {
+    if (!trust_extension) return UpdatePath::CONDITIONED;
+    return has_significant_shape_change ? UpdatePath::TRY_EXTENSION : UpdatePath::NORMAL;
+  }
+
+  const double ALIGNMENT_RATIO_THRESHOLD = 0.2;     // 20% of the larger object's length
+  const double ALIGNMENT_ABSOLUTE_THRESHOLD = 1.0;  // [m] minimum tolerance for small objects
   UpdateStrategy determineUpdateStrategy(
     const types::DynamicObject & measurement, const types::DynamicObject & prediction) const;
 
@@ -97,6 +108,11 @@ class VehicleTracker : public Tracker
     const EdgePositions & meas_edges, const types::DynamicObject & prediction) const;
   geometry_msgs::msg::Point calculateAnchorPoint(
     const EdgeAlignment & alignment, const types::DynamicObject & measurement) const;
+
+  // Re-project a cluster's polygon footprint onto the tracker's current heading.
+  // Returns std::nullopt when the cluster has no footprint points.
+  std::optional<types::DynamicObject> alignClusterToTrackerOrientation(
+    const types::DynamicObject & cluster, double tracker_yaw) const;
 };
 
 }  // namespace autoware::multi_object_tracker

perception/autoware_multi_object_tracker/lib/association/bev_association.cpp

@@ -1,4 +1,4 @@
 // 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.
@@ -178,36 +178,27 @@
   const std::list<std::shared_ptr<Tracker>> & trackers)
 {
   PreparationData prep_data;
+  prep_data.trackers.reserve(trackers.size());
 
-  prep_data.tracked_objects.reserve(trackers.size());
-  prep_data.tracker_labels.reserve(trackers.size());
-  prep_data.tracker_types.reserve(trackers.size());
-
-  {
-    size_t tracker_idx = 0;
-    std::vector<ValueType> rtree_points;
-    rtree_.clear();
-    rtree_points.reserve(trackers.size());
-    for (const auto & tracker : trackers) {
-      types::DynamicObject tracked_object;
-      tracker->getTrackedObject(measurements.header.stamp, tracked_object);
-
-      Point p(tracked_object.pose.position.x, tracked_object.pose.position.y);
-      rtree_points.emplace_back(p, tracker_idx);
-
-      prep_data.tracked_objects.emplace_back(std::move(tracked_object));
-      prep_data.tracker_labels.emplace_back(tracker->getHighestProbLabel());
-      prep_data.tracker_types.emplace_back(tracker->getTrackerType());
-      ++tracker_idx;
-    }
-    rtree_.insert(rtree_points.begin(), rtree_points.end());
-  }
+  std::vector<ValueType> rtree_points;
+  rtree_.clear();
+  rtree_points.reserve(trackers.size());
+
+  size_t tracker_idx = 0;
+  for (const auto & tracker : trackers) {
+    TrackerBevEntry entry;
+    tracker->getTrackedObject(measurements.header.stamp, entry.object);
+    entry.label = tracker->getHighestProbLabel();
+    entry.type = tracker->getTrackerType();
+    entry.inv_cov = precomputeInverseCovarianceFromPose(entry.object.pose_covariance);
+
+    Point p(entry.object.pose.position.x, entry.object.pose.position.y);
+    rtree_points.emplace_back(p, tracker_idx);
 
-  prep_data.tracker_inverse_covariances.reserve(prep_data.tracked_objects.size());
-  for (const auto & tracked_object : prep_data.tracked_objects) {
-    prep_data.tracker_inverse_covariances.emplace_back(
-      precomputeInverseCovarianceFromPose(tracked_object.pose_covariance));
+    prep_data.trackers.emplace_back(std::move(entry));
+    ++tracker_idx;
   }
+  rtree_.insert(rtree_points.begin(), rtree_points.end());
 
   return prep_data;
 }
@@ -231,7 +222,7 @@
   Point measurement_point(measurement_object.pose.position.x, measurement_object.pose.position.y);
 
   std::vector<ValueType> nearby_trackers;
-  nearby_trackers.reserve(std::min(size_t{100}, prep_data.tracked_objects.size()));
+  nearby_trackers.reserve(std::min(size_t{100}, prep_data.trackers.size()));
 
   const double max_dist = std::sqrt(max_squared_dist);
   const Box query_box(
@@ -241,23 +232,21 @@
 
   for (const auto & tracker_value : nearby_trackers) {
     const size_t tracker_idx = tracker_value.second;
-    const auto tracker_type = prep_data.tracker_types[tracker_idx];
+    const auto & tracker_entry = prep_data.trackers[tracker_idx];
 
-    const auto association_params_opt = get_map_value_if_exists(tracker_params_map, tracker_type);
+    const auto association_params_opt =
+      get_map_value_if_exists(tracker_params_map, tracker_entry.type);
     if (!association_params_opt) continue;
 
-    const auto & tracked_object = prep_data.tracked_objects[tracker_idx];
-    const auto tracker_label = prep_data.tracker_labels[tracker_idx];
-
-    bool has_significant_shape_change = false;
-    const double score = calculateBevAssignmentScore(
-      tracked_object, tracker_label, tracker_type, association_params_opt->get(),
-      measurement_object, measurement_label, prep_data.tracker_inverse_covariances[tracker_idx],
-      config_.unknown_association_giou_threshold, has_significant_shape_change);
+    const auto result = calculateBevAssignmentScore(
+      tracker_entry.object, tracker_entry.label, tracker_entry.type, association_params_opt->get(),
+      measurement_object, measurement_label, tracker_entry.inv_cov,
+      config_.unknown_association_giou_threshold);
 
-    if (score > INVALID_SCORE) {
+    if (result.score > INVALID_SCORE) {
       association_data.entries.emplace_back(
-        types::AssociationEntry{tracker_idx, measurement_idx, score, has_significant_shape_change});
+        types::AssociationEntry{
+          tracker_idx, measurement_idx, result.score, result.has_significant_shape_change});
     }
   }
 }

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

@@ -31,76 +31,77 @@
 }
 }  // namespace
 
-double calculateBevAssignmentScore(
+ScoringResult calculateBevAssignmentScore(
   const types::DynamicObject & tracked_object, const classes::Label tracker_label,
   const types::TrackerType tracker_type,
   const AssociatorConfig::TrackerAssociationParameters & association_params,
   const types::DynamicObject & measurement_object, const classes::Label measurement_label,
-  const InverseCovariance2D & inv_cov, const double unknown_association_giou_threshold,
-  bool & has_significant_shape_change)
+  const InverseCovariance2D & inv_cov, const double unknown_association_giou_threshold)
 {
+  ScoringResult result{INVALID_SCORE, false};
+
   // When both tracker and measurement are unknown, use generalized IoU only
   if (tracker_label == classes::Label::UNKNOWN && measurement_label == classes::Label::UNKNOWN) {
     const double generalized_iou = shapes::get2dGeneralizedIoU(tracked_object, measurement_object);
     if (generalized_iou < unknown_association_giou_threshold) {
-      return INVALID_SCORE;
+      return result;
     }
-    // Rescale score to [0, 1]
-    return (generalized_iou - unknown_association_giou_threshold) /
-           (1.0 - unknown_association_giou_threshold);
+    result.score = (generalized_iou - unknown_association_giou_threshold) /
+                   (1.0 - unknown_association_giou_threshold);
+    return result;
   }
 
   const double max_dist_sq = association_params.max_dist_sq;
   const double dx = measurement_object.pose.position.x - tracked_object.pose.position.x;
   const double dy = measurement_object.pose.position.y - tracked_object.pose.position.y;
   const double dist_sq = dx * dx + dy * dy;
 
   // Distance gate
-  if (dist_sq > max_dist_sq) return INVALID_SCORE;
+  if (dist_sq > max_dist_sq) return result;
 
   // Gates for non-vehicle objects
   const double area_meas = measurement_object.area;
   const bool is_vehicle_tracker = isVehicleTrackerType(tracker_type);
   if (!is_vehicle_tracker) {
     // Area gate
     const double max_area = association_params.max_area;
     const double min_area = association_params.min_area;
-    if (area_meas < min_area || area_meas > max_area) return INVALID_SCORE;
+    if (area_meas < min_area || area_meas > max_area) return result;
 
     // Mahalanobis distance gate
     const double mahalanobis_dist = getMahalanobisDistanceFast(dx, dy, inv_cov);
     // Empirical value: 99.6% confidence level for chi-square with 2 DOF
     constexpr double mahalanobis_dist_threshold = 11.62;
-    if (mahalanobis_dist >= mahalanobis_dist_threshold) return INVALID_SCORE;
+    if (mahalanobis_dist >= mahalanobis_dist_threshold) return result;
   }
 
   const double min_iou = association_params.min_iou;
 
   // Use 1D IoU for pedestrian, 3D GIoU if both extension values are trustable, else 2D GIoU
   const bool use_1d_iou = (tracker_label == classes::Label::PEDESTRIAN);
   const bool use_3d_iou = tracked_object.trust_extension && measurement_object.trust_extension;
 
   double iou_score;
   if (use_1d_iou) {
     iou_score = shapes::get1dIoU(measurement_object, tracked_object);
   } else if (use_3d_iou) {
     iou_score = shapes::get3dGeneralizedIoU(measurement_object, tracked_object);
   } else {
     iou_score = shapes::get2dGeneralizedIoU(measurement_object, tracked_object);
   }
-  if (iou_score < min_iou) return INVALID_SCORE;
+  if (iou_score < min_iou) return result;
 
   // Check if shape changes too much for vehicle labels
   if (iou_score < CHECK_GIOU_THRESHOLD && is_vehicle_tracker) {
     const double area_trk = tracked_object.area;
     const double area_ratio = std::max(area_trk, area_meas) / std::min(area_trk, area_meas);
     if (area_ratio > AREA_RATIO_THRESHOLD) {
-      has_significant_shape_change = true;
+      result.has_significant_shape_change = true;
     }
   }
 
-  // Rescale score to [0, 1]
-  return (iou_score - min_iou) / (1.0 - min_iou);
+  result.score = (iou_score - min_iou) / (1.0 - min_iou);
+  return result;
 }
 
 }  // namespace autoware::multi_object_tracker

perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp

@@ -196,44 +196,43 @@
 
   // Pre-allocate boundary values using first point
   float max_x = points[0].x;
   float max_y = points[0].y;
-  float max_z = points[0].z;
   float min_x = points[0].x;
   float min_y = points[0].y;
-  float min_z = points[0].z;
 
   // Start from second point since we used first point for initialization
   for (size_t i = 1; i < points.size(); ++i) {
     const auto & point = points[i];
     // Use direct comparison instead of std::max/min
     if (point.x > max_x) max_x = point.x;
     if (point.y > max_y) max_y = point.y;
-    if (point.z > max_z) max_z = point.z;
     if (point.x < min_x) min_x = point.x;
     if (point.y < min_y) min_y = point.y;
-    if (point.z < min_z) min_z = point.z;
   }
 
   // calc new center in local coordinates - avoid division by 2.0 twice
   const double center_x = (max_x + min_x) * 0.5;
   const double center_y = (max_y + min_y) * 0.5;
 
   // transform to global for the object's position
   const double yaw = tf2::getYaw(input_object.pose.orientation);
   const double cos_yaw = cos(yaw);
   const double sin_yaw = sin(yaw);
   const double dx = center_x * cos_yaw - center_y * sin_yaw;
   const double dy = center_x * sin_yaw + center_y * cos_yaw;
 
   // set output parameters - avoid unnecessary copying
   output_object = input_object;
   output_object.pose.position.x += dx;
   output_object.pose.position.y += dy;
 
   output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
   output_object.shape.dimensions.x = max_x - min_x;
   output_object.shape.dimensions.y = max_y - min_y;
-  output_object.shape.dimensions.z = max_z - min_z;
+
+  //// pose.position.z and shape.dimensions.z (height)
+  // Footprint points are 2D (z=0), so deriving height from
+  // them would always give zero. Preserve the input value unchanged.
 
   // adjust footprint points in local coordinates - use references to avoid copies
   for (auto & point : output_object.shape.footprint.points) {