feat: add support of overwriting signals if harsh backlight is detect…

…ed (autowarefoundation#5852) * feat: add support of overwriting signals if backlit is detected Signed-off-by: ktro2828 <[email protected]> * feat: remove default parameter in nodelet and update lauch for composable node Signed-off-by: ktro2828 <[email protected]> * docs: update README Signed-off-by: ktro2828 <[email protected]> * docs: update README Signed-off-by: ktro2828 <[email protected]> * feat: update confidence to 0.0 corresponding signals overwritten by unkonwn Signed-off-by: ktro2828 <[email protected]> --------- Signed-off-by: ktro2828 <[email protected]>
Interplai · May 26, 2024 · 77bd5c1 · 77bd5c1
1 parent e2b9f69
commit 77bd5c1
Show file tree

Hide file tree

Showing 5 changed files with 54 additions and 9 deletions.
diff --git a/...perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/...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
@@ -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
@@ -86,6 +86,9 @@ class TrafficLightClassifierNodelet : public rclcpp::Node
   rclcpp::Publisher<tier4_perception_msgs::msg::TrafficSignalArray>::SharedPtr
     traffic_signal_array_pub_;
   std::shared_ptr<ClassifierInterface> 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
@@ -13,6 +13,7 @@
   <arg name="classifier_type" default="0" unless="$(var use_gpu)"/>
 
   <arg name="build_only" default="false" description="shutdown node after TensorRT engine file is built"/>
+  <arg name="backlight_threshold" default="0.85" description="overwrite signals if the RoI image intensity is greater than this value"/>
 
   <node pkg="traffic_light_classifier" exec="traffic_light_classifier_node" name="traffic_light_classifier">
     <remap from="~/input/image" to="$(var input/image)"/>
@@ -24,5 +25,6 @@
     <param name="classifier_model_path" value="$(var classifier_model_path)"/>
     <param name="classifier_precision" value="$(var classifier_precision)"/>
     <param name="build_only" value="$(var build_only)"/>
+    <param name="backlight_threshold" value="$(var backlight_threshold)"/>
   </node>
 </launch>
diff --git 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<double>("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<cv::Mat> images;
+  std::vector<size_t> 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 <rclcpp_components/register_node_macro.hpp>