diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index 424e2b2d2c13e..3e3afacadd00f 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -27,6 +27,7 @@ ament_auto_add_library(radar_object_tracker_node SHARED src/radar_object_tracker_node/radar_object_tracker_node.cpp src/tracker/model/tracker_base.cpp src/tracker/model/linear_motion_tracker.cpp + src/tracker/model/constant_turn_rate_motion_tracker.cpp src/utils/utils.cpp src/data_association/data_association.cpp ) diff --git a/perception/radar_object_tracker/README.md b/perception/radar_object_tracker/README.md index 739a4a745ff6c..395bc025c2821 100644 --- a/perception/radar_object_tracker/README.md +++ b/perception/radar_object_tracker/README.md @@ -46,7 +46,7 @@ See more details in the [models.md](models.md). | `publish_rate` | double | 10.0 | The rate at which to publish the output messages | | `world_frame_id` | string | "map" | The frame ID of the world coordinate system | | `enable_delay_compensation` | bool | false | Whether to enable delay compensation. If set to `true`, output topic is published by timer with `publish_rate`. | -| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files | +| `tracking_config_directory` | string | "./config/tracking/" | The directory containing the tracking configuration files | | `enable_logging` | bool | false | Whether to enable logging | | `logging_file_path` | string | "/tmp/association_log.json" | The path to the file where logs should be written | | `tracker_lifetime` | double | 1.0 | The lifetime of the tracker in seconds | @@ -65,6 +65,15 @@ See more details in the [models.md](models.md). See more details in the [models.md](models.md). +### Tracker parameters + +Currently, this package supports the following trackers: + +- `linear_motion_tracker` +- `constant_turn_rate_motion_tracker` + +Default settings for each tracker are defined in the [./config/tracking/](./config/tracking/), and described in [models.md](models.md). + ## Assumptions / Known limits diff --git a/perception/radar_object_tracker/config/data_association_matrix.param.yaml b/perception/radar_object_tracker/config/data_association_matrix.param.yaml index 104d790d185db..69628414e67d4 100644 --- a/perception/radar_object_tracker/config/data_association_matrix.param.yaml +++ b/perception/radar_object_tracker/config/data_association_matrix.param.yaml @@ -14,10 +14,10 @@ max_dist_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [4.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN - 4.0, 4.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 4.0, 8.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #CAR + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #TRUCK + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #BUS + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #TRAILER 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN diff --git a/perception/radar_object_tracker/config/default_tracker.param.yaml b/perception/radar_object_tracker/config/default_tracker.param.yaml index 757125202d69b..6c26034860e1b 100644 --- a/perception/radar_object_tracker/config/default_tracker.param.yaml +++ b/perception/radar_object_tracker/config/default_tracker.param.yaml @@ -1,9 +1,9 @@ /**: ros__parameters: - car_tracker: "linear_motion_tracker" - truck_tracker: "linear_motion_tracker" - bus_tracker: "linear_motion_tracker" - trailer_tracker: "linear_motion_tracker" - pedestrian_tracker: "linear_motion_tracker" - bicycle_tracker: "linear_motion_tracker" - motorcycle_tracker: "linear_motion_tracker" + car_tracker: "constant_turn_rate_motion_tracker" + truck_tracker: "constant_turn_rate_motion_tracker" + bus_tracker: "constant_turn_rate_motion_tracker" + trailer_tracker: "constant_turn_rate_motion_tracker" + pedestrian_tracker: "constant_turn_rate_motion_tracker" + bicycle_tracker: "constant_turn_rate_motion_tracker" + motorcycle_tracker: "constant_turn_rate_motion_tracker" diff --git a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml index f80adffb41090..d2c0841cf372e 100644 --- a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml +++ b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml @@ -4,12 +4,11 @@ # basic settings world_frame_id: "map" tracker_lifetime: 1.0 # [sec] - # if empty, use default config declared in this package - tracking_config_directory: "" + measurement_count_threshold: 3 # object will be published if it is tracked more than this threshold # delay compensate parameters publish_rate: 10.0 - enable_delay_compensation: false + enable_delay_compensation: true # logging enable_logging: false @@ -18,10 +17,10 @@ # filtering ## 1. distance based filtering: remove closer objects than this threshold use_distance_based_noise_filtering: true - minimum_range_threshold: 70.0 # [m] + minimum_range_threshold: 60.0 # [m] ## 2. lanelet map based filtering use_map_based_noise_filtering: true max_distance_from_lane: 5.0 # [m] max_angle_diff_from_lane: 0.785398 # [rad] (45 deg) - max_lateral_velocity: 5.0 # [m/s] + max_lateral_velocity: 7.0 # [m/s] diff --git a/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml b/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml new file mode 100644 index 0000000000000..f80f881cabf03 --- /dev/null +++ b/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml @@ -0,0 +1,36 @@ +default: + # This file defines the parameters for the linear motion tracker. + # All this parameter coordinate is assumed to be in the vehicle coordinate system. + # So, the x axis is pointing to the front of the vehicle, y axis is pointing to the left of the vehicle. + ekf_params: + # random walk noise is used to model the acceleration noise + process_noise_std: # [m/s^2] + x: 0.5 + y: 0.5 + yaw: 0.1 + vx: 1.0 # assume 1m/s velocity noise + wz: 0.4 + measurement_noise_std: + x: 4.0 # [m] + y: 4.0 # [m] + # y: 0.02 # rad/m if use_polar_coordinate_in_measurement_noise is true + yaw: 0.2 # [rad] + vx: 10 # [m/s] + initial_covariance_std: + x: 3.0 # [m] + y: 6.0 # [m] + yaw: 10.0 # [rad] + vx: 100.0 # [m/s] + wz: 10.0 # [rad/s] + # input flag + trust_yaw_input: false # set true if yaw input of sensor is reliable + trust_twist_input: false # set true if twist input of sensor is reliable + use_polar_coordinate_in_measurement_noise: false # set true if you want to define the measurement noise in polar coordinate + assume_zero_yaw_rate: false # set true if you want to assume zero yaw rate + # output limitation + limit: + max_speed: 80.0 # [m/s] + # low pass filter is used to smooth the yaw and shape estimation + low_pass_filter: + time_constant: 1.0 # [s] + sampling_time: 0.1 # [s] diff --git a/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml b/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml index 71367f4575193..5e813558a2bff 100644 --- a/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml +++ b/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml @@ -7,22 +7,28 @@ default: process_noise_std: # [m/s^2] ax: 0.98 # assume 0.1G acceleration noise ay: 0.98 - vx: 0.1 # assume 0.1m/s velocity noise - vy: 0.1 + vx: 1.0 # assume 1m/s velocity noise + vy: 1.0 x: 1.0 # assume 1m position noise y: 1.0 measurement_noise_std: x: 0.6 # [m] - y: 0.9 # [m] - vx: 0.4 # [m/s] - vy: 1 # [m/s] + # y: 4.0 # [m] + y: 0.01 # rad/m if use_polar_coordinate_in_measurement_noise is true + vx: 10 # [m/s] + vy: 100 # [m/s] initial_covariance_std: x: 3.0 # [m] y: 6.0 # [m] - vx: 1.0 # [m/s] - vy: 5.0 # [m/s] - ax: 0.5 # [m/s^2] - ay: 1.0 # [m/s^2] + vx: 1000.0 # [m/s] + vy: 1000.0 # [m/s] + ax: 1000.0 # [m/s^2] + ay: 1000.0 # [m/s^2] + estimate_acc: false # set true if you want to estimate the acceleration + # input flag + trust_yaw_input: false # set true if yaw input of sensor is reliable + trust_twist_input: false # set true if twist input of sensor is reliable + use_polar_coordinate_in_measurement_noise: true # set true if you want to define the measurement noise in polar coordinate # output limitation limit: max_speed: 80.0 # [m/s] diff --git a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp index 415ff24b34cc3..9a3fdf602a3ca 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp @@ -76,6 +76,8 @@ class RadarObjectTrackerNode : public rclcpp::Node float tracker_lifetime_; std::map tracker_map_; + int measurement_count_threshold_; + void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp new file mode 100644 index 0000000000000..ac4b2cd1a0acb --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp @@ -0,0 +1,112 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ +#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ + +#include "radar_object_tracker/tracker/model/tracker_base.hpp" + +#include + +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; +class ConstantTurnRateMotionTracker : public Tracker // means constant turn rate motion tracker +{ +private: + autoware_auto_perception_msgs::msg::DetectedObject object_; + rclcpp::Logger logger_; + +private: + KalmanFilter ekf_; + rclcpp::Time last_update_time_; + enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, WZ = 4 }; + + struct EkfParams + { + // dimension + char dim_x = 5; + // system noise + double q_cov_x; + double q_cov_y; + double q_cov_yaw; + double q_cov_vx; + double q_cov_wz; + // measurement noise + double r_cov_x; + double r_cov_y; + double r_cov_yaw; + double r_cov_vx; + // initial state covariance + double p0_cov_x; + double p0_cov_y; + double p0_cov_yaw; + double p0_cov_vx; + double p0_cov_wz; + }; + static EkfParams ekf_params_; + + // limitation + static double max_vx_; + // rough tracking parameters + float z_; + + // lpf parameter + static double filter_tau_; // time constant of 1st order low pass filter + static double filter_dt_; // sampling time of 1st order low pass filter + + // static flags + static bool is_initialized_; + static bool trust_yaw_input_; + static bool trust_twist_input_; + static bool use_polar_coordinate_in_measurement_noise_; + static bool assume_zero_yaw_rate_; + +private: + struct BoundingBox + { + double length; + double width; + double height; + }; + struct Cylinder + { + double width; + double height; + }; + BoundingBox bounding_box_; + Cylinder cylinder_; + +public: + ConstantTurnRateMotionTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const std::string & tracker_param_file, const std::uint8_t & label); + + static void loadDefaultModelParameters(const std::string & path); + bool predict(const rclcpp::Time & time) override; + bool predict(const double dt, KalmanFilter & ekf) const; + bool measure( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) override; + bool measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); + bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool getTrackedObject( + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + virtual ~ConstantTurnRateMotionTracker() {} +}; + +#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp index 108e79c043ba0..77caa7a266481 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp @@ -56,18 +56,25 @@ class LinearMotionTracker : public Tracker double p0_cov_vy; double p0_cov_ax; double p0_cov_ay; - } ekf_params_; + }; + static EkfParams ekf_params_; // limitation - double max_vx_; - double max_vy_; + static double max_vx_; + static double max_vy_; // rough tracking parameters float z_; float yaw_; // lpf parameter - double filter_tau_; // time constant of 1st order low pass filter - double filter_dt_; // sampling time of 1st order low pass filter + static double filter_tau_; // time constant of 1st order low pass filter + static double filter_dt_; // sampling time of 1st order low pass filter + + static bool is_initialized_; + static bool estimate_acc_; + static bool trust_yaw_input_; + static bool trust_twist_input_; + static bool use_polar_coordinate_in_measurement_noise_; private: struct BoundingBox @@ -89,13 +96,15 @@ class LinearMotionTracker : public Tracker const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const std::string & tracker_param_file, const std::uint8_t & label); - void loadDefaultModelParameters(const std::string & path); + static void loadDefaultModelParameters(const std::string & path); bool predict(const rclcpp::Time & time) override; bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp index 7da940912a253..70045e6b16a07 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp @@ -15,6 +15,7 @@ #ifndef RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ #define RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#include "model/constant_turn_rate_motion_tracker.hpp" #include "model/linear_motion_tracker.hpp" #include "model/tracker_base.hpp" diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml index e2d30c1b69d19..6e6813d3e40ff 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -4,8 +4,9 @@ - + + @@ -17,6 +18,7 @@ + diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp index 6b02836e1f933..6d801302ab6c5 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -207,6 +207,7 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_ // Parameters tracker_lifetime_ = declare_parameter("tracker_lifetime"); double publish_rate = declare_parameter("publish_rate"); + measurement_count_threshold_ = declare_parameter("measurement_count_threshold"); world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; tracker_config_directory_ = declare_parameter("tracking_config_directory"); @@ -363,6 +364,10 @@ std::shared_ptr RadarObjectTrackerNode::createNewTracker( if (tracker == "linear_motion_tracker") { std::string config_file = tracker_config_directory_ + "linear_motion_tracker.yaml"; return std::make_shared(time, object, config_file, label); + } else if (tracker == "constant_turn_rate_motion_tracker") { + std::string config_file = + tracker_config_directory_ + "constant_turn_rate_motion_tracker.yaml"; + return std::make_shared(time, object, config_file, label); } else { // not implemented yet so put warning RCLCPP_WARN(get_logger(), "Tracker %s is not implemented yet", tracker.c_str()); @@ -530,8 +535,7 @@ void RadarObjectTrackerNode::sanitizeTracker( inline bool RadarObjectTrackerNode::shouldTrackerPublish( const std::shared_ptr tracker) const { - constexpr int measurement_count_threshold = 3; - if (tracker->getTotalMeasurementCount() < measurement_count_threshold) { + if (tracker->getTotalMeasurementCount() < measurement_count_threshold_) { return false; } return true; diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp new file mode 100644 index 0000000000000..5815fe34a68a9 --- /dev/null +++ b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -0,0 +1,624 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Yukihiro Saito +// + +#include "radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp" + +#include "radar_object_tracker/utils/utils.hpp" + +#include +#include + +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#define EIGEN_MPL2_ONLY +#include +#include +#include +#include + +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +// init static member variables +bool ConstantTurnRateMotionTracker::is_initialized_ = false; +ConstantTurnRateMotionTracker::EkfParams ConstantTurnRateMotionTracker::ekf_params_; +double ConstantTurnRateMotionTracker::max_vx_; +double ConstantTurnRateMotionTracker::filter_tau_; +double ConstantTurnRateMotionTracker::filter_dt_; +bool ConstantTurnRateMotionTracker::assume_zero_yaw_rate_; +bool ConstantTurnRateMotionTracker::trust_yaw_input_; +bool ConstantTurnRateMotionTracker::trust_twist_input_; +bool ConstantTurnRateMotionTracker::use_polar_coordinate_in_measurement_noise_; + +ConstantTurnRateMotionTracker::ConstantTurnRateMotionTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const std::string & tracker_param_file, const std::uint8_t & /*label*/) +: Tracker(time, object.classification), + logger_(rclcpp::get_logger("ConstantTurnRateMotionTracker")), + last_update_time_(time), + z_(object.kinematics.pose_with_covariance.pose.position.z) +{ + object_ = object; + + // load setting from yaml file + if (!is_initialized_) { + loadDefaultModelParameters(tracker_param_file); // currently not using label + is_initialized_ = true; + } + + // shape initialization + bounding_box_ = {0.5, 0.5, 1.7}; + cylinder_ = {0.3, 1.7}; + + // initialize X matrix and position + Eigen::MatrixXd X(ekf_params_.dim_x, 1); + X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; + X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; + const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + z_ = object.kinematics.pose_with_covariance.pose.position.z; + X(IDX::YAW) = yaw; + // radar object usually have twist + if (object.kinematics.has_twist) { + const auto v = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VX) = v; + } else { + X(IDX::VX) = 0.0; + } + // init turn rate + X(IDX::WZ) = 0.0; + + // initialize P matrix + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + + // create rotation matrix to rotate covariance matrix + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + // 2d rotation matrix + Eigen::Matrix2d R; + R << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; + + // covariance matrix in the target vehicle coordinate system + Eigen::Matrix2d P_xy_local; + P_xy_local << ekf_params_.p0_cov_x, 0.0, 0.0, ekf_params_.p0_cov_y; + + // Rotated covariance matrix + // covariance is rotated by 2D rotation matrix R + // P′=R P RT + Eigen::Matrix2d P_xy, P_v_xy; + + // Rotate the covariance matrix according to the vehicle yaw + // because p0_cov_x and y are in the vehicle coordinate system. + if (object.kinematics.has_position_covariance) { + P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P_xy = R * P_xy_local * R.transpose(); + } else { + // rotate + P_xy = R * P_xy_local * R.transpose(); + } + // put value in P matrix + P.block<2, 2>(IDX::X, IDX::X) = P_xy; + + // covariance often written in object frame + if (object.kinematics.has_twist_covariance) { + const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + // const auto vy_cov = + // object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P(IDX::VX, IDX::VX) = vx_cov; + } else { + P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + } + + P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; + P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; + + // init shape + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + bounding_box_ = { + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; + } + + ekf_.init(X, P); +} + +void ConstantTurnRateMotionTracker::loadDefaultModelParameters(const std::string & path) +{ + YAML::Node config = YAML::LoadFile(path); + // initialize ekf params + const float q_stddev_x = + config["default"]["ekf_params"]["process_noise_std"]["x"].as(); // [m] + const float q_stddev_y = + config["default"]["ekf_params"]["process_noise_std"]["y"].as(); // [m] + const float q_stddev_yaw = + config["default"]["ekf_params"]["process_noise_std"]["yaw"].as(); // [rad] + const float q_stddev_vx = + config["default"]["ekf_params"]["process_noise_std"]["vx"].as(); // [m/s] + const float q_stddev_wz = + config["default"]["ekf_params"]["process_noise_std"]["wz"].as(); // [m/s] + const float r_stddev_x = + config["default"]["ekf_params"]["measurement_noise_std"]["x"].as(); // [m] + const float r_stddev_y = + config["default"]["ekf_params"]["measurement_noise_std"]["y"].as(); // [m] + const float r_stddev_yaw = + config["default"]["ekf_params"]["measurement_noise_std"]["yaw"].as(); // [rad] + const float r_stddev_vx = + config["default"]["ekf_params"]["measurement_noise_std"]["vx"].as(); // [m/s] + const float p0_stddev_x = + config["default"]["ekf_params"]["initial_covariance_std"]["x"].as(); // [m/s] + const float p0_stddev_y = + config["default"]["ekf_params"]["initial_covariance_std"]["y"].as(); // [m/s] + const float p0_stddev_yaw = + config["default"]["ekf_params"]["initial_covariance_std"]["yaw"].as(); // [rad] + const float p0_stddev_vx = + config["default"]["ekf_params"]["initial_covariance_std"]["vx"].as(); // [m/(s)] + const float p0_stddev_wz = + config["default"]["ekf_params"]["initial_covariance_std"]["wz"].as(); // [rad/s] + assume_zero_yaw_rate_ = config["default"]["assume_zero_yaw_rate"].as(); // default false + trust_yaw_input_ = config["default"]["trust_yaw_input"].as(); // default false + trust_twist_input_ = config["default"]["trust_twist_input"].as(); // default false + use_polar_coordinate_in_measurement_noise_ = + config["default"]["use_polar_coordinate_in_measurement_noise"].as(); // default false + ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); + ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + 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); + ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); + ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); + ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); + + // lpf filter parameters + filter_tau_ = config["default"]["low_pass_filter"]["time_constant"].as(); // [s] + filter_dt_ = config["default"]["low_pass_filter"]["sampling_time"].as(); // [s] + + // limitation + // (TODO): this may be written in another yaml file based on classify result + const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] + max_vx_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [m/s] +} + +bool ConstantTurnRateMotionTracker::predict(const rclcpp::Time & time) +{ + const double dt = (time - last_update_time_).seconds(); + bool ret = predict(dt, ekf_); + if (ret) { + last_update_time_ = time; + } + return ret; +} + +bool ConstantTurnRateMotionTracker::predict(const double dt, KalmanFilter & ekf) const +{ + /* == Nonlinear model == + * + * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt + * yaw_{k+1} = yaw_k + (wz_k) * dt + * vx_{k+1} = vx_k + * wz_{k+1} = wz_k + * + */ + + /* == Linearized model == + * + * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] + * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] + * [ 0, 0, 1, 0, dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] + */ + + const double yaw_rate_coeff = assume_zero_yaw_rate_ ? 0.0 : 1.0; + + // X t + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf.getX(X_t); + const auto x = X_t(IDX::X); + const auto y = X_t(IDX::Y); + const auto yaw = X_t(IDX::YAW); + const auto vx = X_t(IDX::VX); + const auto wz = X_t(IDX::WZ); + + // X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); + X_next_t(IDX::X) = x + vx * std::cos(yaw) * dt; + X_next_t(IDX::Y) = y + vx * std::sin(yaw) * dt; + X_next_t(IDX::YAW) = yaw + wz * dt * yaw_rate_coeff; + X_next_t(IDX::VX) = vx; + X_next_t(IDX::WZ) = wz * yaw_rate_coeff; + + // A: state transition matrix + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); + A(IDX::X, IDX::YAW) = -vx * std::sin(yaw) * dt; + A(IDX::Y, IDX::YAW) = vx * std::cos(yaw) * dt; + A(IDX::X, IDX::VX) = std::cos(yaw) * dt; + A(IDX::Y, IDX::VX) = std::sin(yaw) * dt; + A(IDX::YAW, IDX::WZ) = dt * yaw_rate_coeff; + + // Q: system noise + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + + // Rotate the covariance matrix according to the vehicle yaw + // because q_cov_x and y are in the vehicle coordinate system. + Eigen::MatrixXd Q_xy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd Q_xy_global = Eigen::MatrixXd::Zero(2, 2); + Q_xy_local << ekf_params_.q_cov_x, 0.0, 0.0, ekf_params_.q_cov_y; + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(2, 2); + R << cos(yaw), -sin(yaw), sin(yaw), cos(yaw); + Q_xy_global = R * Q_xy_local * R.transpose(); + Q.block<2, 2>(IDX::X, IDX::X) = Q_xy_global; + + Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw; + Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx; + Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz; + + // may be unused + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + + // call kalman filter library + if (!ekf.predict(X_next_t, A, Q)) { + RCLCPP_WARN(logger_, "Cannot predict"); + } + + return true; +} + +bool ConstantTurnRateMotionTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) +{ + // Observation pattern will be: + // 1. x, y, vx, vy + // 2. x, y + // 3. vx, vy (Do not consider this right now) + // + // We handle this measurements by stacking observation matrix, measurement vector and measurement + // covariance + // - observation matrix: C + // - measurement vector : Y + // - measurement covariance: R + + // get current state + Eigen::MatrixXd X(ekf_params_.dim_x, 1); + ekf_.getX(X); + const auto yaw_state = X(IDX::YAW); + + // rotation matrix + Eigen::Matrix2d RotationYaw; + if (trust_yaw_input_) { + const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + RotationYaw << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + } else { + RotationYaw << std::cos(yaw_state), -std::sin(yaw_state), std::sin(yaw_state), + std::cos(yaw_state); + } + const auto base_link_yaw = tf2::getYaw(self_transform.rotation); + Eigen::Matrix2d RotationBaseLink; + RotationBaseLink << std::cos(base_link_yaw), -std::sin(base_link_yaw), std::sin(base_link_yaw), + std::cos(base_link_yaw); + + // depth from base_link to object + const auto dx = + object.kinematics.pose_with_covariance.pose.position.x - self_transform.translation.x; + const auto dy = + object.kinematics.pose_with_covariance.pose.position.y - self_transform.translation.y; + Eigen::Vector2d pose_diff_in_map(dx, dy); + Eigen::Vector2d pose_diff_in_base_link = RotationBaseLink * pose_diff_in_map; + const auto depth = abs(pose_diff_in_base_link(0)); + + // gather matrices as vector + std::vector C_list; + std::vector Y_list; + std::vector R_block_list; + + // 1. add position measurement + const bool enable_position_measurement = true; // assume position is always measured + if (enable_position_measurement) { + Eigen::MatrixXd Cxy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); + Cxy(0, IDX::X) = 1; + Cxy(1, IDX::Y) = 1; + C_list.push_back(Cxy); + + Eigen::MatrixXd Yxy = Eigen::MatrixXd::Zero(2, 1); + Yxy << object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y; + Y_list.push_back(Yxy); + + // covariance need to be rotated since it is in the vehicle coordinate system + Eigen::MatrixXd Rxy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); + if (!object.kinematics.has_position_covariance) { + // switch noise covariance in polar coordinate or cartesian coordinate + const auto r_cov_y = use_polar_coordinate_in_measurement_noise_ + ? depth * depth * ekf_params_.r_cov_x + : ekf_params_.r_cov_y; + Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate + Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); + } else { + Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + object.kinematics.pose_with_covariance + .covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate + Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); + } + R_block_list.push_back(Rxy); + } + + // 2. add yaw measurement + const bool object_has_orientation = object.kinematics.orientation_availability > + 0; // 0: not available, 1: sign unknown, 2: available + const bool enable_yaw_measurement = trust_yaw_input_ && object_has_orientation; + + if (enable_yaw_measurement) { + Eigen::MatrixXd Cyaw = Eigen::MatrixXd::Zero(1, ekf_params_.dim_x); + Cyaw(0, IDX::YAW) = 1; + C_list.push_back(Cyaw); + + Eigen::MatrixXd Yyaw = Eigen::MatrixXd::Zero(1, 1); + const auto yaw = [&] { + auto obj_yaw = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); + while (M_PI_2 <= yaw_state - obj_yaw) { + obj_yaw = obj_yaw + M_PI; + } + while (M_PI_2 <= obj_yaw - yaw_state) { + obj_yaw = obj_yaw - M_PI; + } + return obj_yaw; + }(); + + Yyaw << yaw; + Y_list.push_back(Yyaw); + + Eigen::MatrixXd Ryaw = Eigen::MatrixXd::Zero(1, 1); + Ryaw << ekf_params_.r_cov_yaw; + R_block_list.push_back(Ryaw); + } + + // 3. add linear velocity measurement + const bool enable_velocity_measurement = object.kinematics.has_twist && trust_twist_input_; + if (enable_velocity_measurement) { + Eigen::MatrixXd C_vx = Eigen::MatrixXd::Zero(1, ekf_params_.dim_x); + C_vx(0, IDX::VX) = 1; + C_list.push_back(C_vx); + + // measure absolute velocity + Eigen::MatrixXd Vx = Eigen::MatrixXd::Zero(1, 1); + Vx << object.kinematics.twist_with_covariance.twist.linear.x; + + Eigen::Matrix2d R_vx = Eigen::MatrixXd::Zero(1, 1); + if (!object.kinematics.has_twist_covariance) { + R_vx << ekf_params_.r_cov_vx; + } else { + R_vx << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + R_block_list.push_back(R_vx); + } + + // 4. sum up matrices + const auto row_number = std::accumulate( + C_list.begin(), C_list.end(), 0, + [](const auto & sum, const auto & C) { return sum + C.rows(); }); + if (row_number == 0) { + RCLCPP_WARN(logger_, "No measurement is available"); + return false; + } + + // stacking matrices vertically or diagonally + const auto C = utils::stackMatricesVertically(C_list); + const auto Y = utils::stackMatricesVertically(Y_list); + const auto R = utils::stackMatricesDiagonally(R_block_list); + + // 4. EKF update + if (!ekf_.update(Y, C, R)) { + RCLCPP_WARN(logger_, "Cannot update"); + } + + // 5. normalize: limit vx + { + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); + ekf_.getX(X_t); + ekf_.getP(P_t); + if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { + X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + } + ekf_.init(X_t, P_t); + } + + // 6. Filter z + // first order low pass filter + const float gain = filter_tau_ / (filter_tau_ + filter_dt_); + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + return true; +} + +bool ConstantTurnRateMotionTracker::measureWithShape( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // just use first order low pass filter + const float gain = filter_tau_ / (filter_tau_ + filter_dt_); + + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; + bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + cylinder_.width = gain * cylinder_.width + (1.0 - gain) * object.shape.dimensions.x; + cylinder_.height = gain * cylinder_.height + (1.0 - gain) * object.shape.dimensions.z; + } else { + return false; + } + + return true; +} + +bool ConstantTurnRateMotionTracker::measure( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) +{ + const auto & current_classification = getClassification(); + object_ = object; + if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + setClassification(current_classification); + } + + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + RCLCPP_WARN( + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); + } + + measureWithPose(object, self_transform); + measureWithShape(object); + + // (void)self_transform; // currently do not use self vehicle position + return true; +} + +bool ConstantTurnRateMotionTracker::getTrackedObject( + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const +{ + object = object_recognition_utils::toTrackedObject(object_); + object.object_id = getUUID(); + object.classification = getClassification(); + + // predict kinematics + KalmanFilter tmp_ekf_for_no_update = ekf_; + const double dt = (time - last_update_time_).seconds(); + if (0.001 /*1msec*/ < dt) { + predict(dt, tmp_ekf_for_no_update); + } + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state + tmp_ekf_for_no_update.getX(X_t); + tmp_ekf_for_no_update.getP(P); + + auto & pose_with_cov = object.kinematics.pose_with_covariance; + auto & twist_with_cov = object.kinematics.twist_with_covariance; + // rotation matrix with yaw_ + Eigen::Matrix2d R_yaw = Eigen::Matrix2d::Zero(); + R_yaw << std::cos(X_t(IDX::YAW)), -std::sin(X_t(IDX::YAW)), std::sin(X_t(IDX::YAW)), + std::cos(X_t(IDX::YAW)); + const Eigen::Matrix2d R_yaw_inv = R_yaw.transpose(); + + // position + pose_with_cov.pose.position.x = X_t(IDX::X); + pose_with_cov.pose.position.y = X_t(IDX::Y); + pose_with_cov.pose.position.z = z_; + // quaternion + { + double roll, pitch, yaw; + tf2::Quaternion original_quaternion; + tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); + tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); + tf2::Quaternion filtered_quaternion; + filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); + pose_with_cov.pose.orientation.x = filtered_quaternion.x(); + pose_with_cov.pose.orientation.y = filtered_quaternion.y(); + pose_with_cov.pose.orientation.z = filtered_quaternion.z(); + pose_with_cov.pose.orientation.w = filtered_quaternion.w(); + if (trust_yaw_input_) { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + } else { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + } + } + // twist + // twist need to converted to local coordinate + const auto vx = X_t(IDX::VX); + twist_with_cov.twist.linear.x = vx; + + // ===== covariance transformation ===== + // since covariance in EKF is in map coordinate and output should be in object coordinate, + // we need to transform covariance + Eigen::Matrix2d P_xy_map; + P_xy_map << P(IDX::X, IDX::X), P(IDX::X, IDX::Y), P(IDX::Y, IDX::X), P(IDX::Y, IDX::Y); + + // rotate covariance with -yaw + Eigen::Matrix2d P_xy = R_yaw_inv * P_xy_map * R_yaw_inv.transpose(); + + // position covariance + constexpr double no_info_cov = 1e9; // no information + constexpr double z_cov = 1. * 1.; // TODO(yukkysaito) Currently tentative + constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + + pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); + pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; + pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; + pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; + pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + + // twist covariance + constexpr double vz_cov = 0.2 * 0.2; // TODO(Yoshi Ri) Currently tentative + constexpr double wz_cov = 0.2 * 0.2; // TODO(yukkysaito) Currently tentative + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = 0.0; + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = 0.0; + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = no_info_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + + // set shape + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + object.shape.dimensions.x = bounding_box_.length; + object.shape.dimensions.y = bounding_box_.width; + object.shape.dimensions.z = bounding_box_.height; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + object.shape.dimensions.x = cylinder_.width; + object.shape.dimensions.y = cylinder_.width; + object.shape.dimensions.z = cylinder_.height; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); + const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); + object.shape.footprint = + tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + } + + return true; +} diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index f6814ceb8c246..8369d04fbe225 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -44,9 +44,21 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; +// initialize static parameter +bool LinearMotionTracker::is_initialized_ = false; +LinearMotionTracker::EkfParams LinearMotionTracker::ekf_params_; +double LinearMotionTracker::max_vx_; +double LinearMotionTracker::max_vy_; +double LinearMotionTracker::filter_tau_; +double LinearMotionTracker::filter_dt_; +bool LinearMotionTracker::estimate_acc_; +bool LinearMotionTracker::trust_yaw_input_; +bool LinearMotionTracker::trust_twist_input_; +bool LinearMotionTracker::use_polar_coordinate_in_measurement_noise_; + LinearMotionTracker::LinearMotionTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const std::string & /*tracker_param_file*/, const std::uint8_t & /*label*/) + const std::string & tracker_param_file, const std::uint8_t & /*label*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("LinearMotionTracker")), last_update_time_(time), @@ -55,11 +67,13 @@ LinearMotionTracker::LinearMotionTracker( object_ = object; // load setting from yaml file - const std::string default_setting_file = - ament_index_cpp::get_package_share_directory("radar_object_tracker") + - "/config/tracking/linear_motion_tracker.yaml"; - loadDefaultModelParameters(default_setting_file); - // loadModelParameters(tracker_param_file, label); + if (!is_initialized_) { + loadDefaultModelParameters(tracker_param_file); + is_initialized_ = true; + } + // shape initialization + bounding_box_ = {0.5, 0.5, 1.7}; + cylinder_ = {0.3, 1.7}; // initialize X matrix and position Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -193,6 +207,12 @@ void LinearMotionTracker::loadDefaultModelParameters(const std::string & path) config["default"]["ekf_params"]["initial_covariance_std"]["ax"].as(); // [m/(s*s)] const float p0_stddev_ay = config["default"]["ekf_params"]["initial_covariance_std"]["ay"].as(); // [m/(s*s)] + estimate_acc_ = config["default"]["ekf_params"]["estimate_acc"].as(); + trust_yaw_input_ = config["default"]["trust_yaw_input"].as(false); // default false + trust_twist_input_ = config["default"]["trust_twist_input"].as(false); // default false + use_polar_coordinate_in_measurement_noise_ = + config["default"]["use_polar_coordinate_in_measurement_noise"].as( + false); // default false ekf_params_.q_cov_ax = std::pow(q_stddev_ax, 2.0); ekf_params_.q_cov_ay = std::pow(q_stddev_ay, 2.0); ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); @@ -219,11 +239,6 @@ void LinearMotionTracker::loadDefaultModelParameters(const std::string & path) const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] max_vx_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [m/s] max_vy_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [rad/s] - - // shape initialization - // (TODO): this may be written in another yaml file based on classify result - bounding_box_ = {0.5, 0.5, 1.7}; - cylinder_ = {0.3, 1.7}; } bool LinearMotionTracker::predict(const rclcpp::Time & time) @@ -257,6 +272,8 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const * 0, 0, 0, 0, 1, 0, * 0, 0, 0, 0, 0, 1] */ + // estimate acc + const double acc_coeff = estimate_acc_ ? 1.0 : 0.0; // X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state @@ -270,10 +287,10 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const // X t+1 Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); - X_next_t(IDX::X) = x + vx * dt + 0.5 * ax * dt * dt; - X_next_t(IDX::Y) = y + vy * dt + 0.5 * ay * dt * dt; - X_next_t(IDX::VX) = vx + ax * dt; - X_next_t(IDX::VY) = vy + ay * dt; + X_next_t(IDX::X) = x + vx * dt + 0.5 * ax * dt * dt * acc_coeff; + X_next_t(IDX::Y) = y + vy * dt + 0.5 * ay * dt * dt * acc_coeff; + X_next_t(IDX::VX) = vx + ax * dt * acc_coeff; + X_next_t(IDX::VY) = vy + ay * dt * acc_coeff; X_next_t(IDX::AX) = ax; X_next_t(IDX::AY) = ay; @@ -281,10 +298,10 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); A(IDX::X, IDX::VX) = dt; A(IDX::Y, IDX::VY) = dt; - A(IDX::X, IDX::AX) = 0.5 * dt * dt; - A(IDX::Y, IDX::AY) = 0.5 * dt * dt; - A(IDX::VX, IDX::AX) = dt; - A(IDX::VY, IDX::AY) = dt; + A(IDX::X, IDX::AX) = 0.5 * dt * dt * acc_coeff; + A(IDX::Y, IDX::AY) = 0.5 * dt * dt * acc_coeff; + A(IDX::VX, IDX::AX) = dt * acc_coeff; + A(IDX::VY, IDX::AY) = dt * acc_coeff; // Q: system noise Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -333,7 +350,8 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const } bool LinearMotionTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) { // Observation pattern will be: // 1. x, y, vx, vy @@ -348,8 +366,25 @@ bool LinearMotionTracker::measureWithPose( // rotation matrix Eigen::Matrix2d RotationYaw; - const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - RotationYaw << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + if (trust_yaw_input_) { + const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + RotationYaw << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + } else { + RotationYaw << std::cos(yaw_), -std::sin(yaw_), std::sin(yaw_), std::cos(yaw_); + } + const auto base_link_yaw = tf2::getYaw(self_transform.rotation); + Eigen::Matrix2d RotationBaseLink; + RotationBaseLink << std::cos(base_link_yaw), -std::sin(base_link_yaw), std::sin(base_link_yaw), + std::cos(base_link_yaw); + + // depth from base_link to object + const auto dx = + object.kinematics.pose_with_covariance.pose.position.x - self_transform.translation.x; + const auto dy = + object.kinematics.pose_with_covariance.pose.position.y - self_transform.translation.y; + Eigen::Vector2d pose_diff_in_map(dx, dy); + Eigen::Vector2d pose_diff_in_base_link = RotationBaseLink * pose_diff_in_map; + const auto depth = abs(pose_diff_in_base_link(0)); // gather matrices as vector std::vector C_list; @@ -371,21 +406,27 @@ bool LinearMotionTracker::measureWithPose( // covariance need to be rotated since it is in the vehicle coordinate system Eigen::MatrixXd Rxy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); if (!object.kinematics.has_position_covariance) { - Rxy_local << ekf_params_.r_cov_x, 0, 0, ekf_params_.r_cov_y; + // switch noise covariance in polar coordinate or cartesian coordinate + const auto r_cov_y = use_polar_coordinate_in_measurement_noise_ + ? depth * depth * ekf_params_.r_cov_x + : ekf_params_.r_cov_y; + Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate + Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); } else { Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + object.kinematics.pose_with_covariance + .covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate + Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); } - Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); - Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); R_block_list.push_back(Rxy); } // 2. add linear velocity measurement - const bool enable_velocity_measurement = object.kinematics.has_twist; + const bool enable_velocity_measurement = object.kinematics.has_twist && trust_twist_input_; if (enable_velocity_measurement) { Eigen::MatrixXd C_vx_vy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); C_vx_vy(0, IDX::VX) = 1; @@ -401,14 +442,15 @@ bool LinearMotionTracker::measureWithPose( Y_list.push_back(Vxy); Eigen::Matrix2d R_v_xy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd R_v_xy = Eigen::MatrixXd::Zero(2, 2); if (!object.kinematics.has_twist_covariance) { R_v_xy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy; + R_v_xy = RotationBaseLink * R_v_xy_local * RotationBaseLink.transpose(); } else { R_v_xy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X], 0, 0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose(); } - Eigen::MatrixXd R_v_xy = Eigen::MatrixXd::Zero(2, 2); - R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose(); R_block_list.push_back(R_v_xy); } @@ -453,8 +495,16 @@ bool LinearMotionTracker::measureWithPose( // first order low pass filter const float gain = filter_tau_ / (filter_tau_ + filter_dt_); z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - yaw_ = gain * yaw_ + (1.0 - gain) * yaw; - + // get yaw from twist atan + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + const auto twist_yaw = + std::atan2(X_t(IDX::VY), X_t(IDX::VX)); // calc from lateral and longitudinal velocity + if (trust_yaw_input_) { + yaw_ = gain * yaw_ + (1.0 - gain) * twist_yaw; + } else { + yaw_ = twist_yaw; + } return true; } @@ -494,10 +544,10 @@ bool LinearMotionTracker::measure( (time - last_update_time_).seconds()); } - measureWithPose(object); + measureWithPose(object, self_transform); measureWithShape(object); - (void)self_transform; // currently do not use self vehicle position + // (void)self_transform; // currently do not use self vehicle position return true; } @@ -543,8 +593,13 @@ bool LinearMotionTracker::getTrackedObject( pose_with_cov.pose.orientation.y = filtered_quaternion.y(); pose_with_cov.pose.orientation.z = filtered_quaternion.z(); pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + if (trust_yaw_input_) { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + } else { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + } } // twist // twist need to converted to local coordinate