fix(multi_object_tracker): consider vehicle twist direction (#4637)

* feat(multi_object_tracker): consider vehicle twist direction Signed-off-by: Takayuki Murooka <[email protected]> * fix bicycle model Signed-off-by: Takayuki Murooka <[email protected]> * update doc Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
autowarefoundation · Aug 28, 2023 · 6bc983c · 6bc983c
1 parent 8613b51
commit 6bc983c
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 3 deletions.
diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md
@@ -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}
+$$
+
 ## Anchor point based estimation
 
 To separate the estimation of the position and the shape, we use anchor point based position estimation.

diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
@@ -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

diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
@@ -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

diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
@@ -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