Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(multi_object_tracker): consider vehicle twist direction #4637

Merged
merged 3 commits into from
Aug 28, 2023
Merged

fix(multi_object_tracker): consider vehicle twist direction #4637

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d7215a6
feat(multi_object_tracker): consider vehicle twist direction
takayuki5168 Aug 15, 2023
8ebd3c2
fix bicycle model
takayuki5168 Aug 16, 2023
f2e6b80
update doc
takayuki5168 Aug 16, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 12 additions & 0 deletions perception/multi_object_tracker/models.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,18 @@ $$
\end{array}\right]
$$

#### 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}
$$

Comment on lines +68 to +79
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Image description 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.

## Anchor point based estimation

To separate the estimation of the position and the shape, we use anchor point based position estimation.
Expand Down
3 changes: 2 additions & 1 deletion perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -427,7 +427,8 @@ bool BicycleTracker::getTrackedObject(
pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW);

// twist
twist_with_cov.twist.linear.x = X_t(IDX::VX);
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));
twist_with_cov.twist.angular.z =
X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k)
// twist covariance
Expand Down
3 changes: 2 additions & 1 deletion perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -478,7 +478,8 @@ bool BigVehicleTracker::getTrackedObject(
pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW);

// twist
twist_with_cov.twist.linear.x = X_t(IDX::VX);
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));
twist_with_cov.twist.angular.z =
X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k)
// twist covariance
Expand Down
3 changes: 2 additions & 1 deletion perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -494,7 +494,8 @@ bool NormalVehicleTracker::getTrackedObject(
pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW);

// twist
twist_with_cov.twist.linear.x = X_t(IDX::VX);
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));
twist_with_cov.twist.angular.z =
X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k)
// twist covariance
Expand Down