fix(autoware_traffic_light_visualization): fix to visualize correct c…

…olor and shapes (autowarefoundation#8428)

fix(autoware_traffic_light_visualization): fix vialization to draw correct shapes

Signed-off-by: ktro2828 <[email protected]>
Co-authored-by: Yi-Hsiang Fang (Vivid) <[email protected]>

perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp

@@ -0,0 +1,144 @@
+// Copyright 2024 The Autoware Contributors
+//
+// 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.
+#include "shape_draw.hpp"
+
+#include "opencv2/core/types.hpp"
+#include "opencv2/highgui.hpp"
+#include "opencv2/imgproc.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace autoware::traffic_light::visualization
+{
+void drawShape(
+  cv::Mat & image, const std::vector<ShapeImgParam> & params, int size, const cv::Point & position,
+  const cv::Scalar & color, float probability)
+{
+  // load concatenated shape image
+  const auto shape_img = loadShapeImage(params, size);
+
+  // Calculate the width of the text
+  std::string prob_str = std::to_string(static_cast<int>(round(probability * 100))) + "%";
+
+  int baseline = 0;
+  cv::Size text_size = cv::getTextSize(prob_str, cv::FONT_HERSHEY_SIMPLEX, 0.75, 2, &baseline);
+
+  const int fill_rect_w = shape_img.cols + text_size.width + 20;
+  const int fill_rect_h = std::max(shape_img.rows, text_size.height) + 10;
+
+  const cv::Point rect_position(position.x, position.y - fill_rect_h);
+
+  if (
+    rect_position.x < 0 || rect_position.y < 0 || rect_position.x + fill_rect_w > image.cols ||
+    position.y > image.rows) {
+    // TODO(KhalilSelyan): This error message may flood the terminal logs, so commented out
+    // temporarily. Need to consider a better way.
+
+    // std::cerr << "Adjusted position is out of image bounds." << std::endl;
+    return;
+  }
+  cv::Rect rectangle(rect_position.x, rect_position.y, fill_rect_w, fill_rect_h);
+  cv::rectangle(image, rectangle, color, -1);
+
+  // Position the probability text right next to the shape. (Text origin: bottom-left)
+  const int prob_x_offset = shape_img.cols + 15;
+  const int prob_y_offset = fill_rect_h / 4;
+  cv::putText(
+    image, prob_str, cv::Point(position.x + prob_x_offset, position.y - prob_y_offset),
+    cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0), 2, cv::LINE_AA);
+
+  if (!shape_img.empty()) {
+    // Create ROI on the destination image
+    const int shape_y_offset = fill_rect_h / 4;
+    auto shapeRoi = image(cv::Rect(
+      rect_position.x + 5, rect_position.y + shape_y_offset, shape_img.cols, shape_img.rows));
+
+    // Overlay the image onto the main image
+    for (int y = 0; y < shape_img.rows; ++y) {
+      for (int x = 0; x < shape_img.cols; ++x) {
+        const auto & pixel = shape_img.at<cv::Vec4b>(y, x);
+        if (pixel[3] != 0) {  // Only non-transparent pixels
+          shapeRoi.at<cv::Vec3b>(y, x) = cv::Vec3b(pixel[0], pixel[1], pixel[2]);
+        }
+      }
+    }
+  }
+}
+
+cv::Mat loadShapeImage(const std::vector<ShapeImgParam> & params, int size, double scale_factor)
+{
+  if (params.empty()) {
+    return {};
+  }
+
+  static const auto img_dir =
+    ament_index_cpp::get_package_share_directory("autoware_traffic_light_visualization") +
+    "/images/";
+
+  std::vector<cv::Mat> src_img;
+  for (const auto & param : params) {
+    auto filepath = img_dir + param.filename;
+    auto img = cv::imread(filepath, cv::IMREAD_UNCHANGED);
+
+    cv::resize(img, img, cv::Size(size, size), scale_factor, scale_factor, cv::INTER_AREA);
+
+    if (param.h_flip) {
+      cv::flip(img, img, 1);
+    }
+    if (param.v_flip) {
+      cv::flip(img, img, 0);
+    }
+    src_img.emplace_back(img);
+  }
+
+  cv::Mat dst;
+  cv::hconcat(src_img, dst);  // cspell:ignore hconcat
+
+  return dst;
+}
+
+void drawTrafficLightShape(
+  cv::Mat & image, const std::vector<std::string> & shapes, int size, const cv::Point & position,
+  const cv::Scalar & color, float probability)
+{
+  using ShapeImgParamFunction = std::function<ShapeImgParam()>;
+
+  static const std::unordered_map<std::string, ShapeImgParamFunction> shapeToParamFunction = {
+    {"circle", circleImgParam},
+    {"left", leftArrowImgParam},
+    {"right", rightArrowImgParam},
+    {"straight", straightArrowImgParam},
+    {"down", downArrowImgParam},
+    {"straight_left", straightLeftArrowImgParam},
+    {"straight_right", straightRightArrowImgParam},
+    {"down_left", downLeftArrowImgParam},
+    {"down_right", downRightArrowImgParam},
+    {"cross", crossImgParam},
+    {"unknown", unknownImgParam}};
+
+  std::vector<ShapeImgParam> params;
+  for (const auto & shape : shapes) {
+    if (shapeToParamFunction.find(shape) != shapeToParamFunction.end()) {
+      auto func = shapeToParamFunction.at(shape);
+      params.emplace_back(func());
+    }
+  }
+
+  drawShape(image, params, size, position, color, probability);
+}
+}  // namespace autoware::traffic_light::visualization

perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp

@@ -0,0 +1,186 @@
+// Copyright 2024 The Autoware Contributors
+//
+// 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.
+#pragma once
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/opencv.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/imgproc/imgproc_c.h>
+
+#include <functional>
+#include <string>
+#include <vector>
+
+namespace autoware::traffic_light::visualization
+{
+/**
+ * @brief A struct of parameters to load shape image.
+ */
+struct ShapeImgParam
+{
+  std::string filename;  //!< Filename of shape image.
+  bool h_flip;           //!< Whether to flip horizontally
+  bool v_flip;           //!< Whether to flip vertically
+};
+
+/**
+ * @brief Draw traffic light shapes on the camera view image.
+ * @param image Camera view image.
+ * @param params Shape parameters to load shape image.
+ * @param size Shape image size to resize.
+ * @param position Top-left position of a ROI.
+ * @param color Rectangle color.
+ * @param probability Classification probability.
+ */
+void drawShape(
+  cv::Mat & image, const std::vector<ShapeImgParam> & params, int size, const cv::Point & position,
+  const cv::Scalar & color, float probability);
+
+/**
+ * @brief Load shape images and concatenate them.
+ * @param params Parameters for each shape image.
+ * @param size Image size to resize.
+ * @param scale_factor Scale factor to resize.
+ * @return If no parameter is specified returns empty Mat, otherwise returns horizontally
+ * concatenated image.
+ */
+cv::Mat loadShapeImage(
+  const std::vector<ShapeImgParam> & params, int size, double scale_factor = 0.3);
+
+/**
+ * @brief Load parameter of circle.
+ *
+ * @return Parameter of circle.
+ */
+inline ShapeImgParam circleImgParam()
+{
+  return {"circle.png", false, false};
+}
+
+/**
+ * @brief Load parameter of left-arrow.
+ *
+ * @return Parameter of left-arrow.
+ */
+inline ShapeImgParam leftArrowImgParam()
+{
+  return {"left_arrow.png", false, false};
+}
+
+/**
+ * @brief Load parameter of right-arrow.
+ *
+ * @return Parameter of right-arrow, the image is flipped left-arrow horizontally.
+ */
+inline ShapeImgParam rightArrowImgParam()
+{
+  return {"left_arrow.png", true, false};
+}
+
+/**
+ * @brief Load parameter of straight-arrow.
+ *
+ * @return Parameter of straight-arrow.
+ */
+inline ShapeImgParam straightArrowImgParam()
+{
+  return {"straight_arrow.png", false, false};
+}
+
+/**
+ * @brief Load parameter of down-arrow.
+ *
+ * @return Parameter of down-arrow, the image is flipped straight-arrow vertically.
+ */
+inline ShapeImgParam downArrowImgParam()
+{
+  return {"straight_arrow.png", false, true};
+}
+
+/**
+ * @brief Load parameter of straight-left-arrow.
+ *
+ * @return Parameter of straight-left-arrow, the image is flipped down-left-arrow vertically.
+ */
+inline ShapeImgParam straightLeftArrowImgParam()
+{
+  return {"down_left_arrow.png", false, true};
+}
+
+/**
+ * @brief Load parameter of straight-right-arrow.
+ *
+ * @return Parameter of straight-right-arrow, the image is flipped down-left-arrow both horizontally
+ * and vertically.
+ */
+inline ShapeImgParam straightRightArrowImgParam()
+{
+  return {"down_left_arrow.png", true, true};
+}
+
+/**
+ * @brief Load parameter of down-left-arrow.
+ *
+ * @return Parameter of down-left-arrow.
+ */
+inline ShapeImgParam downLeftArrowImgParam()
+{
+  return {"down_left_arrow.png", false, false};
+}
+
+/**
+ * @brief Load parameter of down-right-arrow.
+ *
+ * @return Parameter of down-right-arrow, the image is flipped straight-arrow horizontally.
+ */
+inline ShapeImgParam downRightArrowImgParam()
+{
+  return {"down_left_arrow.png", true, false};
+}
+
+/**
+ * @brief Load parameter of cross-arrow.
+ *
+ * @return Parameter of cross-arrow.
+ */
+inline ShapeImgParam crossImgParam()
+{
+  return {"cross.png", false, false};
+}
+
+/**
+ * @brief Load parameter of unknown shape.
+ *
+ * @return Parameter of unkown shape.
+ */
+inline ShapeImgParam unknownImgParam()
+{
+  return {"unknown.png", false, false};
+}
+
+/**
+ * @brief Draw traffic light shapes on the camera view image.
+ * @param image Camera view image.
+ * @param shapes Shape names.
+ * @param size Shape image size to resize.
+ * @param position Top-left position of a ROI.
+ * @param color Color of traffic light.
+ * @param probability Classification probability.
+ */
+void drawTrafficLightShape(
+  cv::Mat & image, const std::vector<std::string> & shapes, int size, const cv::Point & position,
+  const cv::Scalar & color, float probability);
+
+}  // namespace autoware::traffic_light::visualization