diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp index eb2942a76e5df..9a382f71e6b33 100644 --- a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp @@ -65,9 +65,9 @@ class CameraPoseInitializer : public rclcpp::Node void on_map(const LaneletMapBin & msg); void on_service( - const RequestPoseAlignment::Request::SharedPtr, - RequestPoseAlignment::Response::SharedPtr request); - PoseCovStamped create_rectified_initial_pose( + const RequestPoseAlignment::Request::SharedPtr request, + RequestPoseAlignment::Response::SharedPtr response); + static PoseCovStamped create_rectified_initial_pose( const Eigen::Vector3f & pos, double yaw_angle_rad, const PoseCovStamped & src_msg); std::optional estimate_pose( diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/semantic_segmentation.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/semantic_segmentation.hpp index 061d0548c6f18..4558a503b8ac4 100644 --- a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/semantic_segmentation.hpp +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/semantic_segmentation.hpp @@ -31,13 +31,13 @@ class SemanticSegmentation static void print_error_message(const rclcpp::Logger & logger); private: - cv::Mat make_blob(const cv::Mat & image); + static cv::Mat make_blob(const cv::Mat & image); - cv::Mat convert_blob_to_image(const cv::Mat & blob); + static cv::Mat convert_blob_to_image(const cv::Mat & blob); - cv::Mat normalize(const cv::Mat & mask, double score_threshold = 0.5); + static cv::Mat normalize(const cv::Mat & mask, double score_threshold = 0.5); - cv::Mat draw_overlay(const cv::Mat & image, const cv::Mat & segmentation); + static cv::Mat draw_overlay(const cv::Mat & image, const cv::Mat & segmentation); struct Impl; std::shared_ptr impl_{nullptr}; diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp index ff251c7bd53b2..6aff744ef4975 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp @@ -32,7 +32,7 @@ namespace yabloc { CameraPoseInitializer::CameraPoseInitializer(const rclcpp::NodeOptions & options) : Node("camera_pose_initializer", options), - angle_resolution_(declare_parameter("angle_resolution")) + angle_resolution_(static_cast(declare_parameter("angle_resolution"))) { using std::placeholders::_1; using std::placeholders::_2; @@ -62,7 +62,7 @@ CameraPoseInitializer::CameraPoseInitializer(const rclcpp::NodeOptions & options } } -cv::Mat bitwise_and_3ch(const cv::Mat src1, const cv::Mat src2) +cv::Mat bitwise_and_3ch(const cv::Mat & src1, const cv::Mat & src2) { std::vector src1_array; std::vector src2_array; @@ -79,7 +79,7 @@ cv::Mat bitwise_and_3ch(const cv::Mat src1, const cv::Mat src2) return merged; } -int count_non_zero(cv::Mat image_3ch) +int count_non_zero(const cv::Mat & image_3ch) { std::vector images; cv::split(image_3ch, images); @@ -153,13 +153,13 @@ std::optional CameraPoseInitializer::estimate_pose( // consider lanelet direction float gain = 1; if (lane_angle_rad) { - gain = 2 + std::cos((lane_angle_rad.value() - angle_rad) / 2.0); + gain = static_cast(2 + std::cos((lane_angle_rad.value() - angle_rad) / 2.0)); } // If count_non_zero() returns 0 everywhere, the orientation is chosen by the only gain - const float score = gain * (1 + count_non_zero(dst)); + const float score = gain * static_cast(1 + count_non_zero(dst)); scores.push_back(score); - angles_rad.push_back(angle_rad); + angles_rad.push_back(static_cast(angle_rad)); } marker_module_->publish_marker(scores, angles_rad, position); @@ -176,7 +176,7 @@ void CameraPoseInitializer::on_map(const LaneletMapBin & msg) lane_image_ = std::make_unique(lanelet_map); const_lanelets_.clear(); - for (auto l : lanelet_map->laneletLayer) { + for (const auto & l : lanelet_map->laneletLayer) { const_lanelets_.push_back(l); } } @@ -191,7 +191,9 @@ void CameraPoseInitializer::on_service( const auto query_pos = request->pose_with_covariance.pose.pose.position; const auto orientation = request->pose_with_covariance.pose.pose.orientation; const double yaw_std_rad = std::sqrt(query_pos_with_cov.pose.covariance.at(35)); - const Eigen::Vector3f pos_vec3f(query_pos.x, query_pos.y, query_pos.z); + const Eigen::Vector3f pos_vec3f( + static_cast(query_pos.x), static_cast(query_pos.y), + static_cast(query_pos.z)); RCLCPP_INFO_STREAM(get_logger(), "Given initial position " << pos_vec3f.transpose()); // Estimate orientation diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp index 52ab8c8205815..d6949c43dbfc5 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp @@ -27,22 +27,22 @@ namespace yabloc { namespace bg = boost::geometry; -typedef bg::model::d2::point_xy point_t; -typedef bg::model::box box_t; -typedef bg::model::polygon polygon_t; +using point_t = bg::model::d2::point_xy; +using box_t = bg::model::box; +using polygon_t = bg::model::polygon; -LaneImage::LaneImage(lanelet::LaneletMapPtr map) : map_(map) +LaneImage::LaneImage(lanelet::LaneletMapPtr map) : map_(std::move(map)) { } cv::Point2i to_cv_point(const Eigen::Vector3f & v) { - const float image_size_ = 800; - const float max_range_ = 30; + const float image_size = 800; + const float max_range = 30; cv::Point pt; - pt.x = -v.y() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f; - pt.y = -v.x() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f; + pt.x = static_cast(-v.y() / max_range * image_size * 0.5f + image_size / 2.f); + pt.y = static_cast(-v.x() / max_range * image_size * 0.5f + image_size / 2.f); return pt; } @@ -59,7 +59,8 @@ void draw_lane(cv::Mat & image, const polygon_t & polygon) { std::vector contour; for (auto p : polygon.outer()) { - cv::Point2i pt = to_cv_point(Eigen::Vector3f(p.x(), p.y(), 0)); + cv::Point2i pt = + to_cv_point(Eigen::Vector3f(static_cast(p.x()), static_cast(p.y()), 0)); contour.push_back(pt); } @@ -71,8 +72,9 @@ void draw_lane(cv::Mat & image, const polygon_t & polygon) void draw_line(cv::Mat & image, const lanelet::LineString2d & line, geometry_msgs::msg::Point xyz) { std::vector contour; - for (auto p : line) { - cv::Point2i pt = to_cv_point(Eigen::Vector3f(p.x() - xyz.x, p.y() - xyz.y, 0)); + for (const auto & p : line) { + cv::Point2i pt = to_cv_point( + Eigen::Vector3f(static_cast(p.x() - xyz.x), static_cast(p.y() - xyz.y), 0)); contour.push_back(pt); } cv::polylines(image, contour, false, cv::Scalar(0, 0, 255), 2); @@ -83,7 +85,9 @@ cv::Mat LaneImage::get_image(const Pose & pose) const auto xyz = pose.position; box_t box(point_t(-20, -20), point_t(20, 20)); - cv::Mat image = cv::Mat::zeros(cv::Size(800, 800), CV_8UC3); + cv::Mat image = cv::Mat::zeros(cv::Size(800, 800), CV_8UC3); // NOLINT + // suppress hicpp-signed-bitwise + // from OpenCV std::vector joint_lanes; for (auto lanelet : map_->laneletLayer) { @@ -91,7 +95,7 @@ cv::Mat LaneImage::get_image(const Pose & pose) for (auto right : lanelet.rightBound2d()) { polygon.outer().push_back(point_t(right.x() - xyz.x, right.y() - xyz.y)); } - for (auto left : boost::adaptors::reverse(lanelet.leftBound2d())) { + for (const auto & left : boost::adaptors::reverse(lanelet.leftBound2d())) { polygon.outer().push_back(point_t(left.x() - xyz.x, left.y() - xyz.y)); } diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp index 8a5fc65e2fac3..0d4dd8bb373e5 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp @@ -28,22 +28,22 @@ void MarkerModule::publish_marker( const std::vector & scores, const std::vector & angles, const Eigen::Vector3f & position) { - const int N = scores.size(); + const int n = static_cast(scores.size()); auto minmax = std::minmax_element(scores.begin(), scores.end()); auto normalize = [minmax](int score) -> float { - return static_cast(score - *minmax.first) / + return static_cast(static_cast(score) - *minmax.first) / std::max(1e-4f, static_cast(*minmax.second - *minmax.first)); }; MarkerArray array; - for (int i = 0; i < N; i++) { + for (int i = 0; i < n; i++) { Marker marker; marker.header.frame_id = "map"; marker.type = Marker::ARROW; marker.id = i; marker.ns = "arrow"; - marker.color = - static_cast(common::color_scale::rainbow(normalize(scores.at(i)))); + marker.color = static_cast( + common::color_scale::rainbow(normalize(static_cast(scores.at(i))))); marker.color.a = 0.5; marker.pose.position.x = position.x(); diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp index baa199d5548a1..449812eff6e09 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp @@ -27,12 +27,12 @@ ProjectorModule::ProjectorModule(rclcpp::Node * node) cv::Point2i to_cv_point(const Eigen::Vector3f & v) { - const float image_size_ = 800; - const float max_range_ = 30; + const float image_size = 800; + const float max_range = 30; cv::Point pt; - pt.x = -v.y() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f; - pt.y = -v.x() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f; + pt.x = static_cast(-v.y() / max_range * image_size * 0.5f + image_size / 2.f); + pt.y = static_cast(-v.x() / max_range * image_size * 0.5f + image_size / 2.f); return pt; } @@ -44,7 +44,10 @@ cv::Mat ProjectorModule::project_image(const cv::Mat & mask_image) std::vector colors = { cv::Scalar(255, 0, 0), cv::Scalar(0, 255, 0), cv::Scalar(0, 0, 255)}; - cv::Mat projected_image = cv::Mat::zeros(cv::Size(800, 800), CV_8UC3); + cv::Mat projected_image = + cv::Mat::zeros(cv::Size(800, 800), CV_8UC3); // NOLINT + // suppress hicpp-signed-bitwise + // from OpenCV for (int i = 0; i < 3; i++) { std::vector > contours; cv::findContours(masks[i], contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE); @@ -90,7 +93,7 @@ bool ProjectorModule::define_project_func() // TODO(KYabuuchi) This will take into account ground tilt and camera vibration someday. project_func_ = [intrinsic_inv, q, t](const cv::Point & u) -> std::optional { - Eigen::Vector3f u3(u.x, u.y, 1); + Eigen::Vector3f u3(static_cast(u.x), static_cast(u.y), 1); Eigen::Vector3f u_bearing = (q * intrinsic_inv * u3).normalized(); if (u_bearing.z() > -0.01) return std::nullopt; float u_distance = -t.z() / u_bearing.z(); diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp index 760ada6f8310a..917015269d3e2 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp @@ -53,7 +53,8 @@ cv::Mat SemanticSegmentation::convert_blob_to_image(const cv::Mat & blob) const int channels = blob.size[1]; const int height = blob.size[2]; const int width = blob.size[3]; - cv::Mat image = cv::Mat(height, width, CV_32FC4); + cv::Mat image = cv::Mat(height, width, CV_32FC4); // NOLINT + // suppress hicpp-signed-bitwise from OpenCV for (int h = 0; h < height; ++h) { for (int w = 0; w < width; ++w) { @@ -92,7 +93,8 @@ cv::Mat SemanticSegmentation::normalize(const cv::Mat & mask, double score_thres for (size_t i = 1; i < masks.size(); ++i) { cv::Mat bin_mask; cv::threshold(masks[i], bin_mask, score_threshold, 255, cv::THRESH_BINARY_INV); - bin_mask.convertTo(bin_mask, CV_8UC1); + bin_mask.convertTo(bin_mask, CV_8UC1); // NOLINT + // suppress hicpp-signed-bitwise from OpenCV bin_masks.push_back(255 - bin_mask); } @@ -109,11 +111,11 @@ cv::Mat SemanticSegmentation::draw_overlay(const cv::Mat & image, const cv::Mat void SemanticSegmentation::print_error_message(const rclcpp::Logger & logger) { - const std::string ERROR_MESSAGE = + const std::string error_message = R"(The yabloc_pose_initializer is not working correctly because the DNN model has not been downloaded correctly. Please check the README of yabloc_pose_initializer to know how download models.)"; - std::istringstream stream(ERROR_MESSAGE); + std::istringstream stream(error_message); std::string line; while (std::getline(stream, line)) { RCLCPP_ERROR_STREAM(logger, line);