From b515100f1d8347cf75975a021e1475704513acc4 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Wed, 5 Jun 2024 11:56:15 +0900 Subject: [PATCH] fix(mulit_object_tracker): revert multi object tracker (#1321) * Revert "fix(multi_object_tracker): calculation of detection delay diagnostics (#1317)" This reverts commit f45652e0a98e9d2d391acc4fde309f837f0268e2. * Revert "fix(multi_object_tracker): huge tracking object bug fix for x2 (#1314)" This reverts commit 4b1f18eeb99834601ea60626a7c0b3337be50b35. --- .../multi_object_tracker/CMakeLists.txt | 21 +- .../include/multi_object_tracker/debugger.hpp | 75 -- .../multi_object_tracker_core.hpp | 79 +- .../processor/processor.hpp | 79 -- .../tracker/model/bicycle_tracker.hpp | 40 +- .../tracker/model/big_vehicle_tracker.hpp | 57 +- .../tracker/model/normal_vehicle_tracker.hpp | 60 +- .../tracker/model/pedestrian_tracker.hpp | 43 +- .../tracker/model/tracker_base.hpp | 2 +- .../tracker/model/unknown_tracker.hpp | 41 +- .../motion_model/bicycle_motion_model.hpp | 108 --- .../motion_model/ctrv_motion_model.hpp | 95 --- .../tracker/motion_model/cv_motion_model.hpp | 88 --- .../motion_model/motion_model_base.hpp | 67 -- .../multi_object_tracker/utils/utils.hpp | 67 +- .../launch/multi_object_tracker.launch.xml | 2 +- perception/multi_object_tracker/package.xml | 4 +- .../multi_object_tracker/src/debugger.cpp | 169 ---- .../src/multi_object_tracker_core.cpp | 456 ++++++++--- .../src/processor/processor.cpp | 242 ------ .../src/tracker/model/bicycle_tracker.cpp | 598 +++++++++----- .../src/tracker/model/big_vehicle_tracker.cpp | 735 +++++++++++------- .../tracker/model/normal_vehicle_tracker.cpp | 733 ++++++++++------- .../src/tracker/model/pedestrian_tracker.cpp | 459 ++++++----- .../src/tracker/model/unknown_tracker.cpp | 355 +++++---- .../motion_model/bicycle_motion_model.cpp | 483 ------------ .../motion_model/ctrv_motion_model.cpp | 386 --------- .../tracker/motion_model/cv_motion_model.cpp | 307 -------- .../motion_model/motion_model_base.cpp | 92 --- 29 files changed, 2397 insertions(+), 3546 deletions(-) delete mode 100644 perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp delete mode 100644 perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp delete mode 100644 perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp delete mode 100644 perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp delete mode 100644 perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp delete mode 100644 perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp delete mode 100644 perception/multi_object_tracker/src/debugger.cpp delete mode 100644 perception/multi_object_tracker/src/processor/processor.cpp delete mode 100644 perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp delete mode 100644 perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp delete mode 100644 perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp delete mode 100644 perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 19f75b5ba2306..3e379bcfd1cf1 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -12,7 +12,6 @@ endif() ### Find Eigen Dependencies find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -find_package(glog REQUIRED) include_directories( SYSTEM @@ -22,15 +21,6 @@ include_directories( # Generate exe file set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp - src/debugger.cpp - src/processor/processor.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 - # cspell: ignore ctrv - 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 @@ -40,20 +30,21 @@ 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(${PROJECT_NAME} SHARED +ament_auto_add_library(multi_object_tracker_node SHARED ${MULTI_OBJECT_TRACKER_SRC} ) -target_link_libraries(${PROJECT_NAME} +target_link_libraries(multi_object_tracker_node Eigen3::Eigen - glog::glog ) -rclcpp_components_register_node(${PROJECT_NAME} +rclcpp_components_register_node(multi_object_tracker_node PLUGIN "MultiObjectTracker" - EXECUTABLE ${PROJECT_NAME}_node + EXECUTABLE multi_object_tracker ) ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp deleted file mode 100644 index 4705f3c0fe0d4..0000000000000 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp +++ /dev/null @@ -1,75 +0,0 @@ -// 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. -// -// - -#ifndef MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ - -#include -#include -#include -#include -// #include - -#include -#include -#include - -#include - -/** - * @brief Debugger class for multi object tracker - * @details This class is used to publish debug information of multi object tracker - */ -class TrackerDebugger -{ -public: - explicit TrackerDebugger(rclcpp::Node & node); - void publishTentativeObjects( - const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; - void startMeasurementTime( - const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp); - void endMeasurementTime(const rclcpp::Time & now); - void startPublishTime(const rclcpp::Time & now); - void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time); - - void setupDiagnostics(); - void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); - struct DEBUG_SETTINGS - { - bool publish_processing_time; - bool publish_tentative_objects; - double diagnostics_warn_delay; - double diagnostics_error_delay; - } debug_settings_; - double pipeline_latency_ms_ = 0.0; - diagnostic_updater::Updater diagnostic_updater_; - bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } - -private: - void loadParameters(); - bool is_initialized_ = false; - rclcpp::Node & node_; - rclcpp::Publisher::SharedPtr - debug_tentative_objects_pub_; - std::unique_ptr processing_time_publisher_; - rclcpp::Time last_input_stamp_; - rclcpp::Time stamp_process_start_; - rclcpp::Time stamp_process_end_; - rclcpp::Time stamp_publish_start_; - rclcpp::Time stamp_publish_output_; -}; - -#endif // MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 2bc432b0d5275..95d33b78ff184 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -20,11 +20,11 @@ #define MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ #include "multi_object_tracker/data_association/data_association.hpp" -#include "multi_object_tracker/debugger.hpp" -#include "multi_object_tracker/processor/processor.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include +#include +#include #include #include @@ -40,6 +40,9 @@ #include #endif +#include +#include + #include #include @@ -47,45 +50,81 @@ #include #include #include -#include #include +/** + * @brief Debugger class for multi object tracker + * @details This class is used to publish debug information of multi object tracker + */ +class TrackerDebugger +{ +public: + explicit TrackerDebugger(rclcpp::Node & node); + void publishProcessingTime(); + void publishTentativeObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; + void startStopWatch(); + void startMeasurementTime(const rclcpp::Time & measurement_header_stamp); + void setupDiagnostics(); + void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); + struct DEBUG_SETTINGS + { + bool publish_processing_time; + bool publish_tentative_objects; + double diagnostics_warn_delay; + double diagnostics_error_delay; + } debug_settings_; + double elapsed_time_from_sensor_input_ = 0.0; + diagnostic_updater::Updater diagnostic_updater_; + +private: + void loadParameters(); + rclcpp::Node & node_; + rclcpp::Publisher::SharedPtr + debug_tentative_objects_pub_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + rclcpp::Time last_input_stamp_; +}; + class MultiObjectTracker : public rclcpp::Node { public: explicit MultiObjectTracker(const rclcpp::NodeOptions & node_options); private: - // ROS interface rclcpp::Publisher::SharedPtr tracked_objects_pub_; rclcpp::Subscription::SharedPtr detected_object_sub_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; + rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer - // debugger + // debugger class std::unique_ptr debugger_; - // std::unique_ptr published_time_publisher_; - // publish timer - rclcpp::TimerBase::SharedPtr publish_timer_; - rclcpp::Time last_published_time_; - double publisher_period_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; - // internal states - std::string world_frame_id_; // tracking frame - std::unique_ptr data_association_; - std::unique_ptr processor_; + std::map tracker_map_; - // callback functions void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); - // publish processes - void checkAndPublish(const rclcpp::Time & time); - void publish(const rclcpp::Time & time) const; + std::string world_frame_id_; // tracking frame + std::list> list_tracker_; + std::unique_ptr data_association_; + + void checkTrackerLifeCycle( + std::list> & list_tracker, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform); + void sanitizeTracker( + std::list> & list_tracker, const rclcpp::Time & time); + std::shared_ptr createNewTracker( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) const; + + void publish(const rclcpp::Time & time); inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp deleted file mode 100644 index 6d6e364d83a41..0000000000000 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// 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. -// -// - -#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ -#define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ - -#include "multi_object_tracker/tracker/model/tracker_base.hpp" - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -class TrackerProcessor -{ -public: - explicit TrackerProcessor(const std::map & tracker_map); - - const std::list> & getListTracker() const { return list_tracker_; } - // tracker processes - void predict(const rclcpp::Time & time); - void update( - const autoware_auto_perception_msgs::msg::DetectedObjects & transformed_objects, - const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment); - void spawn( - const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, - const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment); - void prune(const rclcpp::Time & time); - - // output - bool isConfidentTracker(const std::shared_ptr & tracker) const; - void getTrackedObjects( - const rclcpp::Time & time, - autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects) const; - void getTentativeObjects( - const rclcpp::Time & time, - autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; - -private: - std::map tracker_map_; - std::list> list_tracker_; - - // parameters - float max_elapsed_time_; // [s] - float min_iou_; // [ratio] - float min_iou_for_unknown_object_; // [ratio] - double distance_threshold_; // [m] - int confident_count_threshold_; // [count] - - void removeOldTracker(const rclcpp::Time & time); - void removeOverlappedTracker(const rclcpp::Time & time); - std::shared_ptr createNewTracker( - const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) const; -}; - -#endif // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ 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 2d69334a2f43f..ba50933eddaec 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,7 +20,6 @@ #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 @@ -31,15 +30,36 @@ 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 { - double r_cov_x; - double r_cov_y; - double r_cov_yaw; + 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; } ekf_params_; - double z_; + double max_vel_; + double max_slip_; + double lf_; + double lr_; + float z_; +private: struct BoundingBox { double length; @@ -47,11 +67,7 @@ class BicycleTracker : public Tracker double height; }; BoundingBox bounding_box_; - -private: - BicycleMotionModel motion_model_; - const char DIM = motion_model_.DIM; - using IDX = BicycleMotionModel::IDX; + BoundingBox last_input_bounding_box_; public: BicycleTracker( @@ -59,12 +75,10 @@ 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 8d49138b94a24..e19b6382ad952 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,28 +20,47 @@ #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 { - double r_cov_x; - double r_cov_y; - double r_cov_yaw; - double r_cov_vel; + 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; } ekf_params_; + double max_vel_; + double max_slip_; + double lf_; + double lr_; + float z_; double velocity_deviation_threshold_; - double z_; - +private: struct BoundingBox { double length; @@ -51,12 +70,6 @@ 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( @@ -64,28 +77,18 @@ 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 6ab7e63bce167..05fa0a5ef01ba 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,8 +19,7 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "tracker_base.hpp" #include @@ -29,19 +28,42 @@ 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 { - double r_cov_x; - double r_cov_y; - double r_cov_yaw; - double r_cov_vel; + 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; } ekf_params_; - double velocity_deviation_threshold_; - double z_; + double max_vel_; + double max_slip_; + double lf_; + double lr_; + float z_; + double velocity_deviation_threshold_; +private: struct BoundingBox { double length; @@ -51,12 +73,6 @@ 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( @@ -64,28 +80,18 @@ 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 a56250390e34f..0219d3b227044 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,12 +20,9 @@ #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 -// cspell: ignore CTRV - class PedestrianTracker : public Tracker { private: @@ -33,15 +30,38 @@ 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 { - double r_cov_x; - double r_cov_y; - double r_cov_yaw; + 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; } ekf_params_; - double z_; + double max_vx_; + double max_wz_; + float z_; +private: struct BoundingBox { double length; @@ -56,23 +76,16 @@ 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/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index ab6a747288d4a..d2850793fa4bb 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -81,7 +81,7 @@ class Tracker const rclcpp::Time & time) const; /* - * Pure virtual function + *　Pure virtual function */ protected: 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 73bf7849e13d8..18e6c0606a535 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,14 +13,13 @@ // limitations under the License. // // -// Author: v1.0 Yukihiro Saito +// v1.0 Yukihiro Saito // #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include "tracker_base.hpp" #include @@ -31,20 +30,30 @@ 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 { - double r_cov_x; - double r_cov_y; - double r_cov_vx; - double r_cov_vy; + 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; } ekf_params_; - - double z_; - -private: - CVMotionModel motion_model_; - const char DIM = motion_model_.DIM; - using IDX = CVMotionModel::IDX; + float max_vx_, max_vy_; + float z_; public: UnknownTracker( @@ -52,12 +61,10 @@ 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 deleted file mode 100644 index 5127d0448835c..0000000000000 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ /dev/null @@ -1,108 +0,0 @@ -// 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 deleted file mode 100644 index 6b071eddec7a9..0000000000000 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ /dev/null @@ -1,95 +0,0 @@ -// 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 - -// cspell: ignore CTRV - -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 deleted file mode 100644 index 59032706b00d6..0000000000000 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// 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 deleted file mode 100644 index 1aca602ed66a3..0000000000000 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// 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 5842246bc1313..d2b6ee5de475e 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 @@ -295,43 +295,40 @@ inline void calcAnchorPointOffset( * @param input_object: input convex hull objects * @param output_object: output bounding box objects */ -inline bool convertConvexHullToBoundingBox( +inline void convertConvexHullToBoundingBox( const autoware_auto_perception_msgs::msg::DetectedObject & input_object, autoware_auto_perception_msgs::msg::DetectedObject & output_object) { - // check footprint size - if (input_object.shape.footprint.points.size() < 3) { - return false; - } + const Eigen::Vector2d center{ + input_object.kinematics.pose_with_covariance.pose.position.x, + input_object.kinematics.pose_with_covariance.pose.position.y}; + const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); + const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); - // look for bounding box boundary double max_x = 0; double max_y = 0; double min_x = 0; double min_y = 0; double max_z = 0; + + // look for bounding box boundary for (size_t i = 0; i < input_object.shape.footprint.points.size(); ++i) { - const double foot_x = input_object.shape.footprint.points.at(i).x; - const double foot_y = input_object.shape.footprint.points.at(i).y; - const double foot_z = input_object.shape.footprint.points.at(i).z; - max_x = std::max(max_x, foot_x); - max_y = std::max(max_y, foot_y); - min_x = std::min(min_x, foot_x); - min_y = std::min(min_y, foot_y); - max_z = std::max(max_z, foot_z); + Eigen::Vector2d vertex{ + input_object.shape.footprint.points.at(i).x, input_object.shape.footprint.points.at(i).y}; + + const Eigen::Vector2d local_vertex = R_inv * (vertex - center); + max_x = std::max(max_x, local_vertex.x()); + max_y = std::max(max_y, local_vertex.y()); + min_x = std::min(min_x, local_vertex.x()); + min_y = std::min(min_y, local_vertex.y()); + + max_z = std::max(max_z, static_cast(input_object.shape.footprint.points.at(i).z)); } // calc bounding box state const double length = max_x - min_x; const double width = max_y - min_y; const double height = max_z; - - // calc new center - const Eigen::Vector2d center{ - input_object.kinematics.pose_with_covariance.pose.position.x, - input_object.kinematics.pose_with_covariance.pose.position.y}; - const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); - const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; @@ -344,34 +341,6 @@ inline bool convertConvexHullToBoundingBox( output_object.shape.dimensions.x = length; output_object.shape.dimensions.y = width; output_object.shape.dimensions.z = height; - - return true; -} - -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 diff --git a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml index 8f973b8806a90..dcd080b851c20 100644 --- a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index 1e4a90d8cc08c..e3172dfd22349 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -6,7 +6,6 @@ The ROS 2 multi_object_tracker package Yukihiro Saito Yoshi Ri - Taekjin Lee Apache License 2.0 ament_cmake_auto @@ -14,9 +13,7 @@ eigen3_cmake_module autoware_auto_perception_msgs - diagnostic_updater eigen - glog kalman_filter mussp object_recognition_utils @@ -25,6 +22,7 @@ tf2 tf2_ros tier4_autoware_utils + tier4_perception_msgs unique_identifier_msgs ament_lint_auto diff --git a/perception/multi_object_tracker/src/debugger.cpp b/perception/multi_object_tracker/src/debugger.cpp deleted file mode 100644 index 08394fe80f42a..0000000000000 --- a/perception/multi_object_tracker/src/debugger.cpp +++ /dev/null @@ -1,169 +0,0 @@ -// 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. -// -// - -#include "multi_object_tracker/debugger.hpp" - -#include - -TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&node), node_(node) -{ - // declare debug parameters to decide whether to publish debug topics - loadParameters(); - // initialize debug publishers - if (debug_settings_.publish_processing_time) { - processing_time_publisher_ = - std::make_unique(&node_, "multi_object_tracker"); - } - - if (debug_settings_.publish_tentative_objects) { - debug_tentative_objects_pub_ = - node_.create_publisher( - "debug/tentative_objects", rclcpp::QoS{1}); - } - - // initialize timestamps - const rclcpp::Time now = node_.now(); - last_input_stamp_ = now; - stamp_process_start_ = now; - stamp_process_end_ = now; - stamp_publish_start_ = now; - stamp_publish_output_ = now; - - // setup diagnostics - setupDiagnostics(); -} - -void TrackerDebugger::setupDiagnostics() -{ - diagnostic_updater_.setHardwareID(node_.get_name()); - diagnostic_updater_.add( - "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay); - diagnostic_updater_.setPeriod(0.1); -} - -void TrackerDebugger::loadParameters() -{ - try { - debug_settings_.publish_processing_time = - node_.declare_parameter("publish_processing_time"); - debug_settings_.publish_tentative_objects = - node_.declare_parameter("publish_tentative_objects"); - debug_settings_.diagnostics_warn_delay = - node_.declare_parameter("diagnostics_warn_delay"); - debug_settings_.diagnostics_error_delay = - node_.declare_parameter("diagnostics_error_delay"); - } catch (const std::exception & e) { - RCLCPP_WARN(node_.get_logger(), "Failed to declare parameter: %s", e.what()); - debug_settings_.publish_processing_time = false; - debug_settings_.publish_tentative_objects = false; - debug_settings_.diagnostics_warn_delay = 0.5; - debug_settings_.diagnostics_error_delay = 1.0; - } -} - -void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - if (!is_initialized_) { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Measurement time is not set."); - return; - } - const double & delay = pipeline_latency_ms_ / 1e3; // [s] - - if (delay == 0.0) { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is not calculated."); - } else if (delay < debug_settings_.diagnostics_warn_delay) { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is acceptable"); - } else if (delay < debug_settings_.diagnostics_error_delay) { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::WARN, "Detection delay is over warn threshold."); - } else { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Detection delay is over error threshold."); - } - - stat.add("Detection delay", delay); -} - -void TrackerDebugger::publishTentativeObjects( - const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const -{ - if (debug_settings_.publish_tentative_objects) { - debug_tentative_objects_pub_->publish(tentative_objects); - } -} - -void TrackerDebugger::startMeasurementTime( - const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp) -{ - last_input_stamp_ = measurement_header_stamp; - stamp_process_start_ = now; - if (debug_settings_.publish_processing_time) { - double input_latency_ms = (now - last_input_stamp_).seconds() * 1e3; - processing_time_publisher_->publish( - "debug/input_latency_ms", input_latency_ms); - } - // initialize debug time stamps - if (!is_initialized_) { - stamp_publish_output_ = now; - is_initialized_ = true; - } -} - -void TrackerDebugger::endMeasurementTime(const rclcpp::Time & now) -{ - stamp_process_end_ = now; -} - -void TrackerDebugger::startPublishTime(const rclcpp::Time & now) -{ - stamp_publish_start_ = now; -} - -void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time) -{ - // if the measurement time is not set, do not publish debug information - if (!is_initialized_) return; - - // pipeline latency: time from sensor measurement to publish, used for 'checkDelay' - pipeline_latency_ms_ = (now - last_input_stamp_).seconds() * 1e3; - - if (debug_settings_.publish_processing_time) { - // processing latency: time between input and publish - double processing_latency_ms = ((now - stamp_process_start_).seconds()) * 1e3; - // processing time: only the time spent in the tracking processes - double processing_time_ms = ((now - stamp_publish_start_).seconds() + - (stamp_process_end_ - stamp_process_start_).seconds()) * - 1e3; - // cycle time: time between two consecutive publish - double cyclic_time_ms = (now - stamp_publish_output_).seconds() * 1e3; - // measurement to tracked-object time: time from the sensor measurement to the publishing object - // time If there is not latency compensation, this value is zero - double measurement_to_object_ms = (object_time - last_input_stamp_).seconds() * 1e3; - - // starting from the measurement time - processing_time_publisher_->publish( - "debug/pipeline_latency_ms", pipeline_latency_ms_); - processing_time_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - processing_time_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); - processing_time_publisher_->publish( - "debug/processing_latency_ms", processing_latency_ms); - processing_time_publisher_->publish( - "debug/meas_to_tracked_object_ms", measurement_to_object_ms); - } - stamp_publish_output_ = now; -} 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 416b20bdb91d0..d28833241bd5f 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -16,7 +16,6 @@ #include -#include #include #include @@ -31,6 +30,7 @@ #define EIGEN_MPL2_ONLY #include "multi_object_tracker/multi_object_tracker_core.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -40,20 +40,18 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; namespace { -// Function to get the transform between two frames boost::optional getTransformAnonymous( const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, const std::string & target_frame_id, const rclcpp::Time & time) { try { - // Check if the frames are ready + // check if the frames are ready std::string errstr; // This argument prevents error msg from being displayed in the terminal. if (!tf_buffer.canTransform( target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { return boost::none; } - // Lookup the transform geometry_msgs::msg::TransformStamped self_transform_stamped; self_transform_stamped = tf_buffer.lookupTransform( /*target*/ target_frame_id, /*src*/ source_frame_id, time, @@ -67,16 +65,118 @@ boost::optional getTransformAnonymous( } // namespace +TrackerDebugger::TrackerDebugger(rclcpp::Node & node) +: diagnostic_updater_(&node), node_(node), last_input_stamp_(node.now()) +{ + // declare debug parameters to decide whether to publish debug topics + loadParameters(); + // initialize debug publishers + stop_watch_ptr_ = std::make_unique>(); + if (debug_settings_.publish_processing_time) { + processing_time_publisher_ = + std::make_unique(&node_, "multi_object_tracker"); + } + + if (debug_settings_.publish_tentative_objects) { + debug_tentative_objects_pub_ = + node_.create_publisher( + "debug/tentative_objects", rclcpp::QoS{1}); + } + + // initialize stop watch and diagnostics + startStopWatch(); + setupDiagnostics(); +} + +void TrackerDebugger::setupDiagnostics() +{ + diagnostic_updater_.setHardwareID(node_.get_name()); + diagnostic_updater_.add( + "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay); + diagnostic_updater_.setPeriod(0.1); +} + +void TrackerDebugger::loadParameters() +{ + try { + debug_settings_.publish_processing_time = + node_.declare_parameter("publish_processing_time"); + debug_settings_.publish_tentative_objects = + node_.declare_parameter("publish_tentative_objects"); + debug_settings_.diagnostics_warn_delay = + node_.declare_parameter("diagnostics_warn_delay"); + debug_settings_.diagnostics_error_delay = + node_.declare_parameter("diagnostics_error_delay"); + } catch (const std::exception & e) { + RCLCPP_WARN(node_.get_logger(), "Failed to declare parameter: %s", e.what()); + debug_settings_.publish_processing_time = false; + debug_settings_.publish_tentative_objects = false; + debug_settings_.diagnostics_warn_delay = 0.5; + debug_settings_.diagnostics_error_delay = 1.0; + } +} + +void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + const double delay = elapsed_time_from_sensor_input_; // [s] + + if (delay == 0.0) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is not calculated."); + } else if (delay < debug_settings_.diagnostics_warn_delay) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is acceptable"); + } else if (delay < debug_settings_.diagnostics_error_delay) { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Detection delay is over warn threshold."); + } else { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Detection delay is over error threshold."); + } + + stat.add("Detection delay", delay); +} + +void TrackerDebugger::publishProcessingTime() +{ + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const auto current_time = node_.now(); + elapsed_time_from_sensor_input_ = (current_time - last_input_stamp_).seconds(); + if (debug_settings_.publish_processing_time) { + processing_time_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + processing_time_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + processing_time_publisher_->publish( + "debug/elapsed_time_from_sensor_input_ms", elapsed_time_from_sensor_input_ * 1e3); + } +} + +void TrackerDebugger::publishTentativeObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const +{ + if (debug_settings_.publish_tentative_objects) { + debug_tentative_objects_pub_->publish(tentative_objects); + } +} + +void TrackerDebugger::startStopWatch() +{ + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); +} + +void TrackerDebugger::startMeasurementTime(const rclcpp::Time & measurement_header_stamp) +{ + last_input_stamp_ = measurement_header_stamp; + // start measuring processing time + stop_watch_ptr_->toc("processing_time", true); +} + MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_), - last_published_time_(this->now()) + tf_listener_(tf_buffer_) { - // glog for debug - google::InitGoogleLogging("multi_object_tracker"); - google::InstallFailureSignalHandler(); - // Create publishers and subscribers detected_object_sub_ = create_subscription( "input", rclcpp::QoS{1}, @@ -84,155 +184,260 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); - // Get parameters - double publish_rate = declare_parameter("publish_rate"); // [hz] + // Parameters + double publish_rate = declare_parameter("publish_rate"); world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; - // Create tf timer + // Debug publishers + debugger_ = std::make_unique(*this); + auto cti = std::make_shared( this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_.setCreateTimerInterface(cti); - // Create ROS time based timer. - // If the delay compensation is enabled, the timer is used to publish the output at the correct - // time. + // Create ROS time based timer if (enable_delay_compensation) { - publisher_period_ = 1.0 / publish_rate; // [s] - constexpr double timer_multiplier = 20.0; // 20 times frequent for publish timing check - const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period(); + const auto period_ns = rclcpp::Rate(publish_rate).period(); publish_timer_ = rclcpp::create_timer( - this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this)); + this, get_clock(), period_ns, std::bind(&MultiObjectTracker::onTimer, this)); } - // Initialize processor - { - std::map tracker_map; - tracker_map.insert( - std::make_pair(Label::CAR, this->declare_parameter("car_tracker"))); - tracker_map.insert( - std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); - tracker_map.insert( - std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); - tracker_map.insert( - std::make_pair(Label::TRAILER, this->declare_parameter("trailer_tracker"))); - tracker_map.insert(std::make_pair( - Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); - tracker_map.insert( - std::make_pair(Label::BICYCLE, this->declare_parameter("bicycle_tracker"))); - tracker_map.insert(std::make_pair( - Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); - - processor_ = std::make_unique(tracker_map); - } - - // Data association initialization - { - const auto tmp = this->declare_parameter>("can_assign_matrix"); - const std::vector can_assign_matrix(tmp.begin(), tmp.end()); - const auto max_dist_matrix = this->declare_parameter>("max_dist_matrix"); - const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); - const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); - const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); - const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); - - data_association_ = std::make_unique( - can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, - min_iou_matrix); - } - - // Debugger - debugger_ = std::make_unique(*this); - // published_time_publisher_ = - // std::make_unique(this); + const auto tmp = this->declare_parameter>("can_assign_matrix"); + const std::vector can_assign_matrix(tmp.begin(), tmp.end()); + + const auto max_dist_matrix = this->declare_parameter>("max_dist_matrix"); + const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); + const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); + const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); + + // tracker map + tracker_map_.insert( + std::make_pair(Label::CAR, this->declare_parameter("car_tracker"))); + tracker_map_.insert( + std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); + tracker_map_.insert( + std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); + tracker_map_.insert( + std::make_pair(Label::TRAILER, this->declare_parameter("trailer_tracker"))); + tracker_map_.insert( + std::make_pair(Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); + tracker_map_.insert( + std::make_pair(Label::BICYCLE, this->declare_parameter("bicycle_tracker"))); + tracker_map_.insert( + std::make_pair(Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); + + data_association_ = std::make_unique( + can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, + min_iou_matrix); } void MultiObjectTracker::onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) { - // Get the time of the measurement - const rclcpp::Time measurement_time = - rclcpp::Time(input_objects_msg->header.stamp, this->now().get_clock_type()); - /* keep the latest input stamp and check transform*/ - debugger_->startMeasurementTime(this->now(), measurement_time); - const auto self_transform = - getTransformAnonymous(tf_buffer_, "base_link", world_frame_id_, measurement_time); + debugger_->startMeasurementTime(rclcpp::Time(input_objects_msg->header.stamp)); + const auto self_transform = getTransformAnonymous( + tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp); if (!self_transform) { return; } + /* transform to world coordinate */ autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; if (!object_recognition_utils::transformObjects( *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { return; } + /* tracker prediction */ + rclcpp::Time measurement_time = input_objects_msg->header.stamp; + for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { + (*itr)->predict(measurement_time); + } - ////// Tracker Process - //// Associate and update - /* prediction */ - processor_->predict(measurement_time); - /* object association */ + /* global nearest neighbor */ std::unordered_map direct_assignment, reverse_assignment; - { - const auto & list_tracker = processor_->getListTracker(); - const auto & detected_objects = transformed_objects; - // global nearest neighbor - Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( - detected_objects, list_tracker); // row : tracker, col : measurement - data_association_->assign(score_matrix, direct_assignment, reverse_assignment); + Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( + transformed_objects, list_tracker_); // row : tracker, col : measurement + data_association_->assign(score_matrix, direct_assignment, reverse_assignment); + + /* tracker measurement update */ + int tracker_idx = 0; + for (auto tracker_itr = list_tracker_.begin(); tracker_itr != list_tracker_.end(); + ++tracker_itr, ++tracker_idx) { + if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found + (*(tracker_itr)) + ->updateWithMeasurement( + transformed_objects.objects.at(direct_assignment.find(tracker_idx)->second), + measurement_time, *self_transform); + } else { // not found + (*(tracker_itr))->updateWithoutMeasurement(); + } } - /* tracker update */ - processor_->update(transformed_objects, *self_transform, direct_assignment); - /* tracker pruning */ - processor_->prune(measurement_time); - /* spawn new tracker */ - processor_->spawn(transformed_objects, *self_transform, reverse_assignment); - // debugger time - debugger_->endMeasurementTime(this->now()); + /* life cycle check */ + checkTrackerLifeCycle(list_tracker_, measurement_time, *self_transform); + /* sanitize trackers */ + sanitizeTracker(list_tracker_, measurement_time); + + /* new tracker */ + for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { + if (reverse_assignment.find(i) != reverse_assignment.end()) { // found + continue; + } + std::shared_ptr tracker = + createNewTracker(transformed_objects.objects.at(i), measurement_time, *self_transform); + if (tracker) list_tracker_.push_back(tracker); + } - // Publish objects if the timer is not enabled if (publish_timer_ == nullptr) { - // Publish if the delay compensation is disabled publish(measurement_time); - } else { - // Publish if the next publish time is close - const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period - if ((this->now() - last_published_time_).seconds() > minimum_publish_interval) { - checkAndPublish(this->now()); + } +} + +std::shared_ptr MultiObjectTracker::createNewTracker( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) const +{ + const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (tracker_map_.count(label) != 0) { + const auto tracker = tracker_map_.at(label); + + if (tracker == "bicycle_tracker") { + return std::make_shared(time, object, self_transform); + } else if (tracker == "big_vehicle_tracker") { + return std::make_shared(time, object, self_transform); + } else if (tracker == "multi_vehicle_tracker") { + return std::make_shared(time, object, self_transform); + } else if (tracker == "normal_vehicle_tracker") { + return std::make_shared(time, object, self_transform); + } else if (tracker == "pass_through_tracker") { + return std::make_shared(time, object, self_transform); + } else if (tracker == "pedestrian_and_bicycle_tracker") { + return std::make_shared(time, object, self_transform); + } else if (tracker == "pedestrian_tracker") { + return std::make_shared(time, object, self_transform); } } + return std::make_shared(time, object, self_transform); } void MultiObjectTracker::onTimer() { - const rclcpp::Time current_time = this->now(); - // Check the publish period - const auto elapsed_time = (current_time - last_published_time_).seconds(); - // If the elapsed time is over the period, publish objects with prediction - constexpr double maximum_latency_ratio = 1.11; // 11% margin - const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio; - if (elapsed_time > maximum_publish_latency) { - checkAndPublish(current_time); + rclcpp::Time current_time = this->now(); + const auto self_transform = + getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); + if (!self_transform) { + return; } + + /* life cycle check */ + checkTrackerLifeCycle(list_tracker_, current_time, *self_transform); + /* sanitize trackers */ + sanitizeTracker(list_tracker_, current_time); + + // Publish + publish(current_time); } -void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time) +void MultiObjectTracker::checkTrackerLifeCycle( + std::list> & list_tracker, const rclcpp::Time & time, + [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform) { - /* tracker pruning*/ - processor_->prune(time); + /* params */ + constexpr float max_elapsed_time = 1.0; + + /* delete tracker */ + for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { + const bool is_old = max_elapsed_time < (*itr)->getElapsedTimeFromLastUpdate(time); + if (is_old) { + auto erase_itr = itr; + --itr; + list_tracker.erase(erase_itr); + } + } +} - // Publish - publish(time); +void MultiObjectTracker::sanitizeTracker( + std::list> & list_tracker, const rclcpp::Time & time) +{ + constexpr float min_iou = 0.1; + constexpr float min_iou_for_unknown_object = 0.001; + constexpr double distance_threshold = 5.0; + /* 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); + for (auto itr2 = std::next(itr1); itr2 != list_tracker.end(); ++itr2) { + autoware_auto_perception_msgs::msg::TrackedObject object2; + (*itr2)->getTrackedObject(time, object2); + const double distance = std::hypot( + object1.kinematics.pose_with_covariance.pose.position.x - + object2.kinematics.pose_with_covariance.pose.position.x, + object1.kinematics.pose_with_covariance.pose.position.y - + object2.kinematics.pose_with_covariance.pose.position.y); + if (distance_threshold < distance) { + continue; + } + + const double min_union_iou_area = 1e-2; + const auto iou = object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); + const auto & label1 = (*itr1)->getHighestProbLabel(); + const auto & label2 = (*itr2)->getHighestProbLabel(); + bool should_delete_tracker1 = false; + bool should_delete_tracker2 = false; + + // If at least one of them is UNKNOWN, delete the one with lower IOU. Because the UNKNOWN + // objects are not reliable. + if (label1 == Label::UNKNOWN || label2 == Label::UNKNOWN) { + if (min_iou_for_unknown_object < iou) { + if (label1 == Label::UNKNOWN && label2 == Label::UNKNOWN) { + if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { + should_delete_tracker1 = true; + } else { + should_delete_tracker2 = true; + } + } else if (label1 == Label::UNKNOWN) { + should_delete_tracker1 = true; + } else if (label2 == Label::UNKNOWN) { + should_delete_tracker2 = true; + } + } + } else { // If neither is UNKNOWN, delete the one with lower IOU. + if (min_iou < iou) { + if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { + should_delete_tracker1 = true; + } else { + should_delete_tracker2 = true; + } + } + } + + if (should_delete_tracker1) { + itr1 = list_tracker.erase(itr1); + --itr1; + break; + } else if (should_delete_tracker2) { + itr2 = list_tracker.erase(itr2); + --itr2; + } + } + } +} - // Update last published time - last_published_time_ = this->now(); +inline bool MultiObjectTracker::shouldTrackerPublish( + const std::shared_ptr tracker) const +{ + constexpr int measurement_count_threshold = 3; + if (tracker->getTotalMeasurementCount() < measurement_count_threshold) { + return false; + } + return true; } -void MultiObjectTracker::publish(const rclcpp::Time & time) const +void MultiObjectTracker::publish(const rclcpp::Time & time) { - debugger_->startPublishTime(this->now()); const auto subscriber_count = tracked_objects_pub_->get_subscription_count() + tracked_objects_pub_->get_intra_process_subscription_count(); if (subscriber_count < 1) { @@ -241,22 +446,27 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const // Create output msg autoware_auto_perception_msgs::msg::TrackedObjects output_msg, tentative_objects_msg; output_msg.header.frame_id = world_frame_id_; - processor_->getTrackedObjects(time, output_msg); + output_msg.header.stamp = time; + tentative_objects_msg.header = output_msg.header; + + 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); + tentative_objects_msg.objects.push_back(object); + continue; + } + autoware_auto_perception_msgs::msg::TrackedObject object; + (*itr)->getTrackedObject(time, object); + output_msg.objects.push_back(object); + } // Publish tracked_objects_pub_->publish(output_msg); - // published_time_publisher_->publish_if_subscribed(tracked_objects_pub_, - // output_msg.header.stamp); - // Publish debugger information if enabled - debugger_->endPublishTime(this->now(), time); - - if (debugger_->shouldPublishTentativeObjects()) { - autoware_auto_perception_msgs::msg::TrackedObjects tentative_objects_msg; - tentative_objects_msg.header.frame_id = world_frame_id_; - processor_->getTentativeObjects(time, tentative_objects_msg); - debugger_->publishTentativeObjects(tentative_objects_msg); - } + // Debugger Publish if enabled + debugger_->publishProcessingTime(); + debugger_->publishTentativeObjects(tentative_objects_msg); } RCLCPP_COMPONENTS_REGISTER_NODE(MultiObjectTracker) diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp deleted file mode 100644 index 0d56e16b431e9..0000000000000 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ /dev/null @@ -1,242 +0,0 @@ -// 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. -// -// - -#include "multi_object_tracker/processor/processor.hpp" - -#include "multi_object_tracker/tracker/tracker.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" - -#include - -#include - -using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - -TrackerProcessor::TrackerProcessor(const std::map & tracker_map) -: tracker_map_(tracker_map) -{ - // Set tracker lifetime parameters - max_elapsed_time_ = 1.0; // [s] - - // Set tracker overlap remover parameters - min_iou_ = 0.1; // [ratio] - min_iou_for_unknown_object_ = 0.001; // [ratio] - distance_threshold_ = 5.0; // [m] - - // Set tracker confidence threshold - confident_count_threshold_ = 3; // [count] -} - -void TrackerProcessor::predict(const rclcpp::Time & time) -{ - for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { - (*itr)->predict(time); - } -} - -void TrackerProcessor::update( - const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, - const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment) -{ - int tracker_idx = 0; - const auto & time = detected_objects.header.stamp; - for (auto tracker_itr = list_tracker_.begin(); tracker_itr != list_tracker_.end(); - ++tracker_itr, ++tracker_idx) { - if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found - const auto & associated_object = - detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); - (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform); - } else { // not found - (*(tracker_itr))->updateWithoutMeasurement(); - } - } -} - -void TrackerProcessor::spawn( - const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, - const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment) -{ - const auto & time = detected_objects.header.stamp; - for (size_t i = 0; i < detected_objects.objects.size(); ++i) { - if (reverse_assignment.find(i) != reverse_assignment.end()) { // found - continue; - } - const auto & new_object = detected_objects.objects.at(i); - std::shared_ptr tracker = createNewTracker(new_object, time, self_transform); - if (tracker) list_tracker_.push_back(tracker); - } -} - -std::shared_ptr TrackerProcessor::createNewTracker( - const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) const -{ - const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (tracker_map_.count(label) != 0) { - const auto tracker = tracker_map_.at(label); - if (tracker == "bicycle_tracker") - return std::make_shared(time, object, self_transform); - if (tracker == "big_vehicle_tracker") - return std::make_shared(time, object, self_transform); - if (tracker == "multi_vehicle_tracker") - return std::make_shared(time, object, self_transform); - if (tracker == "normal_vehicle_tracker") - return std::make_shared(time, object, self_transform); - if (tracker == "pass_through_tracker") - return std::make_shared(time, object, self_transform); - if (tracker == "pedestrian_and_bicycle_tracker") - return std::make_shared(time, object, self_transform); - if (tracker == "pedestrian_tracker") - return std::make_shared(time, object, self_transform); - } - return std::make_shared(time, object, self_transform); -} - -void TrackerProcessor::prune(const rclcpp::Time & time) -{ - // Check tracker lifetime: if the tracker is old, delete it - removeOldTracker(time); - // Check tracker overlap: if the tracker is overlapped, delete the one with lower IOU - removeOverlappedTracker(time); -} - -void TrackerProcessor::removeOldTracker(const rclcpp::Time & time) -{ - // Check elapsed time from last update - for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { - const bool is_old = max_elapsed_time_ < (*itr)->getElapsedTimeFromLastUpdate(time); - // If the tracker is old, delete it - if (is_old) { - auto erase_itr = itr; - --itr; - list_tracker_.erase(erase_itr); - } - } -} - -// This function removes overlapped trackers based on distance and IoU criteria -void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) -{ - // Iterate through the list of trackers - for (auto itr1 = list_tracker_.begin(); itr1 != list_tracker_.end(); ++itr1) { - autoware_auto_perception_msgs::msg::TrackedObject object1; - if (!(*itr1)->getTrackedObject(time, object1)) continue; - - // Compare the current tracker with the remaining trackers - for (auto itr2 = std::next(itr1); itr2 != list_tracker_.end(); ++itr2) { - autoware_auto_perception_msgs::msg::TrackedObject object2; - if (!(*itr2)->getTrackedObject(time, object2)) continue; - - // Calculate the distance between the two objects - const double distance = std::hypot( - object1.kinematics.pose_with_covariance.pose.position.x - - object2.kinematics.pose_with_covariance.pose.position.x, - object1.kinematics.pose_with_covariance.pose.position.y - - object2.kinematics.pose_with_covariance.pose.position.y); - - // If the distance is too large, skip - if (distance > distance_threshold_) { - continue; - } - - // Check the Intersection over Union (IoU) between the two objects - const double min_union_iou_area = 1e-2; - const auto iou = object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); - const auto & label1 = (*itr1)->getHighestProbLabel(); - const auto & label2 = (*itr2)->getHighestProbLabel(); - bool should_delete_tracker1 = false; - bool should_delete_tracker2 = false; - - // If both trackers are UNKNOWN, delete the younger tracker - // If one side of the tracker is UNKNOWN, delete UNKNOWN objects - if (label1 == Label::UNKNOWN || label2 == Label::UNKNOWN) { - if (iou > min_iou_for_unknown_object_) { - if (label1 == Label::UNKNOWN && label2 == Label::UNKNOWN) { - if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { - should_delete_tracker1 = true; - } else { - should_delete_tracker2 = true; - } - } else if (label1 == Label::UNKNOWN) { - should_delete_tracker1 = true; - } else if (label2 == Label::UNKNOWN) { - should_delete_tracker2 = true; - } - } - } else { // If neither object is UNKNOWN, delete the younger tracker - if (iou > min_iou_) { - if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { - should_delete_tracker1 = true; - } else { - should_delete_tracker2 = true; - } - } - } - - // Delete the tracker - if (should_delete_tracker1) { - itr1 = list_tracker_.erase(itr1); - --itr1; - break; - } - if (should_delete_tracker2) { - itr2 = list_tracker_.erase(itr2); - --itr2; - } - } - } -} - -bool TrackerProcessor::isConfidentTracker(const std::shared_ptr & tracker) const -{ - // Confidence is determined by counting the number of measurements. - // If the number of measurements is equal to or greater than the threshold, the tracker is - // considered confident. - return tracker->getTotalMeasurementCount() >= confident_count_threshold_; -} - -void TrackerProcessor::getTrackedObjects( - const rclcpp::Time & time, - autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects) const -{ - tracked_objects.header.stamp = time; - for (const auto & tracker : list_tracker_) { - // Skip if the tracker is not confident - if (!isConfidentTracker(tracker)) continue; - // Get the tracked object, extrapolated to the given time - autoware_auto_perception_msgs::msg::TrackedObject tracked_object; - if (tracker->getTrackedObject(time, tracked_object)) { - tracked_objects.objects.push_back(tracked_object); - } - } -} - -void TrackerProcessor::getTentativeObjects( - const rclcpp::Time & time, - autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const -{ - tentative_objects.header.stamp = time; - for (const auto & tracker : list_tracker_) { - if (!isConfidentTracker(tracker)) { - autoware_auto_perception_msgs::msg::TrackedObject tracked_object; - if (tracker->getTrackedObject(time, tracked_object)) { - tentative_objects.objects.push_back(tracked_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 35ce449be4a9a..8a3a5629ec277 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -47,252 +47,399 @@ 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 - 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] + 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] 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}; } else { bounding_box_ = {1.0, 0.5, 1.7}; } - // set maximum and minimum size - constexpr double max_size = 5.0; - constexpr double min_size = 0.3; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); - - // 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); - } - - // 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 - } - - // 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 (!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; - } - - 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); + ekf_.init(X, P); - // initialize motion model - motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); - } + // 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) { - return motion_model_.predictState(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; + } + return ret; } -autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) +bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const { - autoware_auto_perception_msgs::msg::DetectedObject updating_object; - - // OBJECT SHAPE MODEL - // convert to bounding box if input is convex shape - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { - updating_object = object; - } + /* 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 { - updating_object = object; + // 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); } - - // 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 + 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"); } - return updating_object; + return true; } 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); + // yaw measurement + double measurement_yaw = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - // update - bool is_updated = false; - { - 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; + // 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; + } + while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { + measurement_yaw = measurement_yaw - M_PI; + } + use_orientation_information = true; + + } else if ( + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { // know full angle + + use_orientation_information = true; + } + + const int dim_y = + use_orientation_information ? 3 : 2; // pos x, pos y, (pos yaw) depending on Pose output + + // 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 + + // 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 + } 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 (is_yaw_available) { - is_updated = motion_model_.updateStatePoseHead( - x, y, yaw, object.kinematics.pose_with_covariance.covariance); + // 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 { - is_updated = - motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + 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]; + } + } + + // ekf update + if (!ekf_.update(Y, C, R)) { + RCLCPP_WARN(logger_, "Cannot update"); + } + + // normalize yaw and limit vel, slip + { + 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_; } - motion_model_.limitStates(); + 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_; + } + ekf_.init(X_t, P_t); } // position z - constexpr double gain = 0.1; - z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; + constexpr float gain = 0.9; + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - return is_updated; + return true; } 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) { - // do not update shape if the input is not a bounding box - return false; - } - - // check bound box size abnormality - constexpr double size_max = 30.0; // [m] - constexpr double size_min = 0.1; // [m] - if ( - bbox_object.shape.dimensions.x > size_max || bbox_object.shape.dimensions.y > size_max || - bbox_object.shape.dimensions.z > size_max) { - return false; - } else if ( - bbox_object.shape.dimensions.x < size_min || bbox_object.shape.dimensions.y < size_min || - bbox_object.shape.dimensions.z < size_min) { - return false; + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + utils::convertConvexHullToBoundingBox(object, bbox_object); + } else { + bbox_object = object; } - // update object size - constexpr double gain = 0.1; - constexpr double gain_inv = 1.0 - gain; - bounding_box_.length = gain_inv * bounding_box_.length + gain * bbox_object.shape.dimensions.x; - bounding_box_.width = gain_inv * bounding_box_.width + gain * bbox_object.shape.dimensions.y; - bounding_box_.height = gain_inv * bounding_box_.height + gain * bbox_object.shape.dimensions.z; + 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}; - // set maximum and minimum size - constexpr double max_size = 5.0; - constexpr double min_size = 0.3; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); - - // update motion model - motion_model_.updateExtendedState(bounding_box_.length); + // 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 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*/) { - // keep the latest input object - object_ = object; - - // update classification const auto & current_classification = getClassification(); + object_ = object; if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - // check time gap - const double dt = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < dt) { + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( - logger_, - "BicycleTracker::measure There is a large gap between predicted time and measurement time. " - "(%f)", - dt); + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); } - // update object - const autoware_auto_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); - measureWithPose(updating_object); - measureWithShape(updating_object); + measureWithPose(object); + measureWithShape(object); return true; } @@ -304,19 +451,92 @@ 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 f5c15369c2287..80c0df7e5ffb1 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,6 +47,7 @@ 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()) { @@ -56,304 +57,407 @@ 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); // in map coordinate [rad] - float r_stddev_vel = 1.0; // in object coordinate [m/s] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad] + float r_stddev_vel = 1.0; // [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); - - // velocity deviation threshold - // if the predicted velocity is close to the observed velocity, - // the observed velocity is used as the measurement. + // 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] - // OBJECT SHAPE MODEL + // 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; + } + 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; - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { - RCLCPP_WARN( - logger_, - "BigVehicleTracker::BigVehicleTracker: Failed to convert convex hull to bounding box."); - bounding_box_ = {6.0, 2.0, 2.0}; // default value - } else { - bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; - } + utils::convertConvexHullToBoundingBox(object, bbox_object); + bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, + bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - // set maximum and minimum size - constexpr double max_size = 30.0; - constexpr double min_size = 1.0; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); - - // 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); - } + ekf_.init(X, P); - // 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 - } - - // 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; - } + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step - 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; - } + // 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 +} - 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]; +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; + } + // 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; } - - 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); } - - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + if (ret) { + last_update_time_ = time; + } + return ret; } -bool BigVehicleTracker::predict(const rclcpp::Time & time) +bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - return motion_model_.predictState(time); + /* 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"); + } + + return true; } -autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) +bool BigVehicleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) { - autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; + 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; + + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { + // Velocity deviation is small + enable_velocity_measurement = true; + } + } - // 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); + // 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); - // OBJECT SHAPE MODEL - // convert to bounding box if input is convex shape + // convert to boundingbox 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) { - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { - RCLCPP_WARN( - logger_, - "BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); - bbox_object = object; - } + utils::convertConvexHullToBoundingBox(object, bbox_object); } else { bbox_object = object; } - // get offset measurement - int nearest_corner_index = utils::getNearestCornerOrSurface( - tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); + /* get offset measurement*/ + autoware_auto_perception_msgs::msg::DetectedObject offset_object; utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, - bbox_object, tracked_yaw, updating_object, tracking_offset_); - - // UNCERTAINTY MODEL + 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) { - // 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_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 + 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]; } - return updating_object; -} + // 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 -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; + if (!object.kinematics.has_twist_covariance) { + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel + } else { + R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - // update - bool is_updated = false; + // ekf update + if (!ekf_.update(Y, C, R)) { + RCLCPP_WARN(logger_, "Cannot update"); + } + + // normalize yaw and limit vel, 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); + 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_; } - motion_model_.limitStates(); + ekf_.init(X_t, P_t); } // position z - constexpr double gain = 0.1; - z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; + constexpr float gain = 0.9; + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - return is_updated; + return true; } bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & 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 false; - } - - // check object size abnormality - constexpr double size_max = 40.0; // [m] - constexpr double size_min = 1.0; // [m] - if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) { - return false; - } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) { - return false; + // 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; - - // 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; + 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_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - - // set maximum and minimum size - constexpr double max_size = 30.0; - constexpr double min_size = 1.0; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - // 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(); + // 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 return true; } @@ -362,30 +466,20 @@ bool BigVehicleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - // keep the latest input object - object_ = object; - - // update classification const auto & current_classification = getClassification(); + object_ = object; if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - // check time gap - const double dt = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < dt) { + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( - logger_, - "BigVehicleTracker::measure There is a large gap between predicted time and measurement " - "time. (%f)", - dt); + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); } - // update object - const autoware_auto_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); - measureWithPose(updating_object); - measureWithShape(updating_object); + measureWithPose(object); + measureWithShape(object); /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -400,28 +494,100 @@ 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( - 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(); + 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(); // 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; @@ -434,3 +600,32 @@ 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 b5a303d790a83..7714c381894f0 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,6 +47,7 @@ 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()) { @@ -62,300 +63,401 @@ 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); - - // velocity deviation threshold - // if the predicted velocity is close to the observed velocity, - // the observed velocity is used as the measurement. + // 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] - // OBJECT SHAPE MODEL + // 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; + } + 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; - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { - RCLCPP_WARN( - logger_, - "NormalVehicleTracker::NormalVehicleTracker: Failed to convert convex hull to bounding " - "box."); - bounding_box_ = {3.0, 2.0, 1.8}; // default value - } else { - bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; - } + utils::convertConvexHullToBoundingBox(object, bbox_object); + bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, + bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - // set maximum and minimum size - constexpr double max_size = 20.0; - constexpr double min_size = 1.0; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); - - // 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); - } + ekf_.init(X, P); - // 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 - } - - // 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; - } + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step - 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; - } + // 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 +} - 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]; +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; + } + // 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; } - - 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); } - - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + if (ret) { + last_update_time_ = time; + } + return ret; } -bool NormalVehicleTracker::predict(const rclcpp::Time & time) +bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - return motion_model_.predictState(time); + /* 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"); + } + + return true; } -autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) +bool NormalVehicleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) { - autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; + 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; + } - // 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); + // 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; - // 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) { - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { - RCLCPP_WARN( - logger_, - "NormalVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); - bbox_object = object; + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { + // Velocity deviation is small + enable_velocity_measurement = true; } + } + + // 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); + // convert to boundingbox 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); } else { bbox_object = object; } - // get offset measurement - int nearest_corner_index = utils::getNearestCornerOrSurface( - tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); + /* get offset measurement*/ + autoware_auto_perception_msgs::msg::DetectedObject offset_object; utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, - bbox_object, tracked_yaw, updating_object, tracking_offset_); - - // UNCERTAINTY MODEL + 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) { - // 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_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 + 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]; } - return updating_object; -} + // 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 -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; + if (!object.kinematics.has_twist_covariance) { + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel + } else { + R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - // update - bool is_updated = false; + // ekf update + if (!ekf_.update(Y, C, R)) { + RCLCPP_WARN(logger_, "Cannot update"); + } + + // normalize yaw and limit vel, 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); + 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_; } - motion_model_.limitStates(); + ekf_.init(X_t, P_t); } // position z - constexpr double gain = 0.1; - z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; + constexpr float gain = 0.9; + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - return is_updated; + return true; } bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & 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 false; - } - - // check object size abnormality - constexpr double size_max = 30.0; // [m] - constexpr double size_min = 1.0; // [m] - if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) { - return false; - } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) { - return false; + // 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; - - // 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; + 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_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - // set maximum and minimum size - constexpr double max_size = 20.0; - constexpr double min_size = 1.0; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); - - // 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(); + // 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 return true; } @@ -364,30 +466,20 @@ bool NormalVehicleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - // keep the latest input object - object_ = object; - - // update classification const auto & current_classification = getClassification(); + object_ = object; if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - // check time gap - const double dt = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < dt) { + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( - logger_, - "NormalVehicleTracker::measure There is a large gap between predicted time and measurement " - "time. (%f)", - dt); + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); } - // update object - const autoware_auto_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); - measureWithPose(updating_object); - measureWithShape(updating_object); + measureWithPose(object); + measureWithShape(object); /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -402,28 +494,100 @@ 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( - 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(); + 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(); // 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; @@ -436,3 +600,32 @@ 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 c03ab867cd8d1..57c2e8f899951 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -47,6 +47,7 @@ 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; @@ -59,196 +60,262 @@ 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.5, 1.7}; + cylinder_ = {0.3, 1.7}; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - // do not update polygon shape - } - // set maximum and minimum size - constexpr double max_size = 5.0; - constexpr double min_size = 0.3; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); - cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size); - cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size); - // 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); - } + ekf_.init(X, P); } bool PedestrianTracker::predict(const rclcpp::Time & time) { - return motion_model_.predictState(time); + const double dt = (time - last_update_time_).seconds(); + bool ret = predict(dt, ekf_); + if (ret) { + last_update_time_ = time; + } + return ret; } -autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) +bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const { - autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - - // 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 + /* 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"); } - return updating_object; + + return true; } bool PedestrianTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // update motion model - bool is_updated = false; - { - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; + 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]; + } - is_updated = - motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); - motion_model_.limitStates(); + // ekf update + if (!ekf_.update(Y, C, R)) { + RCLCPP_WARN(logger_, "Cannot update"); + } + + // normalize yaw and limit vx, wz + { + 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); } // position z - constexpr double gain = 0.1; - z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; + constexpr float gain = 0.9; + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - return is_updated; + return true; } bool PedestrianTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr double gain = 0.1; - constexpr double gain_inv = 1.0 - gain; - + constexpr float gain = 0.9; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - // check bound box size abnormality - constexpr double size_max = 30.0; // [m] - constexpr double size_min = 0.1; // [m] - if ( - object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max || - object.shape.dimensions.z > size_max) { - return false; - } else if ( - object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min || - object.shape.dimensions.z < size_min) { - return false; - } - // update bounding box 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; + bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; + bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - // check cylinder size abnormality - constexpr double size_max = 30.0; // [m] - constexpr double size_min = 0.1; // [m] - if (object.shape.dimensions.x > size_max || object.shape.dimensions.z > size_max) { - return false; - } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.z < size_min) { - return false; - } - cylinder_.width = gain_inv * cylinder_.width + gain * object.shape.dimensions.x; - cylinder_.height = gain_inv * cylinder_.height + gain * object.shape.dimensions.z; + cylinder_.width = gain * cylinder_.width + (1.0 - gain) * object.shape.dimensions.x; + cylinder_.height = gain * cylinder_.height + (1.0 - gain) * object.shape.dimensions.z; } else { - // do not update polygon shape return false; } - // set maximum and minimum size - constexpr double max_size = 5.0; - constexpr double min_size = 0.3; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); - cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size); - cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size); - return true; } @@ -256,29 +323,20 @@ bool PedestrianTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - // keep the latest input object - object_ = object; - const auto & current_classification = getClassification(); + object_ = object; if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - // check time gap - const double dt = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < dt) { + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( - logger_, - "PedestrianTracker::measure There is a large gap between predicted time and measurement " - "time. (%f)", - dt); + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); } - // update object - const autoware_auto_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); - measureWithPose(updating_object); - measureWithShape(updating_object); + measureWithPose(object); + measureWithShape(object); (void)self_transform; // currently do not use self vehicle position return true; @@ -291,19 +349,70 @@ 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 450bb2d94abb6..897b858a6aabe 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -12,14 +12,6 @@ // 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 @@ -30,179 +22,227 @@ #else #include #endif -#include "object_recognition_utils/object_recognition_utils.hpp" #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" + #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 - // measurement noise covariance - constexpr double r_stddev_x = 1.0; // [m] - constexpr double r_stddev_y = 1.0; // [m] + 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); ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.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); + 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 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; + // 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; } - - // 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); } + + ekf_.init(X, P); } bool UnknownTracker::predict(const rclcpp::Time & time) { - return motion_model_.predictState(time); + const double dt = (time - last_update_time_).seconds(); + bool ret = predict(dt, ekf_); + if (ret) { + last_update_time_ = time; + } + return ret; } -autoware_auto_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) +bool UnknownTracker::predict(const double dt, KalmanFilter & ekf) const { - autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - - // 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 + /* == 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"); } - return updating_object; + + return true; } bool UnknownTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // update motion model - bool is_updated = false; - { - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; + 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; - is_updated = - motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); - motion_model_.limitStates(); + /* 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 + { + 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); } // position z - constexpr double gain = 0.1; - z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; + constexpr float gain = 0.9; + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - return is_updated; + return true; } 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; - // check time gap - const double dt = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < dt) { + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { RCLCPP_WARN( logger_, - "UnknownTracker::measure There is a large gap between predicted time and measurement time. " - "(%f)", - dt); + "Pedestrian : There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); } - // update object - const autoware_auto_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); - measureWithPose(updating_object); + measureWithPose(object); return true; } @@ -214,19 +254,54 @@ bool UnknownTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - 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; + // 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; // 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 deleted file mode 100644 index ef8a45f608098..0000000000000 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ /dev/null @@ -1,483 +0,0 @@ -// 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 - -// cspell: ignore CTRV -// 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); - 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(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 deleted file mode 100644 index 9e8327e381057..0000000000000 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ /dev/null @@ -1,386 +0,0 @@ -// 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); - 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(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 deleted file mode 100644 index a9ad03e7875c5..0000000000000 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ /dev/null @@ -1,307 +0,0 @@ -// 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); - ekf.getX(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 deleted file mode 100644 index 4d51021de634b..0000000000000 --- a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// 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 < 1e-6 /*1usec*/) { - return true; - } - - // multi-step prediction - // if dt is too large, shorten dt and repeat prediction - const uint32_t repeat = std::floor(dt / dt_max_) + 1; - const double dt_ = dt / repeat; - for (uint32_t i = 0; i < repeat; ++i) { - if (!predictStateStep(dt_, ekf_)) return false; - last_update_time_ += rclcpp::Duration::from_seconds(dt_); - } - // reset the last_update_time_ to the prediction 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; - - const double dt = getDeltaTime(time); - if (dt < 1e-6 /*1usec*/) { - // no prediction, return the current state - ekf_.getX(X); - ekf_.getP(P); - return true; - } - - // copy the predicted state and covariance - KalmanFilter tmp_ekf_for_no_update = ekf_; - // multi-step prediction - // if dt is too large, shorten dt and repeat prediction - const uint32_t repeat = std::floor(dt / dt_max_) + 1; - 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; -}