diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 3e379bcfd1cf1..19f75b5ba2306 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -12,6 +12,7 @@ endif() ### Find Eigen Dependencies find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) +find_package(glog REQUIRED) include_directories( SYSTEM @@ -21,6 +22,15 @@ 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 @@ -30,21 +40,20 @@ set(MULTI_OBJECT_TRACKER_SRC src/tracker/model/pedestrian_and_bicycle_tracker.cpp src/tracker/model/unknown_tracker.cpp src/tracker/model/pass_through_tracker.cpp - src/data_association/data_association.cpp - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp ) -ament_auto_add_library(multi_object_tracker_node SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED ${MULTI_OBJECT_TRACKER_SRC} ) -target_link_libraries(multi_object_tracker_node +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen + glog::glog ) -rclcpp_components_register_node(multi_object_tracker_node +rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "MultiObjectTracker" - EXECUTABLE multi_object_tracker + EXECUTABLE ${PROJECT_NAME}_node ) 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 new file mode 100644 index 0000000000000..4705f3c0fe0d4 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp @@ -0,0 +1,75 @@ +// 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 95d33b78ff184..2bc432b0d5275 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,9 +40,6 @@ #include #endif -#include -#include - #include #include @@ -50,81 +47,45 @@ #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_; - rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer - - // debugger class - std::unique_ptr debugger_; - tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - std::map tracker_map_; + // debugger + std::unique_ptr debugger_; + // std::unique_ptr published_time_publisher_; - void onMeasurement( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); - void onTimer(); + // publish timer + rclcpp::TimerBase::SharedPtr publish_timer_; + rclcpp::Time last_published_time_; + double publisher_period_; + // internal states std::string world_frame_id_; // tracking frame - std::list> list_tracker_; std::unique_ptr data_association_; + std::unique_ptr processor_; - 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; + // callback functions + void onMeasurement( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); + void onTimer(); - void publish(const rclcpp::Time & time); + // publish processes + void checkAndPublish(const rclcpp::Time & time); + void publish(const rclcpp::Time & time) const; 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 new file mode 100644 index 0000000000000..6d6e364d83a41 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp @@ -0,0 +1,79 @@ +// 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 ba50933eddaec..2d69334a2f43f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -20,6 +20,7 @@ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include @@ -30,36 +31,15 @@ class BicycleTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { - char dim_x = 5; - float q_stddev_acc_long; - float q_stddev_acc_lat; - float q_stddev_yaw_rate_min; - float q_stddev_yaw_rate_max; - float q_cov_slip_rate_min; - float q_cov_slip_rate_max; - float q_max_slip_angle; - float p0_cov_vel; - float p0_cov_slip; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; } ekf_params_; - double max_vel_; - double max_slip_; - double lf_; - double lr_; - float z_; + double z_; -private: struct BoundingBox { double length; @@ -67,7 +47,11 @@ class BicycleTracker : public Tracker double height; }; BoundingBox bounding_box_; - BoundingBox last_input_bounding_box_; + +private: + BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: BicycleTracker( @@ -75,10 +59,12 @@ class BicycleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index e19b6382ad952..8d49138b94a24 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -20,47 +20,28 @@ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include + class BigVehicleTracker : public Tracker { private: autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; - int last_nearest_corner_index_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { - char dim_x = 5; - float q_stddev_acc_long; - float q_stddev_acc_lat; - float q_stddev_yaw_rate_min; - float q_stddev_yaw_rate_max; - float q_cov_slip_rate_min; - float q_cov_slip_rate_max; - float q_max_slip_angle; - float p0_cov_vel; - float p0_cov_slip; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; - float r_cov_vel; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; + double r_cov_vel; } ekf_params_; - double max_vel_; - double max_slip_; - double lf_; - double lr_; - float z_; double velocity_deviation_threshold_; -private: + double z_; + struct BoundingBox { double length; @@ -70,6 +51,12 @@ class BigVehicleTracker : public Tracker BoundingBox bounding_box_; BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; + int last_nearest_corner_index_; + +private: + BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: BigVehicleTracker( @@ -77,18 +64,28 @@ class BigVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; - double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); virtual ~BigVehicleTracker() {} + +private: + void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) + { + Eigen::MatrixXd X_t(DIM, 1); + motion_model_.getStateVector(X_t); + last_nearest_corner_index_ = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); + } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 05fa0a5ef01ba..6ab7e63bce167 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -19,7 +19,8 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ -#include "tracker_base.hpp" +#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include @@ -28,42 +29,19 @@ class NormalVehicleTracker : public Tracker private: autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; - int last_nearest_corner_index_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; - struct EkfParams { - char dim_x = 5; - float q_stddev_acc_long; - float q_stddev_acc_lat; - float q_stddev_yaw_rate_min; - float q_stddev_yaw_rate_max; - float q_cov_slip_rate_min; - float q_cov_slip_rate_max; - float q_max_slip_angle; - float p0_cov_vel; - float p0_cov_slip; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; - float r_cov_vel; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; + double r_cov_vel; } ekf_params_; - - double max_vel_; - double max_slip_; - double lf_; - double lr_; - float z_; double velocity_deviation_threshold_; -private: + double z_; + struct BoundingBox { double length; @@ -73,6 +51,12 @@ class NormalVehicleTracker : public Tracker BoundingBox bounding_box_; BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; + int last_nearest_corner_index_; + +private: + BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: NormalVehicleTracker( @@ -80,18 +64,28 @@ class NormalVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); - double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); virtual ~NormalVehicleTracker() {} + +private: + void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) + { + Eigen::MatrixXd X_t(DIM, 1); + motion_model_.getStateVector(X_t); + last_nearest_corner_index_ = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); + } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 0219d3b227044..a56250390e34f 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,9 +20,12 @@ #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: @@ -30,38 +33,15 @@ class PedestrianTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - YAW = 2, - VX = 3, - WZ = 4, - }; struct EkfParams { - char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_wz; - float q_cov_vx; - float p0_cov_vx; - float p0_cov_wz; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; } ekf_params_; - double max_vx_; - double max_wz_; - float z_; + double z_; -private: struct BoundingBox { double length; @@ -76,16 +56,23 @@ class PedestrianTracker : public Tracker BoundingBox bounding_box_; Cylinder cylinder_; +private: + CTRVMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = CTRVMotionModel::IDX; + public: PedestrianTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index d2850793fa4bb..ab6a747288d4a 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 18e6c0606a535..73bf7849e13d8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -13,13 +13,14 @@ // limitations under the License. // // -// v1.0 Yukihiro Saito +// Author: v1.0 Yukihiro Saito // #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#include "tracker_base.hpp" +#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include @@ -30,30 +31,20 @@ class UnknownTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - VX = 2, - VY = 3, - }; struct EkfParams { - char dim_x = 4; - float q_cov_x; - float q_cov_y; - float q_cov_vx; - float q_cov_vy; - float p0_cov_vx; - float p0_cov_vy; - float r_cov_x; - float r_cov_y; - float p0_cov_x; - float p0_cov_y; + double r_cov_x; + double r_cov_y; + double r_cov_vx; + double r_cov_vy; } ekf_params_; - float max_vx_, max_vy_; - float z_; + + double z_; + +private: + CVMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = CVMotionModel::IDX; public: UnknownTracker( @@ -61,10 +52,12 @@ class UnknownTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp new file mode 100644 index 0000000000000..5127d0448835c --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -0,0 +1,108 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +class BicycleMotionModel : public MotionModel +{ +private: + // attributes + rclcpp::Logger logger_; + + // extended state + double lf_; + double lr_; + + // motion parameters + struct MotionParams + { + double q_stddev_acc_long; + double q_stddev_acc_lat; + double q_stddev_yaw_rate_min; + double q_stddev_yaw_rate_max; + double q_cov_slip_rate_min; + double q_cov_slip_rate_max; + double q_max_slip_angle; + double lf_ratio; + double lr_ratio; + double lf_min; + double lr_min; + double max_vel; + double max_slip; + } motion_params_; + +public: + BicycleMotionModel(); + + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; + const char DIM = 5; + + bool initialize( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & slip, const double & slip_cov, const double & length); + + void setDefaultParams(); + + void setMotionParams( + const double & q_stddev_acc_long, const double & q_stddev_acc_lat, + const double & q_stddev_yaw_rate_min, const double & q_stddev_yaw_rate_max, + const double & q_stddev_slip_rate_min, const double & q_stddev_slip_rate_max, + const double & q_max_slip_angle, const double & lf_ratio, const double & lf_min, + const double & lr_ratio, const double & lr_min); + + void setMotionLimits(const double & max_vel, const double & max_slip); + + bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); + + bool updateStatePoseHead( + const double & x, const double & y, const double & yaw, + const std::array & pose_cov); + + bool updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov); + + bool adjustPosition(const double & x, const double & y); + + bool limitStates(); + + bool updateExtendedState(const double & length); + + bool predictStateStep(const double dt, KalmanFilter & ekf) const override; + + bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp new file mode 100644 index 0000000000000..6b071eddec7a9 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -0,0 +1,95 @@ +// 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 new file mode 100644 index 0000000000000..59032706b00d6 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -0,0 +1,88 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +class CVMotionModel : public MotionModel +{ +private: + // attributes + rclcpp::Logger logger_; + + // motion parameters + struct MotionParams + { + double q_cov_x; + double q_cov_y; + double q_cov_vx; + double q_cov_vy; + double max_vx; + double max_vy; + } motion_params_; + +public: + CVMotionModel(); + + enum IDX { X = 0, Y = 1, VX = 2, VY = 3 }; + const char DIM = 4; + + bool initialize( + const rclcpp::Time & time, const double & x, const double & y, + const std::array & pose_cov, const double & vx, const double & vy, + const std::array & twist_cov); + + void setDefaultParams(); + + void setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, + const double & q_stddev_vy); + + void setMotionLimits(const double & max_vx, const double & max_vy); + + bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); + + bool updateStatePoseVel( + const double & x, const double & y, const std::array & pose_cov, const double & vx, + const double & vy, const std::array & twist_cov); + + bool adjustPosition(const double & x, const double & y); + + bool limitStates(); + + bool predictStateStep(const double dt, KalmanFilter & ekf) const override; + + bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp new file mode 100644 index 0000000000000..1aca602ed66a3 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp @@ -0,0 +1,67 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +class MotionModel +{ +private: + bool is_initialized_{false}; + double dt_max_{0.11}; // [s] maximum time interval for prediction + +protected: + rclcpp::Time last_update_time_; + KalmanFilter ekf_; + +public: + MotionModel(); + virtual ~MotionModel() = default; + + bool checkInitialized() const { return is_initialized_; } + double getDeltaTime(const rclcpp::Time & time) const + { + return (time - last_update_time_).seconds(); + } + void setMaxDeltaTime(const double dt_max) { dt_max_ = dt_max; } + double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } + void getStateVector(Eigen::MatrixXd & X) const { ekf_.getX(X); } + + bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); + + bool predictState(const rclcpp::Time & time); + bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; + + virtual bool predictStateStep(const double dt, KalmanFilter & ekf) const = 0; + virtual bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const = 0; +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index d2b6ee5de475e..5842246bc1313 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,40 +295,43 @@ inline void calcAnchorPointOffset( * @param input_object: input convex hull objects * @param output_object: output bounding box objects */ -inline void convertConvexHullToBoundingBox( +inline bool convertConvexHullToBoundingBox( const autoware_auto_perception_msgs::msg::DetectedObject & input_object, autoware_auto_perception_msgs::msg::DetectedObject & output_object) { - 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(); + // check footprint size + if (input_object.shape.footprint.points.size() < 3) { + return false; + } + // 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) { - 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)); + 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); } // 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; @@ -341,6 +344,34 @@ inline void 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 dcd080b851c20..8f973b8806a90 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 e3172dfd22349..1e4a90d8cc08c 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -6,6 +6,7 @@ The ROS 2 multi_object_tracker package Yukihiro Saito Yoshi Ri + Taekjin Lee Apache License 2.0 ament_cmake_auto @@ -13,7 +14,9 @@ eigen3_cmake_module autoware_auto_perception_msgs + diagnostic_updater eigen + glog kalman_filter mussp object_recognition_utils @@ -22,7 +25,6 @@ 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 new file mode 100644 index 0000000000000..b5632a13e78fc --- /dev/null +++ b/perception/multi_object_tracker/src/debugger.cpp @@ -0,0 +1,169 @@ +// 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_; // [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 d28833241bd5f..416b20bdb91d0 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -16,6 +16,7 @@ #include +#include #include #include @@ -30,7 +31,6 @@ #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,18 +40,20 @@ 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, @@ -65,118 +67,16 @@ 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_) + tf_listener_(tf_buffer_), + last_published_time_(this->now()) { + // glog for debug + google::InitGoogleLogging("multi_object_tracker"); + google::InstallFailureSignalHandler(); + // Create publishers and subscribers detected_object_sub_ = create_subscription( "input", rclcpp::QoS{1}, @@ -184,260 +84,155 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); - // Parameters - double publish_rate = declare_parameter("publish_rate"); + // Get parameters + double publish_rate = declare_parameter("publish_rate"); // [hz] world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; - // Debug publishers - debugger_ = std::make_unique(*this); - + // Create tf timer auto cti = std::make_shared( this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_.setCreateTimerInterface(cti); - // Create ROS time based timer + // Create ROS time based timer. + // If the delay compensation is enabled, the timer is used to publish the output at the correct + // time. if (enable_delay_compensation) { - const auto period_ns = rclcpp::Rate(publish_rate).period(); + 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(); publish_timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&MultiObjectTracker::onTimer, this)); + this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, 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); + // 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); } 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(rclcpp::Time(input_objects_msg->header.stamp)); - const auto self_transform = getTransformAnonymous( - tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp); + debugger_->startMeasurementTime(this->now(), measurement_time); + const auto self_transform = + getTransformAnonymous(tf_buffer_, "base_link", world_frame_id_, measurement_time); 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); - } - /* global nearest neighbor */ + ////// Tracker Process + //// Associate and update + /* prediction */ + processor_->predict(measurement_time); + /* object association */ std::unordered_map 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(); - } + { + 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); } + /* 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); - /* 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); - } + // debugger time + debugger_->endMeasurementTime(this->now()); + // Publish objects if the timer is not enabled if (publish_timer_ == nullptr) { + // Publish if the delay compensation is disabled publish(measurement_time); - } -} - -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); + } 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()); } } - return std::make_shared(time, object, self_transform); } void MultiObjectTracker::onTimer() { - rclcpp::Time current_time = this->now(); - const auto self_transform = - getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); - if (!self_transform) { - return; + 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); } - - /* life cycle check */ - checkTrackerLifeCycle(list_tracker_, current_time, *self_transform); - /* sanitize trackers */ - sanitizeTracker(list_tracker_, current_time); - - // Publish - publish(current_time); } -void MultiObjectTracker::checkTrackerLifeCycle( - std::list> & list_tracker, const rclcpp::Time & time, - [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform) +void MultiObjectTracker::checkAndPublish(const rclcpp::Time & 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); - } - } -} + /* tracker pruning*/ + processor_->prune(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; - } - } - } -} + // Publish + publish(time); -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; + // Update last published time + last_published_time_ = this->now(); } -void MultiObjectTracker::publish(const rclcpp::Time & time) +void MultiObjectTracker::publish(const rclcpp::Time & time) const { + 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) { @@ -446,27 +241,22 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) // Create output msg autoware_auto_perception_msgs::msg::TrackedObjects output_msg, tentative_objects_msg; output_msg.header.frame_id = world_frame_id_; - 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); - } + processor_->getTrackedObjects(time, output_msg); // Publish tracked_objects_pub_->publish(output_msg); + // published_time_publisher_->publish_if_subscribed(tracked_objects_pub_, + // output_msg.header.stamp); - // Debugger Publish if enabled - debugger_->publishProcessingTime(); - debugger_->publishTentativeObjects(tentative_objects_msg); + // 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); + } } 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 new file mode 100644 index 0000000000000..0d56e16b431e9 --- /dev/null +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -0,0 +1,242 @@ +// 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 8a3a5629ec277..35ce449be4a9a 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -47,399 +47,252 @@ BicycleTracker::BicycleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BicycleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - float r_stddev_x = 0.5; // in vehicle coordinate [m] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + double r_stddev_x = 0.5; // in vehicle coordinate [m] + double r_stddev_y = 0.4; // in vehicle coordinate [m] + double r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // in map coordinate [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - // initial state covariance - float p0_stddev_x = 0.8; // [m] - float p0_stddev_y = 0.5; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(1); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - - // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } + // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { bounding_box_ = {1.0, 0.5, 1.7}; } - ekf_.init(X, P); - - // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m - lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m -} - -bool BicycleTracker::predict(const rclcpp::Time & time) -{ - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); - return false; - } - // if dt is too large, shorten dt and repeat prediction - const double dt_max = 0.11; - const uint32_t repeat = std::ceil(dt / dt_max); - const double dt_ = dt / repeat; - bool ret = false; - for (uint32_t i = 0; i < repeat; ++i) { - ret = predict(dt_, ekf_); - if (!ret) { - return false; - } - } - if (ret) { - last_update_time_ = time; + // set 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); } - return ret; -} -bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); + // Set motion limits + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(80); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } - return true; -} - -bool BicycleTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - // yaw measurement - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - - // prediction - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); + // 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; + } - // validate if orientation is available - bool use_orientation_information = false; - if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { // has 180 degree - // uncertainty - // fix orientation - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 0.8; // in object coordinate [m] + constexpr double p0_stddev_y = 0.5; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } - use_orientation_information = true; - } else if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { // know full angle + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); - use_orientation_information = true; + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } +} - const int dim_y = - use_orientation_information ? 3 : 2; // pos x, pos y, (pos yaw) depending on Pose output +bool BicycleTracker::predict(const rclcpp::Time & time) +{ + return motion_model_.predictState(time); +} - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y +autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) +{ + autoware_auto_perception_msgs::msg::DetectedObject updating_object; - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x + // OBJECT SHAPE MODEL + // convert to bounding box if input is convex shape + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { + updating_object = object; + } } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + updating_object = object; } - // if there are orientation available - if (dim_y == 3) { - // fill yaw observation and measurement matrix - Y(IDX::YAW, 0) = measurement_yaw; - C(2, IDX::YAW) = 1.0; - - // fill yaw covariance - if (!object.kinematics.has_position_covariance) { - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + // fill covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + pose_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_x; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0; // x - y + pose_cov[utils::MSG_COV_IDX::Y_X] = 0; // y - x + pose_cov[utils::MSG_COV_IDX::Y_Y] = ekf_params_.r_cov_y; // y - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw } - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); - } + return updating_object; +} + +bool BicycleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // get measurement yaw angle to update + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); + double measurement_yaw = 0.0; + bool is_yaw_available = utils::getMeasurementYaw(object, tracked_yaw, measurement_yaw); - // normalize yaw and limit vel, slip + // update + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = measurement_yaw; + + if (is_yaw_available) { + is_updated = motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); + } else { + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); } - ekf_.init(X_t, P_t); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { - bbox_object = 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; } - 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}; + // 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; + } + + // 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; - // 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 + // 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); return true; } bool BicycleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "BicycleTracker::measure There is a large gap between predicted time and measurement time. " + "(%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); return true; } @@ -451,92 +304,19 @@ bool BicycleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 80c0df7e5ffb1..f5c15369c2287 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -47,7 +47,6 @@ BigVehicleTracker::BigVehicleTracker( const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -57,407 +56,304 @@ BigVehicleTracker::BigVehicleTracker( // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty float r_stddev_x = 0.5; // in vehicle coordinate [m] float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad] - float r_stddev_vel = 1.0; // [m/s] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); - // initial state covariance - float p0_stddev_x = 1.5; // [m] - float p0_stddev_y = 0.5; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } + // velocity deviation threshold + // if the predicted velocity is close to the observed velocity, + // the observed velocity is used as the measurement. + velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } else { - // past default value - // bounding_box_ = {7.0, 2.0, 2.0}; autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - utils::convertConvexHullToBoundingBox(object, bbox_object); - bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + 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}; + } last_input_bounding_box_ = bounding_box_; } - ekf_.init(X, P); + // 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); + } - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + // 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 lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m - lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m -} + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double vel_cov; + const double & length = bounding_box_.length; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } -bool 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; + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 1.5; // in object coordinate [m] + constexpr double p0_stddev_y = 0.5; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; } - } - if (ret) { - last_update_time_ = time; - } - return ret; -} -bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); + + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - return true; + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } -bool BigVehicleTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) +bool BigVehicleTracker::predict(const rclcpp::Time & time) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - - float r_cov_x; - float r_cov_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - - if (utils::isLargeVehicleLabel(label)) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (label == Label::CAR) { - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } - - // Decide dimension of measurement vector - bool enable_velocity_measurement = false; - if (object.kinematics.has_twist) { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - const double predicted_vel = X_t(IDX::VEL); - const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + return motion_model_.predictState(time); +} - if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { - // Velocity deviation is small - enable_velocity_measurement = true; - } - } +autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) +{ + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - // pos x, pos y, yaw, vel depending on pose measurement - const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); + // current (predicted) state + const double tracked_x = motion_model_.getStateElement(IDX::X); + const double tracked_y = motion_model_.getStateElement(IDX::Y); + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); - // convert to boundingbox if input is convex shape + // OBJECT SHAPE MODEL + // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); + if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + RCLCPP_WARN( + logger_, + "BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); + bbox_object = object; + } } else { bbox_object = object; } - /* get offset measurement*/ - autoware_auto_perception_msgs::msg::DetectedObject offset_object; + // get offset measurement + int nearest_corner_index = utils::getNearestCornerOrSurface( + tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, - bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; - Y(IDX::YAW, 0) = measurement_yaw; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(measurement_yaw); - const double sin_yaw = std::sin(measurement_yaw); - const double sin_2yaw = std::sin(2.0f * measurement_yaw); - R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - R(1, 0) = R(0, 1); // y - x - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } + last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, + bbox_object, tracked_yaw, updating_object, tracking_offset_); - // 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 - - if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vel; // vel -vel + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + // measurement noise covariance + float r_cov_x; + float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (utils::isLargeVehicleLabel(label)) { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } else if (label == Label::CAR) { + // if label is changed, enlarge the measurement noise covariance + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); } else { - R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; } + + // yaw angle fix + double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + + // fill covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + pose_cov[utils::MSG_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + if (!is_yaw_available) { + pose_cov[utils::MSG_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value + } + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel } - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); + return updating_object; +} + +bool BigVehicleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // current (predicted) state + const double tracked_vel = motion_model_.getStateElement(IDX::VEL); + + // velocity capability is checked only when the object has velocity measurement + // and the predicted velocity is close to the observed velocity + bool is_velocity_available = false; + if (object.kinematics.has_twist) { + const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { + // Velocity deviation is small + is_velocity_available = true; + } } - // normalize yaw and limit vel, slip + // update + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double vel = object.kinematics.twist_with_covariance.twist.linear.x; + + if (is_velocity_available) { + is_updated = motion_model_.updateStatePoseHeadVel( + x, y, yaw, object.kinematics.pose_with_covariance.covariance, vel, + object.kinematics.twist_with_covariance.covariance); + } else { + is_updated = motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); } - ekf_.init(X_t, P_t); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type - autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { - bbox_object = object; + 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; } - 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; + // 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; + } + + 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; last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + + // set 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); - // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m - lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m + // update motion model + motion_model_.updateExtendedState(bounding_box_.length); + + // // update offset into object position + // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); + // // update offset + // tracking_offset_.x() = gain_inv * tracking_offset_.x(); + // tracking_offset_.y() = gain_inv * tracking_offset_.y(); return true; } @@ -466,20 +362,30 @@ bool BigVehicleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "BigVehicleTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -494,100 +400,28 @@ bool BigVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "BigVehicleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // recover bounding box from tracking point const double dl = bounding_box_.length - last_input_bounding_box_.length; const double dw = bounding_box_.width - last_input_bounding_box_.width; const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - X_t(IDX::X) = recovered_pose.x(); - X_t(IDX::Y) = recovered_pose.y(); + pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, + motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); + pose_with_cov.pose.position.x = recovered_pose.x(); + pose_with_cov.pose.position.y = recovered_pose.y(); // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; @@ -600,32 +434,3 @@ bool BigVehicleTracker::getTrackedObject( return true; } - -void BigVehicleTracker::setNearestCornerOrSurfaceIndex( - const geometry_msgs::msg::Transform & self_transform) -{ - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); -} - -double BigVehicleTracker::getMeasurementYaw( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } - return measurement_yaw; -} diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 7714c381894f0..b5a303d790a83 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -47,7 +47,6 @@ NormalVehicleTracker::NormalVehicleTracker( const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -63,401 +62,300 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); - // initial state covariance - float p0_stddev_x = 1.0; // in object coordinate [m] - float p0_stddev_y = 0.3; // in object coordinate [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } + // velocity deviation threshold + // if the predicted velocity is close to the observed velocity, + // the observed velocity is used as the measurement. + velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } else { - // past default value - // bounding_box_ = {4.0, 1.7, 2.0}; autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - utils::convertConvexHullToBoundingBox(object, bbox_object); - bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + 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}; + } last_input_bounding_box_ = bounding_box_; } - ekf_.init(X, P); + // 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); + } - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + // 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 lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m - lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m -} + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double vel_cov; + const double & length = bounding_box_.length; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } -bool 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; + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 1.0; // in object coordinate [m] + constexpr double p0_stddev_y = 0.3; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; } - } - if (ret) { - last_update_time_ = time; - } - return ret; -} -bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); + + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - return true; + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } -bool NormalVehicleTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) +bool NormalVehicleTracker::predict(const rclcpp::Time & time) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - - float r_cov_x; - float r_cov_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - - if (label == Label::CAR) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (utils::isLargeVehicleLabel(label)) { - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } - - // Decide dimension of measurement vector - bool enable_velocity_measurement = false; - if (object.kinematics.has_twist) { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - const double predicted_vel = X_t(IDX::VEL); - const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + return motion_model_.predictState(time); +} - if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { - // Velocity deviation is small - enable_velocity_measurement = true; - } - } +autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) +{ + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - // pos x, pos y, yaw, vel depending on pose measurement - const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); + // current (predicted) state + const double tracked_x = motion_model_.getStateElement(IDX::X); + const double tracked_y = motion_model_.getStateElement(IDX::Y); + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); - // convert to boundingbox if input is convex shape + // OBJECT SHAPE MODEL + // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); + if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + RCLCPP_WARN( + logger_, + "NormalVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); + bbox_object = object; + } + } else { bbox_object = object; } - /* get offset measurement*/ - autoware_auto_perception_msgs::msg::DetectedObject offset_object; + // get offset measurement + int nearest_corner_index = utils::getNearestCornerOrSurface( + tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, - bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; - Y(IDX::YAW, 0) = measurement_yaw; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(measurement_yaw); - const double sin_yaw = std::sin(measurement_yaw); - const double sin_2yaw = std::sin(2.0f * measurement_yaw); - R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - R(1, 0) = R(0, 1); // y - x - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } - - // Update the velocity when necessary - if (dim_y == 4) { - Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VEL) = 1.0; // for vel + last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, + bbox_object, tracked_yaw, updating_object, tracking_offset_); - if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vel; // vel -vel + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + // measurement noise covariance + float r_cov_x; + float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (label == Label::CAR) { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } else if (utils::isLargeVehicleLabel(label)) { + // if label is changed, enlarge the measurement noise covariance + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); } else { - R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } + + // yaw angle fix + double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + + // fill covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + pose_cov[utils::MSG_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + if (!is_yaw_available) { + pose_cov[utils::MSG_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value } + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel } - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); + return updating_object; +} + +bool NormalVehicleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // current (predicted) state + const double tracked_vel = motion_model_.getStateElement(IDX::VEL); + + // velocity capability is checked only when the object has velocity measurement + // and the predicted velocity is close to the observed velocity + bool is_velocity_available = false; + if (object.kinematics.has_twist) { + const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { + // Velocity deviation is small + is_velocity_available = true; + } } - // normalize yaw and limit vel, slip + // update + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double vel = object.kinematics.twist_with_covariance.twist.linear.x; + + if (is_velocity_available) { + is_updated = motion_model_.updateStatePoseHeadVel( + x, y, yaw, object.kinematics.pose_with_covariance.covariance, vel, + object.kinematics.twist_with_covariance.covariance); + } else { + is_updated = motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); } - ekf_.init(X_t, P_t); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type - autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { - bbox_object = object; + 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; } - 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; + 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; last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - // 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 + // 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(); return true; } @@ -466,20 +364,30 @@ bool NormalVehicleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "NormalVehicleTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -494,100 +402,28 @@ bool NormalVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "NormalVehicleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // recover bounding box from tracking point const double dl = bounding_box_.length - last_input_bounding_box_.length; const double dw = bounding_box_.width - last_input_bounding_box_.width; const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - X_t(IDX::X) = recovered_pose.x(); - X_t(IDX::Y) = recovered_pose.y(); + pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, + motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); + pose_with_cov.pose.position.x = recovered_pose.x(); + pose_with_cov.pose.position.y = recovered_pose.y(); // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; @@ -600,32 +436,3 @@ bool NormalVehicleTracker::getTrackedObject( return true; } - -void NormalVehicleTracker::setNearestCornerOrSurfaceIndex( - const geometry_msgs::msg::Transform & self_transform) -{ - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); -} - -double NormalVehicleTracker::getMeasurementYaw( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } - return measurement_yaw; -} diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 57c2e8f899951..c03ab867cd8d1 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -47,7 +47,6 @@ PedestrianTracker::PedestrianTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("PedestrianTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -60,262 +59,196 @@ PedestrianTracker::PedestrianTracker( ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - // initial state covariance - float p0_stddev_x = 2.0; // [m] - float p0_stddev_y = 2.0; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(120); // [m/s] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(360); // [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); - // process noise covariance - float q_stddev_x = 0.4; // [m/s] - float q_stddev_y = 0.4; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); - // limitations - max_vx_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - - // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z; - } else { - X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::WZ, IDX::WZ) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; - } - } + // OBJECT SHAPE MODEL bounding_box_ = {0.5, 0.5, 1.7}; - cylinder_ = {0.3, 1.7}; + cylinder_ = {0.5, 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); } - ekf_.init(X, P); + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ + 30.0 /* [deg/s] maximum turn rate */ + ); + + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double wz = 0.0; + double vel_cov; + double wz_cov; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + wz = object.kinematics.twist_with_covariance.twist.angular.z; + } + + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 2.0; // in object coordinate [m] + constexpr double p0_stddev_y = 2.0; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(1000); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(120); // in object coordinate [m/s] + constexpr double p0_stddev_wz = + tier4_autoware_utils::deg2rad(360); // in object coordinate [rad/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + wz_cov = std::pow(p0_stddev_wz, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + wz_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; + } + + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, wz, wz_cov); + } } bool PedestrianTracker::predict(const rclcpp::Time & time) { - const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); - if (ret) { - last_update_time_ = time; - } - return ret; + return motion_model_.predictState(time); } -bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const +autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) { - /* Motion model: Constant turn-rate motion model (CTRV) - * - * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt - * yaw_{k+1} = yaw_k + (wz_k) * dt - * vx_{k+1} = vx_k - * wz_{k+1} = wz_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] - * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] - * [ 0, 0, 1, 0, dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW)); - const double sin_yaw = std::sin(X_t(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::WZ) = X_t(IDX::WZ); - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::YAW, IDX::WZ) = dt; - - // Process noise covariance Q - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); - } + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - return true; + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + const double & r_cov_x = ekf_params_.r_cov_x; + const double & r_cov_y = ekf_params_.r_cov_y; + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + } + return updating_object; } bool PedestrianTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr int dim_y = 2; // pos x, pos y depending on Pose output - // double measurement_yaw = - // tier4_autoware_utils::normalizeRadian(tf2::getYaw(object.state.pose_covariance.pose.orientation)); - // { - // Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - // ekf_.getX(X_t); - // while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - // measurement_yaw = measurement_yaw + M_PI; - // } - // while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - // measurement_yaw = measurement_yaw - M_PI; - // } - // float theta = std::acos( - // std::cos(X_t(IDX::YAW)) * std::cos(measurement_yaw) + - // std::sin(X_t(IDX::YAW)) * std::sin(measurement_yaw)); - // if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false; - // } - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - // C(2, IDX::YAW) = 1.0; // for yaw - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x - // R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - // R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - // R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - // R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - // R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - // R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } - - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); - } - - // normalize yaw and limit vx, wz + // update motion model + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; - } - if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) { - X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_; - } - ekf_.init(X_t, P_t); + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool PedestrianTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr float gain = 0.9; + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + // 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; } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - cylinder_.width = gain * cylinder_.width + (1.0 - gain) * object.shape.dimensions.x; - cylinder_.height = gain * cylinder_.height + (1.0 - gain) * object.shape.dimensions.z; + // 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; } 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; } @@ -323,20 +256,29 @@ bool PedestrianTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "PedestrianTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); (void)self_transform; // currently do not use self vehicle position return true; @@ -349,70 +291,19 @@ bool PedestrianTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "PedestrianTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX); - twist_with_cov.twist.angular.z = X_t(IDX::WZ); - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); // set shape if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 897b858a6aabe..450bb2d94abb6 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -12,6 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -22,227 +30,179 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("UnknownTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; // initialize params - float q_stddev_x = 0.0; // [m/s] - float q_stddev_y = 0.0; // [m/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(20); // [m/(s*s)] - float q_stddev_vy = tier4_autoware_utils::kmph2mps(20); // [m/(s*s)] - float r_stddev_x = 1.0; // [m] - float r_stddev_y = 1.0; // [m] - float p0_stddev_x = 1.0; // [m/s] - float p0_stddev_y = 1.0; // [m/s] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float p0_stddev_vy = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); + // measurement noise covariance + constexpr double r_stddev_x = 1.0; // [m] + constexpr double r_stddev_y = 1.0; // [m] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_vy = std::pow(p0_stddev_vy, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(60); // [m/s] - max_vy_ = tier4_autoware_utils::kmph2mps(60); // [m/s] - - // initialize X matrix - Eigen::MatrixXd X(ekf_params_.dim_x, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::VY) = object.kinematics.twist_with_covariance.twist.linear.y; - } else { - X(IDX::VX) = 0.0; - X(IDX::VY) = 0.0; + + // Set motion model parameters + { + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] + motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); } - // initialize P matrix - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = ekf_params_.p0_cov_x; - P(IDX::X, IDX::Y) = 0.0; - P(IDX::Y, IDX::Y) = ekf_params_.p0_cov_y; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::VY, IDX::VY) = ekf_params_.p0_cov_vy; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::VY, IDX::VY) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::VY, IDX::VY) = ekf_params_.p0_cov_vy; + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ + tier4_autoware_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ + ); + + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + auto twist_cov = object.kinematics.twist_with_covariance.covariance; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + double vx = 0.0; + double vy = 0.0; + if (object.kinematics.has_twist) { + const double & vel_x = object.kinematics.twist_with_covariance.twist.linear.x; + const double & vel_y = object.kinematics.twist_with_covariance.twist.linear.y; + vx = std::cos(yaw) * vel_x - std::sin(yaw) * vel_y; + vy = std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; + } + + if (!object.kinematics.has_position_covariance) { + constexpr double p0_stddev_x = 1.0; // [m] + constexpr double p0_stddev_y = 1.0; // [m] + + const double p0_cov_x = std::pow(p0_stddev_x, 2.0); + const double p0_cov_y = std::pow(p0_stddev_y, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vy = tier4_autoware_utils::kmph2mps(10); // [m/s] + const double p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + const double p0_cov_vy = std::pow(p0_stddev_vy, 2.0); + twist_cov[utils::MSG_COV_IDX::X_X] = p0_cov_vx; + twist_cov[utils::MSG_COV_IDX::X_Y] = 0.0; + twist_cov[utils::MSG_COV_IDX::Y_X] = 0.0; + twist_cov[utils::MSG_COV_IDX::Y_Y] = p0_cov_vy; } - } - ekf_.init(X, P); + // rotate twist covariance matrix, since it is in the vehicle coordinate system + Eigen::MatrixXd twist_cov_rotate(2, 2); + twist_cov_rotate(0, 0) = twist_cov[utils::MSG_COV_IDX::X_X]; + twist_cov_rotate(0, 1) = twist_cov[utils::MSG_COV_IDX::X_Y]; + twist_cov_rotate(1, 0) = twist_cov[utils::MSG_COV_IDX::Y_X]; + twist_cov_rotate(1, 1) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + Eigen::MatrixXd twist_cov_rotated = R_yaw * twist_cov_rotate * R_yaw.transpose(); + twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_rotated(0, 0); + twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_rotated(0, 1); + twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_rotated(1, 0); + twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); + + // initialize motion model + motion_model_.initialize(time, x, y, pose_cov, vx, vy, twist_cov); + } } bool UnknownTracker::predict(const rclcpp::Time & time) { - const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); - if (ret) { - last_update_time_ = time; - } - return ret; + return motion_model_.predictState(time); } -bool UnknownTracker::predict(const double dt, KalmanFilter & ekf) const +autoware_auto_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) { - /* == Nonlinear model == - * - * x_{k+1} = x_k + vx_k * dt - * y_{k+1} = y_k + vx_k * dt - * vx_{k+1} = vx_k - * vy_{k+1} = vy_k - * - */ - - /* == Linearized model == - * - * A = [ 1, 0, dt, 0] - * [ 0, 1, 0, dt] - * [ 0, 0, 1, 0] - * [ 0, 0, 0, 1] - */ - - // X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * dt; - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VY) * dt; - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::VY) = X_t(IDX::VY); - - // A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::VX) = dt; - A(IDX::Y, IDX::VY) = dt; - - // Q - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = ekf_params_.q_cov_x * dt * dt; - Q(IDX::X, IDX::Y) = 0.0; - Q(IDX::Y, IDX::Y) = ekf_params_.q_cov_y * dt * dt; - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::VY, IDX::VY) = ekf_params_.q_cov_vy * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Pedestrian : Cannot predict"); - } + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - return true; + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + const double & r_cov_x = ekf_params_.r_cov_x; + const double & r_cov_y = ekf_params_.r_cov_y; + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + } + return updating_object; } bool UnknownTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr int dim_y = 2; // pos x, pos y depending on Pose output - - /* Set measurement matrix */ - Eigen::MatrixXd Y(dim_y, 1); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y; - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - - /* Set measurement noise covariance */ - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - } - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Pedestrian : Cannot update"); - } - - // limit vx, vy + // update motion model + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; - } - if (!(-max_vy_ <= X_t(IDX::VY) && X_t(IDX::VY) <= max_vy_)) { - X_t(IDX::VY) = X_t(IDX::VY) < 0 ? -max_vy_ : max_vy_; - } - ekf_.init(X_t, P_t); + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool UnknownTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & self_transform) { + // keep the latest input object object_ = object; - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( logger_, - "Pedestrian : There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + "UnknownTracker::measure There is a large gap between predicted time and measurement time. " + "(%f)", + dt); } - measureWithPose(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); return true; } @@ -254,54 +214,19 @@ bool UnknownTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "UnknownTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; - - // twist - const auto pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); - const double cos = std::cos(-pose_yaw); - const double sin = std::sin(-pose_yaw); - twist_with_cov.twist.linear.x = cos * X_t(IDX::VX) - sin * X_t(IDX::VY); - twist_with_cov.twist.linear.y = sin * X_t(IDX::VX) + cos * X_t(IDX::VY); - - // twist covariance - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::VY, IDX::VY); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; return true; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp new file mode 100644 index 0000000000000..ef8a45f608098 --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -0,0 +1,483 @@ +// 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 new file mode 100644 index 0000000000000..9e8327e381057 --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -0,0 +1,386 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +// cspell: ignore CTRV +// Constant Turn Rate and constant Velocity (CTRV) motion model + +CTRVMotionModel::CTRVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CTRVMotionModel")) +{ + // Initialize motion parameters + setDefaultParams(); +} + +void CTRVMotionModel::setDefaultParams() +{ + // process noise covariance + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + + setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); + + // set motion limitations + constexpr double max_vel = tier4_autoware_utils::kmph2mps(10); // [m/s] maximum velocity + constexpr double max_wz = 30.0; // [deg] maximum yaw rate + setMotionLimits(max_vel, max_wz); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] maximum time interval for prediction + setMaxDeltaTime(dt_max); +} + +void CTRVMotionModel::setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_yaw, + const double & q_stddev_vel, const double & q_stddev_wz) +{ + // set process noise covariance parameters + motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + motion_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); + motion_params_.q_cov_vel = std::pow(q_stddev_vel, 2.0); + motion_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); +} + +void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max_wz) +{ + // set motion limitations + motion_params_.max_vel = max_vel; + motion_params_.max_wz = tier4_autoware_utils::deg2rad(max_wz); +} + +bool CTRVMotionModel::initialize( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & wz, const double & wz_cov) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, yaw, vel, wz; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + P(IDX::VEL, IDX::VEL) = vel_cov; + P(IDX::WZ, IDX::WZ) = wz_cov; + + return MotionModel::initialize(time, X, P); +} + +bool CTRVMotionModel::updateStatePose( + const double & x, const double & y, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 2; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::updateStatePoseHead( + const double & x, const double & y, const double & yaw, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 3; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, fixed_yaw; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, with velocity + constexpr int DIM_Y = 4; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, yaw, vel; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + C(3, IDX::VEL) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + R(3, 3) = twist_cov[utils::MSG_COV_IDX::X_X]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::limitStates() +{ + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); + if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; + } + if (!(-motion_params_.max_wz <= X_t(IDX::WZ) && X_t(IDX::WZ) <= motion_params_.max_wz)) { + X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -motion_params_.max_wz : motion_params_.max_wz; + } + ekf_.init(X_t, P_t); + + return true; +} + +bool CTRVMotionModel::adjustPosition(const double & x, const double & y) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // adjust position + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) += x; + X_t(IDX::Y) += y; + ekf_.init(X_t, P_t); + + return true; +} + +bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const +{ + /* Motion model: Constant Turn Rate and constant Velocity model (CTRV) + * + * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt + * yaw_{k+1} = yaw_k + (wz_k) * dt + * vx_{k+1} = vx_k + * wz_{k+1} = wz_k + * + */ + + /* Jacobian Matrix + * + * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] + * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] + * [ 0, 0, 1, 0, dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] + */ + + // Current state vector X t + Eigen::MatrixXd X_t(DIM, 1); + 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 new file mode 100644 index 0000000000000..a9ad03e7875c5 --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -0,0 +1,307 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include + +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +// cspell: ignore CV +// Constant Velocity (CV) motion model + +CVMotionModel::CVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CVMotionModel")) +{ + // Initialize motion parameters + setDefaultParams(); +} + +void CVMotionModel::setDefaultParams() +{ + // process noise covariance + constexpr double q_stddev_x = 0.5; // [m/s] standard deviation of x + constexpr double q_stddev_y = 0.5; // [m/s] standard deviation of y + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] standard deviation of vx + constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] standard deviation of vy + setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); + + // set motion limitations + constexpr double max_vx = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum x velocity + constexpr double max_vy = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum y velocity + setMotionLimits(max_vx, max_vy); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] maximum time interval for prediction + setMaxDeltaTime(dt_max); +} + +void CVMotionModel::setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, + const double & q_stddev_vy) +{ + // set process noise covariance parameters + motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + motion_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + motion_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); +} + +void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy) +{ + // set motion limitations + motion_params_.max_vx = max_vx; + motion_params_.max_vy = max_vy; +} + +bool CVMotionModel::initialize( + const rclcpp::Time & time, const double & x, const double & y, + const std::array & pose_cov, const double & vx, const double & vy, + const std::array & twist_cov) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, vx, vy; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::VX, IDX::VX) = twist_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::VY, IDX::VY) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + + return MotionModel::initialize(time, X, P); +} + +bool CVMotionModel::updateStatePose( + const double & x, const double & y, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 2; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CVMotionModel::updateStatePoseVel( + const double & x, const double & y, const std::array & pose_cov, const double & vx, + const double & vy, const std::array & twist_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state with velocity + constexpr int DIM_Y = 4; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, vx, vy; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::VX) = 1.0; + C(3, IDX::VY) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(2, 2) = twist_cov[utils::MSG_COV_IDX::X_X]; + R(2, 3) = twist_cov[utils::MSG_COV_IDX::X_Y]; + R(3, 2) = twist_cov[utils::MSG_COV_IDX::Y_X]; + R(3, 3) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CVMotionModel::limitStates() +{ + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + if (!(-motion_params_.max_vx <= X_t(IDX::VX) && X_t(IDX::VX) <= motion_params_.max_vx)) { + X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -motion_params_.max_vx : motion_params_.max_vx; + } + if (!(-motion_params_.max_vy <= X_t(IDX::VY) && X_t(IDX::VY) <= motion_params_.max_vy)) { + X_t(IDX::VY) = X_t(IDX::VY) < 0 ? -motion_params_.max_vy : motion_params_.max_vy; + } + ekf_.init(X_t, P_t); + + return true; +} + +bool CVMotionModel::adjustPosition(const double & x, const double & y) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // adjust position + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) += x; + X_t(IDX::Y) += y; + ekf_.init(X_t, P_t); + + return true; +} + +bool CVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const +{ + /* Motion model: Constant velocity model + * + * x_{k+1} = x_k + vx_k * dt + * y_{k+1} = y_k + vx_k * dt + * vx_{k+1} = vx_k + * vy_{k+1} = vy_k + * + */ + + /* Jacobian Matrix + * + * A = [ 1, 0, dt, 0] + * [ 0, 1, 0, dt] + * [ 0, 0, 1, 0] + * [ 0, 0, 0, 1] + */ + + // Current state vector X t + Eigen::MatrixXd X_t(DIM, 1); + 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 new file mode 100644 index 0000000000000..4d51021de634b --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp @@ -0,0 +1,92 @@ +// 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; +}