refactor(yabloc_image_processing): apply static analysis (#7489)

* refactor based on linter

Signed-off-by: a-maumau <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: a-maumau <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp

@@ -27,7 +27,7 @@ struct Histogram
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   explicit Histogram(int bin = 10) : bin(bin) { data = Eigen::MatrixXf::Zero(3, bin); }
 
-  Eigen::MatrixXf eval() const
+  [[nodiscard]] Eigen::MatrixXf eval() const
   {
     float sum = data.sum();
     if (sum < 1e-6f) throw std::runtime_error("invalid division");
@@ -37,7 +37,9 @@ struct Histogram
   void add(const cv::Vec3b & rgb)
   {
     for (int ch = 0; ch < 3; ++ch) {
-      int index = std::clamp(static_cast<int>(rgb[ch] * bin / 255.f), 0, bin - 1);
+      int index = std::clamp(
+        static_cast<int>(static_cast<float>(rgb[ch]) * static_cast<float>(bin) / 255.f), 0,
+        bin - 1);
       data(ch, index) += 1.0f;
     }
   }

localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp

@@ -76,7 +76,7 @@ class Lanelet2Overlay : public rclcpp::Node
   void draw_overlay(
     const cv::Mat & image, const std::optional<Pose> & pose, const rclcpp::Time & stamp);
   void draw_overlay_line_segments(
-    cv::Mat & image, const Pose & pose, const LineSegments & line_segments);
+    cv::Mat & image, const Pose & pose, const LineSegments & near_segments);
 
   void make_vis_marker(const LineSegments & ls, const Pose & pose, const rclcpp::Time & stamp);
 };

localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp

@@ -51,8 +51,8 @@ class LineSegmentDetector : public rclcpp::Node
 
   cv::Ptr<cv::LineSegmentDetector> line_segment_detector_;
 
-  std::vector<cv::Mat> remove_too_outer_elements(
-    const cv::Mat & lines, const cv::Size & size) const;
+  static std::vector<cv::Mat> remove_too_outer_elements(
+    const cv::Mat & lines, const cv::Size & size);
   void on_image(const sensor_msgs::msg::Image & msg);
   void execute(const cv::Mat & image, const rclcpp::Time & stamp);
 };

localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp

@@ -63,14 +63,14 @@ class SegmentFilter : public rclcpp::Node
   bool define_project_func();
 
   pcl::PointCloud<pcl::PointNormal> project_lines(
-    const pcl::PointCloud<pcl::PointNormal> & lines, const std::set<int> & indices,
+    const pcl::PointCloud<pcl::PointNormal> & points, const std::set<int> & indices,
     bool negative = false) const;
 
-  std::set<int> filter_by_mask(
+  static std::set<int> filter_by_mask(
     const cv::Mat & mask, const pcl::PointCloud<pcl::PointNormal> & edges);
 
   cv::Point2i to_cv_point(const Eigen::Vector3f & v) const;
-  void execute(const PointCloud2 & msg1, const Image & msg2);
+  void execute(const PointCloud2 & line_segments_msg, const Image & segment_msg);
 
   bool is_near_element(const pcl::PointNormal & pn, pcl::PointNormal & truncated_pn) const;
 };

localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp

@@ -25,8 +25,9 @@ namespace yabloc::graph_segment
 {
 GraphSegment::GraphSegment(const rclcpp::NodeOptions & options)
 : Node("graph_segment", options),
-  target_height_ratio_(declare_parameter<float>("target_height_ratio")),
-  target_candidate_box_width_(declare_parameter<int>("target_candidate_box_width"))
+  target_height_ratio_(static_cast<float>(declare_parameter<float>("target_height_ratio"))),
+  target_candidate_box_width_(
+    static_cast<int>(declare_parameter<int>("target_candidate_box_width")))
 {
   using std::placeholders::_1;
 
@@ -38,8 +39,8 @@ GraphSegment::GraphSegment(const rclcpp::NodeOptions & options)
   pub_debug_image_ = create_publisher<Image>("~/debug/segmented_image", 10);
 
   const double sigma = declare_parameter<double>("sigma");
-  const float k = declare_parameter<float>("k");
-  const int min_size = declare_parameter<double>("min_size");
+  const float k = static_cast<float>(declare_parameter<float>("k"));
+  const int min_size = static_cast<int>(declare_parameter<double>("min_size"));
   segmentation_ = cv::ximgproc::segmentation::createGraphSegmentation(sigma, k, min_size);
 
   // additional area pickup module
@@ -52,24 +53,28 @@ GraphSegment::GraphSegment(const rclcpp::NodeOptions & options)
 cv::Vec3b random_hsv(int index)
 {
   // It generates colors that are not too bright or too vivid, but rich in hues.
-  double base = static_cast<double>(index);
-  return cv::Vec3b(std::fmod(base * 0.7, 1.0) * 180, 0.7 * 255, 0.5 * 255);
+  auto base = static_cast<double>(index);
+  return {
+    static_cast<unsigned char>(std::fmod(base * 0.7, 1.0) * 180),
+    static_cast<unsigned char>(0.7 * 255), static_cast<unsigned char>(0.5 * 255)};
 };
 
 int GraphSegment::search_most_road_like_class(const cv::Mat & segmented) const
 {
-  const int W = target_candidate_box_width_;
-  const float R = target_height_ratio_;
-  cv::Point2i target_px(segmented.cols * 0.5, segmented.rows * R);
-  cv::Rect2i rect(target_px + cv::Point2i(-W, -W), target_px + cv::Point2i(W, W));
+  const int bw = target_candidate_box_width_;
+  const float r = target_height_ratio_;
+  cv::Point2i target_px(
+    static_cast<int>(static_cast<float>(segmented.cols) * 0.5),
+    static_cast<int>(static_cast<float>(segmented.rows) * r));
+  cv::Rect2i rect(target_px + cv::Point2i(-bw, -bw), target_px + cv::Point2i(bw, bw));
 
   std::unordered_map<int, int> areas;
   std::unordered_set<int> candidates;
   for (int h = 0; h < segmented.rows; h++) {
     const int * seg_ptr = segmented.ptr<int>(h);
     for (int w = 0; w < segmented.cols; w++) {
       int key = seg_ptr[w];
-      if (areas.count(key) == 0) areas[key] = 0;
+      areas.try_emplace(key, 0);
       areas[key]++;
       if (rect.contains(cv::Point2i{w, h})) candidates.insert(key);
     }
@@ -111,8 +116,8 @@ void GraphSegment::on_image(const Image & msg)
   cv::Mat debug_image = cv::Mat::zeros(resized.size(), CV_8UC3);
   for (int h = 0; h < resized.rows; h++) {
     // NOTE: Accessing through ptr() is faster than at()
-    cv::Vec3b * const debug_image_ptr = debug_image.ptr<cv::Vec3b>(h);
-    uchar * const output_image_ptr = output_image.ptr<uchar>(h);
+    auto * const debug_image_ptr = debug_image.ptr<cv::Vec3b>(h);
+    auto * const output_image_ptr = output_image.ptr<uchar>(h);
     const int * const segmented_image_ptr = segmented.ptr<int>(h);
 
     for (int w = 0; w < resized.cols; w++) {
@@ -148,10 +153,10 @@ void GraphSegment::draw_and_publish_image(
 
   // Draw target rectangle
   {
-    const int W = target_candidate_box_width_;
-    const float R = target_height_ratio_;
-    cv::Point2i target(size.width / 2, size.height * R);
-    cv::Rect2i rect(target + cv::Point2i(-W, -W), target + cv::Point2i(W, W));
+    const int w = target_candidate_box_width_;
+    const float r = target_height_ratio_;
+    cv::Point2i target(size.width / 2, static_cast<int>(static_cast<float>(size.height) * r));
+    cv::Rect2i rect(target + cv::Point2i(-w, -w), target + cv::Point2i(w, w));
     cv::rectangle(show_image, rect, cv::Scalar::all(0), 2);
   }
 

localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp

@@ -37,7 +37,7 @@ std::set<int> SimilarAreaSearcher::search(
 
   for (int h = 0; h < rgb_image.rows; h++) {
     const int * seg_ptr = segmented.ptr<int>(h);
-    const cv::Vec3b * rgb_ptr = rgb_image.ptr<cv::Vec3b>(h);
+    const auto * rgb_ptr = rgb_image.ptr<cv::Vec3b>(h);
 
     for (int w = 0; w < rgb_image.cols; w++) {
       int key = seg_ptr[w];

localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp

@@ -72,11 +72,11 @@ void Lanelet2Overlay::on_image(const sensor_msgs::msg::Image & msg)
   // Search synchronized pose
   float min_abs_dt = std::numeric_limits<float>::max();
   std::optional<Pose> synched_pose{std::nullopt};
-  for (auto pose : pose_buffer_) {
+  for (const auto & pose : pose_buffer_) {
     auto dt = (rclcpp::Time(pose.header.stamp) - stamp);
     auto abs_dt = std::abs(dt.seconds());
     if (abs_dt < min_abs_dt) {
-      min_abs_dt = abs_dt;
+      min_abs_dt = static_cast<float>(abs_dt);
       synched_pose = pose.pose;
     }
   }
@@ -93,19 +93,17 @@ void Lanelet2Overlay::on_line_segments(const PointCloud2 & msg)
   // Search synchronized pose
   float min_dt = std::numeric_limits<float>::max();
   geometry_msgs::msg::PoseStamped synched_pose;
-  for (auto pose : pose_buffer_) {
+  for (const auto & pose : pose_buffer_) {
     auto dt = (rclcpp::Time(pose.header.stamp) - stamp);
     auto abs_dt = std::abs(dt.seconds());
     if (abs_dt < min_dt) {
-      min_dt = abs_dt;
+      min_dt = static_cast<float>(abs_dt);
       synched_pose = pose;
     }
   }
   if (min_dt > 0.1) return;
   auto latest_pose_stamp = rclcpp::Time(pose_buffer_.back().header.stamp);
 
-  std::vector<int> a;
-
   LineSegments line_segments_cloud;
   pcl::fromROSMsg(msg, line_segments_cloud);
   make_vis_marker(line_segments_cloud, synched_pose.pose, stamp);
@@ -139,32 +137,35 @@ void Lanelet2Overlay::draw_overlay_line_segments(
   if (!camera_extrinsic_.has_value()) return;
   if (!info_.has_value()) return;
 
-  Eigen::Matrix3f K =
+  Eigen::Matrix3f k =
     Eigen::Map<Eigen::Matrix<double, 3, 3> >(info_->k.data()).cast<float>().transpose();
-  Eigen::Affine3f T = camera_extrinsic_.value();
+  Eigen::Affine3f t = camera_extrinsic_.value();
 
   Eigen::Affine3f transform = ground_plane_.align_with_slope(common::pose_to_affine(pose));
 
-  auto projectLineSegment =
-    [K, T, transform](
+  auto project_line_segment =
+    [k, t, transform](
       const Eigen::Vector3f & p1,
       const Eigen::Vector3f & p2) -> std::tuple<bool, cv::Point2i, cv::Point2i> {
-    Eigen::Vector3f from_camera1 = K * T.inverse() * transform.inverse() * p1;
-    Eigen::Vector3f from_camera2 = K * T.inverse() * transform.inverse() * p2;
-    constexpr float EPSILON = 0.1f;
-    bool p1_is_visible = from_camera1.z() > EPSILON;
-    bool p2_is_visible = from_camera2.z() > EPSILON;
+    Eigen::Vector3f from_camera1 = k * t.inverse() * transform.inverse() * p1;
+    Eigen::Vector3f from_camera2 = k * t.inverse() * transform.inverse() * p2;
+    constexpr float epsilon = 0.1f;
+    bool p1_is_visible = from_camera1.z() > epsilon;
+    bool p2_is_visible = from_camera2.z() > epsilon;
     if ((!p1_is_visible) && (!p2_is_visible)) return {false, cv::Point2i{}, cv::Point2i{}};
 
-    Eigen::Vector3f uv1, uv2;
+    Eigen::Vector3f uv1;
+    Eigen::Vector3f uv2;
     if (p1_is_visible) uv1 = from_camera1 / from_camera1.z();
     if (p2_is_visible) uv2 = from_camera2 / from_camera2.z();
 
     if ((p1_is_visible) && (p2_is_visible))
-      return {true, cv::Point2i(uv1.x(), uv1.y()), cv::Point2i(uv2.x(), uv2.y())};
+      return {
+        true, cv::Point2i(static_cast<int>(uv1.x()), static_cast<int>(uv1.y())),
+        cv::Point2i(static_cast<int>(uv2.x()), static_cast<int>(uv2.y()))};
 
     Eigen::Vector3f tangent = from_camera2 - from_camera1;
-    float mu = (EPSILON - from_camera1.z()) / (tangent.z());
+    float mu = (epsilon - from_camera1.z()) / (tangent.z());
     if (!p1_is_visible) {
       from_camera1 = from_camera1 + mu * tangent;
       uv1 = from_camera1 / from_camera1.z();
@@ -173,11 +174,13 @@ void Lanelet2Overlay::draw_overlay_line_segments(
       from_camera2 = from_camera1 + mu * tangent;
       uv2 = from_camera2 / from_camera2.z();
     }
-    return {true, cv::Point2i(uv1.x(), uv1.y()), cv::Point2i(uv2.x(), uv2.y())};
+    return {
+      true, cv::Point2i(static_cast<int>(uv1.x()), static_cast<int>(uv1.y())),
+      cv::Point2i(static_cast<int>(uv2.x()), static_cast<int>(uv2.y()))};
   };
 
   for (const pcl::PointNormal & pn : near_segments) {
-    auto [success, u1, u2] = projectLineSegment(pn.getVector3fMap(), pn.getNormalVector3fMap());
+    auto [success, u1, u2] = project_line_segment(pn.getVector3fMap(), pn.getNormalVector3fMap());
     if (success) cv::line(image, u1, u2, cv::Scalar(0, 255, 255), 2);
   }
 }
@@ -197,7 +200,8 @@ void Lanelet2Overlay::make_vis_marker(
   marker.color.a = 0.7f;
 
   for (const auto pn : ls) {
-    geometry_msgs::msg::Point p1, p2;
+    geometry_msgs::msg::Point p1;
+    geometry_msgs::msg::Point p2;
     p1.x = pn.x;
     p1.y = pn.y;
     p1.z = pn.z;

localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp

@@ -67,7 +67,8 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st
   std::vector<cv::Mat> filtered_lines = remove_too_outer_elements(lines, image.size());
 
   for (const cv::Mat & xy_xy : filtered_lines) {
-    Eigen::Vector3f xy1, xy2;
+    Eigen::Vector3f xy1;
+    Eigen::Vector3f xy2;
     xy1 << xy_xy.at<float>(0), xy_xy.at<float>(1), 0;
     xy2 << xy_xy.at<float>(2), xy_xy.at<float>(3), 0;
     pcl::PointNormal pn;
@@ -79,7 +80,7 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st
 }
 
 std::vector<cv::Mat> LineSegmentDetector::remove_too_outer_elements(
-  const cv::Mat & lines, const cv::Size & size) const
+  const cv::Mat & lines, const cv::Size & size)
 {
   std::vector<cv::Rect2i> rect_vector;
   rect_vector.emplace_back(0, 0, size.width, 3);

localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp

@@ -30,11 +30,11 @@ LineSegmentsOverlay::LineSegmentsOverlay(const rclcpp::NodeOptions & options)
   using std::placeholders::_1;
 
   auto cb_image = std::bind(&LineSegmentsOverlay::on_image, this, _1);
-  auto cb_line_segments_ = std::bind(&LineSegmentsOverlay::on_line_segments, this, _1);
+  auto cb_line_segments = std::bind(&LineSegmentsOverlay::on_line_segments, this, _1);
 
   sub_image_ = create_subscription<Image>("~/input/image_raw", 10, cb_image);
   sub_line_segments_ =
-    create_subscription<PointCloud2>("~/input/line_segments", 10, cb_line_segments_);
+    create_subscription<PointCloud2>("~/input/line_segments", 10, cb_line_segments);
 
   pub_debug_image_ = create_publisher<Image>("~/debug/image_with_colored_line_segments", 10);
 }
@@ -73,8 +73,7 @@ void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & l
   LineSegments line_segments_cloud;
   pcl::fromROSMsg(*line_segments_msg, line_segments_cloud);
 
-  for (size_t index = 0; index < line_segments_cloud.size(); ++index) {
-    const LineSegment & pn = line_segments_cloud.at(index);
+  for (auto & pn : line_segments_cloud) {
     Eigen::Vector3f xy1 = pn.getVector3fMap();
     Eigen::Vector3f xy2 = pn.getNormalVector3fMap();
 
@@ -83,7 +82,9 @@ void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & l
       color = cv::Scalar(0, 0, 255);  // Red
     }
 
-    cv::line(image, cv::Point(xy1(0), xy1(1)), cv::Point(xy2(0), xy2(1)), color, 2);
+    cv::line(
+      image, cv::Point(static_cast<int>(xy1(0)), static_cast<int>(xy1(1))),
+      cv::Point(static_cast<int>(xy2(0)), static_cast<int>(xy2(1))), color, 2);
   }
 
   common::publish_image(*pub_debug_image_, image, stamp);