diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/bev_association.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/bev_association.hpp index ab4800cf2fcb8..7beb5d1b7a8c9 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/bev_association.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/bev_association.hpp @@ -50,13 +50,19 @@ typedef bg::model::point Point; typedef bg::model::box Box; typedef std::pair 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 tracked_objects; - std::vector tracker_labels; - std::vector tracker_types; - std::vector tracker_inverse_covariances; + std::vector trackers; }; class BevAssociation : public AssociationBase diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/scoring/bev_assignment_scoring.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/scoring/bev_assignment_scoring.hpp index 7de2c9ff7389d..637ed501faf73 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/scoring/bev_assignment_scoring.hpp +++ b/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 diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/scoring/scoring_types.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/scoring/scoring_types.hpp new file mode 100644 index 0000000000000..29f0b603f117e --- /dev/null +++ b/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_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 34259bee98d3b..377492a1dd681 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/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 diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 7ae772f4c1fea..e9df67879d534 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/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, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index d5b3e016625ce..1ba39b0cedd0b 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -71,7 +71,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 +106,10 @@ 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 + types::DynamicObject alignClusterToTrackerOrientation( + const types::DynamicObject & cluster, double tracker_yaw) const; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/association/bev_association.cpp b/perception/autoware_multi_object_tracker/lib/association/bev_association.cpp index a07cd9e430d31..5cde0f50456cf 100644 --- a/perception/autoware_multi_object_tracker/lib/association/bev_association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/bev_association.cpp @@ -178,36 +178,27 @@ PreparationData BevAssociation::prepareAssociationData( const std::list> & 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 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 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 @@ void BevAssociation::processMeasurement( Point measurement_point(measurement_object.pose.position.x, measurement_object.pose.position.y); std::vector 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 @@ void BevAssociation::processMeasurement( 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}); } } } diff --git a/perception/autoware_multi_object_tracker/lib/association/scoring/bev_assignment_scoring.cpp b/perception/autoware_multi_object_tracker/lib/association/scoring/bev_assignment_scoring.cpp index 20dd352ef64c6..2339dd5264e50 100644 --- a/perception/autoware_multi_object_tracker/lib/association/scoring/bev_assignment_scoring.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/scoring/bev_assignment_scoring.cpp @@ -31,23 +31,24 @@ inline double getMahalanobisDistanceFast(double dx, double dy, const InverseCova } } // 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; @@ -56,7 +57,7 @@ double calculateBevAssignmentScore( 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; @@ -65,13 +66,13 @@ double calculateBevAssignmentScore( // 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; @@ -88,19 +89,19 @@ double calculateBevAssignmentScore( } 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 diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 1f89337f83c7b..7b6677c85fa1c 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -197,10 +197,8 @@ bool convertConvexHullToBoundingBox( // 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) { @@ -208,10 +206,8 @@ bool convertConvexHullToBoundingBox( // 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 @@ -233,7 +229,10 @@ bool convertConvexHullToBoundingBox( 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) { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index 0939f874b3129..eefdaf474731a 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -200,45 +200,39 @@ bool Tracker::updateWithMeasurement( } setOrientationAvailability(object_.kinematics.orientation_availability); - // Update strategies: - // 1. Normal update: Update position and shape by Kalman filter - // 2. Extension update: Apply the new stable shape and update position - // 3. Conditioned update: Ignore the noisy shape info and partially update position with below 3 - // conditions - // - FRONT_WHEEL_UPDATE: Update anchor point of front wheel - // - REAR_WHEEL_UPDATE: Update anchor point of rear wheel - // - WEAK_UPDATE: Update tending to predicted position - - if (!has_significant_shape_change) { + // Select update path: NORMAL / TRY_EXTENSION / CONDITIONED + const UpdatePath path = + selectUpdatePath(channel_info.trust_extension, has_significant_shape_change); + + if (path == UpdatePath::NORMAL) { unstable_shape_filter_.processNormalMeasurement(object); - // 1. Normal update measure(object, measurement_time, channel_info); object_.trust_extension = object.trust_extension; - } else { + } else if (path == UpdatePath::TRY_EXTENSION) { unstable_shape_filter_.processNoisyMeasurement(object); if (unstable_shape_filter_.isStable()) { - // 2. Extension update - autoware_perception_msgs::msg::Shape smoothed_shape = unstable_shape_filter_.getShape(); - + // Extension update: apply stabilized shape and update with smoothed measurement + const auto smoothed_shape = unstable_shape_filter_.getShape(); setObjectShape(smoothed_shape); - auto smoothed_object = object; smoothed_object.shape = smoothed_shape; measure(smoothed_object, measurement_time, channel_info); object_.trust_extension = smoothed_object.trust_extension; - unstable_shape_filter_.clear(); - } else { - // 3. Conditioned update + // Filter not yet stable — fall back to conditioned update const auto tracker_shape = object_.shape; - types::DynamicObject predicted_object; getTrackedObject(measurement_time, predicted_object); - conditionedUpdate(object, predicted_object, tracker_shape, measurement_time, channel_info); } + + } else { // UpdatePath::CONDITIONED + const auto tracker_shape = object_.shape; + types::DynamicObject predicted_object; + getTrackedObject(measurement_time, predicted_object); + conditionedUpdate(object, predicted_object, tracker_shape, measurement_time, channel_info); } // Update object status diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index aa5a3c1bd80b8..2549f56146bbd 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -31,6 +31,7 @@ #include #include +#include namespace autoware::multi_object_tracker { @@ -136,14 +137,21 @@ bool VehicleTracker::predict(const rclcpp::Time & time) bool VehicleTracker::measureWithPose( const types::DynamicObject & object, const types::InputChannel & channel_info) { - // get measurement yaw angle to update - bool is_yaw_available = + // Shape update is only valid when the channel guarantees reliable size information + // AND the measurement is a bounding box. + // Use the measurement length only when its shape is trustworthy. + const bool is_bbox = (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX); + const bool can_update_shape = channel_info.trust_extension && is_bbox; + constexpr double min_length = 1.0; + const double length = can_update_shape ? std::max(object.shape.dimensions.x, min_length) + : std::max(motion_model_.getLength(), min_length); + + const bool is_yaw_available = object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE && channel_info.trust_orientation; + const bool is_velocity_available = object.kinematics.has_twist; - bool is_velocity_available = object.kinematics.has_twist; - - // update + // Update kinematics via EKF bool is_updated = false; { const double & x = object.pose.position.x; @@ -151,24 +159,17 @@ bool VehicleTracker::measureWithPose( const double & yaw = tf2::getYaw(object.pose.orientation); const double & vel_x = object.twist.linear.x; const double & vel_y = object.twist.linear.y; - constexpr double min_length = 1.0; // minimum length to avoid division by zero - const double length = std::max(object.shape.dimensions.x, min_length); if (is_yaw_available && is_velocity_available) { - // update with yaw angle and velocity is_updated = motion_model_.updateStatePoseHeadVel( x, y, yaw, object.pose_covariance, vel_x, vel_y, object.twist_covariance, length); } else if (is_yaw_available && !is_velocity_available) { - // update with yaw angle, but without velocity is_updated = motion_model_.updateStatePoseHead(x, y, yaw, object.pose_covariance, length); } else if (!is_yaw_available && is_velocity_available) { - // update without yaw angle, but with velocity is_updated = motion_model_.updateStatePoseVel( x, y, object.pose_covariance, yaw, vel_x, vel_y, object.twist_covariance, length); } else { - // update without yaw angle and velocity - is_updated = motion_model_.updateStatePose( - x, y, object.pose_covariance, length); // update without yaw angle and velocity + is_updated = motion_model_.updateStatePose(x, y, object.pose_covariance, length); } motion_model_.limitStates(); } @@ -180,8 +181,7 @@ bool VehicleTracker::measureWithPose( (1.0 - gain) * object_.pose.position.z + gain * object.pose.position.z; } - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - // do not update shape if the input is not a bounding box + if (!can_update_shape) { return false; } @@ -206,10 +206,7 @@ bool VehicleTracker::measureWithPose( object_extension.z = gain_inv * object_extension.z + gain * object.shape.dimensions.z; } - // set maximum and minimum size limitObjectExtension(object_model_); - - // set shape type, which is bounding box object_.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; object_.area = types::getArea(object.shape); @@ -310,13 +307,70 @@ bool VehicleTracker::getTrackedObject( return true; } +types::DynamicObject VehicleTracker::alignClusterToTrackerOrientation( + const types::DynamicObject & cluster, const double tracker_yaw) const +{ + const auto & pts = cluster.shape.footprint.points; + if (pts.empty()) return cluster; + + // footprint.points are in the cluster's local frame (baselink orientation). + // Transform each point to a map-relative offset, then project onto the tracker's axes. + const double cluster_yaw = tf2::getYaw(cluster.pose.orientation); + const double c_cl = std::cos(cluster_yaw); + const double s_cl = std::sin(cluster_yaw); + const double c_tr = std::cos(tracker_yaw); + const double s_tr = std::sin(tracker_yaw); + + double long_min = std::numeric_limits::max(); + double long_max = std::numeric_limits::lowest(); + double lat_min = std::numeric_limits::max(); + double lat_max = std::numeric_limits::lowest(); + + for (const auto & pt : pts) { + // cluster local -> map-relative offset + const double mx = pt.x * c_cl - pt.y * s_cl; + const double my = pt.x * s_cl + pt.y * c_cl; + // project onto tracker heading (longitudinal) and lateral axes + const double along = mx * c_tr + my * s_tr; + const double lat = -mx * s_tr + my * c_tr; + long_min = std::min(long_min, along); + long_max = std::max(long_max, along); + lat_min = std::min(lat_min, lat); + lat_max = std::max(lat_max, lat); + } + + const double long_center = (long_min + long_max) * 0.5; + const double lat_center = (lat_min + lat_max) * 0.5; + + types::DynamicObject aligned = cluster; + aligned.pose.position.x = cluster.pose.position.x + long_center * c_tr - lat_center * s_tr; + aligned.pose.position.y = cluster.pose.position.y + long_center * s_tr + lat_center * c_tr; + + tf2::Quaternion q; + q.setRPY(0.0, 0.0, tracker_yaw); + aligned.pose.orientation = tf2::toMsg(q); + + aligned.shape.dimensions.x = long_max - long_min; + aligned.shape.dimensions.y = lat_max - lat_min; + + return aligned; +} + bool VehicleTracker::conditionedUpdate( const types::DynamicObject & measurement, const types::DynamicObject & prediction, const autoware_perception_msgs::msg::Shape & tracker_shape, const rclcpp::Time & measurement_time, const types::InputChannel & channel_info) { + // For cluster measurements, the bounding box orientation is in baselink frame. + // Re-project the polygon footprint onto the tracker's current heading so that + // determineUpdateStrategy receives correctly-oriented edge centers. + const types::DynamicObject & meas_for_strategy = + !measurement.shape.footprint.points.empty() + ? alignClusterToTrackerOrientation(measurement, motion_model_.getYawState()) + : measurement; + // Determine update strategy - UpdateStrategy strategy = determineUpdateStrategy(measurement, prediction); + UpdateStrategy strategy = determineUpdateStrategy(meas_for_strategy, prediction); // Handle weak update strategy (no edge alignment - use weak update with pseudo measurement) if (strategy.type == UpdateStrategyType::WEAK_UPDATE) { @@ -366,10 +420,15 @@ UpdateStrategy VehicleTracker::determineUpdateStrategy( // 2. Calculate alignment distances between measurement and prediction edges const EdgeAlignment alignment = findAlignedEdges(meas_edges, prediction); - // 3. Check if any edge is well-aligned (within threshold ratio of vehicle length) + // 3. Check if any edge is well-aligned. + // Use the larger of predicted and measured length so size-mismatch cases are handled + // symmetrically. const double predicted_length = prediction.shape.dimensions.x; - const bool is_edge_aligned = - (alignment.min_alignment_distance / predicted_length) < ALIGNMENT_RATIO_THRESHOLD; + const double measured_length = measurement.shape.dimensions.x; + const double max_length = std::max(predicted_length, measured_length); + const double alignment_threshold = + std::max(ALIGNMENT_RATIO_THRESHOLD * max_length, ALIGNMENT_ABSOLUTE_THRESHOLD); + const bool is_edge_aligned = alignment.min_alignment_distance < alignment_threshold; // 4. If no edge is aligned, use weak update strategy if (!is_edge_aligned) {