feat: add support of overwriting signals if harsh backlight is detected

launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py

@@ -42,87 +42,98 @@
     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
     add_launch_arg(
         "fine_detector_model_path",
         os.path.join(fine_detector_share_dir, "data", "tlr_yolox_s.onnx"),
     )
     add_launch_arg(
         "fine_detector_label_path",
         os.path.join(fine_detector_share_dir, "data", "tlr_labels.txt"),
     )
     add_launch_arg("fine_detector_precision", "fp16")
     add_launch_arg("fine_detector_score_thresh", "0.3")
     add_launch_arg("fine_detector_nms_thresh", "0.65")
 
     add_launch_arg("approximate_sync", "False")
 
     # traffic_light_classifier
     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")
 
     def create_parameter_dict(*args):
         result = {}
         for x in args:
             result[x] = LaunchConfiguration(x)
         return result
 
     container = ComposableNodeContainer(
         name="traffic_light_node_container",
         namespace="",
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=[
             ComposableNode(
                 package="traffic_light_classifier",
                 plugin="traffic_light::TrafficLightClassifierNodelet",
                 name="traffic_light_classifier",
                 namespace="classification",
                 parameters=[
                     create_parameter_dict(
                         "approximate_sync",
                         "classifier_type",
                         "classifier_model_path",
                         "classifier_label_path",
                         "classifier_precision",
                         "classifier_mean",
                         "classifier_std",
+                        "backlight_threshold",
                     )
                 ],
                 remappings=[
                     ("~/input/image", LaunchConfiguration("input/image")),
                     ("~/input/rois", LaunchConfiguration("output/rois")),
                     ("~/output/traffic_signals", "classified/traffic_signals"),
                 ],
                 extra_arguments=[
                     {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
                 ],
             ),
             ComposableNode(
                 package="traffic_light_visualization",
                 plugin="traffic_light::TrafficLightRoiVisualizerNodelet",
                 name="traffic_light_roi_visualizer",
                 parameters=[create_parameter_dict("enable_fine_detection")],
                 remappings=[
                     ("~/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"),

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
 

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

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>

perception/traffic_light_classifier/src/nodelet.cpp

@@ -26,6 +26,8 @@
   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 @@
   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>