fix(multi_object_tracker): consider vehicle twist direction #4637

takayuki5168 · 2023-08-15T14:56:59Z

Description

Fixed the twist direction bug, which was the object's orientation, but actually the slip angle should be considered.
The kinematics model used in the tracker is CoG frame where the direction of twist and object orientation is not the same.

Tests performed

planning simulator
scenario simulator
- https://evaluation.tier4.jp/evaluation/reports/cb5db4b2-c286-5eb3-996d-676f01957581?project_id=prd_jt

Effects on system behavior

Nothing

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

Summary by CodeRabbit

Release Notes:

Bug fix: Fixed a bug in the twist direction calculation in the multi-object tracker code.
Refactor: Updated the calculation of the twist values in the getTrackedObject function of the BicycleTracker, BigVehicleTracker, and NormalVehicleTracker classes to consider the slip angle.

"Bug fixed, twists refined,
Slip angles now aligned.
Objects tracked with precision,
A bug's demise, a coder's mission."

YoshiRi

Make sense.
Could you add following two changes?

add same changes to bicycle_tracker

  twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP));
  twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP));

add document description in models.md

#### remarks on the output twist

Remarks that the velocity $v_{k}$ is the norm of velocity of vehicle, not the longitudinal velocity.
So the output twist in the object coordinate $(x,y)$ is calculated as follows.

$$
\begin{aligned}
v_{x} &= v_{k} \cos \left(\beta_{k}\right) \\
v_{y} &= v_{k} \sin \left(\beta_{k}\right) 
\end{aligned}
$$

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

takayuki5168 · 2023-08-16T02:50:58Z

@YoshiRi Thank you. I added them.

github-actions · 2023-08-16T02:53:56Z

Documentation URL: https://autowarefoundation.github.io/autoware.universe/pr-4637/
Modified URLs:

https://autowarefoundation.github.io/autoware.universe/pr-4637/perception/multi_object_tracker/models/

codecov · 2023-08-16T03:00:24Z

Codecov Report

Patch and project coverage have no change.

Comparison is base (31e10cd) 14.89% compared to head (f2e6b80) 14.89%.
Report is 2 commits behind head on main.

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #4637   +/-   ##
=======================================
  Coverage   14.89%   14.89%           
=======================================
  Files        1547     1547           
  Lines      106613   106599   -14     
  Branches    32603    32595    -8     
=======================================
+ Hits        15876    15878    +2     
+ Misses      73432    73417   -15     
+ Partials    17305    17304    -1

Flag	Coverage Δ		*Carryforward flag
differential	`0.00% <0.00%> (?)`
total	`14.89% <ø> (+<0.01%)`	⬆️	Carriedforward from 1d39724

*This pull request uses carry forward flags. Click here to find out more.

Files Changed	Coverage Δ
...ject_tracker/src/tracker/model/bicycle_tracker.cpp	`0.00% <0.00%> (ø)`
..._tracker/src/tracker/model/big_vehicle_tracker.cpp	`0.00% <0.00%> (ø)`
...acker/src/tracker/model/normal_vehicle_tracker.cpp	`0.00% <0.00%> (ø)`

... and 4 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

YoshiRi

LGTM

github-actions · 2023-08-16T16:05:44Z

CodeRabbit

Walkthrough

The overall change focuses on fixing a bug in the twist direction calculation in the multi-object tracker code. The calculation of the linear and angular components of the twist is updated based on the slip angle (X_t(IDX::SLIP)). The linear x-component is multiplied by the cosine of the slip angle, and the linear y-component is multiplied by the sine of the slip angle. The angular z-component is calculated using the updated linear x-component and a constant value (lr_).

Changes

File	Summary
perception/multi_object_tracker/models.md	Fixes a bug in the twist direction calculation. Introduces a new calculation for the output twist in the object coordinate system. Remarks about velocity and the calculation of the output twist. No changes to exported functions or global data structures.
perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp	Changes the calculation of `twist_with_cov.twist.linear.x` and `twist_with_cov.twist.linear.y` in the `getTrackedObject` function of the `BicycleTracker` class. Multiplies `twist_with_cov.twist.linear.x` by the cosine of `X_t(IDX::SLIP)`, and `twist_with_cov.twist.linear.y` by the sine of `X_t(IDX::SLIP)`. Accounts for slip angle when calculating the twist direction.
perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp	Modifies the `getTrackedObject` function in the `BigVehicleTracker` and `NormalVehicleTracker` classes. Updates the calculation of the linear and angular components of the twist based on the slip angle (`X_t(IDX::SLIP)`). Multiplies the linear x-component by the cosine of the slip angle, and the linear y-component by the sine of the slip angle. Calculates the angular z-component using the updated linear x-component and a constant value (`lr_`). No changes to function signature or external interface.

github-actions

CodeRabbit

Commits

Files that changed from the base of the PR and between 31e10cd and f2e6b80 commits.

Files selected (4)

perception/multi_object_tracker/models.md (1)
perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp (1)
perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp (1)
perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp (1)

Files skipped from review due to trivial changes (2)

perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

Review comments generated (2)

Review: 1
LGTM: 1

Tips

Chat with CodeRabbit Bot (`@coderabbitai`)

Reply on review comments left by this bot to ask follow-up questions. A review comment is a comment on a diff or a file.
Invite the bot into a review comment chain by tagging @coderabbitai in a reply.

Code suggestions

The bot may make code suggestions, but please review them carefully before committing since the line number ranges may be misaligned.
You can edit the comment made by the bot and manually tweak the suggestion if it is slightly off.

Pausing incremental reviews

Add @coderabbitai: ignore anywhere in the PR description to pause further reviews from the bot.

github-actions · 2023-08-16T16:06:42Z

perception/multi_object_tracker/models.md

+#### remarks on the output twist
+
+Remarks that the velocity $v_{k}$ is the norm of velocity of vehicle, not the longitudinal velocity.
+So the output twist in the object coordinate $(x,y)$ is calculated as follows.
+
+$$
+\begin{aligned}
+v_{x} &= v_{k} \cos \left(\beta_{k}\right) \\
+v_{y} &= v_{k} \sin \left(\beta_{k}\right)
+\end{aligned}
+$$
+


CodeRabbit

The comments in this section provide an explanation of how the output twist in the object coordinate system is calculated. However, the comments are not formatted correctly and are missing the necessary syntax for mathematical equations. Additionally, the comments refer to variables and symbols that are not defined in this context.

To improve clarity, please update the comments as follows:

68: #### Remarks on the output twist 69: The velocity `v_k` represents the norm of the vehicle's velocity, not the longitudinal velocity. 70: To calculate the output twist in the object coordinate `(x, y)`, use the following formulas: 71: 72: ``` 73: v_x = v_k * cos(beta_k) 74: v_y = v_k * sin(beta_k) 75: ``` 76: Where: 77: - `v_x` is the linear x-component of the twist. 78: - `v_y` is the linear y-component of the twist. 79: - `beta_k` is the slip angle of the vehicle.

Please ensure that the variables and symbols used in the comments are consistent with their definitions in the code.

yukkysaito

LGTM

…foundation#4637) * feat(multi_object_tracker): consider vehicle twist direction Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix bicycle model Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update doc Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

github-actions bot added the component:perception Advanced sensor data processing and environment understanding. (auto-assigned) label Aug 15, 2023

takayuki5168 changed the title ~~feat(multi_object_tracker): consider vehicle twist direction~~ fix(multi_object_tracker): consider vehicle twist direction Aug 15, 2023

takayuki5168 marked this pull request as ready for review August 15, 2023 15:02

takayuki5168 requested review from YoshiRi and yukkysaito as code owners August 15, 2023 15:02

takayuki5168 force-pushed the feat/tracker-vehicle-twist-direction branch from 7f19375 to 489493d Compare August 15, 2023 15:20

YoshiRi requested changes Aug 15, 2023

View reviewed changes

takayuki5168 added 3 commits August 16, 2023 11:48

feat(multi_object_tracker): consider vehicle twist direction

d7215a6

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

fix bicycle model

8ebd3c2

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

update doc

f2e6b80

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

takayuki5168 force-pushed the feat/tracker-vehicle-twist-direction branch from 489493d to f2e6b80 Compare August 16, 2023 02:50

takayuki5168 added run:deploy-docs Mark for deploy-docs action generation. (used-by-ci) run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) labels Aug 16, 2023

github-actions bot added the type:documentation Creating or refining documentation. (auto-assigned) label Aug 16, 2023

YoshiRi approved these changes Aug 16, 2023

View reviewed changes

miursh added the openai-pr-reviewer label Aug 16, 2023

github-actions bot reviewed Aug 16, 2023

View reviewed changes

takayuki5168 added run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) and removed run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) labels Aug 16, 2023

yukkysaito reviewed Aug 21, 2023

View reviewed changes

YoshiRi mentioned this pull request Aug 23, 2023

feat(map_based_prediciton): consider y component of the velocity vector #4728

Merged

4 tasks

takayuki5168 merged commit 6bc983c into autowarefoundation:main Aug 28, 2023

takayuki5168 deleted the feat/tracker-vehicle-twist-direction branch August 28, 2023 05:17

tier4-autoware-public-bot bot mentioned this pull request Aug 29, 2023

chore: sync upstream tier4/autoware.universe#776

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(multi_object_tracker): consider vehicle twist direction #4637

fix(multi_object_tracker): consider vehicle twist direction #4637

takayuki5168 commented Aug 15, 2023 •

edited by github-actions bot

Loading

YoshiRi left a comment

takayuki5168 commented Aug 16, 2023

github-actions bot commented Aug 16, 2023

codecov bot commented Aug 16, 2023 •

edited

Loading

YoshiRi left a comment

github-actions bot commented Aug 16, 2023 •

edited

Loading

github-actions bot left a comment

github-actions bot Aug 16, 2023

yukkysaito left a comment

fix(multi_object_tracker): consider vehicle twist direction #4637

fix(multi_object_tracker): consider vehicle twist direction #4637

Conversation

takayuki5168 commented Aug 15, 2023 • edited by github-actions bot Loading

Description

Tests performed

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

Summary by CodeRabbit

YoshiRi left a comment

Choose a reason for hiding this comment

takayuki5168 commented Aug 16, 2023

github-actions bot commented Aug 16, 2023

codecov bot commented Aug 16, 2023 • edited Loading

Codecov Report

YoshiRi left a comment

Choose a reason for hiding this comment

github-actions bot commented Aug 16, 2023 • edited Loading

Walkthrough

Changes

github-actions bot left a comment

Choose a reason for hiding this comment

Chat with CodeRabbit Bot (@coderabbitai)

Code suggestions

Pausing incremental reviews

github-actions bot Aug 16, 2023

Choose a reason for hiding this comment

yukkysaito left a comment

Choose a reason for hiding this comment

takayuki5168 commented Aug 15, 2023 •

edited by github-actions bot

Loading

codecov bot commented Aug 16, 2023 •

edited

Loading

github-actions bot commented Aug 16, 2023 •

edited

Loading

Chat with CodeRabbit Bot (`@coderabbitai`)