From 35da6d89b03b4a4667071eb6124f1585b4095faa Mon Sep 17 00:00:00 2001 From: jakor97 <51270271+jakor97@users.noreply.github.com> Date: Mon, 16 Dec 2024 01:29:43 +0100 Subject: [PATCH] refactor(autoware_multi_object_tracker): add configurable tracker parameters (#9621) * refactor(autoware_multi_object_tracker): add configurable tracker parameters Signed-off-by: jkoronczok * style(pre-commit): autofix * refactor(autoware_multi_object_tracker): remove default values from parameter declarations Signed-off-by: jkoronczok * refactor(autoware_multi_object_tracker): update schema file Signed-off-by: jkoronczok * style(pre-commit): autofix * Update perception/autoware_multi_object_tracker/src/processor/processor.cpp * Update perception/autoware_multi_object_tracker/src/processor/processor.cpp --------- Signed-off-by: jkoronczok Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Taekjin LEE --- .../multi_object_tracker_node.param.yaml | 15 ++++ .../multi_object_tracker_node.schema.json | 69 +++++++++++++++++++ .../src/multi_object_tracker_node.cpp | 42 ++++++++--- .../src/processor/processor.cpp | 50 ++++++-------- .../src/processor/processor.hpp | 27 ++++---- 5 files changed, 152 insertions(+), 51 deletions(-) diff --git a/perception/autoware_multi_object_tracker/config/multi_object_tracker_node.param.yaml b/perception/autoware_multi_object_tracker/config/multi_object_tracker_node.param.yaml index 7dd588ec8eeba..c9d0a3676b526 100644 --- a/perception/autoware_multi_object_tracker/config/multi_object_tracker_node.param.yaml +++ b/perception/autoware_multi_object_tracker/config/multi_object_tracker_node.param.yaml @@ -15,6 +15,21 @@ enable_delay_compensation: false consider_odometry_uncertainty: false + # tracker parameters + tracker_lifetime: 1.0 # [s] + min_known_object_removal_iou: 0.1 # [ratio] + min_unknown_object_removal_iou: 0.001 # [ratio] + distance_threshold: 5.0 # [m] + confident_count_threshold: + UNKNOWN: 3 + CAR: 3 + TRUCK: 3 + BUS: 3 + TRAILER: 3 + MOTORBIKE: 3 + BICYCLE: 3 + PEDESTRIAN: 3 + # debug parameters publish_processing_time: false publish_tentative_objects: false diff --git a/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json b/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json index a40eb12d99b38..f32db284c1738 100644 --- a/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json +++ b/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json @@ -56,6 +56,69 @@ "description": "If True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp.", "default": false }, + "consider_odometry_uncertainty": { + "type": "boolean", + "description": "If True, consider odometry uncertainty in tracking.", + "default": false + }, + "tracker_lifetime": { + "type": "number", + "description": "Lifetime of the tracker in seconds.", + "default": 1.0 + }, + "min_known_object_removal_iou": { + "type": "number", + "description": "Minimum IOU between associated objects with known label to remove younger tracker", + "default": 0.1 + }, + "min_unknown_object_removal_iou": { + "type": "number", + "description": "Minimum IOU between associated objects with unknown label to remove unknown tracker", + "default": 0.001 + }, + "distance_threshold": { + "type": "number", + "description": "Distance threshold in meters.", + "default": 5.0 + }, + "confident_count_threshold": { + "type": "object", + "properties": { + "UNKNOWN": { + "type": "number", + "default": 3 + }, + "CAR": { + "type": "number", + "default": 3 + }, + "TRUCK": { + "type": "number", + "default": 3 + }, + "BUS": { + "type": "number", + "default": 3 + }, + "TRAILER": { + "type": "number", + "default": 3 + }, + "MOTORBIKE": { + "type": "number", + "default": 3 + }, + "BICYCLE": { + "type": "number", + "default": 3 + }, + "PEDESTRIAN": { + "type": "number", + "default": 3 + } + }, + "description": "Number of measurements to consider tracker as confident for different object classes." + }, "publish_processing_time": { "type": "boolean", "description": "Enable to publish debug message of process time information.", @@ -93,6 +156,12 @@ "publish_rate", "world_frame_id", "enable_delay_compensation", + "consider_odometry_uncertainty", + "tracker_lifetime", + "min_known_object_removal_iou", + "min_unknown_object_removal_iou", + "distance_threshold", + "confident_count_threshold", "publish_processing_time", "publish_tentative_objects", "publish_debug_markers", diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index a8e27ed4f0146..d2d2d3e087c4e 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -64,12 +64,12 @@ boost::optional getTransformAnonymous( return boost::none; } } - } // namespace namespace autoware::multi_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; +using LabelType = autoware_perception_msgs::msg::ObjectClassification::_label_type; MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), @@ -166,23 +166,45 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // Initialize processor { - std::map tracker_map; - tracker_map.insert( + TrackerProcessorConfig config; + config.tracker_map.insert( std::make_pair(Label::CAR, this->declare_parameter("car_tracker"))); - tracker_map.insert( + config.tracker_map.insert( std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); - tracker_map.insert( + config.tracker_map.insert( std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); - tracker_map.insert( + config.tracker_map.insert( std::make_pair(Label::TRAILER, this->declare_parameter("trailer_tracker"))); - tracker_map.insert(std::make_pair( + config.tracker_map.insert(std::make_pair( Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); - tracker_map.insert( + config.tracker_map.insert( std::make_pair(Label::BICYCLE, this->declare_parameter("bicycle_tracker"))); - tracker_map.insert(std::make_pair( + config.tracker_map.insert(std::make_pair( Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); + config.channel_size = input_channel_size_; + + // Declare parameters + config.tracker_lifetime = declare_parameter("tracker_lifetime"); + config.min_known_object_removal_iou = declare_parameter("min_known_object_removal_iou"); + config.min_unknown_object_removal_iou = + declare_parameter("min_unknown_object_removal_iou"); + config.distance_threshold = declare_parameter("distance_threshold"); + + // Map from class name to label + std::map class_name_to_label = { + {"UNKNOWN", Label::UNKNOWN}, {"CAR", Label::CAR}, + {"TRUCK", Label::TRUCK}, {"BUS", Label::BUS}, + {"TRAILER", Label::TRAILER}, {"MOTORBIKE", Label::MOTORCYCLE}, + {"BICYCLE", Label::BICYCLE}, {"PEDESTRIAN", Label::PEDESTRIAN}}; + + // Declare parameters and initialize confident_count_threshold_map + for (const auto & [class_name, class_label] : class_name_to_label) { + int64_t value = declare_parameter("confident_count_threshold." + class_name); + config.confident_count_threshold[class_label] = static_cast(value); + } - processor_ = std::make_unique(tracker_map, input_channel_size_); + // Initialize processor with parameters + processor_ = std::make_unique(config); } // Data association initialization diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index ebc11ea63e199..b3bcd018faf9d 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -30,21 +30,10 @@ namespace autoware::multi_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; +using LabelType = autoware_perception_msgs::msg::ObjectClassification::_label_type; -TrackerProcessor::TrackerProcessor( - const std::map & tracker_map, const size_t & channel_size) -: tracker_map_(tracker_map), channel_size_(channel_size) +TrackerProcessor::TrackerProcessor(const TrackerProcessorConfig & config) : config_(config) { - // 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) @@ -95,34 +84,36 @@ std::shared_ptr TrackerProcessor::createNewTracker( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const { - const std::uint8_t label = + const LabelType label = autoware::object_recognition_utils::getHighestProbLabel(object.classification); - if (tracker_map_.count(label) != 0) { - const auto tracker = tracker_map_.at(label); + if (config_.tracker_map.count(label) != 0) { + const auto tracker = config_.tracker_map.at(label); if (tracker == "bicycle_tracker") return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + time, object, self_transform, config_.channel_size, channel_index); if (tracker == "big_vehicle_tracker") return std::make_shared( - object_model::big_vehicle, time, object, self_transform, channel_size_, channel_index); + object_model::big_vehicle, time, object, self_transform, config_.channel_size, + channel_index); if (tracker == "multi_vehicle_tracker") return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + time, object, self_transform, config_.channel_size, channel_index); if (tracker == "normal_vehicle_tracker") return std::make_shared( - object_model::normal_vehicle, time, object, self_transform, channel_size_, channel_index); + object_model::normal_vehicle, time, object, self_transform, config_.channel_size, + channel_index); if (tracker == "pass_through_tracker") return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + time, object, self_transform, config_.channel_size, channel_index); if (tracker == "pedestrian_and_bicycle_tracker") return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + time, object, self_transform, config_.channel_size, channel_index); if (tracker == "pedestrian_tracker") return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + time, object, self_transform, config_.channel_size, channel_index); } return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + time, object, self_transform, config_.channel_size, channel_index); } void TrackerProcessor::prune(const rclcpp::Time & time) @@ -137,7 +128,7 @@ 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); + const bool is_old = config_.tracker_lifetime < (*itr)->getElapsedTimeFromLastUpdate(time); // If the tracker is old, delete it if (is_old) { auto erase_itr = itr; @@ -168,7 +159,7 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) object2.kinematics.pose_with_covariance.pose.position.y); // If the distance is too large, skip - if (distance > distance_threshold_) { + if (distance > config_.distance_threshold) { continue; } @@ -184,7 +175,7 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) // 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 (iou > config_.min_unknown_object_removal_iou) { if (label1 == Label::UNKNOWN && label2 == Label::UNKNOWN) { if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { should_delete_tracker1 = true; @@ -198,7 +189,7 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) } } } else { // If neither object is UNKNOWN, delete the younger tracker - if (iou > min_iou_) { + if (iou > config_.min_known_object_removal_iou) { if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { should_delete_tracker1 = true; } else { @@ -226,7 +217,8 @@ bool TrackerProcessor::isConfidentTracker(const std::shared_ptr & track // 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_; + auto label = tracker->getHighestProbLabel(); + return tracker->getTotalMeasurementCount() >= config_.confident_count_threshold.at(label); } void TrackerProcessor::getTrackedObjects( diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index 851a0f6a26717..80ca92bc43671 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -31,11 +31,23 @@ namespace autoware::multi_object_tracker { +using LabelType = autoware_perception_msgs::msg::ObjectClassification::_label_type; + +struct TrackerProcessorConfig +{ + std::map tracker_map; + size_t channel_size; + float tracker_lifetime; // [s] + float min_known_object_removal_iou; // ratio [0, 1] + float min_unknown_object_removal_iou; // ratio [0, 1] + double distance_threshold; // [m] + std::map confident_count_threshold; // [count] +}; + class TrackerProcessor { public: - explicit TrackerProcessor( - const std::map & tracker_map, const size_t & channel_size); + explicit TrackerProcessor(const TrackerProcessorConfig & config); const std::list> & getListTracker() const { return list_tracker_; } // tracker processes @@ -62,17 +74,8 @@ class TrackerProcessor void getExistenceProbabilities(std::vector> & existence_vectors) const; private: - std::map tracker_map_; + TrackerProcessorConfig config_; std::list> list_tracker_; - const size_t channel_size_; - - // 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(