Skip to content

Commit 335d917

Browse files
committed
refactor(vehicle_tracker): enhance bounding box orientation logic in conditionedUpdate
- Updated the logic in the `conditionedUpdate` method to ensure that the bounding box orientation is correctly handled for cluster measurements. - Improved the condition for aligning the cluster to the tracker's orientation by checking for polygon shape type and non-empty footprint points. These changes aim to enhance the accuracy of the vehicle tracking system's update strategy. Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent fb75459 commit 335d917

1 file changed

Lines changed: 4 additions & 3 deletions

File tree

perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -367,11 +367,12 @@ bool VehicleTracker::conditionedUpdate(
367367
const autoware_perception_msgs::msg::Shape & tracker_shape, const rclcpp::Time & measurement_time,
368368
const types::InputChannel & channel_info)
369369
{
370-
// For cluster measurements (trust_extension=false), the bounding box orientation is in baselink
371-
// frame. Re-project the polygon footprint onto the tracker's current heading so that
370+
// For cluster measurements, the bounding box orientation is in baselink frame.
371+
// Re-project the polygon footprint onto the tracker's current heading so that
372372
// determineUpdateStrategy receives correctly-oriented edge centers.
373373
const types::DynamicObject & meas_for_strategy =
374-
(!channel_info.trust_extension && !measurement.shape.footprint.points.empty())
374+
((measurement.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) &&
375+
!measurement.shape.footprint.points.empty())
375376
? alignClusterToTrackerOrientation(measurement, motion_model_.getYawState())
376377
: measurement;
377378

0 commit comments

Comments
 (0)