diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index f5fbc8ff950e8..94687dd6b61c7 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -22,6 +22,12 @@ include_directories( # Generate exe file set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp + src/data_association/data_association.cpp + src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp + src/tracker/motion_model/motion_model_base.cpp + src/tracker/motion_model/bicycle_motion_model.cpp + src/tracker/motion_model/ctrv_motion_model.cpp + src/tracker/motion_model/cv_motion_model.cpp src/tracker/model/tracker_base.cpp src/tracker/model/big_vehicle_tracker.cpp src/tracker/model/normal_vehicle_tracker.cpp @@ -31,8 +37,6 @@ set(MULTI_OBJECT_TRACKER_SRC src/tracker/model/pedestrian_and_bicycle_tracker.cpp src/tracker/model/unknown_tracker.cpp src/tracker/model/pass_through_tracker.cpp - src/data_association/data_association.cpp - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp ) ament_auto_add_library(multi_object_tracker_node SHARED diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index ba50933eddaec..2d69334a2f43f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -20,6 +20,7 @@ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include @@ -30,36 +31,15 @@ class BicycleTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { - char dim_x = 5; - 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 p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; } ekf_params_; - double max_vel_; - double max_slip_; - double lf_; - double lr_; - float z_; + double z_; -private: struct BoundingBox { double length; @@ -67,7 +47,11 @@ class BicycleTracker : public Tracker double height; }; BoundingBox bounding_box_; - BoundingBox last_input_bounding_box_; + +private: + BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: BicycleTracker( @@ -75,10 +59,12 @@ class BicycleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); 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; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index e19b6382ad952..8d49138b94a24 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -20,47 +20,28 @@ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include + class BigVehicleTracker : public Tracker { private: autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; - int last_nearest_corner_index_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { - char dim_x = 5; - 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_vel; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; + double r_cov_vel; } ekf_params_; - double max_vel_; - double max_slip_; - double lf_; - double lr_; - float z_; double velocity_deviation_threshold_; -private: + double z_; + struct BoundingBox { double length; @@ -70,6 +51,12 @@ class BigVehicleTracker : public Tracker BoundingBox bounding_box_; BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; + int last_nearest_corner_index_; + +private: + BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: BigVehicleTracker( @@ -77,18 +64,28 @@ class BigVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); 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; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); 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; - double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); virtual ~BigVehicleTracker() {} + +private: + void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) + { + Eigen::MatrixXd X_t(DIM, 1); + motion_model_.getStateVector(X_t); + last_nearest_corner_index_ = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); + } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 05fa0a5ef01ba..6ab7e63bce167 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -19,7 +19,8 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ -#include "tracker_base.hpp" +#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include @@ -28,42 +29,19 @@ class NormalVehicleTracker : public Tracker private: autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; - int last_nearest_corner_index_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; - struct EkfParams { - char dim_x = 5; - 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_vel; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; + double r_cov_vel; } ekf_params_; - - double max_vel_; - double max_slip_; - double lf_; - double lr_; - float z_; double velocity_deviation_threshold_; -private: + double z_; + struct BoundingBox { double length; @@ -73,6 +51,12 @@ class NormalVehicleTracker : public Tracker BoundingBox bounding_box_; BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; + int last_nearest_corner_index_; + +private: + BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: NormalVehicleTracker( @@ -80,18 +64,28 @@ class NormalVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); 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; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); 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; - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); - double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); virtual ~NormalVehicleTracker() {} + +private: + void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) + { + Eigen::MatrixXd X_t(DIM, 1); + motion_model_.getStateVector(X_t); + last_nearest_corner_index_ = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); + } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 0219d3b227044..c81f594ae461a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -20,6 +20,7 @@ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" #include @@ -30,38 +31,15 @@ class PedestrianTracker : public Tracker 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 { - char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_wz; - float q_cov_vx; - float p0_cov_vx; - float p0_cov_wz; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; } ekf_params_; - double max_vx_; - double max_wz_; - float z_; + double z_; -private: struct BoundingBox { double length; @@ -76,16 +54,23 @@ class PedestrianTracker : public Tracker BoundingBox bounding_box_; Cylinder cylinder_; +private: + CTRVMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = CTRVMotionModel::IDX; + public: PedestrianTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform); 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; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index 18e6c0606a535..73bf7849e13d8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -13,13 +13,14 @@ // limitations under the License. // // -// v1.0 Yukihiro Saito +// Author: v1.0 Yukihiro Saito // #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#include "tracker_base.hpp" +#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include @@ -30,30 +31,20 @@ class UnknownTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - VX = 2, - VY = 3, - }; struct EkfParams { - char dim_x = 4; - float q_cov_x; - float q_cov_y; - float q_cov_vx; - float q_cov_vy; - float p0_cov_vx; - float p0_cov_vy; - float r_cov_x; - float r_cov_y; - float p0_cov_x; - float p0_cov_y; + double r_cov_x; + double r_cov_y; + double r_cov_vx; + double r_cov_vy; } ekf_params_; - float max_vx_, max_vy_; - float z_; + + double z_; + +private: + CVMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = CVMotionModel::IDX; public: UnknownTracker( @@ -61,10 +52,12 @@ class UnknownTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); 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; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp new file mode 100644 index 0000000000000..5127d0448835c --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -0,0 +1,108 @@ +// Copyright 2024 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 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +class BicycleMotionModel : public MotionModel +{ +private: + // attributes + rclcpp::Logger logger_; + + // extended state + double lf_; + double lr_; + + // motion parameters + struct MotionParams + { + double q_stddev_acc_long; + double q_stddev_acc_lat; + double q_stddev_yaw_rate_min; + double q_stddev_yaw_rate_max; + double q_cov_slip_rate_min; + double q_cov_slip_rate_max; + double q_max_slip_angle; + double lf_ratio; + double lr_ratio; + double lf_min; + double lr_min; + double max_vel; + double max_slip; + } motion_params_; + +public: + BicycleMotionModel(); + + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; + const char DIM = 5; + + bool initialize( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & slip, const double & slip_cov, const double & length); + + void setDefaultParams(); + + void setMotionParams( + const double & q_stddev_acc_long, const double & q_stddev_acc_lat, + const double & q_stddev_yaw_rate_min, const double & q_stddev_yaw_rate_max, + const double & q_stddev_slip_rate_min, const double & q_stddev_slip_rate_max, + const double & q_max_slip_angle, const double & lf_ratio, const double & lf_min, + const double & lr_ratio, const double & lr_min); + + void setMotionLimits(const double & max_vel, const double & max_slip); + + bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); + + bool updateStatePoseHead( + const double & x, const double & y, const double & yaw, + const std::array & pose_cov); + + bool updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov); + + bool adjustPosition(const double & x, const double & y); + + bool limitStates(); + + bool updateExtendedState(const double & length); + + bool predictStateStep(const double dt, KalmanFilter & ekf) const override; + + bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp new file mode 100644 index 0000000000000..560d165bba1b5 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -0,0 +1,93 @@ +// Copyright 2024 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 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +class CTRVMotionModel : public MotionModel +{ +private: + // attributes + rclcpp::Logger logger_; + + // motion parameters + struct MotionParams + { + double q_cov_x; + double q_cov_y; + double q_cov_yaw; + double q_cov_vel; + double q_cov_wz; + double max_vel; + double max_wz; + } motion_params_; + +public: + CTRVMotionModel(); + + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, WZ = 4 }; + const char DIM = 5; + + bool initialize( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & wz, const double & wz_cov); + + void setDefaultParams(); + + void setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_yaw, + const double & q_stddev_vx, const double & q_stddev_wz); + + void setMotionLimits(const double & max_vel, const double & max_wz); + + bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); + + bool updateStatePoseHead( + const double & x, const double & y, const double & yaw, + const std::array & pose_cov); + + bool updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov); + + bool adjustPosition(const double & x, const double & y); + + bool limitStates(); + + bool predictStateStep(const double dt, KalmanFilter & ekf) const override; + + bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp new file mode 100644 index 0000000000000..59032706b00d6 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -0,0 +1,88 @@ +// Copyright 2024 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 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +class CVMotionModel : public MotionModel +{ +private: + // attributes + rclcpp::Logger logger_; + + // motion parameters + struct MotionParams + { + double q_cov_x; + double q_cov_y; + double q_cov_vx; + double q_cov_vy; + double max_vx; + double max_vy; + } motion_params_; + +public: + CVMotionModel(); + + enum IDX { X = 0, Y = 1, VX = 2, VY = 3 }; + const char DIM = 4; + + bool initialize( + const rclcpp::Time & time, const double & x, const double & y, + const std::array & pose_cov, const double & vx, const double & vy, + const std::array & twist_cov); + + void setDefaultParams(); + + void setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, + const double & q_stddev_vy); + + void setMotionLimits(const double & max_vx, const double & max_vy); + + bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); + + bool updateStatePoseVel( + const double & x, const double & y, const std::array & pose_cov, const double & vx, + const double & vy, const std::array & twist_cov); + + bool adjustPosition(const double & x, const double & y); + + bool limitStates(); + + bool predictStateStep(const double dt, KalmanFilter & ekf) const override; + + bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp new file mode 100644 index 0000000000000..1aca602ed66a3 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp @@ -0,0 +1,67 @@ +// Copyright 2024 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 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +class MotionModel +{ +private: + bool is_initialized_{false}; + double dt_max_{0.11}; // [s] maximum time interval for prediction + +protected: + rclcpp::Time last_update_time_; + KalmanFilter ekf_; + +public: + MotionModel(); + virtual ~MotionModel() = default; + + bool checkInitialized() const { return is_initialized_; } + double getDeltaTime(const rclcpp::Time & time) const + { + return (time - last_update_time_).seconds(); + } + void setMaxDeltaTime(const double dt_max) { dt_max_ = dt_max; } + double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } + void getStateVector(Eigen::MatrixXd & X) const { ekf_.getX(X); } + + bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); + + bool predictState(const rclcpp::Time & time); + bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; + + virtual bool predictStateStep(const double dt, KalmanFilter & ekf) const = 0; + virtual bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const = 0; +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index d2b6ee5de475e..b19b0ba7ee10c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -343,6 +343,32 @@ inline void convertConvexHullToBoundingBox( output_object.shape.dimensions.z = height; } +inline bool getMeasurementYaw( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const double & predicted_yaw, + double & measurement_yaw) +{ + measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + // check orientation sign is known or not, and fix the limiting delta yaw + double limiting_delta_yaw = M_PI_2; + if ( + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + limiting_delta_yaw = M_PI; + } + // limiting delta yaw, even the availability is unknown + while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { + if (measurement_yaw < predicted_yaw) { + measurement_yaw += 2 * limiting_delta_yaw; + } else { + measurement_yaw -= 2 * limiting_delta_yaw; + } + } + // return false if the orientation is unknown + return object.kinematics.orientation_availability != + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; +} + } // namespace utils #endif // MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index d955745bb8c47..cc662768ce197 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -256,7 +256,7 @@ void MultiObjectTracker::onMeasurement( return; } /* tracker prediction */ - rclcpp::Time measurement_time = input_objects_msg->header.stamp; + const rclcpp::Time measurement_time = input_objects_msg->header.stamp; for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { (*itr)->predict(measurement_time); } @@ -330,7 +330,7 @@ std::shared_ptr MultiObjectTracker::createNewTracker( void MultiObjectTracker::onTimer() { - rclcpp::Time current_time = this->now(); + const rclcpp::Time current_time = this->now(); const auto self_transform = getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); if (!self_transform) { @@ -373,10 +373,10 @@ void MultiObjectTracker::sanitizeTracker( /* delete collision tracker */ for (auto itr1 = list_tracker.begin(); itr1 != list_tracker.end(); ++itr1) { autoware_auto_perception_msgs::msg::TrackedObject object1; - (*itr1)->getTrackedObject(time, object1); + if (!(*itr1)->getTrackedObject(time, object1)) continue; for (auto itr2 = std::next(itr1); itr2 != list_tracker.end(); ++itr2) { autoware_auto_perception_msgs::msg::TrackedObject object2; - (*itr2)->getTrackedObject(time, object2); + if (!(*itr2)->getTrackedObject(time, object2)) continue; const double distance = std::hypot( object1.kinematics.pose_with_covariance.pose.position.x - object2.kinematics.pose_with_covariance.pose.position.x, @@ -457,12 +457,12 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { if (!shouldTrackerPublish(*itr)) { // for debug purpose autoware_auto_perception_msgs::msg::TrackedObject object; - (*itr)->getTrackedObject(time, object); + if (!(*itr)->getTrackedObject(time, object)) continue; tentative_objects_msg.objects.push_back(object); continue; } autoware_auto_perception_msgs::msg::TrackedObject object; - (*itr)->getTrackedObject(time, object); + if (!(*itr)->getTrackedObject(time, object)) continue; output_msg.objects.push_back(object); } diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 2d732db6bb709..12c340516c6c1 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -47,93 +47,20 @@ BicycleTracker::BicycleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BicycleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - float r_stddev_x = 0.5; // in vehicle coordinate [m] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + double r_stddev_x = 0.5; // in vehicle coordinate [m] + double r_stddev_y = 0.4; // in vehicle coordinate [m] + double r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // in map coordinate [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); - // initial state covariance - float p0_stddev_x = 0.8; // [m] - float p0_stddev_y = 0.5; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - 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_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(1); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - - // initialize state vector X - 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; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } + // OBJECT SHAPE MODEL 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}; @@ -145,316 +72,207 @@ BicycleTracker::BicycleTracker( bounding_box_.width = std::max(bounding_box_.width, 0.3); bounding_box_.height = std::max(bounding_box_.height, 0.3); - ekf_.init(X, P); - - // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m - lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m -} - -bool BicycleTracker::predict(const rclcpp::Time & time) -{ - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); - return false; - } - // if dt is too large, shorten dt and repeat prediction - const double dt_max = 0.11; - const uint32_t repeat = std::ceil(dt / dt_max); - const double dt_ = dt / repeat; - bool ret = false; - for (uint32_t i = 0; i < repeat; ++i) { - ret = predict(dt_, ekf_); - if (!ret) { - return false; - } - } - if (ret) { - last_update_time_ = time; + // Set motion model parameters + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 5.0; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 1.0; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 0.3; // [m] minimum front wheel position + constexpr double lr_ratio = 0.3; // [-] ratio of rear wheel position + constexpr double lr_min = 0.3; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); } - return ret; -} -bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - 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. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // 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); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); + // Set motion limits + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(80); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } - return true; -} + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double vel_cov; + const double & length = bounding_box_.length; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } -bool BicycleTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - // yaw measurement - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - - // prediction - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - - // validate if orientation is available - bool use_orientation_information = false; - if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { // has 180 degree - // uncertainty - // fix orientation - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 0.8; // in object coordinate [m] + constexpr double p0_stddev_y = 0.5; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } - use_orientation_information = true; - } else if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { // know full angle + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); - use_orientation_information = true; + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } +} - const int dim_y = - use_orientation_information ? 3 : 2; // pos x, pos y, (pos yaw) depending on Pose output +bool BicycleTracker::predict(const rclcpp::Time & time) +{ + return motion_model_.predictState(time); +} - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y +autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) +{ + autoware_auto_perception_msgs::msg::DetectedObject updating_object; - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x + // OBJECT SHAPE MODEL + // convert to bounding box if input is convex shape + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + utils::convertConvexHullToBoundingBox(object, updating_object); } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + updating_object = object; } - // if there are orientation available - if (dim_y == 3) { - // fill yaw observation and measurement matrix - Y(IDX::YAW, 0) = measurement_yaw; - C(2, IDX::YAW) = 1.0; - - // fill yaw covariance - if (!object.kinematics.has_position_covariance) { - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + // fill covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + pose_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_x; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0; // x - y + pose_cov[utils::MSG_COV_IDX::Y_X] = 0; // y - x + pose_cov[utils::MSG_COV_IDX::Y_Y] = ekf_params_.r_cov_y; // y - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw } - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); - } + return updating_object; +} + +bool BicycleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // get measurement yaw angle to update + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); + double measurement_yaw = 0.0; + bool is_yaw_available = utils::getMeasurementYaw(object, tracked_yaw, measurement_yaw); - // normalize yaw and limit vel, slip + // update + bool is_updated = false; { - 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); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = measurement_yaw; + + if (is_yaw_available) { + is_updated = motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); + } else { + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); } - ekf_.init(X_t, P_t); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type - autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - bbox_object = object; - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - // convert cylinder to bbox - bbox_object.shape.dimensions.x = object.shape.dimensions.x; - bbox_object.shape.dimensions.y = object.shape.dimensions.x; - bbox_object.shape.dimensions.z = object.shape.dimensions.z; - } else { - utils::convertConvexHullToBoundingBox(object, bbox_object); + if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // do not update shape if the input is not a bounding box + return true; } - constexpr float gain = 0.9; - bounding_box_.length = - gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; - bounding_box_.height = - gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; + // update object size + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); bounding_box_.height = std::max(bounding_box_.height, 0.3); - // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m - lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m + // update motion model + motion_model_.updateExtendedState(bounding_box_.length); return true; } bool BicycleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "BicycleTracker::measure There is a large gap between predicted time and measurement time. " + "(%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); return true; } @@ -466,92 +284,19 @@ bool BicycleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - 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); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // 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(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(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] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - 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::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; 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 index 8516e9a8d02c9..3c73b9f19cfbb 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -47,7 +47,6 @@ BigVehicleTracker::BigVehicleTracker( const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -57,94 +56,24 @@ BigVehicleTracker::BigVehicleTracker( // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty float r_stddev_x = 0.5; // in vehicle coordinate [m] float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad] - float r_stddev_vel = 1.0; // [m/s] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] 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_vel = std::pow(r_stddev_vel, 2.0); - // initial state covariance - float p0_stddev_x = 1.5; // [m] - float p0_stddev_y = 0.5; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - 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_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize state vector X - 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; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } + // velocity deviation threshold + // if the predicted velocity is close to the observed velocity, + // the observed velocity is used as the measurement. + velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + // OBJECT SHAPE MODEL 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}; last_input_bounding_box_ = bounding_box_; } else { - // past default value - // bounding_box_ = {7.0, 2.0, 2.0}; autoware_auto_perception_msgs::msg::DetectedObject bbox_object; utils::convertConvexHullToBoundingBox(object, bbox_object); bounding_box_ = { @@ -152,208 +81,114 @@ BigVehicleTracker::BigVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - ekf_.init(X, P); - - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step - // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); bounding_box_.height = std::max(bounding_box_.height, 0.3); - // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m - lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m -} + // Set motion model parameters + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 0.3; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 1.5; // [m] minimum front wheel position + constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position + constexpr double lr_min = 1.5; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + } -bool BigVehicleTracker::predict(const rclcpp::Time & time) -{ - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); - return false; + // Set motion limits + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } - // if dt is too large, shorten dt and repeat prediction - const double dt_max = 0.11; - const uint32_t repeat = std::ceil(dt / dt_max); - const double dt_ = dt / repeat; - bool ret = false; - for (uint32_t i = 0; i < repeat; ++i) { - ret = predict(dt_, ekf_); - if (!ret) { - return false; + + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double vel_cov; + const double & length = bounding_box_.length; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; } - } - if (ret) { - last_update_time_ = time; - } - return ret; -} -bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - 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. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // 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); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 1.5; // in object coordinate [m] + constexpr double p0_stddev_y = 0.5; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); + + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - return true; + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } -bool BigVehicleTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) +bool BigVehicleTracker::predict(const rclcpp::Time & time) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - - float r_cov_x; - float r_cov_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - - if (utils::isLargeVehicleLabel(label)) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (label == Label::CAR) { - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } - - // Decide dimension of measurement vector - bool enable_velocity_measurement = false; - if (object.kinematics.has_twist) { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - const double predicted_vel = X_t(IDX::VEL); - const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + return motion_model_.predictState(time); +} - if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { - // Velocity deviation is small - enable_velocity_measurement = true; - } - } +autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) +{ + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - // pos x, pos y, yaw, vel depending on pose measurement - const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); + // current (predicted) state + const double tracked_x = motion_model_.getStateElement(IDX::X); + const double tracked_y = motion_model_.getStateElement(IDX::Y); + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); - // convert to boundingbox if input is convex shape + // OBJECT SHAPE MODEL + // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); @@ -361,113 +196,134 @@ bool BigVehicleTracker::measureWithPose( bbox_object = object; } - /* get offset measurement*/ - autoware_auto_perception_msgs::msg::DetectedObject offset_object; + // get offset measurement + int nearest_corner_index = utils::getNearestCornerOrSurface( + tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, - bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; - Y(IDX::YAW, 0) = measurement_yaw; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(measurement_yaw); - const double sin_yaw = std::sin(measurement_yaw); - const double sin_2yaw = std::sin(2.0f * measurement_yaw); - R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - R(1, 0) = R(0, 1); // y - x - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } - - // Update the velocity when necessary - if (dim_y == 4) { - Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VEL) = 1.0; // for vel + last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, + bbox_object, tracked_yaw, updating_object, tracking_offset_); - if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vel; // vel -vel + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + // measurement noise covariance + float r_cov_x; + float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (utils::isLargeVehicleLabel(label)) { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } else if (label == Label::CAR) { + // if label is changed, enlarge the measurement noise covariance + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); } else { - R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_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_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + + // fill covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + 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[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + pose_cov[utils::MSG_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + if (!is_yaw_available) { + pose_cov[utils::MSG_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value } + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel } - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); + return updating_object; +} + +bool BigVehicleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // current (predicted) state + const double tracked_vel = motion_model_.getStateElement(IDX::VEL); + + // velocity capability is checked only when the object has velocity measurement + // and the predicted velocity is close to the observed velocity + bool is_velocity_available = false; + if (object.kinematics.has_twist) { + const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { + // Velocity deviation is small + is_velocity_available = true; + } } - // normalize yaw and limit vel, slip + // update + bool is_updated = false; { - 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); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double vel = object.kinematics.twist_with_covariance.twist.linear.x; + + if (is_velocity_available) { + is_updated = motion_model_.updateStatePoseHeadVel( + x, y, yaw, object.kinematics.pose_with_covariance.covariance, vel, + object.kinematics.twist_with_covariance.covariance); + } else { + is_updated = motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); } - ekf_.init(X_t, P_t); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type - autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { - bbox_object = object; - } + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; - constexpr float gain = 0.9; - bounding_box_.length = - gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; - bounding_box_.height = - gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + // update object size + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); bounding_box_.height = std::max(bounding_box_.height, 0.3); - // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m - lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m + // update motion model + motion_model_.updateExtendedState(bounding_box_.length); + + // // update offset into object position + // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); + // // update offset + // tracking_offset_.x() = gain_inv * tracking_offset_.x(); + // tracking_offset_.y() = gain_inv * tracking_offset_.y(); return true; } @@ -476,20 +332,30 @@ bool BigVehicleTracker::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(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "BigVehicleTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -504,100 +370,28 @@ bool BigVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - 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); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "BigVehicleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // recover bounding box from tracking point const double dl = bounding_box_.length - last_input_bounding_box_.length; const double dw = bounding_box_.width - last_input_bounding_box_.width; const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - X_t(IDX::X) = recovered_pose.x(); - X_t(IDX::Y) = recovered_pose.y(); + pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, + motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); + pose_with_cov.pose.position.x = recovered_pose.x(); + pose_with_cov.pose.position.y = recovered_pose.y(); // 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(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(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] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - 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::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; @@ -610,32 +404,3 @@ bool BigVehicleTracker::getTrackedObject( return true; } - -void BigVehicleTracker::setNearestCornerOrSurfaceIndex( - const geometry_msgs::msg::Transform & self_transform) -{ - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); -} - -double BigVehicleTracker::getMeasurementYaw( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } - return measurement_yaw; -} 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 index 18f73c8610f04..19fc5a2f71122 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -47,7 +47,6 @@ NormalVehicleTracker::NormalVehicleTracker( const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -63,88 +62,18 @@ NormalVehicleTracker::NormalVehicleTracker( 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_vel = std::pow(r_stddev_vel, 2.0); - // initial state covariance - float p0_stddev_x = 1.0; // in object coordinate [m] - float p0_stddev_y = 0.3; // in object coordinate [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - 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_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize state vector X - 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; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } + // velocity deviation threshold + // if the predicted velocity is close to the observed velocity, + // the observed velocity is used as the measurement. + velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + // OBJECT SHAPE MODEL 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}; last_input_bounding_box_ = bounding_box_; } else { - // past default value - // bounding_box_ = {4.0, 1.7, 2.0}; autoware_auto_perception_msgs::msg::DetectedObject bbox_object; utils::convertConvexHullToBoundingBox(object, bbox_object); bounding_box_ = { @@ -152,208 +81,114 @@ NormalVehicleTracker::NormalVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - ekf_.init(X, P); - - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step - // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); bounding_box_.height = std::max(bounding_box_.height, 0.3); - // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m - lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m -} + // Set motion model parameters + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 0.3; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 1.0; // [m] minimum front wheel position + constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position + constexpr double lr_min = 1.0; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + } -bool NormalVehicleTracker::predict(const rclcpp::Time & time) -{ - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); - return false; + // Set motion limits + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } - // if dt is too large, shorten dt and repeat prediction - const double dt_max = 0.11; - const uint32_t repeat = std::ceil(dt / dt_max); - const double dt_ = dt / repeat; - bool ret = false; - for (uint32_t i = 0; i < repeat; ++i) { - ret = predict(dt_, ekf_); - if (!ret) { - return false; + + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double vel_cov; + const double & length = bounding_box_.length; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; } - } - if (ret) { - last_update_time_ = time; - } - return ret; -} -bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - 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. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // 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); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 1.0; // in object coordinate [m] + constexpr double p0_stddev_y = 0.3; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); + + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - return true; + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } -bool NormalVehicleTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) +bool NormalVehicleTracker::predict(const rclcpp::Time & time) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - - float r_cov_x; - float r_cov_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - - if (label == Label::CAR) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (utils::isLargeVehicleLabel(label)) { - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } - - // Decide dimension of measurement vector - bool enable_velocity_measurement = false; - if (object.kinematics.has_twist) { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - const double predicted_vel = X_t(IDX::VEL); - const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + return motion_model_.predictState(time); +} - if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { - // Velocity deviation is small - enable_velocity_measurement = true; - } - } +autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) +{ + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - // pos x, pos y, yaw, vel depending on pose measurement - const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); + // current (predicted) state + const double tracked_x = motion_model_.getStateElement(IDX::X); + const double tracked_y = motion_model_.getStateElement(IDX::Y); + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); - // convert to boundingbox if input is convex shape + // OBJECT SHAPE MODEL + // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); @@ -361,113 +196,134 @@ bool NormalVehicleTracker::measureWithPose( bbox_object = object; } - /* get offset measurement*/ - autoware_auto_perception_msgs::msg::DetectedObject offset_object; + // get offset measurement + int nearest_corner_index = utils::getNearestCornerOrSurface( + tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, - bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; - Y(IDX::YAW, 0) = measurement_yaw; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(measurement_yaw); - const double sin_yaw = std::sin(measurement_yaw); - const double sin_2yaw = std::sin(2.0f * measurement_yaw); - R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - R(1, 0) = R(0, 1); // y - x - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } - - // Update the velocity when necessary - if (dim_y == 4) { - Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VEL) = 1.0; // for vel + last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, + bbox_object, tracked_yaw, updating_object, tracking_offset_); - if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vel; // vel -vel + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + // measurement noise covariance + float r_cov_x; + float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (label == Label::CAR) { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } else if (utils::isLargeVehicleLabel(label)) { + // if label is changed, enlarge the measurement noise covariance + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); } else { - R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_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_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + + // fill covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + 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[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + pose_cov[utils::MSG_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + if (!is_yaw_available) { + pose_cov[utils::MSG_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value } + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel } - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); + return updating_object; +} + +bool NormalVehicleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // current (predicted) state + const double tracked_vel = motion_model_.getStateElement(IDX::VEL); + + // velocity capability is checked only when the object has velocity measurement + // and the predicted velocity is close to the observed velocity + bool is_velocity_available = false; + if (object.kinematics.has_twist) { + const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { + // Velocity deviation is small + is_velocity_available = true; + } } - // normalize yaw and limit vel, slip + // update + bool is_updated = false; { - 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); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double vel = object.kinematics.twist_with_covariance.twist.linear.x; + + if (is_velocity_available) { + is_updated = motion_model_.updateStatePoseHeadVel( + x, y, yaw, object.kinematics.pose_with_covariance.covariance, vel, + object.kinematics.twist_with_covariance.covariance); + } else { + is_updated = motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); } - ekf_.init(X_t, P_t); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type - autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { - bbox_object = object; - } + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; - constexpr float gain = 0.9; - bounding_box_.length = - gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; - bounding_box_.height = - gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + // update object size + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); bounding_box_.height = std::max(bounding_box_.height, 0.3); - // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m - lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m + // update motion model + motion_model_.updateExtendedState(bounding_box_.length); + + // // update offset into object position + // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); + // // update offset + // tracking_offset_.x() = gain_inv * tracking_offset_.x(); + // tracking_offset_.y() = gain_inv * tracking_offset_.y(); return true; } @@ -476,20 +332,30 @@ bool NormalVehicleTracker::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(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "NormalVehicleTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -504,100 +370,28 @@ bool NormalVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - 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); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "NormalVehicleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // recover bounding box from tracking point const double dl = bounding_box_.length - last_input_bounding_box_.length; const double dw = bounding_box_.width - last_input_bounding_box_.width; const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - X_t(IDX::X) = recovered_pose.x(); - X_t(IDX::Y) = recovered_pose.y(); + pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, + motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); + pose_with_cov.pose.position.x = recovered_pose.x(); + pose_with_cov.pose.position.y = recovered_pose.y(); // 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(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(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] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - 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::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; @@ -610,32 +404,3 @@ bool NormalVehicleTracker::getTrackedObject( return true; } - -void NormalVehicleTracker::setNearestCornerOrSurfaceIndex( - const geometry_msgs::msg::Transform & self_transform) -{ - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); -} - -double NormalVehicleTracker::getMeasurementYaw( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } - return measurement_yaw; -} diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index b5b3bd5826e69..e1c07388836f6 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -47,7 +47,6 @@ PedestrianTracker::PedestrianTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("PedestrianTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -60,80 +59,8 @@ PedestrianTracker::PedestrianTracker( 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); - // initial state covariance - float p0_stddev_x = 2.0; // [m] - float p0_stddev_y = 2.0; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(120); // [m/s] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(360); // [rad/s] - 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); - // process noise covariance - float q_stddev_x = 0.4; // [m/s] - float q_stddev_y = 0.4; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] - 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); - // limitations - max_vx_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - - // initialize state vector X - 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; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z; - } else { - X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::WZ, IDX::WZ) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; - } - } + // OBJECT SHAPE MODEL bounding_box_ = {0.5, 0.5, 1.7}; cylinder_ = {0.3, 1.7}; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -142,7 +69,6 @@ PedestrianTracker::PedestrianTracker( } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; } - // set minimum size bounding_box_.length = std::max(bounding_box_.length, 0.3); bounding_box_.width = std::max(bounding_box_.width, 0.3); @@ -150,176 +76,141 @@ PedestrianTracker::PedestrianTracker( cylinder_.width = std::max(cylinder_.width, 0.3); cylinder_.height = std::max(cylinder_.height, 0.3); - ekf_.init(X, P); + // 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)] + motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); + } + + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ + 30.0 /* [deg/s] maximum turn rate */ + ); + + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double wz = 0.0; + double vel_cov; + double wz_cov; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + wz = object.kinematics.twist_with_covariance.twist.angular.z; + } + + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 2.0; // in object coordinate [m] + constexpr double p0_stddev_y = 2.0; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(1000); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(120); // in object coordinate [m/s] + constexpr double p0_stddev_wz = + tier4_autoware_utils::deg2rad(360); // in object coordinate [rad/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + wz_cov = std::pow(p0_stddev_wz, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + wz_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; + } + + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, wz, wz_cov); + } } bool PedestrianTracker::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; + return motion_model_.predictState(time); } -bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const +autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) { - // cspell: ignore CTRV - /* Motion model: Constant turn-rate motion model (CTRV) - * - * 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 - * - */ - - /* Jacobian Matrix - * - * 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] - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW)); - const double sin_yaw = std::sin(X_t(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::WZ) = X_t(IDX::WZ); - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::YAW, IDX::WZ) = dt; - - // Process noise covariance Q - 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. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt; - 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); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); - } + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - return true; + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + 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[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + } + return updating_object; } bool PedestrianTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr int dim_y = 2; // pos x, pos y depending on Pose output - // double measurement_yaw = - // tier4_autoware_utils::normalizeRadian(tf2::getYaw(object.state.pose_covariance.pose.orientation)); - // { - // Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - // ekf_.getX(X_t); - // while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - // measurement_yaw = measurement_yaw + M_PI; - // } - // while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - // measurement_yaw = measurement_yaw - M_PI; - // } - // float theta = std::acos( - // std::cos(X_t(IDX::YAW)) * std::cos(measurement_yaw) + - // std::sin(X_t(IDX::YAW)) * std::sin(measurement_yaw)); - // if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false; - // } - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - // C(2, IDX::YAW) = 1.0; // for yaw - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x - // R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - // R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - // R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - // R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - // R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - // R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } - - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); - } - - // normalize yaw and limit vx, wz + // update motion model + bool is_updated = false; { - 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); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - 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_; - } - if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) { - X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_; - } - ekf_.init(X_t, P_t); + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool PedestrianTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr float gain = 0.9; + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; + 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; + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + 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; + cylinder_.width = gain_inv * cylinder_.width + gain * object.shape.dimensions.x; + cylinder_.height = gain_inv * cylinder_.height + gain * object.shape.dimensions.z; } else { return false; } @@ -338,20 +229,29 @@ bool PedestrianTracker::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(); + // keep the latest input object object_ = object; + + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "PedestrianTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); (void)self_transform; // currently do not use self vehicle position return true; @@ -364,70 +264,19 @@ bool PedestrianTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - 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); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "PedestrianTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // 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(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(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] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - 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.angular.z = X_t(IDX::WZ); - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // 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::Y_Y] = vy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); // set shape if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 897b858a6aabe..450bb2d94abb6 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -12,6 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -22,227 +30,179 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("UnknownTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; // initialize params - float q_stddev_x = 0.0; // [m/s] - float q_stddev_y = 0.0; // [m/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(20); // [m/(s*s)] - float q_stddev_vy = tier4_autoware_utils::kmph2mps(20); // [m/(s*s)] - float r_stddev_x = 1.0; // [m] - float r_stddev_y = 1.0; // [m] - float p0_stddev_x = 1.0; // [m/s] - float p0_stddev_y = 1.0; // [m/s] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float p0_stddev_vy = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - 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_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); + // measurement noise covariance + constexpr double r_stddev_x = 1.0; // [m] + constexpr double r_stddev_y = 1.0; // [m] 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_.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_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_vy = std::pow(p0_stddev_vy, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(60); // [m/s] - max_vy_ = tier4_autoware_utils::kmph2mps(60); // [m/s] - - // initialize X matrix - 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; - if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::VY) = object.kinematics.twist_with_covariance.twist.linear.y; - } else { - X(IDX::VX) = 0.0; - X(IDX::VY) = 0.0; + + // 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_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] + motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); } - // initialize P matrix - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = ekf_params_.p0_cov_x; - P(IDX::X, IDX::Y) = 0.0; - P(IDX::Y, IDX::Y) = ekf_params_.p0_cov_y; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::VY, IDX::VY) = ekf_params_.p0_cov_vy; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::VY, IDX::VY) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::VY, IDX::VY) = ekf_params_.p0_cov_vy; + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ + tier4_autoware_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ + ); + + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + auto twist_cov = object.kinematics.twist_with_covariance.covariance; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + double vx = 0.0; + double vy = 0.0; + if (object.kinematics.has_twist) { + const double & vel_x = object.kinematics.twist_with_covariance.twist.linear.x; + const double & vel_y = object.kinematics.twist_with_covariance.twist.linear.y; + vx = std::cos(yaw) * vel_x - std::sin(yaw) * vel_y; + vy = std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; + } + + if (!object.kinematics.has_position_covariance) { + constexpr double p0_stddev_x = 1.0; // [m] + constexpr double p0_stddev_y = 1.0; // [m] + + const double p0_cov_x = std::pow(p0_stddev_x, 2.0); + const double p0_cov_y = std::pow(p0_stddev_y, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vy = tier4_autoware_utils::kmph2mps(10); // [m/s] + const double p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + const double p0_cov_vy = std::pow(p0_stddev_vy, 2.0); + twist_cov[utils::MSG_COV_IDX::X_X] = p0_cov_vx; + twist_cov[utils::MSG_COV_IDX::X_Y] = 0.0; + twist_cov[utils::MSG_COV_IDX::Y_X] = 0.0; + twist_cov[utils::MSG_COV_IDX::Y_Y] = p0_cov_vy; } - } - ekf_.init(X, P); + // rotate twist covariance matrix, since it is in the vehicle coordinate system + Eigen::MatrixXd twist_cov_rotate(2, 2); + twist_cov_rotate(0, 0) = twist_cov[utils::MSG_COV_IDX::X_X]; + twist_cov_rotate(0, 1) = twist_cov[utils::MSG_COV_IDX::X_Y]; + twist_cov_rotate(1, 0) = twist_cov[utils::MSG_COV_IDX::Y_X]; + twist_cov_rotate(1, 1) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + Eigen::MatrixXd twist_cov_rotated = R_yaw * twist_cov_rotate * R_yaw.transpose(); + twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_rotated(0, 0); + twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_rotated(0, 1); + twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_rotated(1, 0); + twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); + + // initialize motion model + motion_model_.initialize(time, x, y, pose_cov, vx, vy, twist_cov); + } } bool UnknownTracker::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; + return motion_model_.predictState(time); } -bool UnknownTracker::predict(const double dt, KalmanFilter & ekf) const +autoware_auto_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) { - /* == Nonlinear model == - * - * x_{k+1} = x_k + vx_k * dt - * y_{k+1} = y_k + vx_k * dt - * vx_{k+1} = vx_k - * vy_{k+1} = vy_k - * - */ - - /* == Linearized model == - * - * A = [ 1, 0, dt, 0] - * [ 0, 1, 0, dt] - * [ 0, 0, 1, 0] - * [ 0, 0, 0, 1] - */ - - // X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * dt; - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VY) * dt; - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::VY) = X_t(IDX::VY); - - // A - 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; - - // Q - 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. - Q(IDX::X, IDX::X) = ekf_params_.q_cov_x * dt * dt; - Q(IDX::X, IDX::Y) = 0.0; - Q(IDX::Y, IDX::Y) = ekf_params_.q_cov_y * dt * dt; - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::VY, IDX::VY) = ekf_params_.q_cov_vy * dt * dt; - 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); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Pedestrian : Cannot predict"); - } + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - return true; + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + 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[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + } + return updating_object; } bool UnknownTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr int dim_y = 2; // pos x, pos y depending on Pose output - - /* Set measurement matrix */ - Eigen::MatrixXd Y(dim_y, 1); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y; - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - - /* Set measurement noise covariance */ - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - } - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Pedestrian : Cannot update"); - } - - // limit vx, vy + // update motion model + bool is_updated = false; { - 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_; - } - if (!(-max_vy_ <= X_t(IDX::VY) && X_t(IDX::VY) <= max_vy_)) { - X_t(IDX::VY) = X_t(IDX::VY) < 0 ? -max_vy_ : max_vy_; - } - ekf_.init(X_t, P_t); + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool UnknownTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & self_transform) { + // keep the latest input object object_ = object; - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( logger_, - "Pedestrian : There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + "UnknownTracker::measure There is a large gap between predicted time and measurement time. " + "(%f)", + dt); } - measureWithPose(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); return true; } @@ -254,54 +214,19 @@ bool UnknownTracker::getTrackedObject( 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; + + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "UnknownTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // 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_; - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.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(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(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] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; - - // twist - const auto pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); - const double cos = std::cos(-pose_yaw); - const double sin = std::sin(-pose_yaw); - twist_with_cov.twist.linear.x = cos * X_t(IDX::VX) - sin * X_t(IDX::VY); - twist_with_cov.twist.linear.y = sin * X_t(IDX::VX) + cos * X_t(IDX::VY); - - // twist covariance - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wz_cov = 0.1 * 0.1; // 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::Y_Y] = P(IDX::VY, IDX::VY); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; return true; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp new file mode 100644 index 0000000000000..d992ea926746e --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -0,0 +1,482 @@ +// Copyright 2024 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 Taekjin Lee +// + +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +// Bicycle CTRV motion model +// CTRV : Constant Turn Rate and constant Velocity + +BicycleMotionModel::BicycleMotionModel() +: MotionModel(), logger_(rclcpp::get_logger("BicycleMotionModel")) +{ + // Initialize motion parameters + setDefaultParams(); +} + +void BicycleMotionModel::setDefaultParams() +{ + // set default motion parameters + constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate + constexpr double q_stddev_slip_rate_min = 0.3; // [deg/s] uncertain slip angle change rate + constexpr double q_stddev_slip_rate_max = 10.0; // [deg/s] uncertain slip angle change rate + constexpr double q_max_slip_angle = 30.0; // [deg] max slip angle + // extended state parameters + constexpr double lf_ratio = 0.3; // 30% front from the center + constexpr double lf_min = 1.0; // minimum of 1.0m + constexpr double lr_ratio = 0.25; // 25% rear from the center + constexpr double lr_min = 1.0; // minimum of 1.0m + setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + + // set motion limitations + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30.0; // [deg] maximum slip angle + setMotionLimits(max_vel, max_slip); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] maximum time interval for prediction + setMaxDeltaTime(dt_max); +} + +void BicycleMotionModel::setMotionParams( + const double & q_stddev_acc_long, const double & q_stddev_acc_lat, + const double & q_stddev_yaw_rate_min, const double & q_stddev_yaw_rate_max, + const double & q_stddev_slip_rate_min, const double & q_stddev_slip_rate_max, + const double & q_max_slip_angle, const double & lf_ratio, const double & lf_min, + const double & lr_ratio, const double & lr_min) +{ + // set process noise covariance parameters + motion_params_.q_stddev_acc_long = q_stddev_acc_long; + motion_params_.q_stddev_acc_lat = q_stddev_acc_lat; + motion_params_.q_stddev_yaw_rate_min = tier4_autoware_utils::deg2rad(q_stddev_yaw_rate_min); + motion_params_.q_stddev_yaw_rate_max = tier4_autoware_utils::deg2rad(q_stddev_yaw_rate_max); + motion_params_.q_cov_slip_rate_min = + std::pow(tier4_autoware_utils::deg2rad(q_stddev_slip_rate_min), 2.0); + motion_params_.q_cov_slip_rate_max = + std::pow(tier4_autoware_utils::deg2rad(q_stddev_slip_rate_max), 2.0); + motion_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(q_max_slip_angle); + + constexpr double minimum_wheel_pos = 0.01; // minimum of 0.01m + if (lf_min < minimum_wheel_pos || lr_min < minimum_wheel_pos) { + RCLCPP_WARN( + logger_, + "BicycleMotionModel::setMotionParams: minimum wheel position should be greater than 0.01m."); + } + motion_params_.lf_min = (lf_min < minimum_wheel_pos) ? minimum_wheel_pos : lf_min; + motion_params_.lr_min = (lr_min < minimum_wheel_pos) ? minimum_wheel_pos : lr_min; + motion_params_.lf_ratio = lf_ratio; + motion_params_.lr_ratio = lr_ratio; +} + +void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & max_slip) +{ + // set motion limitations + motion_params_.max_vel = max_vel; + motion_params_.max_slip = tier4_autoware_utils::deg2rad(max_slip); +} + +bool BicycleMotionModel::initialize( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & slip, const double & slip_cov, const double & length) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, yaw, vel, slip; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + P(IDX::VEL, IDX::VEL) = vel_cov; + P(IDX::SLIP, IDX::SLIP) = slip_cov; + + // set initial extended state + if (!updateExtendedState(length)) return false; + + return MotionModel::initialize(time, X, P); +} + +bool BicycleMotionModel::updateStatePose( + const double & x, const double & y, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 2; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool BicycleMotionModel::updateStatePoseHead( + const double & x, const double & y, const double & yaw, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 3; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, fixed_yaw; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + + return ekf_.update(Y, C, R); +} + +bool BicycleMotionModel::updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, with velocity + constexpr int DIM_Y = 4; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, yaw, vel; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + C(3, IDX::VEL) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + R(3, 3) = twist_cov[utils::MSG_COV_IDX::X_X]; + + return ekf_.update(Y, C, R); +} + +bool BicycleMotionModel::limitStates() +{ + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); + if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; + } + if (!(-motion_params_.max_slip <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= motion_params_.max_slip)) { + X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -motion_params_.max_slip : motion_params_.max_slip; + } + ekf_.init(X_t, P_t); + + return true; +} + +bool BicycleMotionModel::updateExtendedState(const double & length) +{ + lf_ = std::max(length * motion_params_.lf_ratio, motion_params_.lf_min); + lr_ = std::max(length * motion_params_.lr_ratio, motion_params_.lr_min); + return true; +} + +bool BicycleMotionModel::adjustPosition(const double & x, const double & y) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // adjust position + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) += x; + X_t(IDX::Y) += y; + ekf_.init(X_t, P_t); + + return true; +} + +bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const +{ + /* Motion model: static bicycle model (constant slip angle, constant velocity) + * + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k + * slip_{k+1} = slip_k + * + */ + + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] + * + */ + + // Current state vector X t + Eigen::MatrixXd X_t(DIM, 1); + getStateVector(X_t); + + const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); + const double cos_slip = std::cos(X_t(IDX::SLIP)); + const double sin_slip = std::sin(X_t(IDX::SLIP)); + + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(DIM, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(DIM, DIM); + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + double q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = motion_params_.q_stddev_yaw_rate_min; + } else { + /* uncertainty of the yaw rate is limited by the following: + * - centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + * - or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + */ + q_stddev_yaw_rate = std::min( + motion_params_.q_stddev_acc_lat / vel, + vel * std::sin(motion_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_min); + } + double q_cov_slip_rate{0.0}; + if (vel <= 0.01) { + q_cov_slip_rate = motion_params_.q_cov_slip_rate_min; + } else { + /* The slip angle rate uncertainty is modeled as follows: + * d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + * where sin(slip) = w * l_r / v + * + * d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + * d(v)/dt and d(w)/t are considered to be uncorrelated + */ + q_cov_slip_rate = + std::pow(motion_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, motion_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, motion_params_.q_cov_slip_rate_min); + } + const double q_cov_x = std::pow(0.5 * motion_params_.q_stddev_acc_long * dt * dt, 2); + const double q_cov_y = std::pow(0.5 * motion_params_.q_stddev_acc_lat * dt * dt, 2); + const double q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const double q_cov_vel = std::pow(motion_params_.q_stddev_acc_long * dt, 2); + const double q_cov_slip = q_cov_slip_rate * dt * dt; + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); + // Rotate the covariance matrix according to the vehicle yaw + // because q_cov_x and y are in the vehicle coordinate system. + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); + Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(DIM, DIM); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(DIM, 1); + + // predict state + return ekf.predict(X_next_t, A, Q); +} + +bool BicycleMotionModel::getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const +{ + // get predicted state + Eigen::MatrixXd X(DIM, 1); + Eigen::MatrixXd P(DIM, DIM); + if (!MotionModel::getPredictedState(time, X, P)) { + return false; + } + + // set position + pose.position.x = X(IDX::X); + pose.position.y = X(IDX::Y); + pose.position.z = 0.0; + + // set orientation + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, X(IDX::YAW)); + pose.orientation.x = quaternion.x(); + pose.orientation.y = quaternion.y(); + pose.orientation.z = quaternion.z(); + pose.orientation.w = quaternion.w(); + + // set twist + twist.linear.x = X(IDX::VEL) * std::cos(X(IDX::SLIP)); + twist.linear.y = X(IDX::VEL) * std::sin(X(IDX::SLIP)); + twist.linear.z = 0.0; + twist.angular.x = 0.0; + twist.angular.y = 0.0; + twist.angular.z = twist.linear.y / lr_; + + // set pose covariance + constexpr double zz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); + pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; + pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; + + // set twist covariance + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X(IDX::SLIP)), -X(IDX::VEL) * std::sin(X(IDX::SLIP)), + std::sin(X(IDX::SLIP)), X(IDX::VEL) * std::cos(X(IDX::SLIP)), std::sin(X(IDX::SLIP)) / lr_, + X(IDX::VEL) * std::cos(X(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_cov[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_cov[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_cov[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_cov[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_cov[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); + twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + + return true; +} diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp new file mode 100644 index 0000000000000..b10fc70d1bba7 --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -0,0 +1,386 @@ +// Copyright 2024 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 Taekjin Lee +// + +#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +// cspell: ignore CTRV +// Constant Turn Rate and constant Velocity (CTRV) motion model + +CTRVMotionModel::CTRVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CTRVMotionModel")) +{ + // Initialize motion parameters + setDefaultParams(); +} + +void CTRVMotionModel::setDefaultParams() +{ + // process noise covariance + 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_vel = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + + setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); + + // set motion limitations + constexpr double max_vel = tier4_autoware_utils::kmph2mps(10); // [m/s] maximum velocity + constexpr double max_wz = 30.0; // [deg] maximum yaw rate + setMotionLimits(max_vel, max_wz); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] maximum time interval for prediction + setMaxDeltaTime(dt_max); +} + +void CTRVMotionModel::setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_yaw, + const double & q_stddev_vel, const double & q_stddev_wz) +{ + // set process noise covariance parameters + motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + motion_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); + motion_params_.q_cov_vel = std::pow(q_stddev_vel, 2.0); + motion_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); +} + +void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max_wz) +{ + // set motion limitations + motion_params_.max_vel = max_vel; + motion_params_.max_wz = tier4_autoware_utils::deg2rad(max_wz); +} + +bool CTRVMotionModel::initialize( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & wz, const double & wz_cov) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, yaw, vel, wz; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + P(IDX::VEL, IDX::VEL) = vel_cov; + P(IDX::WZ, IDX::WZ) = wz_cov; + + return MotionModel::initialize(time, X, P); +} + +bool CTRVMotionModel::updateStatePose( + const double & x, const double & y, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 2; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::updateStatePoseHead( + const double & x, const double & y, const double & yaw, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 3; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, fixed_yaw; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, with velocity + constexpr int DIM_Y = 4; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, yaw, vel; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + C(3, IDX::VEL) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + R(3, 3) = twist_cov[utils::MSG_COV_IDX::X_X]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::limitStates() +{ + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); + if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; + } + if (!(-motion_params_.max_wz <= X_t(IDX::WZ) && X_t(IDX::WZ) <= motion_params_.max_wz)) { + X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -motion_params_.max_wz : motion_params_.max_wz; + } + ekf_.init(X_t, P_t); + + return true; +} + +bool CTRVMotionModel::adjustPosition(const double & x, const double & y) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // adjust position + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) += x; + X_t(IDX::Y) += y; + ekf_.init(X_t, P_t); + + return true; +} + +bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const +{ + /* Motion model: Constant Turn Rate and constant Velocity model (CTRV) + * + * 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 + * + */ + + /* Jacobian Matrix + * + * 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] + */ + + // Current state vector X t + Eigen::MatrixXd X_t(DIM, 1); + getStateVector(X_t); + + const double cos_yaw = std::cos(X_t(IDX::YAW)); + const double sin_yaw = std::sin(X_t(IDX::YAW)); + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(DIM, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VEL) * cos_yaw * dt; // dx = v * cos(yaw) + X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VEL) * sin_yaw * dt; // dy = v * sin(yaw) + X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::WZ) = X_t(IDX::WZ); + + // State transition matrix A + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(DIM, DIM); + A(IDX::X, IDX::YAW) = -X_t(IDX::VEL) * sin_yaw * dt; + A(IDX::X, IDX::VEL) = cos_yaw * dt; + A(IDX::Y, IDX::YAW) = X_t(IDX::VEL) * cos_yaw * dt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt; + A(IDX::YAW, IDX::WZ) = dt; + + // Process noise covariance Q + const double q_cov_x = std::pow(0.5 * motion_params_.q_cov_x * dt, 2); + const double q_cov_y = std::pow(0.5 * motion_params_.q_cov_y * dt, 2); + const double q_cov_yaw = std::pow(motion_params_.q_cov_yaw * dt, 2); + const double q_cov_vel = std::pow(motion_params_.q_cov_vel * dt, 2); + const double q_cov_wz = std::pow(motion_params_.q_cov_wz * dt, 2); + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); + // Rotate the covariance matrix according to the vehicle yaw + // because q_cov_x and y are in the vehicle coordinate system. + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); + Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::WZ, IDX::WZ) = q_cov_wz; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(DIM, DIM); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(DIM, 1); + + // predict state + return ekf.predict(X_next_t, A, Q); +} + +bool CTRVMotionModel::getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const +{ + // get predicted state + Eigen::MatrixXd X(DIM, 1); + Eigen::MatrixXd P(DIM, DIM); + if (!MotionModel::getPredictedState(time, X, P)) { + return false; + } + + // set position + pose.position.x = X(IDX::X); + pose.position.y = X(IDX::Y); + pose.position.z = 0.0; + + // set orientation + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, X(IDX::YAW)); + pose.orientation.x = quaternion.x(); + pose.orientation.y = quaternion.y(); + pose.orientation.z = quaternion.z(); + pose.orientation.w = quaternion.w(); + + // set twist + twist.linear.x = X(IDX::VEL); + twist.linear.y = 0.0; + twist.linear.z = 0.0; + twist.angular.x = 0.0; + twist.angular.y = 0.0; + twist.angular.z = X(IDX::WZ); + + // set pose covariance + constexpr double zz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); + pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; + pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; + + // set twist covariance + constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + twist_cov[utils::MSG_COV_IDX::X_X] = P(IDX::VEL, IDX::VEL); + twist_cov[utils::MSG_COV_IDX::X_Y] = 0.0; + twist_cov[utils::MSG_COV_IDX::X_YAW] = P(IDX::VEL, IDX::WZ); + twist_cov[utils::MSG_COV_IDX::Y_X] = 0.0; + twist_cov[utils::MSG_COV_IDX::Y_Y] = vy_cov; + twist_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; + twist_cov[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VEL); + twist_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; + twist_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + + return true; +} diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp new file mode 100644 index 0000000000000..28ba59f4f19f9 --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -0,0 +1,307 @@ +// Copyright 2024 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 Taekjin Lee +// + +#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include + +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +// cspell: ignore CV +// Constant Velocity (CV) motion model + +CVMotionModel::CVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CVMotionModel")) +{ + // Initialize motion parameters + setDefaultParams(); +} + +void CVMotionModel::setDefaultParams() +{ + // process noise covariance + constexpr double q_stddev_x = 0.5; // [m/s] standard deviation of x + constexpr double q_stddev_y = 0.5; // [m/s] standard deviation of y + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] standard deviation of vx + constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] standard deviation of vy + setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); + + // set motion limitations + constexpr double max_vx = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum x velocity + constexpr double max_vy = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum y velocity + setMotionLimits(max_vx, max_vy); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] maximum time interval for prediction + setMaxDeltaTime(dt_max); +} + +void CVMotionModel::setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, + const double & q_stddev_vy) +{ + // set process noise covariance parameters + motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + motion_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + motion_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); +} + +void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy) +{ + // set motion limitations + motion_params_.max_vx = max_vx; + motion_params_.max_vy = max_vy; +} + +bool CVMotionModel::initialize( + const rclcpp::Time & time, const double & x, const double & y, + const std::array & pose_cov, const double & vx, const double & vy, + const std::array & twist_cov) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, vx, vy; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::VX, IDX::VX) = twist_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::VY, IDX::VY) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + + return MotionModel::initialize(time, X, P); +} + +bool CVMotionModel::updateStatePose( + const double & x, const double & y, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 2; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CVMotionModel::updateStatePoseVel( + const double & x, const double & y, const std::array & pose_cov, const double & vx, + const double & vy, const std::array & twist_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state with velocity + constexpr int DIM_Y = 4; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, vx, vy; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::VX) = 1.0; + C(3, IDX::VY) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(2, 2) = twist_cov[utils::MSG_COV_IDX::X_X]; + R(2, 3) = twist_cov[utils::MSG_COV_IDX::X_Y]; + R(3, 2) = twist_cov[utils::MSG_COV_IDX::Y_X]; + R(3, 3) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CVMotionModel::limitStates() +{ + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + if (!(-motion_params_.max_vx <= X_t(IDX::VX) && X_t(IDX::VX) <= motion_params_.max_vx)) { + X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -motion_params_.max_vx : motion_params_.max_vx; + } + if (!(-motion_params_.max_vy <= X_t(IDX::VY) && X_t(IDX::VY) <= motion_params_.max_vy)) { + X_t(IDX::VY) = X_t(IDX::VY) < 0 ? -motion_params_.max_vy : motion_params_.max_vy; + } + ekf_.init(X_t, P_t); + + return true; +} + +bool CVMotionModel::adjustPosition(const double & x, const double & y) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // adjust position + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) += x; + X_t(IDX::Y) += y; + ekf_.init(X_t, P_t); + + return true; +} + +bool CVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const +{ + /* Motion model: Constant velocity model + * + * x_{k+1} = x_k + vx_k * dt + * y_{k+1} = y_k + vx_k * dt + * vx_{k+1} = vx_k + * vy_{k+1} = vy_k + * + */ + + /* Jacobian Matrix + * + * A = [ 1, 0, dt, 0] + * [ 0, 1, 0, dt] + * [ 0, 0, 1, 0] + * [ 0, 0, 0, 1] + */ + + // Current state vector X t + Eigen::MatrixXd X_t(DIM, 1); + getStateVector(X_t); + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(DIM, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * dt; + X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VY) * dt; + X_next_t(IDX::VX) = X_t(IDX::VX); + X_next_t(IDX::VY) = X_t(IDX::VY); + + // State transition matrix A + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(DIM, DIM); + A(IDX::X, IDX::VX) = dt; + A(IDX::Y, IDX::VY) = dt; + + // Process noise covariance Q + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); + Q(IDX::X, IDX::X) = motion_params_.q_cov_x * dt * dt; + Q(IDX::X, IDX::Y) = 0.0; + Q(IDX::Y, IDX::Y) = motion_params_.q_cov_y * dt * dt; + Q(IDX::Y, IDX::X) = 0.0; + Q(IDX::VX, IDX::VX) = motion_params_.q_cov_vx * dt * dt; + Q(IDX::VY, IDX::VY) = motion_params_.q_cov_vy * dt * dt; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(DIM, DIM); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(DIM, 1); + + // predict state + return ekf.predict(X_next_t, A, Q); +} + +bool CVMotionModel::getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const +{ + // get predicted state + Eigen::MatrixXd X(DIM, 1); + Eigen::MatrixXd P(DIM, DIM); + if (!MotionModel::getPredictedState(time, X, P)) { + return false; + } + + // get yaw from pose + const double yaw = tf2::getYaw(pose.orientation); + + // set position + pose.position.x = X(IDX::X); + pose.position.y = X(IDX::Y); + pose.position.z = 0.0; + + // set twist + twist.linear.x = X(IDX::VX) * std::cos(-yaw) - X(IDX::VY) * std::sin(-yaw); + twist.linear.y = X(IDX::VX) * std::sin(-yaw) + X(IDX::VY) * std::cos(-yaw); + twist.linear.z = 0.0; + twist.angular.x = 0.0; + twist.angular.y = 0.0; + twist.angular.z = 0.0; + + // set pose covariance + constexpr double zz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; + pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + + // set twist covariance + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + // rotate covariance matrix + Eigen::MatrixXd twist_cov_rotate(2, 2); + twist_cov_rotate(0, 0) = P(IDX::VX, IDX::VX); + twist_cov_rotate(0, 1) = P(IDX::VX, IDX::VY); + twist_cov_rotate(1, 0) = P(IDX::VY, IDX::VX); + twist_cov_rotate(1, 1) = P(IDX::VY, IDX::VY); + Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + Eigen::MatrixXd twist_cov_rotated = R_yaw * twist_cov_rotate * R_yaw.transpose(); + twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_rotated(0, 0); + twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_rotated(0, 1); + twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_rotated(1, 0); + twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); + twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + twist_cov[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + + return true; +} diff --git a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp b/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp new file mode 100644 index 0000000000000..b4af422fd2329 --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp @@ -0,0 +1,93 @@ +// Copyright 2024 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 Taekjin Lee +// + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +MotionModel::MotionModel() : last_update_time_(rclcpp::Time(0, 0)) +{ +} + +bool MotionModel::initialize( + const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) +{ + // initialize Kalman filter + if (!ekf_.init(X, P)) return false; + + // set last_update_time_ + last_update_time_ = time; + + // set initialized flag + is_initialized_ = true; + + return true; +} + +bool MotionModel::predictState(const rclcpp::Time & time) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + const double dt = getDeltaTime(time); + if (dt < 0.0) { + return false; + } + // if dt is too large, shorten dt and repeat prediction + const uint32_t repeat = std::ceil(dt / dt_max_); + const double dt_ = dt / repeat; + for (uint32_t i = 0; i < repeat; ++i) { + if (!predictStateStep(dt_, ekf_)) { + return false; + } + // add interval to last_update_time_ + last_update_time_ += rclcpp::Duration::from_seconds(dt_); + } + // update last_update_time_ to the estimation time + last_update_time_ = time; + return true; +} + +bool MotionModel::getPredictedState( + const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // copy the predicted state and covariance + KalmanFilter tmp_ekf_for_no_update = ekf_; + + double dt = getDeltaTime(time); + if (dt < 0.0) { + // a naive way to handle the case when the required prediction time is in the past + dt = 0.0; + } + + // predict only when dt is small enough + if (0.001 /*1msec*/ < dt) { + // if dt is too large, shorten dt and repeat prediction + const uint32_t repeat = std::ceil(dt / dt_max_); + const double dt_ = dt / repeat; + for (uint32_t i = 0; i < repeat; ++i) { + if (!predictStateStep(dt_, tmp_ekf_for_no_update)) { + return false; + } + } + } + tmp_ekf_for_no_update.getX(X); + tmp_ekf_for_no_update.getP(P); + return true; +}