refactor(autoware_traffic_light_map_based_detector): unify ROI computation and use std::optional

Treat the single-transform ROI calculation as the one-element case of
the multi-transform bounding ROI calculation, exposing a single
get_traffic_light_roi() returning std::optional<TrafficLightRoi>.
Promote the per-transform projection to project_traffic_light_to_roi()
returning std::optional.

Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp>

Author: Takahisa.Ishikawa

perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector.cpp

@@ -27,6 +27,7 @@
 #include <algorithm>
 #include <cstdlib>
 #include <memory>
+#include <optional>
 #include <stdexcept>
 #include <string>
 #include <vector>
@@ -150,7 +151,7 @@ std::optional<SetRouteError> TrafficLightMapBasedDetector::set_route(
   return std::nullopt;
 }
 
-const tf2::Transform & find_closest_transform(
+const StampedTransform & find_closest_transform(
   const std::vector<StampedTransform> & samples, const rclcpp::Time & target_time)
 {
   const auto closest_iter = std::min_element(
@@ -160,7 +161,7 @@ const tf2::Transform & find_closest_transform(
       const auto diff_rhs = std::abs((rhs.stamp - target_time).nanoseconds());
       return diff_lhs < diff_rhs;
     });
-  return closest_iter->transform;
+  return *closest_iter;
 }
 
 const TrafficLightMapBasedDetector::TrafficLightSet &
@@ -202,27 +203,27 @@ DetectionResult TrafficLightMapBasedDetector::detect(
   const auto visible_traffic_lights = get_visible_traffic_lights(
     select_target_traffic_lights(), tf_map2camera_samples, pinhole_camera_model);
 
-  const tf2::Transform tf_map2camera_closest =
+  const StampedTransform tf_map2camera_closest =
     find_closest_transform(tf_map2camera_samples, camera_info.header.stamp);
   const auto expect_roi_config = make_expect_roi_config(config_);
 
   for (const auto & traffic_light : visible_traffic_lights) {
-    tier4_perception_msgs::msg::TrafficLightRoi rough_roi, expect_roi;
-    if (!get_traffic_light_roi(
-          tf_map2camera_closest, pinhole_camera_model, traffic_light, expect_roi_config,
-          expect_roi)) {
+    const auto expect_roi = get_traffic_light_roi(
+      {tf_map2camera_closest}, pinhole_camera_model, traffic_light, expect_roi_config);
+    if (!expect_roi) {
       continue;
     }
-    if (!get_traffic_light_roi(
-          tf_map2camera_samples, pinhole_camera_model, traffic_light, config_, rough_roi)) {
+    const auto rough_roi =
+      get_traffic_light_roi(tf_map2camera_samples, pinhole_camera_model, traffic_light, config_);
+    if (!rough_roi) {
       continue;
     }
-    result.rough_rois.rois.push_back(rough_roi);
-    result.expect_rois.rois.push_back(expect_roi);
+    result.rough_rois.rois.push_back(*rough_roi);
+    result.expect_rois.rois.push_back(*expect_roi);
   }
 
-  result.markers =
-    create_traffic_light_markers(tf_map2camera_closest, camera_info.header, visible_traffic_lights);
+  result.markers = create_traffic_light_markers(
+    tf_map2camera_closest.transform, camera_info.header, visible_traffic_lights);
 
   return result;
 }
@@ -287,12 +288,14 @@ std::vector<lanelet::ConstLineString3d> TrafficLightMapBasedDetector::get_visibl
   return visible_traffic_lights;
 }
 
-bool TrafficLightMapBasedDetector::get_traffic_light_roi(
+std::optional<tier4_perception_msgs::msg::TrafficLightRoi>
+TrafficLightMapBasedDetector::project_traffic_light_to_roi(
   const tf2::Transform & tf_map2camera,
   const image_geometry::PinholeCameraModel & pinhole_camera_model,
-  const lanelet::ConstLineString3d traffic_light, const TrafficLightMapBasedDetectorConfig & config,
-  tier4_perception_msgs::msg::TrafficLightRoi & roi) const
+  const lanelet::ConstLineString3d traffic_light,
+  const TrafficLightMapBasedDetectorConfig & config) const
 {
+  tier4_perception_msgs::msg::TrafficLightRoi roi;
   roi.traffic_light_id = traffic_light.id();
   if (pedestrian_traffic_light_id_.find(traffic_light.id()) != pedestrian_traffic_light_id_.end()) {
     roi.traffic_light_type = tier4_perception_msgs::msg::TrafficLightRoi::PEDESTRIAN_TRAFFIC_LIGHT;
@@ -310,7 +313,7 @@ bool TrafficLightMapBasedDetector::get_traffic_light_roi(
       config.max_vibration_height, config.max_vibration_width, config.max_vibration_depth);
     tf2::Vector3 point3d = top_left_camera_optical - margin;
     if (point3d.z() <= 0.0) {
-      return false;
+      return std::nullopt;
     }
     // enlarged target position in camera coordinate
     {
@@ -333,7 +336,7 @@ bool TrafficLightMapBasedDetector::get_traffic_light_roi(
       config.max_vibration_height, config.max_vibration_width, config.max_vibration_depth);
     tf2::Vector3 point3d = bottom_right_camera_optical + margin;
     if (point3d.z() <= 0.0) {
-      return false;
+      return std::nullopt;
     }
     // enlarged target position in camera coordinate
     {
@@ -346,34 +349,35 @@ bool TrafficLightMapBasedDetector::get_traffic_light_roi(
     }
 
     if (roi.roi.width < 1 || roi.roi.height < 1) {
-      return false;
+      return std::nullopt;
     }
   }
-  return true;
+  return roi;
 }
 
-bool TrafficLightMapBasedDetector::get_traffic_light_roi(
+std::optional<tier4_perception_msgs::msg::TrafficLightRoi>
+TrafficLightMapBasedDetector::get_traffic_light_roi(
   const std::vector<StampedTransform> & tf_map2camera_samples,
   const image_geometry::PinholeCameraModel & pinhole_camera_model,
-  const lanelet::ConstLineString3d traffic_light, const TrafficLightMapBasedDetectorConfig & config,
-  tier4_perception_msgs::msg::TrafficLightRoi & out_roi) const
+  const lanelet::ConstLineString3d traffic_light,
+  const TrafficLightMapBasedDetectorConfig & config) const
 {
   std::vector<tier4_perception_msgs::msg::TrafficLightRoi> rois;
   for (const auto & sample : tf_map2camera_samples) {
-    const auto & tf_map2camera = sample.transform;
-    tier4_perception_msgs::msg::TrafficLightRoi roi;
-    if (get_traffic_light_roi(tf_map2camera, pinhole_camera_model, traffic_light, config, roi)) {
-      rois.push_back(roi);
+    if (
+      auto projected = project_traffic_light_to_roi(
+        sample.transform, pinhole_camera_model, traffic_light, config)) {
+      rois.push_back(*projected);
     }
   }
   if (rois.empty()) {
-    return false;
+    return std::nullopt;
   }
-  out_roi = rois.front();
+  tier4_perception_msgs::msg::TrafficLightRoi out_roi = rois.front();
   utils::compute_bounding_roi(
     pinhole_camera_model.cameraInfo().width, pinhole_camera_model.cameraInfo().height, rois,
     out_roi);
-  return true;
+  return out_roi;
 }
 
 visualization_msgs::msg::MarkerArray TrafficLightMapBasedDetector::create_traffic_light_markers(

perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector.hpp

@@ -127,16 +127,13 @@ class TrafficLightMapBasedDetector
    * @param pinhole_camera_model  pinhole model calculated from camera_info
    * @param traffic_light         lanelet traffic light object
    * @param config                offset configuration
-   * @param roi                   computed result
-   * @return true                 the computation succeeded
-   * @return false                the computation failed
+   * @return                      the projected ROI, or std::nullopt if the projection fails
    */
-  bool get_traffic_light_roi(
+  std::optional<tier4_perception_msgs::msg::TrafficLightRoi> project_traffic_light_to_roi(
     const tf2::Transform & tf_map2camera,
     const image_geometry::PinholeCameraModel & pinhole_camera_model,
     const lanelet::ConstLineString3d traffic_light,
-    const TrafficLightMapBasedDetectorConfig & config,
-    tier4_perception_msgs::msg::TrafficLightRoi & roi) const;
+    const TrafficLightMapBasedDetectorConfig & config) const;
 
   /**
    * @brief Compute the bounding ROI from multiple transforms
@@ -145,16 +142,14 @@ class TrafficLightMapBasedDetector
    * @param pinhole_camera_model  pinhole model calculated from camera_info
    * @param traffic_light         lanelet traffic light object
    * @param config                offset configuration
-   * @param out_roi               computed result
-   * @return true                 the computation succeeded
-   * @return false                the computation failed
+   * @return                      the bounding ROI, or std::nullopt if no sample yields a valid
+   *                              projection
    */
-  bool get_traffic_light_roi(
+  std::optional<tier4_perception_msgs::msg::TrafficLightRoi> get_traffic_light_roi(
     const std::vector<StampedTransform> & tf_map2camera_samples,
     const image_geometry::PinholeCameraModel & pinhole_camera_model,
     const lanelet::ConstLineString3d traffic_light,
-    const TrafficLightMapBasedDetectorConfig & config,
-    tier4_perception_msgs::msg::TrafficLightRoi & out_roi) const;
+    const TrafficLightMapBasedDetectorConfig & config) const;
 
   /**
    * @brief Create visualization markers for visible traffic lights