Skip to content

Commit f098d3a

Browse files
committed
fix(tracking_object_merger): merge velocity base on target object coordinate system (autowarefoundation#6457)
fix(tracking_object_merger): merge velocity base on target object coordinate Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent db82695 commit f098d3a

File tree

1 file changed

+45
-23
lines changed
  • perception/tracking_object_merger/src/utils

1 file changed

+45
-23
lines changed

perception/tracking_object_merger/src/utils/utils.cpp

+45-23
Original file line numberDiff line numberDiff line change
@@ -324,43 +324,61 @@ autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMe
324324
// copy main object at first
325325
output_kinematics = main_obj.kinematics;
326326
auto sub_obj_ = sub_obj;
327-
// do not merge reverse direction objects
327+
// do not merge if motion direction is different
328328
if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) {
329329
return output_kinematics;
330-
} else if (objectsYawIsReverted(main_obj, sub_obj)) {
331-
// revert velocity (revert pose is not necessary because it is not used in tracking)
332-
sub_obj_.kinematics.twist_with_covariance.twist.linear.x *= -1.0;
333-
sub_obj_.kinematics.twist_with_covariance.twist.linear.y *= -1.0;
334330
}
335331

336332
// currently only merge vx
337333
if (policy == MergePolicy::SKIP) {
338334
return output_kinematics;
339335
} else if (policy == MergePolicy::OVERWRITE) {
340-
output_kinematics.twist_with_covariance.twist.linear.x =
341-
sub_obj_.kinematics.twist_with_covariance.twist.linear.x;
336+
// use main_obj's orientation
337+
// take sub_obj's velocity vector and convert into main_obj's frame, but take only x component
338+
const auto sub_vx = sub_obj_.kinematics.twist_with_covariance.twist.linear.x;
339+
const auto sub_vy = sub_obj_.kinematics.twist_with_covariance.twist.linear.y;
340+
const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation);
341+
const auto sub_yaw = tf2::getYaw(sub_obj_.kinematics.pose_with_covariance.pose.orientation);
342+
const auto sub_vx_in_main_frame =
343+
sub_vx * std::cos(sub_yaw - main_yaw) + sub_vy * std::sin(sub_yaw - main_yaw);
344+
output_kinematics.twist_with_covariance.twist.linear.x = sub_vx_in_main_frame;
345+
342346
return output_kinematics;
343347
} else if (policy == MergePolicy::FUSION) {
348+
// use main_obj's orientation
349+
// take main_obj's velocity vector and convert into sub_obj's frame, but take only x component
344350
const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x;
345351
const auto sub_vx = sub_obj_.kinematics.twist_with_covariance.twist.linear.x;
352+
const auto sub_vy = sub_obj_.kinematics.twist_with_covariance.twist.linear.y;
353+
const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation);
354+
const auto sub_yaw = tf2::getYaw(sub_obj_.kinematics.pose_with_covariance.pose.orientation);
355+
const auto sub_vel_in_main_frame_x =
356+
sub_vx * std::cos(sub_yaw - main_yaw) + sub_vy * std::sin(sub_yaw - main_yaw);
346357
// inverse weight
347358
const auto main_vx_cov = main_obj.kinematics.twist_with_covariance.covariance[0];
348359
const auto sub_vx_cov = sub_obj_.kinematics.twist_with_covariance.covariance[0];
360+
const auto sub_vy_cov = sub_obj_.kinematics.twist_with_covariance.covariance[7];
361+
const auto sub_vel_cov_in_main_frame_x =
362+
sub_vx_cov * std::cos(sub_yaw - main_yaw) * std::cos(sub_yaw - main_yaw) +
363+
sub_vy_cov * std::sin(sub_yaw - main_yaw) * std::sin(sub_yaw - main_yaw);
349364
double main_vx_weight, sub_vx_weight;
350365
if (main_vx_cov == 0.0) {
351366
main_vx_weight = 1.0 * 1e6;
352367
} else {
353368
main_vx_weight = 1.0 / main_vx_cov;
354369
}
355-
if (sub_vx_cov == 0.0) {
370+
if (sub_vel_cov_in_main_frame_x == 0.0) {
356371
sub_vx_weight = 1.0 * 1e6;
357372
} else {
358-
sub_vx_weight = 1.0 / sub_vx_cov;
373+
sub_vx_weight = 1.0 / sub_vel_cov_in_main_frame_x;
359374
}
360-
// merge with covariance
375+
376+
// merge velocity with covariance
361377
output_kinematics.twist_with_covariance.twist.linear.x =
362-
(main_vx * main_vx_weight + sub_vx * sub_vx_weight) / (main_vx_weight + sub_vx_weight);
378+
(main_vx * main_vx_weight + sub_vel_in_main_frame_x * sub_vx_weight) /
379+
(main_vx_weight + sub_vx_weight);
363380
output_kinematics.twist_with_covariance.covariance[0] = 1.0 / (main_vx_weight + sub_vx_weight);
381+
364382
return output_kinematics;
365383
} else {
366384
std::cerr << "unknown merge policy in objectKinematicsMerger function." << std::endl;
@@ -453,21 +471,25 @@ autoware_auto_perception_msgs::msg::Shape shapeMerger(
453471

454472
void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj)
455473
{
474+
// do not update if motion direction is different
456475
if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) {
457-
// do not update velocity
458-
return;
459-
} else if (objectsYawIsReverted(main_obj, sub_obj)) {
460-
// revert velocity (revert pose is not necessary because it is not used in tracking)
461-
const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x;
462-
main_obj = sub_obj;
463-
main_obj.kinematics.twist_with_covariance.twist.linear.x = -vx_temp;
464476
return;
465-
} else {
466-
// update velocity
467-
const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x;
468-
main_obj = sub_obj;
469-
main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp;
470477
}
478+
// take main_obj's velocity vector and convert into sub_obj's frame
479+
// use sub_obj's orientation, but take only x component
480+
const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x;
481+
const auto main_vy = main_obj.kinematics.twist_with_covariance.twist.linear.y;
482+
const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation);
483+
const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation);
484+
const auto main_vx_in_sub_frame =
485+
main_vx * std::cos(main_yaw - sub_yaw) + main_vy * std::sin(main_yaw - sub_yaw);
486+
487+
// copy sub object to fused object
488+
main_obj = sub_obj;
489+
// recover main object's velocity
490+
main_obj.kinematics.twist_with_covariance.twist.linear.x = main_vx_in_sub_frame;
491+
492+
return;
471493
}
472494

473495
void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj)

0 commit comments

Comments
 (0)