@@ -324,43 +324,61 @@ autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMe
324
324
// copy main object at first
325
325
output_kinematics = main_obj.kinematics ;
326
326
auto sub_obj_ = sub_obj;
327
- // do not merge reverse direction objects
327
+ // do not merge if motion direction is different
328
328
if (!objectsHaveSameMotionDirections (main_obj, sub_obj)) {
329
329
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 ;
334
330
}
335
331
336
332
// currently only merge vx
337
333
if (policy == MergePolicy::SKIP) {
338
334
return output_kinematics;
339
335
} 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
+
342
346
return output_kinematics;
343
347
} 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
344
350
const auto main_vx = main_obj.kinematics .twist_with_covariance .twist .linear .x ;
345
351
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);
346
357
// inverse weight
347
358
const auto main_vx_cov = main_obj.kinematics .twist_with_covariance .covariance [0 ];
348
359
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);
349
364
double main_vx_weight, sub_vx_weight;
350
365
if (main_vx_cov == 0.0 ) {
351
366
main_vx_weight = 1.0 * 1e6 ;
352
367
} else {
353
368
main_vx_weight = 1.0 / main_vx_cov;
354
369
}
355
- if (sub_vx_cov == 0.0 ) {
370
+ if (sub_vel_cov_in_main_frame_x == 0.0 ) {
356
371
sub_vx_weight = 1.0 * 1e6 ;
357
372
} else {
358
- sub_vx_weight = 1.0 / sub_vx_cov ;
373
+ sub_vx_weight = 1.0 / sub_vel_cov_in_main_frame_x ;
359
374
}
360
- // merge with covariance
375
+
376
+ // merge velocity with covariance
361
377
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);
363
380
output_kinematics.twist_with_covariance .covariance [0 ] = 1.0 / (main_vx_weight + sub_vx_weight);
381
+
364
382
return output_kinematics;
365
383
} else {
366
384
std::cerr << " unknown merge policy in objectKinematicsMerger function." << std::endl;
@@ -453,21 +471,25 @@ autoware_auto_perception_msgs::msg::Shape shapeMerger(
453
471
454
472
void updateExceptVelocity (TrackedObject & main_obj, const TrackedObject & sub_obj)
455
473
{
474
+ // do not update if motion direction is different
456
475
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;
464
476
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;
470
477
}
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 ;
471
493
}
472
494
473
495
void updateOnlyObjectVelocity (TrackedObject & main_obj, const TrackedObject & sub_obj)
0 commit comments