feat: cherry pick to improve stability of tracker

perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp

@@ -19,7 +19,8 @@
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_perception_msgs/msg/traffic_signal_array.hpp>

perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp

@@ -16,7 +16,6 @@
 #define OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -15,7 +15,7 @@
 #include "detected_object_filter/object_lanelet_filter.hpp"
 
 #include <object_recognition_utils/object_recognition_utils.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 
 #include <boost/geometry/algorithms/convex_hull.hpp>
 #include <boost/geometry/algorithms/disjoint.hpp>

perception/detected_object_validation/src/object_position_filter.cpp

@@ -14,8 +14,6 @@
 
 #include "detected_object_filter/object_position_filter.hpp"
 
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
-
 namespace object_position_filter
 {
 ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options)

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

@@ -15,7 +15,7 @@
 #include "obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp"
 
 #include <object_recognition_utils/object_recognition_utils.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 
 #include <boost/geometry.hpp>
 

perception/detected_object_validation/src/occupancy_grid_based_validator.cpp

@@ -16,7 +16,7 @@
 
 #include <object_recognition_utils/object_classification.hpp>
 #include <object_recognition_utils/object_recognition_utils.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 
 #include <boost/optional.hpp>
 

perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp

@@ -20,7 +20,6 @@
 #include <euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <shape_estimation/shape_estimator.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>

perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp

@@ -22,7 +22,6 @@
 #include <euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <shape_estimation/shape_estimator.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>

perception/detection_by_tracker/src/detection_by_tracker_core.cpp

@@ -16,6 +16,9 @@
 
 #include "object_recognition_utils/object_recognition_utils.hpp"
 
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
+
 #include <chrono>
 #include <memory>
 #include <optional>

perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp

@@ -19,7 +19,7 @@
 #include "pcl/point_types.h"
 #include "pcl_conversions/pcl_conversions.h"
 #include "rclcpp/logger.hpp"
-#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
 
 #include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
 #include "nav_msgs/msg/odometry.hpp"

perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp

@@ -14,6 +14,8 @@
 
 #include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp"
 
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+
 #include <boost/geometry.hpp>
 
 #include <memory>

perception/heatmap_visualizer/src/utils.cpp

@@ -15,7 +15,8 @@
 #include "heatmap_visualizer/utils.hpp"
 
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/math/normalization.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
 
 #include <tf2/utils.h>
 

perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp

@@ -16,8 +16,7 @@
 
 #include "object_recognition_utils/geometry.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
-#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
-
+#include "tier4_autoware_utils/geometry/geometry.hpp"
 namespace centerpoint
 {
 

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp

@@ -32,16 +32,18 @@ class BicycleTracker : public Tracker
 private:
   KalmanFilter ekf_;
   rclcpp::Time last_update_time_;
-  enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
+  enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
   struct EkfParams
   {
     char dim_x = 5;
-    float q_cov_x;
-    float q_cov_y;
-    float q_cov_yaw;
-    float q_cov_slip;
-    float q_cov_vx;
-    float p0_cov_vx;
+    float q_stddev_acc_long;
+    float q_stddev_acc_lat;
+    float q_stddev_yaw_rate_min;
+    float q_stddev_yaw_rate_max;
+    float q_cov_slip_rate_min;
+    float q_cov_slip_rate_max;
+    float q_max_slip_angle;
+    float p0_cov_vel;
     float p0_cov_slip;
     float r_cov_x;
     float r_cov_y;
@@ -51,7 +53,7 @@ class BicycleTracker : public Tracker
     float p0_cov_yaw;
   } ekf_params_;
 
-  double max_vx_;
+  double max_vel_;
   double max_slip_;
   double lf_;
   double lr_;

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp

@@ -32,26 +32,28 @@ class BigVehicleTracker : public Tracker
 private:
   KalmanFilter ekf_;
   rclcpp::Time last_update_time_;
-  enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
+  enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
   struct EkfParams
   {
     char dim_x = 5;
-    float q_cov_x;
-    float q_cov_y;
-    float q_cov_yaw;
-    float q_cov_slip;
-    float q_cov_vx;
-    float p0_cov_vx;
+    float q_stddev_acc_long;
+    float q_stddev_acc_lat;
+    float q_stddev_yaw_rate_min;
+    float q_stddev_yaw_rate_max;
+    float q_cov_slip_rate_min;
+    float q_cov_slip_rate_max;
+    float q_max_slip_angle;
+    float p0_cov_vel;
     float p0_cov_slip;
     float r_cov_x;
     float r_cov_y;
     float r_cov_yaw;
-    float r_cov_vx;
+    float r_cov_vel;
     float p0_cov_x;
     float p0_cov_y;
     float p0_cov_yaw;
   } ekf_params_;
-  double max_vx_;
+  double max_vel_;
   double max_slip_;
   double lf_;
   double lr_;

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp

@@ -33,28 +33,30 @@ class NormalVehicleTracker : public Tracker
 private:
   KalmanFilter ekf_;
   rclcpp::Time last_update_time_;
-  enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
+  enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
 
   struct EkfParams
   {
     char dim_x = 5;
-    float q_cov_x;
-    float q_cov_y;
-    float q_cov_yaw;
-    float q_cov_slip;
-    float q_cov_vx;
-    float p0_cov_vx;
+    float q_stddev_acc_long;
+    float q_stddev_acc_lat;
+    float q_stddev_yaw_rate_min;
+    float q_stddev_yaw_rate_max;
+    float q_cov_slip_rate_min;
+    float q_cov_slip_rate_max;
+    float q_max_slip_angle;
+    float p0_cov_vel;
     float p0_cov_slip;
     float r_cov_x;
     float r_cov_y;
     float r_cov_yaw;
-    float r_cov_vx;
+    float r_cov_vel;
     float p0_cov_x;
     float p0_cov_y;
     float p0_cov_yaw;
   } ekf_params_;
 
-  double max_vx_;
+  double max_vel_;
   double max_slip_;
   double lf_;
   double lr_;
@@ -88,6 +90,7 @@ class NormalVehicleTracker : public Tracker
     const rclcpp::Time & time,
     autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
   void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform);
+  double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   virtual ~NormalVehicleTracker() {}
 };
 

perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp

@@ -21,14 +21,16 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_object.hpp>
 #include <autoware_auto_perception_msgs/msg/shape.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_object.hpp>
 #include <geometry_msgs/msg/polygon.hpp>
+#include <geometry_msgs/msg/transform.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 
+#include <tf2/utils.h>
+
 #include <algorithm>
 #include <cmath>
 #include <tuple>

perception/multi_object_tracker/models.md

@@ -2,28 +2,28 @@
 
 ## Tracking model
 
 ### CTRV model [1]
 
 CTRV model is a model that assumes constant turn rate and velocity magnitude.
 
 - state transition equation
 
 $$
-\begin{align*}
-x_{k+1}   &= x_k + v_{x_k} \cos(\psi_k) \cdot dt \\
-y_{k+1}   &= y_k + v_{x_k} \sin(\psi_k) \cdot dt \\
-\psi_{k+1} &= \psi_k + \dot{\psi}_k \cdot dt \\
-v_{x_{k+1}}  &= v_{x_k} \\
-\dot{\psi}_{k+1}  &= \dot{\psi}_k \\
-\end{align*}
+\begin{aligned}
+x_{k+1} & = x_{k} + v_{k} \cos(\psi_k) \cdot {d t} \\
+y_{k+1} & = y_{k} + v_{k} \sin(\psi_k) \cdot {d t} \\
+\psi_{k+1} & = \psi_k + \dot\psi_{k} \cdot {d t} \\
+v_{k+1} & = v_{k} \\
+\dot\psi_{k+1} & = \dot\psi_{k} \\
+\end{aligned}
 $$
 
 - jacobian
 
 $$
 A = \begin{bmatrix}
-1 & 0 & -v_x \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\
-0 & 1 & v_x \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\
+1 & 0 & -v \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\
+0 & 1 & v \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\
 0 & 0 & 1 & 0 & dt \\
 0 & 0 & 0 & 1 & 0 \\
 0 & 0 & 0 & 0 & 1 \\
@@ -38,17 +38,20 @@
 ![kinematic_bicycle_model](image/kinematic_bicycle_model.png)
 
 - **state variable**
-  - pose( $x,y$ ), velocity( $v$ ), yaw( $\psi$ ), and slip angle ( $\beta$ )
-  - $[x_{k} ,y_{k} , v_{k} , \psi_{k} , \beta_{k} ]^\mathrm{T}$
+  - pose( $x,y$ ), yaw( $\psi$ ), velocity( $v$ ), and slip angle ( $\beta$ )
+  - $[x_{k}, y_{k}, \psi_{k}, v_{k}, \beta_{k} ]^\mathrm{T}$
 - **Prediction Equation**
   - $dt$: sampling time
+  - $w_{k} = \dot\psi_{k} = \frac{ v_{k} \sin \left( \beta_{k} \right) }{ l_r }$ : angular velocity
 
 $$
 \begin{aligned}
-x_{k+1} & =x_{k}+v_{k} \cos \left(\psi_{k}+\beta_{k}\right) d t \\
-y_{k+1} & =y_{k}+v_{k} \sin \left(\psi_{k}+\beta_{k}\right) d t \\
-v_{k+1} & =v_{k} \\
-\psi_{k+1} & =\psi_{k}+\frac{v_{k}}{l_{r}} \sin \beta_{k} d t \\
+x_{k+1} & = x_{k} + v_{k} \cos \left( \psi_{k}+\beta_{k} \right) {d t}
+            -\frac{1}{2}  \left\lbrace w_k v_k \sin \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\
+y_{k+1} & = y_{k} + v_{k} \sin \left( \psi_{k}+\beta_{k} \right) {d t}
+            +\frac{1}{2}  \left\lbrace w_k v_k \cos \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\
+\psi_{k+1} & =\psi_{k} + w_k {d t} \\
+v_{k+1} & = v_{k} \\
 \beta_{k+1} & =\beta_{k}
 \end{aligned}
 $$
@@ -57,9 +60,15 @@
 
 $$
 \frac{\partial f}{\partial \mathrm x}=\left[\begin{array}{ccccc}
-1 & 0 & -v \sin (\psi+\beta) d t & v \cos (\psi+\beta) & -v \sin (\psi+\beta) d t \\
-0 & 1 & v \cos (\psi+\beta) d t & v \sin (\psi+\beta) & v \cos (\psi+\beta) d t \\
-0 & 0 & 1 & \frac{1}{l_r} \sin \beta d t & \frac{v}{l_r} \cos \beta d t \\
+1 & 0
+ & v \cos (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \cos \left( \psi+\beta \right) \right\rbrace {d t}^2
+ & \sin (\psi+\beta) {d t} - \left\lbrace w \sin \left( \psi+\beta \right) \right\rbrace {d t}^2
+ & -v \sin (\psi+\beta) {d t} - \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\sin(\psi+\beta)+\sin(\beta)\cos(\psi+\beta) \right\rbrace {d t}^2 \\
+0 & 1
+ & v \sin (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \sin \left( \psi+\beta \right) \right\rbrace {d t}^2
+ & \cos (\psi+\beta) {d t} + \left\lbrace w \cos \left( \psi+\beta \right) \right\rbrace {d t}^2
+ & v \cos (\psi+\beta) {d t} + \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\cos(\psi+\beta)-\sin(\beta)\sin(\psi+\beta) \right\rbrace {d t}^2 \\
+0 & 0 & 1 & \frac{1}{l_r} \sin \beta {d t} & \frac{v}{l_r} \cos \beta d t \\
 0 & 0 & 0 & 1 & 0 \\
 0 & 0 & 0 & 0 & 1
 \end{array}\right]
@@ -106,5 +115,5 @@
 
 ## References
 
 [1] Schubert, Robin & Richter, Eric & Wanielik, Gerd. (2008). Comparison and evaluation of advanced motion models for vehicle tracking. 1 - 6. 10.1109/ICIF.2008.4632283.  
 [2] Kong, Jason & Pfeiffer, Mark & Schildbach, Georg & Borrelli, Francesco. (2015). Kinematic and dynamic vehicle models for autonomous driving control design. 1094-1099. 10.1109/IVS.2015.7225830.

perception/multi_object_tracker/src/data_association/data_association.cpp

@@ -201,7 +201,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
             measurement_object.kinematics.pose_with_covariance.pose.position,
             tracked_object.kinematics.pose_with_covariance.pose.position,
             getXYCovariance(tracked_object.kinematics.pose_with_covariance));
-          if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = false;
+          if (3.035 /*99%*/ <= mahalanobis_dist) passed_gate = false;
         }
         // 2d iou gate
         if (passed_gate) {