From a44805cf8ce1018e90bdc807066905b52a28c3d0 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Mon, 17 Jun 2024 08:32:00 +0900 Subject: [PATCH] refactor(yabloc_image_processing): apply static analysis (#7489) * refactor based on linter Signed-off-by: a-maumau * style(pre-commit): autofix --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../graph_segment/histogram.hpp | 6 ++- .../lanelet2_overlay/lanelet2_overlay.hpp | 2 +- .../line_segment_detector.hpp | 4 +- .../segment_filter/segment_filter.hpp | 6 +-- .../src/graph_segment/graph_segment_core.cpp | 39 ++++++++------- .../graph_segment/similar_area_searcher.cpp | 2 +- .../lanelet2_overlay_core.cpp | 46 ++++++++++-------- .../line_segment_detector_core.cpp | 5 +- .../line_segments_overlay_core.cpp | 11 +++-- .../segment_filter/segment_filter_core.cpp | 34 +++++++------ .../src/undistort/undistort_node.cpp | 48 ++++++++++--------- 11 files changed, 112 insertions(+), 91 deletions(-) diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp index c3ac2475f7e08..18b06b08cb55e 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp +++ b/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(rgb[ch] * bin / 255.f), 0, bin - 1); + int index = std::clamp( + static_cast(static_cast(rgb[ch]) * static_cast(bin) / 255.f), 0, + bin - 1); data(ch, index) += 1.0f; } } diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp index 7d644376ba591..d6d737a2808c4 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp +++ b/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, 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); }; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp index 761c581200369..7da51cc5daf35 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp +++ b/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 line_segment_detector_; - std::vector remove_too_outer_elements( - const cv::Mat & lines, const cv::Size & size) const; + static std::vector 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); }; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp index c8f036f2b12de..6b76db6751d26 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp +++ b/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 project_lines( - const pcl::PointCloud & lines, const std::set & indices, + const pcl::PointCloud & points, const std::set & indices, bool negative = false) const; - std::set filter_by_mask( + static std::set filter_by_mask( const cv::Mat & mask, const pcl::PointCloud & 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; }; diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp index f8d4c74dd5bf8..4cc2a9de4acc7 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp +++ b/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("target_height_ratio")), - target_candidate_box_width_(declare_parameter("target_candidate_box_width")) + target_height_ratio_(static_cast(declare_parameter("target_height_ratio"))), + target_candidate_box_width_( + static_cast(declare_parameter("target_candidate_box_width"))) { using std::placeholders::_1; @@ -38,8 +39,8 @@ GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) pub_debug_image_ = create_publisher("~/debug/segmented_image", 10); const double sigma = declare_parameter("sigma"); - const float k = declare_parameter("k"); - const int min_size = declare_parameter("min_size"); + const float k = static_cast(declare_parameter("k")); + const int min_size = static_cast(declare_parameter("min_size")); segmentation_ = cv::ximgproc::segmentation::createGraphSegmentation(sigma, k, min_size); // additional area pickup module @@ -52,16 +53,20 @@ 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(index); - return cv::Vec3b(std::fmod(base * 0.7, 1.0) * 180, 0.7 * 255, 0.5 * 255); + auto base = static_cast(index); + return { + static_cast(std::fmod(base * 0.7, 1.0) * 180), + static_cast(0.7 * 255), static_cast(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(static_cast(segmented.cols) * 0.5), + static_cast(static_cast(segmented.rows) * r)); + cv::Rect2i rect(target_px + cv::Point2i(-bw, -bw), target_px + cv::Point2i(bw, bw)); std::unordered_map areas; std::unordered_set candidates; @@ -69,7 +74,7 @@ int GraphSegment::search_most_road_like_class(const cv::Mat & segmented) const const int * seg_ptr = segmented.ptr(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(h); - uchar * const output_image_ptr = output_image.ptr(h); + auto * const debug_image_ptr = debug_image.ptr(h); + auto * const output_image_ptr = output_image.ptr(h); const int * const segmented_image_ptr = segmented.ptr(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(static_cast(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); } diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp index 85ce9e354362a..c7bf41bff459b 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp @@ -37,7 +37,7 @@ std::set SimilarAreaSearcher::search( for (int h = 0; h < rgb_image.rows; h++) { const int * seg_ptr = segmented.ptr(h); - const cv::Vec3b * rgb_ptr = rgb_image.ptr(h); + const auto * rgb_ptr = rgb_image.ptr(h); for (int w = 0; w < rgb_image.cols; w++) { int key = seg_ptr[w]; diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp index 107d861364038..d31b19406e320 100644 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp +++ b/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::max(); std::optional 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(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::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(abs_dt); synched_pose = pose; } } if (min_dt > 0.1) return; auto latest_pose_stamp = rclcpp::Time(pose_buffer_.back().header.stamp); - std::vector 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 >(info_->k.data()).cast().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 { - 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(uv1.x()), static_cast(uv1.y())), + cv::Point2i(static_cast(uv2.x()), static_cast(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(uv1.x()), static_cast(uv1.y())), + cv::Point2i(static_cast(uv2.x()), static_cast(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; diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index c613642628499..9854fa9ab7106 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/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 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(0), xy_xy.at(1), 0; xy2 << xy_xy.at(2), xy_xy.at(3), 0; pcl::PointNormal pn; @@ -79,7 +80,7 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st } std::vector LineSegmentDetector::remove_too_outer_elements( - const cv::Mat & lines, const cv::Size & size) const + const cv::Mat & lines, const cv::Size & size) { std::vector rect_vector; rect_vector.emplace_back(0, 0, size.width, 3); diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp index 7e06de81fbd18..70e1bf2d6e6e2 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp +++ b/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("~/input/image_raw", 10, cb_image); sub_line_segments_ = - create_subscription("~/input/line_segments", 10, cb_line_segments_); + create_subscription("~/input/line_segments", 10, cb_line_segments); pub_debug_image_ = create_publisher("~/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(xy1(0)), static_cast(xy1(1))), + cv::Point(static_cast(xy2(0)), static_cast(xy2(1))), color, 2); } common::publish_image(*pub_debug_image_, image, stamp); diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp index df0aa7d65c617..b844fb69285b5 100644 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp @@ -25,11 +25,11 @@ namespace yabloc::segment_filter { SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) : Node("segment_filter", options), - image_size_(declare_parameter("image_size")), - max_range_(declare_parameter("max_range")), - min_segment_length_(declare_parameter("min_segment_length")), - max_segment_distance_(declare_parameter("max_segment_distance")), - max_lateral_distance_(declare_parameter("max_lateral_distance")), + image_size_(static_cast(declare_parameter("image_size"))), + max_range_(static_cast(declare_parameter("max_range"))), + min_segment_length_(static_cast(declare_parameter("min_segment_length"))), + max_segment_distance_(static_cast(declare_parameter("max_segment_distance"))), + max_lateral_distance_(static_cast(declare_parameter("max_lateral_distance"))), info_(this), synchro_subscriber_(this, "~/input/line_segments_cloud", "~/input/graph_segmented"), tf_subscriber_(this->get_clock()) @@ -47,9 +47,12 @@ SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) cv::Point2i SegmentFilter::to_cv_point(const Eigen::Vector3f & v) const { - cv::Point pt; - pt.x = -v.y() / max_range_ * image_size_ * 0.5f + image_size_ / 2; - pt.y = -v.x() / max_range_ * image_size_ * 0.5f + image_size_; + cv::Point2i pt; + pt.x = static_cast( + -v.y() / max_range_ * static_cast(image_size_) * 0.5f + + static_cast(image_size_) / 2.0); + pt.y = static_cast( + -v.x() / max_range_ * static_cast(image_size_) * 0.5f + static_cast(image_size_)); return pt; } @@ -149,7 +152,7 @@ void SegmentFilter::execute(const PointCloud2 & line_segments_msg, const Image & pcl::PointXYZLNormal pln; pln.getVector3fMap() = pn.getVector3fMap(); pln.getNormalVector3fMap() = pn.getNormalVector3fMap(); - if (indices.count(index) > 0) + if (indices.count(static_cast(index)) > 0) pln.label = 255; else pln.label = 0; @@ -194,7 +197,7 @@ std::set get_unique_pixel_value(cv::Mat & image) auto last = std::unique(begin, image.end()); std::sort(begin, last); last = std::unique(begin, last); - return std::set(begin, last); + return {begin, last}; } pcl::PointCloud SegmentFilter::project_lines( @@ -204,9 +207,9 @@ pcl::PointCloud SegmentFilter::project_lines( pcl::PointCloud projected_points; for (size_t index = 0; index < points.size(); ++index) { if (negative) { - if (indices.count(index) > 0) continue; + if (indices.count(static_cast(index)) > 0) continue; } else { - if (indices.count(index) == 0) continue; + if (indices.count(static_cast(index)) == 0) continue; } pcl::PointNormal truncated_pn = points.at(index); @@ -254,9 +257,10 @@ std::set SegmentFilter::filter_by_mask( auto & pn = edges.at(i); Eigen::Vector3f p1 = pn.getVector3fMap(); Eigen::Vector3f p2 = pn.getNormalVector3fMap(); - cv::Scalar color = cv::Scalar::all(i + 1); + cv::Scalar color = cv::Scalar::all(static_cast(i + 1)); cv::line( - line_image, cv::Point2i(p1.x(), p1.y()), cv::Point2i(p2.x(), p2.y()), color, 1, + line_image, cv::Point2i(static_cast(p1.x()), static_cast(p1.y())), + cv::Point2i(static_cast(p2.x()), static_cast(p2.y())), color, 1, cv::LineTypes::LINE_4); } @@ -275,7 +279,7 @@ std::set SegmentFilter::filter_by_mask( // Extract edges within masks std::set reliable_indices; for (size_t i = 0; i < edges.size(); i++) { - if (pixel_values.count(i + 1) != 0) reliable_indices.insert(i); + if (pixel_values.count(i + 1) != 0) reliable_indices.insert(static_cast(i)); } return reliable_indices; diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 0714b6c8091c8..c4b868289eb16 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -39,8 +39,8 @@ class UndistortNode : public rclcpp::Node explicit UndistortNode(const rclcpp::NodeOptions & options) : Node("undistort", options), - OUTPUT_WIDTH(declare_parameter("width")), - OVERRIDE_FRAME_ID(declare_parameter("override_frame_id")) + OUTPUT_WIDTH_(static_cast(declare_parameter("width"))), + OVERRIDE_FRAME_ID_(declare_parameter("override_frame_id")) { using std::placeholders::_1; @@ -63,8 +63,8 @@ class UndistortNode : public rclcpp::Node } private: - const int OUTPUT_WIDTH; - const std::string OVERRIDE_FRAME_ID; + const int OUTPUT_WIDTH_; + const std::string OVERRIDE_FRAME_ID_; rclcpp::Subscription::SharedPtr sub_image_; rclcpp::Subscription::SharedPtr sub_compressed_image_; @@ -74,29 +74,33 @@ class UndistortNode : public rclcpp::Node std::optional info_{std::nullopt}; std::optional scaled_info_{std::nullopt}; - cv::Mat undistort_map_x, undistort_map_y; + cv::Mat undistort_map_x_, undistort_map_y_; void make_remap_lut() { if (!info_.has_value()) return; - cv::Mat K = cv::Mat(cv::Size(3, 3), CV_64FC1, reinterpret_cast(info_->k.data())); - cv::Mat D = cv::Mat(cv::Size(5, 1), CV_64FC1, reinterpret_cast(info_->d.data())); - cv::Size size(info_->width, info_->height); + cv::Mat k = cv::Mat(cv::Size(3, 3), CV_64FC1, info_->k.data()); + cv::Mat d = cv::Mat(cv::Size(5, 1), CV_64FC1, info_->d.data()); + cv::Size size(static_cast(info_->width), static_cast(info_->height)); cv::Size new_size = size; - if (OUTPUT_WIDTH > 0) - new_size = cv::Size(OUTPUT_WIDTH, 1.0f * OUTPUT_WIDTH / size.width * size.height); + if (OUTPUT_WIDTH_ > 0) + // set new_size along with aspect ratio + new_size = cv::Size( + OUTPUT_WIDTH_, static_cast( + static_cast(OUTPUT_WIDTH_) * + (static_cast(size.height) / static_cast(size.width)))); - cv::Mat new_K = cv::getOptimalNewCameraMatrix(K, D, size, 0, new_size); + cv::Mat new_k = cv::getOptimalNewCameraMatrix(k, d, size, 0, new_size); cv::initUndistortRectifyMap( - K, D, cv::Mat(), new_K, new_size, CV_32FC1, undistort_map_x, undistort_map_y); + k, d, cv::Mat(), new_k, new_size, CV_32FC1, undistort_map_x_, undistort_map_y_); scaled_info_ = sensor_msgs::msg::CameraInfo{}; - scaled_info_->k.at(0) = new_K.at(0, 0); - scaled_info_->k.at(2) = new_K.at(0, 2); - scaled_info_->k.at(4) = new_K.at(1, 1); - scaled_info_->k.at(5) = new_K.at(1, 2); + scaled_info_->k.at(0) = new_k.at(0, 0); + scaled_info_->k.at(2) = new_k.at(0, 2); + scaled_info_->k.at(4) = new_k.at(1, 1); + scaled_info_->k.at(5) = new_k.at(1, 2); scaled_info_->k.at(8) = 1; scaled_info_->d.resize(5); scaled_info_->width = new_size.width; @@ -106,12 +110,12 @@ class UndistortNode : public rclcpp::Node void remap_and_publish(const cv::Mat & image, const std_msgs::msg::Header & header) { cv::Mat undistorted_image; - cv::remap(image, undistorted_image, undistort_map_x, undistort_map_y, cv::INTER_LINEAR); + cv::remap(image, undistorted_image, undistort_map_x_, undistort_map_y_, cv::INTER_LINEAR); // Publish CameraInfo { scaled_info_->header = info_->header; - if (OVERRIDE_FRAME_ID != "") scaled_info_->header.frame_id = OVERRIDE_FRAME_ID; + if (!OVERRIDE_FRAME_ID_.empty()) scaled_info_->header.frame_id = OVERRIDE_FRAME_ID_; pub_info_->publish(scaled_info_.value()); } @@ -119,8 +123,8 @@ class UndistortNode : public rclcpp::Node { cv_bridge::CvImage bridge; bridge.header.stamp = header.stamp; - if (OVERRIDE_FRAME_ID != "") - bridge.header.frame_id = OVERRIDE_FRAME_ID; + if (!OVERRIDE_FRAME_ID_.empty()) + bridge.header.frame_id = OVERRIDE_FRAME_ID_; else bridge.header.frame_id = header.frame_id; bridge.encoding = "bgr8"; @@ -134,7 +138,7 @@ class UndistortNode : public rclcpp::Node if (!info_.has_value()) { return; } - if (undistort_map_x.empty()) { + if (undistort_map_x_.empty()) { make_remap_lut(); } @@ -153,7 +157,7 @@ class UndistortNode : public rclcpp::Node if (!info_.has_value()) { return; } - if (undistort_map_x.empty()) { + if (undistort_map_x_.empty()) { make_remap_lut(); }