feat: impl. object model to pedestrian

autowarefoundation · Jun 13, 2024 · 7e617cc · 7e617cc
1 parent d588a5e
commit 7e617cc
Show file tree

Hide file tree

Showing 7 changed files with 83 additions and 69 deletions.
diff --git a/...ption/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/...ption/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp
@@ -32,13 +32,6 @@ class BicycleTracker : public Tracker
 
   object_model::ObjectModel object_model_ = object_model::bicycle;
 
-  struct EkfParams
-  {
-    double r_cov_x;
-    double r_cov_y;
-    double r_cov_yaw;
-  } ekf_params_;
-
   double z_;
 
   struct BoundingBox

diff --git a/...on/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/...on/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp
@@ -22,6 +22,7 @@
 #include "kalman_filter/kalman_filter.hpp"
 #include "multi_object_tracker/tracker/model/tracker_base.hpp"
 #include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp"
+#include "multi_object_tracker/tracker/object_model/object_model.hpp"
 
 // cspell: ignore CTRV
 
@@ -31,12 +32,7 @@ class PedestrianTracker : public Tracker
   autoware_perception_msgs::msg::DetectedObject object_;
   rclcpp::Logger logger_;
 
-  struct EkfParams
-  {
-    double r_cov_x;
-    double r_cov_y;
-    double r_cov_yaw;
-  } ekf_params_;
+  object_model::ObjectModel object_model_ = object_model::pedestrian;
 
   double z_;
 

diff --git a/...n/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp b/...n/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp
@@ -67,17 +67,22 @@ struct ObjectSizeLimit
 };
 struct MotionProcessNoise
 {
+  double vel_long{0.0};      // [m/s] uncertain longitudinal velocity
+  double vel_lat{0.0};       // [m/s] uncertain lateral velocity
+  double yaw_rate{0.0};      // [rad/s] uncertain yaw rate
+  double yaw_rate_min{0.0};  // [rad/s] uncertain yaw rate, minimum
+  double yaw_rate_max{0.0};  // [rad/s] uncertain yaw rate, maximum
   double acc_long{0.0};      // [m/s^2] uncertain longitudinal acceleration
   double acc_lat{0.0};       // [m/s^2] uncertain lateral acceleration
-  double yaw_rate_min{0.0};  // [rad/s] uncertain yaw change rate, minimum
-  double yaw_rate_max{0.0};  // [rad/s] uncertain yaw change rate, maximum
+  double rotate_rate{0.0};   // [rad/s^2] uncertain rotation change rate
 };
 struct MotionProcessLimit
 {
   double acc_long_max{0.0};  // [m/s^2]
   double acc_lat_max{0.0};   // [m/s^2]
   double vel_long_max{0.0};  // [m/s]
   double vel_lat_max{0.0};   // [m/s]
+  double yaw_rate_max{0.0};  // [rad/s]
 };
 struct StateCovariance
 {
@@ -256,17 +261,17 @@ class ObjectModel
         size_limit.length_max = 2.0;
         size_limit.width_min = 0.3;
         size_limit.width_max = 1.0;
-        size_limit.height_min = 1.0;
+        size_limit.height_min = 0.6;
         size_limit.height_max = 2.0;
 
-        process_noise.acc_long = const_g * 0.35;
-        process_noise.acc_lat = const_g * 0.15;
-        process_noise.yaw_rate_min = 1.5;   // deg2rad(1.5);
-        process_noise.yaw_rate_max = 15.0;  // deg2rad(15.0);
+        process_noise.vel_long = 0.5;
+        process_noise.vel_lat = 0.5;
+        process_noise.yaw_rate = deg2rad(20.0);
+        process_noise.acc_long = const_g * 0.3;
+        process_noise.rotate_rate = deg2rad(30.0);
 
-        process_limit.acc_long_max = const_g;
-        process_limit.acc_lat_max = const_g;
         process_limit.vel_long_max = kmph2mps(100.0);
+        process_limit.yaw_rate_max = deg2rad(30.0);
 
         // initial covariance
         initial_covariance.pos_x = sq(1.0);
@@ -276,10 +281,10 @@ class ObjectModel
         initial_covariance.vel_lat = sq(0.2);
 
         // measurement noise model
-        measurement_covariance.pos_x = sq(0.5);
+        measurement_covariance.pos_x = sq(0.4);
         measurement_covariance.pos_y = sq(0.4);
-        measurement_covariance.yaw = sq(deg2rad(20.0));
-        measurement_covariance.vel_long = sq(kmph2mps(10.0));
+        measurement_covariance.yaw = sq(deg2rad(30.0));
+
         break;
 
       default:

diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
@@ -166,9 +166,10 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject(
     auto r_cov_y = object_model_.measurement_covariance.pos_y;
 
     // yaw angle fix
-    double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
-    bool is_yaw_available = object.kinematics.orientation_availability !=
-                            autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
+    const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
+    const bool is_yaw_available =
+      object.kinematics.orientation_availability !=
+      autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
 
     // fill covariance matrix
     using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;

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
@@ -206,9 +206,10 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje
     }
 
     // yaw angle fix
-    double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
-    bool is_yaw_available = object.kinematics.orientation_availability !=
-                            autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
+    const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
+    const bool is_yaw_available =
+      object.kinematics.orientation_availability !=
+      autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
 
     // fill covariance matrix
     using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;

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
@@ -207,9 +207,10 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO
     }
 
     // yaw angle fix
-    double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
-    bool is_yaw_available = object.kinematics.orientation_availability !=
-                            autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
+    const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
+    const bool is_yaw_available =
+      object.kinematics.orientation_availability !=
+      autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
 
     // fill covariance matrix
     using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;

diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp
@@ -55,18 +55,11 @@ PedestrianTracker::PedestrianTracker(
   // initialize existence probability
   initializeExistenceProbabilities(channel_index, object.existence_probability);
 
-  // Initialize parameters
-  // measurement noise covariance
-  float r_stddev_x = 0.4;                                  // [m]
-  float r_stddev_y = 0.4;                                  // [m]
-  float r_stddev_yaw = tier4_autoware_utils::deg2rad(30);  // [rad]
-  ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
-  ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
-  ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
-
   // OBJECT SHAPE MODEL
-  bounding_box_ = {0.5, 0.5, 1.7};
-  cylinder_ = {0.5, 1.7};
+  bounding_box_ = {
+    object_model_.init_size.length, object_model_.init_size.width,
+    object_model_.init_size.height};                                             // default value
+  cylinder_ = {object_model_.init_size.length, object_model_.init_size.height};  // default value
   if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
     bounding_box_ = {
       object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
@@ -76,21 +69,24 @@ PedestrianTracker::PedestrianTracker(
     // do not update polygon shape
   }
   // set maximum and minimum size
-  constexpr double max_size = 5.0;
-  constexpr double min_size = 0.3;
-  bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
-  bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
-  bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
-  cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size);
-  cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size);
+  bounding_box_.length = std::clamp(
+    bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max);
+  bounding_box_.width = std::clamp(
+    bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max);
+  bounding_box_.height = std::clamp(
+    bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max);
+  cylinder_.width = std::clamp(
+    cylinder_.width, object_model_.size_limit.length_min, object_model_.size_limit.length_max);
+  cylinder_.height = std::clamp(
+    cylinder_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max);
 
   // Set motion model parameters
   {
-    constexpr double q_stddev_x = 0.5;                                  // [m/s]
-    constexpr double q_stddev_y = 0.5;                                  // [m/s]
-    constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20);  // [rad/s]
-    constexpr double q_stddev_vx = 9.8 * 0.3;                           // [m/(s*s)]
-    constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30);   // [rad/(s*s)]
+    const double q_stddev_x = object_model_.process_noise.vel_long;
+    const double q_stddev_y = object_model_.process_noise.vel_lat;
+    const double q_stddev_yaw = object_model_.process_noise.yaw_rate;
+    const double q_stddev_vx = object_model_.process_noise.acc_long;
+    const double q_stddev_wz = object_model_.process_noise.rotate_rate;
     motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz);
   }
 
@@ -167,22 +163,40 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje
 
   // UNCERTAINTY MODEL
   if (!object.kinematics.has_position_covariance) {
+    // measurement noise covariance
+    auto r_cov_x = object_model_.measurement_covariance.pos_x;
+    auto r_cov_y = object_model_.measurement_covariance.pos_y;
+
+    // yaw angle fix
+    const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
+    const bool is_yaw_available =
+      object.kinematics.orientation_availability !=
+      autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
+
     // fill covariance matrix
     using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
-    const double & r_cov_x = ekf_params_.r_cov_x;
-    const double & r_cov_y = ekf_params_.r_cov_y;
     auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance;
-    const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
     const double cos_yaw = std::cos(pose_yaw);
     const double sin_yaw = std::sin(pose_yaw);
     const double sin_2yaw = std::sin(2.0f * pose_yaw);
     pose_cov[XYZRPY_COV_IDX::X_X] =
       r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw;            // x - x
     pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw;  // x - y
     pose_cov[XYZRPY_COV_IDX::Y_Y] =
-      r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw;    // y - y
-    pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y];  // y - x
+      r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw;                   // y - y
+    pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y];                 // y - x
+    pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0;                                         // x - yaw
+    pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0;                                         // y - yaw
+    pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0;                                         // yaw - x
+    pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0;                                         // yaw - y
+    pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw;  // yaw - yaw
+    if (!is_yaw_available) {
+      pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3;  // yaw is not available, multiply large value
+    }
+    auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance;
+    twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long;  // vel - vel
   }
+
   return updating_object;
 }
 
@@ -246,13 +260,16 @@ bool PedestrianTracker::measureWithShape(
   }
 
   // set maximum and minimum size
-  constexpr double max_size = 5.0;
-  constexpr double min_size = 0.3;
-  bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size);
-  bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size);
-  bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size);
-  cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size);
-  cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size);
+  bounding_box_.length = std::clamp(
+    bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max);
+  bounding_box_.width = std::clamp(
+    bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max);
+  bounding_box_.height = std::clamp(
+    bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max);
+  cylinder_.width = std::clamp(
+    cylinder_.width, object_model_.size_limit.length_min, object_model_.size_limit.length_max);
+  cylinder_.height = std::clamp(
+    cylinder_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max);
 
   return true;
 }