From 84d3790a0172439d877173d3e91d822a1cfbb3d8 Mon Sep 17 00:00:00 2001 From: Yukihiro Saito Date: Thu, 10 Mar 2022 18:07:31 +0900 Subject: [PATCH] feat(detection_by_tracker): add debug and modify existence_probability (#475) * add debugger Signed-off-by: Yukihiro Saito * change iou Signed-off-by: Yukihiro Saito --- .../include/detection_by_tracker/debugger.hpp | 101 ++++++++++++++++++ .../detection_by_tracker_core.hpp | 3 + .../src/detection_by_tracker_core.cpp | 29 ++--- 3 files changed, 121 insertions(+), 12 deletions(-) create mode 100644 perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp new file mode 100644 index 0000000000000..c0289a4ffde6f --- /dev/null +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -0,0 +1,101 @@ +// Copyright 2021 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 DETECTION_BY_TRACKER__DEBUGGER_HPP_ +#define DETECTION_BY_TRACKER__DEBUGGER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +class Debugger +{ +public: + explicit Debugger(rclcpp::Node * node) + { + initial_objects_pub_ = + node->create_publisher( + "debug/initial_objects", 1); + tracked_objects_pub_ = + node->create_publisher( + "debug/tracked_objects", 1); + merged_objects_pub_ = + node->create_publisher( + "debug/merged_objects", 1); + divided_objects_pub_ = + node->create_publisher( + "debug/divided_objects", 1); + } + + ~Debugger() {} + void publishInitialObjects(const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) + { + initial_objects_pub_->publish(removeFeature(input)); + } + void publishTrackedObjects(const autoware_auto_perception_msgs::msg::DetectedObjects & input) + { + tracked_objects_pub_->publish(input); + } + void publishMergedObjects(const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) + { + merged_objects_pub_->publish(removeFeature(input)); + } + void publishDividedObjects(const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) + { + divided_objects_pub_->publish(removeFeature(input)); + } + +private: + rclcpp::Publisher::SharedPtr + initial_objects_pub_; + rclcpp::Publisher::SharedPtr + tracked_objects_pub_; + rclcpp::Publisher::SharedPtr + merged_objects_pub_; + rclcpp::Publisher::SharedPtr + divided_objects_pub_; + + autoware_auto_perception_msgs::msg::DetectedObjects removeFeature( + const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) + { + autoware_auto_perception_msgs::msg::DetectedObjects objects; + objects.header = input.header; + for (const auto & feature_object : input.feature_objects) { + objects.objects.push_back(feature_object.object); + } + return objects; + } +}; + +#endif // DETECTION_BY_TRACKER__DEBUGGER_HPP_ diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 02500e2f3652a..299829a41dd52 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -15,6 +15,8 @@ #ifndef DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_ #define DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_ +#include "detection_by_tracker/debugger.hpp" + #include #include #include @@ -69,6 +71,7 @@ class DetectionByTracker : public rclcpp::Node TrackerHandler tracker_handler_; std::shared_ptr shape_estimator_; std::shared_ptr cluster_; + std::shared_ptr debugger_; void onObjects( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg); diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 25227c7a0fa08..91036ca5b0b91 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -215,6 +215,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) shape_estimator_ = std::make_shared(true, true); cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); + debugger_ = std::make_shared(this); } void DetectionByTracker::onObjects( @@ -224,34 +225,36 @@ void DetectionByTracker::onObjects( detected_objects.header = input_msg->header; // get objects from tracking module - autoware_auto_perception_msgs::msg::TrackedObjects tracked_objects; + autoware_auto_perception_msgs::msg::DetectedObjects tracked_objects; { - autoware_auto_perception_msgs::msg::TrackedObjects objects; + autoware_auto_perception_msgs::msg::TrackedObjects objects, transformed_objects; const bool available_trackers = tracker_handler_.estimateTrackedObjects(input_msg->header.stamp, objects); if ( !available_trackers || - !transformTrackedObjects(objects, input_msg->header.frame_id, tf_buffer_, tracked_objects)) { + !transformTrackedObjects( + objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) { objects_pub_->publish(detected_objects); return; } + // to simplify post processes, convert tracked_objects to DetectedObjects message. + tracked_objects = toDetectedObjects(transformed_objects); } - - // to simplify post processes, convert tracked_objects to DetectedObjects message. - autoware_auto_perception_msgs::msg::DetectedObjects tracked_objects_dt; - tracked_objects_dt = toDetectedObjects(tracked_objects); + debugger_->publishInitialObjects(*input_msg); + debugger_->publishTrackedObjects(tracked_objects); // merge over segmented objects tier4_perception_msgs::msg::DetectedObjectsWithFeature merged_objects; autoware_auto_perception_msgs::msg::DetectedObjects no_found_tracked_objects; - mergeOverSegmentedObjects( - tracked_objects_dt, *input_msg, no_found_tracked_objects, merged_objects); + mergeOverSegmentedObjects(tracked_objects, *input_msg, no_found_tracked_objects, merged_objects); + debugger_->publishMergedObjects(merged_objects); // divide under segmented objects tier4_perception_msgs::msg::DetectedObjectsWithFeature divided_objects; autoware_auto_perception_msgs::msg::DetectedObjects temp_no_found_tracked_objects; divideUnderSegmentedObjects( no_found_tracked_objects, *input_msg, temp_no_found_tracked_objects, divided_objects); + debugger_->publishDividedObjects(divided_objects); // merge under/over segmented objects to build output objects for (const auto & merged_object : merged_objects.feature_objects) { @@ -387,8 +390,9 @@ float DetectionByTracker::optimizeUnderSegmentedObject( // build output highest_iou_object.object.classification = target_object.classification; - // TODO(yukkysaito): It is necessary to consider appropriate values in the future. - highest_iou_object.object.existence_probability = 0.1f; + highest_iou_object.object.existence_probability = + utils::get2dIoU(target_object, highest_iou_object.object); + output = highest_iou_object; return highest_iou; } @@ -437,7 +441,6 @@ void DetectionByTracker::mergeOverSegmentedObjects( // build output clusters tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; - feature_object.object.existence_probability = 0.1f; feature_object.object.classification = tracked_object.classification; bool is_shape_estimated = shape_estimator_->estimateShapeAndPose( @@ -450,6 +453,8 @@ void DetectionByTracker::mergeOverSegmentedObjects( continue; } + feature_object.object.existence_probability = + utils::get2dIoU(tracked_object, feature_object.object); setClusterInObjectWithFeature(in_cluster_objects.header, pcl_merged_cluster, feature_object); out_objects.feature_objects.push_back(feature_object); }