From 76e09d58ec1b75041f52c5adb40b3b3e457683fa Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Thu, 28 Dec 2023 11:32:43 +0900 Subject: [PATCH] feat: add support of overwriting signals if harsh backlight is detected (#5852) * feat: add support of overwriting signals if backlit is detected Signed-off-by: ktro2828 * feat: remove default parameter in nodelet and update lauch for composable node Signed-off-by: ktro2828 * docs: update README Signed-off-by: ktro2828 * docs: update README Signed-off-by: ktro2828 * feat: update confidence to 0.0 corresponding signals overwritten by unkonwn Signed-off-by: ktro2828 --------- Signed-off-by: ktro2828 --- .../traffic_light_node_container.launch.py | 19 +++++++++--- perception/traffic_light_classifier/README.md | 9 +++--- .../traffic_light_classifier/nodelet.hpp | 3 ++ .../traffic_light_classifier.launch.xml | 2 ++ .../traffic_light_classifier/src/nodelet.cpp | 30 ++++++++++++++++++- 5 files changed, 54 insertions(+), 9 deletions(-) diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index a6bcb40e81252..30eeff04985e5 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -42,7 +42,8 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw") add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois") add_launch_arg( - "output/traffic_signals", "/perception/traffic_light_recognition/traffic_signals" + "output/traffic_signals", + "/perception/traffic_light_recognition/traffic_signals", ) # traffic_light_fine_detector @@ -64,14 +65,20 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("classifier_type", "1") add_launch_arg( "classifier_model_path", - os.path.join(classifier_share_dir, "data", "traffic_light_classifier_efficientNet_b1.onnx"), + os.path.join( + classifier_share_dir, + "data", + "traffic_light_classifier_efficientNet_b1.onnx", + ), ) add_launch_arg( - "classifier_label_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt") + "classifier_label_path", + os.path.join(classifier_share_dir, "data", "lamp_labels.txt"), ) add_launch_arg("classifier_precision", "fp16") add_launch_arg("classifier_mean", "[123.675, 116.28, 103.53]") add_launch_arg("classifier_std", "[58.395, 57.12, 57.375]") + add_launch_arg("backlight_threshold", "0.85") add_launch_arg("use_intra_process", "False") add_launch_arg("use_multithread", "False") @@ -102,6 +109,7 @@ def create_parameter_dict(*args): "classifier_precision", "classifier_mean", "classifier_std", + "backlight_threshold", ) ], remappings=[ @@ -122,7 +130,10 @@ def create_parameter_dict(*args): ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), ("~/input/rough/rois", "detection/rough/rois"), - ("~/input/traffic_signals", LaunchConfiguration("output/traffic_signals")), + ( + "~/input/traffic_signals", + LaunchConfiguration("output/traffic_signals"), + ), ("~/output/image", "debug/rois"), ("~/output/image/compressed", "debug/rois/compressed"), ("~/output/image/compressedDepth", "debug/rois/compressedDepth"), diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index 2aecf66f8fb7b..758234f129f2a 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -52,10 +52,11 @@ These colors and shapes are assigned to the message as follows: ### Node Parameters -| Name | Type | Description | -| ----------------- | ---- | ------------------------------------------- | -| `classifier_type` | int | if the value is `1`, cnn_classifier is used | -| `data_path` | str | packages data and artifacts directory path | +| Name | Type | Description | +| --------------------- | ----- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `classifier_type` | int | if the value is `1`, cnn_classifier is used | +| `data_path` | str | packages data and artifacts directory path | +| `backlight_threshold` | float | If the intensity get grater than this overwrite with UNKNOWN in corresponding RoI. Note that, if the value is much higher, the node only overwrites in the harsher backlight situations. Therefore, If you wouldn't like to use this feature set this value to `1.0`. The value can be `[0.0, 1.0]`. The confidence of overwritten signal is set to `0.0`. | ### Core Parameters diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp index e076ff5c69378..89b91c5fb666c 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp @@ -86,6 +86,9 @@ class TrafficLightClassifierNodelet : public rclcpp::Node rclcpp::Publisher::SharedPtr traffic_signal_array_pub_; std::shared_ptr classifier_ptr_; + + double backlight_threshold_; + bool is_harsh_backlight(const cv::Mat & img) const; }; } // namespace traffic_light diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml index 10aa04cc585af..8ba0990b3efc6 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -13,6 +13,7 @@ + @@ -24,5 +25,6 @@ + diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/nodelet.cpp index 90cca87245d22..d0d7e13b928c8 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/traffic_light_classifier/src/nodelet.cpp @@ -26,6 +26,8 @@ TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeO using std::placeholders::_1; using std::placeholders::_2; is_approximate_sync_ = this->declare_parameter("approximate_sync", false); + backlight_threshold_ = this->declare_parameter("backlight_threshold"); + if (is_approximate_sync_) { approximate_sync_.reset(new ApproximateSync(ApproximateSyncPolicy(10), image_sub_, roi_sub_)); approximate_sync_->registerCallback( @@ -94,19 +96,45 @@ void TrafficLightClassifierNodelet::imageRoiCallback( output_msg.signals.resize(input_rois_msg->rois.size()); std::vector images; + std::vector backlight_indices; for (size_t i = 0; i < input_rois_msg->rois.size(); i++) { output_msg.signals[i].traffic_light_id = input_rois_msg->rois.at(i).traffic_light_id; const sensor_msgs::msg::RegionOfInterest & roi = input_rois_msg->rois.at(i).roi; - images.emplace_back(cv_ptr->image, cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height)); + auto roi_img = cv_ptr->image(cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height)); + if (is_harsh_backlight(roi_img)) { + backlight_indices.emplace_back(i); + } + images.emplace_back(roi_img); } if (!classifier_ptr_->getTrafficSignals(images, output_msg)) { RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback"); return; } + + for (const auto & idx : backlight_indices) { + auto & elements = output_msg.signals.at(idx).elements; + for (auto & element : elements) { + element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.confidence = 0.0; + } + } + output_msg.header = input_image_msg->header; traffic_signal_array_pub_->publish(output_msg); } +bool TrafficLightClassifierNodelet::is_harsh_backlight(const cv::Mat & img) const +{ + cv::Mat y_cr_cb; + cv::cvtColor(img, y_cr_cb, cv::COLOR_RGB2YCrCb); + + const cv::Scalar mean_values = cv::mean(y_cr_cb); + const double intensity = (mean_values[0] - 112.5) / 112.5; + + return backlight_threshold_ <= intensity; +} + } // namespace traffic_light #include