From 246002e555c51c81dd4c9f676771ee5163c09b96 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 16 Jan 2026 22:40:00 +0100 Subject: [PATCH 01/13] line detection package structure --- line_detection_sonar/CMakeLists.txt | 86 +++++++++++++++++++ line_detection_sonar/README.md | 0 .../config/line_detection_sonar.yaml | 2 + .../ros/line_detection_sonar_ros.hpp | 0 line_detection_sonar/package.xml | 22 +++++ .../src/ros/line_detection_sonar_ros.cpp | 0 6 files changed, 110 insertions(+) create mode 100644 line_detection_sonar/CMakeLists.txt create mode 100644 line_detection_sonar/README.md create mode 100644 line_detection_sonar/config/line_detection_sonar.yaml create mode 100644 line_detection_sonar/include/line_detection_sonar/ros/line_detection_sonar_ros.hpp create mode 100644 line_detection_sonar/package.xml create mode 100644 line_detection_sonar/src/ros/line_detection_sonar_ros.cpp diff --git a/line_detection_sonar/CMakeLists.txt b/line_detection_sonar/CMakeLists.txt new file mode 100644 index 0000000..011ef4b --- /dev/null +++ b/line_detection_sonar/CMakeLists.txt @@ -0,0 +1,86 @@ +cmake_minimum_required(VERSION 3.8) +project(line_detection_sonar) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(vortex_msgs REQUIRED) + +include_directories(include) + +set(CORE_LIB_NAME "${PROJECT_NAME}") + +add_library(${CORE_LIB_NAME} SHARED + src/lib/houghp_line_detection.cpp +) + +target_include_directories(${CORE_LIB_NAME} + PUBLIC + $ + $ +) + +ament_target_dependencies(${CORE_LIB_NAME} + vortex_utils + vortex_utils_ros + vortex_msgs +) + +set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component") + +add_library(${COMPONENT_LIB_NAME} SHARED + src/ros/${PROJECT_NAME}_ros.cpp +) + +ament_target_dependencies(${COMPONENT_LIB_NAME} + rclcpp + rclcpp_components + vortex_utils + vortex_utils_ros +) + +target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME}) + +rclcpp_components_register_node( + ${COMPONENT_LIB_NAME} + PLUGIN "LineDetectionSonarNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +install( + TARGETS + ${CORE_LIB_NAME} + ${COMPONENT_LIB_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +ament_export_include_directories(include) +ament_export_libraries(${CORE_LIB_NAME}) + +ament_package() diff --git a/line_detection_sonar/README.md b/line_detection_sonar/README.md new file mode 100644 index 0000000..e69de29 diff --git a/line_detection_sonar/config/line_detection_sonar.yaml b/line_detection_sonar/config/line_detection_sonar.yaml new file mode 100644 index 0000000..c3067f3 --- /dev/null +++ b/line_detection_sonar/config/line_detection_sonar.yaml @@ -0,0 +1,2 @@ +/**: + ros_ diff --git a/line_detection_sonar/include/line_detection_sonar/ros/line_detection_sonar_ros.hpp b/line_detection_sonar/include/line_detection_sonar/ros/line_detection_sonar_ros.hpp new file mode 100644 index 0000000..e69de29 diff --git a/line_detection_sonar/package.xml b/line_detection_sonar/package.xml new file mode 100644 index 0000000..3f40a55 --- /dev/null +++ b/line_detection_sonar/package.xml @@ -0,0 +1,22 @@ + + + + line_detection_sonar + 0.0.0 + Package for detecting lines on images produces by an FLS sonar. + + jorgen + MIT + + ament_cmake + + rclcpp + rclcpp_components + vortex_utils + vortex_utils_ros + vortex_msgs + + + ament_cmake + + diff --git a/line_detection_sonar/src/ros/line_detection_sonar_ros.cpp b/line_detection_sonar/src/ros/line_detection_sonar_ros.cpp new file mode 100644 index 0000000..e69de29 From 6955ee2fdd6436d34247a60a2039b799da342a01 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 19 Jan 2026 21:48:26 +0100 Subject: [PATCH 02/13] line detection typedefs and class declaration --- .gitignore | 2 - line_detection_sonar/CMakeLists.txt | 7 +- .../lib/houghp_line_detection.hpp | 42 +++++++ .../line_detection_sonar/lib/typedefs.hpp | 106 ++++++++++++++++++ line_detection_sonar/package.xml | 1 + .../src/lib/houghp_line_detection.cpp | 39 +++++++ 6 files changed, 194 insertions(+), 3 deletions(-) create mode 100644 line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp create mode 100644 line_detection_sonar/include/line_detection_sonar/lib/typedefs.hpp create mode 100644 line_detection_sonar/src/lib/houghp_line_detection.cpp diff --git a/.gitignore b/.gitignore index 5d05913..2a027b3 100644 --- a/.gitignore +++ b/.gitignore @@ -25,7 +25,6 @@ dist/ downloads/ eggs/ .eggs/ -lib/ lib64/ parts/ sdist/ @@ -210,7 +209,6 @@ devel/ logs/ build/ bin/ -lib/ msg_gen/ srv_gen/ msg/*Action.msg diff --git a/line_detection_sonar/CMakeLists.txt b/line_detection_sonar/CMakeLists.txt index 011ef4b..bee2e3b 100644 --- a/line_detection_sonar/CMakeLists.txt +++ b/line_detection_sonar/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(rclcpp_components REQUIRED) find_package(vortex_utils REQUIRED) find_package(vortex_utils_ros REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(OpenCV REQUIRED) include_directories(include) @@ -28,7 +29,8 @@ target_include_directories(${CORE_LIB_NAME} PUBLIC $ $ -) + ${OpenCV_INCLUDE_DIRS} + ) ament_target_dependencies(${CORE_LIB_NAME} vortex_utils @@ -36,6 +38,9 @@ ament_target_dependencies(${CORE_LIB_NAME} vortex_msgs ) +target_link_libraries(${CORE_LIB_NAME} + ${OpenCV_LIBS}) + set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component") add_library(${COMPONENT_LIB_NAME} SHARED diff --git a/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp b/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp new file mode 100644 index 0000000..38741db --- /dev/null +++ b/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp @@ -0,0 +1,42 @@ +#ifndef LINE_DETECTION_SONAR__LIB__HOUGHP_LINE_DETECTION_HPP_ +#define LINE_DETECTION_SONAR__LIB__HOUGHP_LINE_DETECTION_HPP_ + +#include +#include +#include +#include "typedefs.hpp" + +namespace vortex::line_detection { + +class HoughPLineDetector { + public: + explicit HoughPLineDetector(EdgeDetectionConfig edge_cfg, + LineDetectionConfig line_cfg); + + Result detect(const cv::Mat& input_image, Mode mode = Mode::standard) const; + + private: + EdgeDetectionConfig edge_config_; + LineDetectionConfig config_; + + void detect_edges(const cv::Mat& input_image, cv::Mat& edges) const; + + void detect_line_segments(const cv::Mat& edges, + std::vector& lines) const; + + static std::vector to_line_segments( + const std::vector& lines); + + static void make_overlay_color(const cv::Mat& input_bgr, + const cv::Mat& edges, + const std::vector& lines, + cv::Mat& out_bgr); + + static void make_overlay_canny(const cv::Mat& edges, + const std::vector& lines, + cv::Mat& out_bgr); +}; + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_SONAR__LIB__HOUGHP_LINE_DETECTION_HPP_ diff --git a/line_detection_sonar/include/line_detection_sonar/lib/typedefs.hpp b/line_detection_sonar/include/line_detection_sonar/lib/typedefs.hpp new file mode 100644 index 0000000..6d7e73b --- /dev/null +++ b/line_detection_sonar/include/line_detection_sonar/lib/typedefs.hpp @@ -0,0 +1,106 @@ +#ifndef LINE_DETECTION_SONAR__LIB__TYPEDEFS_HPP_ +#define LINE_DETECTION_SONAR__LIB__TYPEDEFS_HPP_ + +#include +#include +#include + +namespace vortex::line_detection { + +/** + * @brief Config for the OpenCV Canny edge algorithm + * used to detect edges in an 8-bit single channel image. + */ +struct EdgeDetectionConfig { + int low_threshold{50}; // Low threshold for the hysteresis procedure. + int high_threshold{150}; // High threshold for the hysteresis procedure. + int aperture_size{3}; // Aperture size for the Sobel operator. + bool L2_gradient{false}; // L2_gradient flag, indicating whether to use a + // more accurate edge equation algorithm. +}; + +/** + * @brief Config for the OpenCV HoughLinesP algorithm + * used to detect lines segments in an 8-bit single channel image. + */ +struct LineDetectionConfig { + double rho{1.0}; // Distance resolution of the accumulator in pixels. + double theta{CV_PI / 180.0}; // Theta angle resolution + // of the accumulator in radians. + int threshold{50}; // Accumulator threshold used for accepting lines. + double min_line_length{0.0}; // Minimum allowed length for returned lines. + double max_line_gap{0.0}; // Allowed gap between points + // on the same line to link them. +}; + +/** + * @brief Operating mode of the detector. + * + * - standard: compute and return line segments only. + * - visualize: return line segments + + * an overlay image on the input color image. + * - debug: return line segments + edge map + + * overlay color and edge image for debugging. + */ +enum class Mode { standard, visualize, debug }; + +/** + * @brief Empty output payload for Mode::standard. + * + * Used as the "no extra outputs" alternative in the Output variant. + */ +struct NoOutput {}; + +/** + * @brief Visualization payload for Mode::visualize. + * + * - overlay_color: a BGR visualization derived from the input color image with + * drawings applied. + */ +struct VisualizeOutput { + cv::Mat overlay_color; // Input image with line segments overlaid + // (typically CV_8UC3). +}; + +/** + * @brief Debug payload for Mode::debug. + * + * - canny: the edge map produced by Canny (8-bit, single-channel). + * - overlay_canny: a BGR visualization derived from the edge map with drawings + * applied. + * - overlay_color: a BGR visualization derived from the input color image with + * drawings applied. + */ +struct DebugOutput { + cv::Mat canny; // Canny edge map (CV_8UC1). + cv::Mat overlay_canny; // Edge map with line segments overlaid (typically + // CV_8UC3). + cv::Mat overlay_color; // Input image with line segments overlaid + // (typically CV_8UC3). +}; + +/** + * @brief Variant holding the mode-dependent output payload. + * + * Exactly one alternative is active: + * - NoOutput for Mode::standard + * - VisualizeOutput for Mode::visualize + * - DebugOutput for Mode::debug + */ +using Output = std::variant; + +/** + * @brief Result returned from a detection call. + * + * line_segments is always populated. output contains additional products + * depending on the requested Mode. + */ +struct Result { + std::vector + line_segments; // Detected line segments. + Output output; // Mode-dependent extra outputs. +}; + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_SONAR__LIB__TYPEDEFS_HPP_ diff --git a/line_detection_sonar/package.xml b/line_detection_sonar/package.xml index 3f40a55..9721aaa 100644 --- a/line_detection_sonar/package.xml +++ b/line_detection_sonar/package.xml @@ -15,6 +15,7 @@ vortex_utils vortex_utils_ros vortex_msgs + opencv ament_cmake diff --git a/line_detection_sonar/src/lib/houghp_line_detection.cpp b/line_detection_sonar/src/lib/houghp_line_detection.cpp new file mode 100644 index 0000000..339e3d5 --- /dev/null +++ b/line_detection_sonar/src/lib/houghp_line_detection.cpp @@ -0,0 +1,39 @@ +#include "line_detection_sonar/lib/houghp_line_detection.hpp" + +namespace vortex::line_detection { + +Result HoughPLineDetector::detect(const cv::Mat& input_image, Mode mode) const { + cv::Mat edge_image; + detect_edges(input_image, edge_image); + + std::vector lines; + detect_line_segments(edge_image, lines); + + Result r; + r.line_segments = to_line_segments(lines); + + switch (mode) { + case Mode::standard: { + r.output = NoOutput{}; + break; + } + case Mode::visualize: { + VisualizeOutput v; + make_overlay_color(input_image, edge_image, lines, v.overlay_color); + r.output = std::move(v); + break; + } + case Mode::debug: { + DebugOutput d; + d.canny = + edge_image; // shallow copy; use edge_image.clone() if needed + make_overlay_canny(edge_image, lines, d.overlay_canny); + make_overlay_color(input_image, edge_image, lines, d.overlay_color); + r.output = std::move(d); + break; + } + } + return r; +} + +} // namespace vortex::line_detection From 6f88b638ff23eaafa6a5c4c6188bc50205843296 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 20 Jan 2026 14:15:47 +0100 Subject: [PATCH 03/13] HoughPLineDetector class impl --- .../lib/houghp_line_detection.hpp | 28 ++-- .../include/line_detection_sonar/lib/utils.h | 81 ++++++++++ .../line_detection_sonar/lib/utils.hpp | 145 ++++++++++++++++++ .../src/lib/houghp_line_detection.cpp | 62 ++++++-- 4 files changed, 293 insertions(+), 23 deletions(-) create mode 100644 line_detection_sonar/include/line_detection_sonar/lib/utils.h create mode 100644 line_detection_sonar/include/line_detection_sonar/lib/utils.hpp diff --git a/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp b/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp index 38741db..0bd6142 100644 --- a/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp +++ b/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp @@ -10,31 +10,31 @@ namespace vortex::line_detection { class HoughPLineDetector { public: - explicit HoughPLineDetector(EdgeDetectionConfig edge_cfg, - LineDetectionConfig line_cfg); + explicit HoughPLineDetector(const EdgeDetectionConfig& edge_config, + const LineDetectionConfig& line_config); Result detect(const cv::Mat& input_image, Mode mode = Mode::standard) const; private: EdgeDetectionConfig edge_config_; - LineDetectionConfig config_; + LineDetectionConfig line_config_; - void detect_edges(const cv::Mat& input_image, cv::Mat& edges) const; + void detect_edges(const cv::Mat& input_image, cv::Mat& edge_image) const; - void detect_line_segments(const cv::Mat& edges, - std::vector& lines) const; + void detect_line_segments(const cv::Mat& edge_image, + std::vector& cv_lines) const; static std::vector to_line_segments( - const std::vector& lines); + const std::vector& cv_lines); - static void make_overlay_color(const cv::Mat& input_bgr, - const cv::Mat& edges, - const std::vector& lines, - cv::Mat& out_bgr); + // static void make_overlay_color(const cv::Mat& input_bgr, + // const cv::Mat& edge_image, + // const std::vector& cv_lines, + // cv::Mat& out_bgr); - static void make_overlay_canny(const cv::Mat& edges, - const std::vector& lines, - cv::Mat& out_bgr); + // static void make_overlay_canny(const cv::Mat& edge_image, + // const std::vector& cv_lines, + // cv::Mat& out_bgr); }; } // namespace vortex::line_detection diff --git a/line_detection_sonar/include/line_detection_sonar/lib/utils.h b/line_detection_sonar/include/line_detection_sonar/lib/utils.h new file mode 100644 index 0000000..8f2bdad --- /dev/null +++ b/line_detection_sonar/include/line_detection_sonar/lib/utils.h @@ -0,0 +1,81 @@ +#ifndef LINE_DETECTION_SONAR__LIB__UTILS_H_ +#define LINE_DETECTION_SONAR__LIB__UTILS_H_ + +#include + +namespace vortex::line_detection { + +/** + * @brief Convert an image to 8-bit, single-channel grayscale (CV_8UC1). + * + * This helper is intended to produce a suitable input for edge detection (e.g., + * Canny) from a variety of common OpenCV image formats. + * + * Conversion rules: + * - If @p input is already CV_8UC1, the function returns a **shallow copy** (no + * pixel data copied). + * - If @p input has 3 channels, it is converted using @c cv::COLOR_BGR2GRAY + * (assumes BGR ordering). + * - If @p input has 4 channels, it is converted using @c cv::COLOR_BGRA2GRAY + * (assumes BGRA ordering). + * + * - If the resulting grayscale image is 16-bit unsigned (CV_16U), it is scaled + * to 8-bit using + * @c convertTo with @c alpha = 1/256 (i.e., approximately a right shift by 8 + * bits). + * - For other depths (signed integers or floating point), the image is min-max + * normalized to [0,255] using @c cv::normalize(..., cv::NORM_MINMAX) and + * written as CV_8U. + * + * @param input Input image. Supported channel counts: 1, 3 (BGR), or 4 (BGRA). + * Supported depths: any OpenCV depth; non-8U depths are converted + * as described above. + * + * @return A grayscale image of type CV_8UC1. May share data with @p input if no + * conversion is needed. + * + * @throws std::runtime_error If @p input is empty or has an unsupported channel + * count. + */ +inline cv::Mat to_gray8(const cv::Mat& input); + +/** + * @brief Convert an image to 8-bit, 3-channel BGR (CV_8UC3). + * + * Conversion rules: + * - If @p input is already CV_8UC3, the function returns a **shallow copy** (no + * pixel data copied). + * - If @p input is single-channel, it is expanded to BGR using @c + * cv::COLOR_GRAY2BGR. In this case, the already-computed @p gray8 (CV_8UC1) + * must be provided to avoid recomputing grayscale conversion / scaling. + * - If @p input has 4 channels, it is converted using @c cv::COLOR_BGRA2BGR + * (assumes BGRA ordering). + * - If the resulting BGR image is 16-bit unsigned (CV_16U), it is scaled to + * 8-bit using + * @c convertTo with @c alpha = 1/256 (i.e., approximately a right shift by 8 + * bits). + * - For other depths (signed integers or floating point), the image is min-max + * normalized to [0,255] using @c cv::normalize(..., cv::NORM_MINMAX) and + * written as CV_8U. + * + * @note The normalization step computes min/max per call. This is usually fine + * for visualization, but it may produce different brightness/contrast per frame + * when viewing a stream. + * + * @param input Input image. Supported channel counts: 1, 3 (assumed BGR), or 4 + * (assumed BGRA). + * @param gray8 A grayscale image of type CV_8UC1 corresponding to @p input (or + * derived from it). Required when @p input is single-channel; ignored for 3- or + * 4-channel inputs. + * + * @return A BGR image of type CV_8UC3. May share data with @p input if no + * conversion is needed. + * + * @throws std::runtime_error If @p input is empty, has an unsupported channel + * count, or if @p input is single-channel and @p gray8 is empty or not CV_8UC1. + */ +inline cv::Mat to_bgr8(const cv::Mat& input, const cv::Mat& gray8 /*CV_8UC1*/); + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_SONAR__LIB__UTILS_H_ diff --git a/line_detection_sonar/include/line_detection_sonar/lib/utils.hpp b/line_detection_sonar/include/line_detection_sonar/lib/utils.hpp new file mode 100644 index 0000000..16b5258 --- /dev/null +++ b/line_detection_sonar/include/line_detection_sonar/lib/utils.hpp @@ -0,0 +1,145 @@ +#ifndef LINE_DETECTION_SONAR__LIB__UTILS_HPP_ +#define LINE_DETECTION_SONAR__LIB__UTILS_HPP_ + +#include +#include "utils.h" + +namespace vortex::line_detection { + +inline cv::Mat to_gray8(const cv::Mat& input) { + if (input.empty()) { + throw std::runtime_error("to_gray8: input is empty"); + } + + if (input.type() == CV_8UC1) { + return input; + } + + cv::Mat gray; + const int ch = input.channels(); + + // Convert to grayscale + if (ch == 1) { + gray = input; + } else if (ch == 3) { + cv::cvtColor(input, gray, cv::COLOR_BGR2GRAY); // assumes BGR + } else if (ch == 4) { + cv::cvtColor(input, gray, cv::COLOR_BGRA2GRAY); // assumes BGRA + } else { + throw std::runtime_error("to_gray8: unsupported channel count"); + } + + // Now ensure 8-bit depth. + if (gray.depth() == CV_8U) { + return gray; + } + + if (gray.depth() == CV_16U) { + cv::Mat out; + gray.convertTo(out, CV_8U, 1.0 / 256.0); // quick/common 16U->8U + return out; + } + + // Generic fallback for signed ints / floats etc. + cv::Mat out; + cv::normalize(gray, out, 0, 255, cv::NORM_MINMAX, CV_8U); + return out; +} + +inline cv::Mat to_bgr8(const cv::Mat& input, const cv::Mat& gray8 /*CV_8UC1*/) { + if (input.empty()) { + throw std::runtime_error("to_bgr8_for_overlay: input is empty"); + } + + if (input.type() == CV_8UC3) { + return input; + } + + const int ch = input.channels(); + + if (ch == 1) { + if (gray8.empty() || gray8.type() != CV_8UC1) { + throw std::runtime_error( + "to_bgr8_for_overlay: gray8 must be CV_8UC1"); + } + cv::Mat bgr; + cv::cvtColor(gray8, bgr, cv::COLOR_GRAY2BGR); + return bgr; + } + + if (ch == 4) { + cv::Mat bgr; + cv::cvtColor(input, bgr, cv::COLOR_BGRA2BGR); // assumes BGRA + + if (bgr.depth() == CV_8U) { + return bgr; + } + if (bgr.depth() == CV_16U) { + cv::Mat out; + bgr.convertTo(out, CV_8U, 1.0 / 256.0); + return out; + } + cv::Mat out; + cv::normalize(bgr, out, 0, 255, cv::NORM_MINMAX, CV_8U); + return out; + } + + if (ch == 3) { + if (input.depth() == CV_8U) { + return input; + } + if (input.depth() == CV_16U) { + cv::Mat out; + input.convertTo(out, CV_8U, 1.0 / 256.0); + return out; + } + cv::Mat out; + cv::normalize(input, out, 0, 255, cv::NORM_MINMAX, CV_8U); + return out; + } + + throw std::runtime_error("to_bgr8_for_overlay: unsupported channel count"); +} + +constexpr int kLineThickness = 2; +constexpr int kLineType = cv::LINE_AA; +const cv::Scalar kLineColor(0, 0, 255); // BGR red + +inline void make_overlay_color(const cv::Mat& input_bgr, + const cv::Mat& gray8, + const std::vector& lines, + cv::Mat& out_bgr) { + if (input_bgr.empty()) { + throw std::runtime_error("make_overlay_color: input_bgr is empty"); + } + out_bgr = vortex::line_detection::to_bgr8(input_bgr, gray8); + + for (const auto& l : lines) { + const cv::Point p0(l[0], l[1]); + const cv::Point p1(l[2], l[3]); + cv::line(out_bgr, p0, p1, kLineColor, kLineThickness, kLineType); + } +} + +inline void make_overlay_canny(const cv::Mat& edges, + const std::vector& lines, + cv::Mat& out_bgr) { + if (edges.empty()) { + throw std::runtime_error("make_overlay_canny: edges is empty"); + } + if (edges.type() != CV_8UC1) { + throw std::runtime_error("make_overlay_canny: edges must be CV_8UC1"); + } + + cv::cvtColor(edges, out_bgr, cv::COLOR_GRAY2BGR); + + for (const auto& l : lines) { + const cv::Point p0(l[0], l[1]); + const cv::Point p1(l[2], l[3]); + cv::line(out_bgr, p0, p1, kLineColor, kLineThickness, kLineType); + } +} + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_SONAR__LIB__UTILS_HPP_ diff --git a/line_detection_sonar/src/lib/houghp_line_detection.cpp b/line_detection_sonar/src/lib/houghp_line_detection.cpp index 339e3d5..12bf99e 100644 --- a/line_detection_sonar/src/lib/houghp_line_detection.cpp +++ b/line_detection_sonar/src/lib/houghp_line_detection.cpp @@ -1,16 +1,31 @@ #include "line_detection_sonar/lib/houghp_line_detection.hpp" +#include +#include +#include +#include "line_detection_sonar/lib/typedefs.hpp" +#include "line_detection_sonar/lib/utils.hpp" namespace vortex::line_detection { +HoughPLineDetector::HoughPLineDetector(const EdgeDetectionConfig& edge_config, + const LineDetectionConfig& line_config) + : edge_config_(edge_config), line_config_(line_config) {} + Result HoughPLineDetector::detect(const cv::Mat& input_image, Mode mode) const { + if (input_image.empty()) { + throw std::runtime_error( + "HoughPLineDetector::detect: input_image is empty"); + } + cv::Mat gray8 = vortex::line_detection::to_gray8(input_image); cv::Mat edge_image; - detect_edges(input_image, edge_image); - std::vector lines; - detect_line_segments(edge_image, lines); + detect_edges(gray8, edge_image); + + std::vector cv_lines; + detect_line_segments(edge_image, cv_lines); Result r; - r.line_segments = to_line_segments(lines); + r.line_segments = to_line_segments(cv_lines); switch (mode) { case Mode::standard: { @@ -19,16 +34,18 @@ Result HoughPLineDetector::detect(const cv::Mat& input_image, Mode mode) const { } case Mode::visualize: { VisualizeOutput v; - make_overlay_color(input_image, edge_image, lines, v.overlay_color); + vortex::line_detection::make_overlay_color( + input_image, gray8, cv_lines, v.overlay_color); r.output = std::move(v); break; } case Mode::debug: { DebugOutput d; - d.canny = - edge_image; // shallow copy; use edge_image.clone() if needed - make_overlay_canny(edge_image, lines, d.overlay_canny); - make_overlay_color(input_image, edge_image, lines, d.overlay_color); + d.canny = edge_image; + vortex::line_detection::make_overlay_canny(edge_image, cv_lines, + d.overlay_canny); + vortex::line_detection::make_overlay_color( + input_image, gray8, cv_lines, d.overlay_color); r.output = std::move(d); break; } @@ -36,4 +53,31 @@ Result HoughPLineDetector::detect(const cv::Mat& input_image, Mode mode) const { return r; } +void HoughPLineDetector::detect_edges(const cv::Mat& input_image, + cv::Mat& edge_image) const { + cv::Canny(input_image, edge_image, edge_config_.low_threshold, + edge_config_.high_threshold, edge_config_.aperture_size, + edge_config_.L2_gradient); +} + +void HoughPLineDetector::detect_line_segments( + const cv::Mat& edge_image, + std::vector& cv_lines) const { + cv::HoughLinesP(edge_image, cv_lines, line_config_.rho, line_config_.theta, + line_config_.threshold); +} + +std::vector to_line_segments( + const std::vector& cv_lines) { + std::vector line_segments; + line_segments.reserve(cv_lines.size()); + for (const auto& cv_line : cv_lines) { + line_segments.push_back(vortex::utils::types::LineSegment2D{ + {static_cast(cv_line[0]), static_cast(cv_line[1])}, + {static_cast(cv_line[2]), + static_cast(cv_line[3])}}); + } + return line_segments; +} + } // namespace vortex::line_detection From e3a33805cd417b5439923aa42d38ade755ef4a87 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 20 Jan 2026 17:22:59 +0100 Subject: [PATCH 04/13] line detection node impl --- line_detection_sonar/CMakeLists.txt | 21 ++- .../config/line_detection_sonar.yaml | 23 ++- .../lib/houghp_line_detection.hpp | 21 +-- .../line_detection_sonar/lib/typedefs.hpp | 7 +- .../line_detection_sonar/lib/utils.hpp | 3 +- .../ros/line_detection_sonar_ros.hpp | 91 ++++++++++++ .../launch/line_detection_sonar.launch.py | 28 ++++ line_detection_sonar/package.xml | 1 + .../src/lib/houghp_line_detection.cpp | 31 ++-- .../src/ros/line_detection_sonar_ros.cpp | 135 +++++++++++++++++ .../ros/line_detection_sonar_ros_utils.cpp | 139 ++++++++++++++++++ 11 files changed, 460 insertions(+), 40 deletions(-) create mode 100644 line_detection_sonar/launch/line_detection_sonar.launch.py create mode 100644 line_detection_sonar/src/ros/line_detection_sonar_ros_utils.cpp diff --git a/line_detection_sonar/CMakeLists.txt b/line_detection_sonar/CMakeLists.txt index bee2e3b..b5ecb95 100644 --- a/line_detection_sonar/CMakeLists.txt +++ b/line_detection_sonar/CMakeLists.txt @@ -15,7 +15,10 @@ find_package(rclcpp_components REQUIRED) find_package(vortex_utils REQUIRED) find_package(vortex_utils_ros REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(OpenCV REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) include_directories(include) @@ -27,15 +30,13 @@ add_library(${CORE_LIB_NAME} SHARED target_include_directories(${CORE_LIB_NAME} PUBLIC - $ - $ - ${OpenCV_INCLUDE_DIRS} - ) + $ + $ + ${OpenCV_INCLUDE_DIRS} +) ament_target_dependencies(${CORE_LIB_NAME} vortex_utils - vortex_utils_ros - vortex_msgs ) target_link_libraries(${CORE_LIB_NAME} @@ -45,6 +46,7 @@ set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component") add_library(${COMPONENT_LIB_NAME} SHARED src/ros/${PROJECT_NAME}_ros.cpp + src/ros/${PROJECT_NAME}_ros_utils.cpp ) ament_target_dependencies(${COMPONENT_LIB_NAME} @@ -52,6 +54,11 @@ ament_target_dependencies(${COMPONENT_LIB_NAME} rclcpp_components vortex_utils vortex_utils_ros + vortex_msgs + cv_bridge + sensor_msgs + spdlog + fmt ) target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME}) diff --git a/line_detection_sonar/config/line_detection_sonar.yaml b/line_detection_sonar/config/line_detection_sonar.yaml index c3067f3..200d2e5 100644 --- a/line_detection_sonar/config/line_detection_sonar.yaml +++ b/line_detection_sonar/config/line_detection_sonar.yaml @@ -1,2 +1,23 @@ /**: - ros_ + ros__parameters: + topic: + image_sub_topic: /fls_publisher/display_mono + line_segments_pub_topic: line_detection/line_segments + color_overlay_pub_topic: line_detection/visualization + canny_debug_pub_topic: line_detection/canny_image + canny_overlay_pub_topic: line_detection/canny_overlay + + canny: + low_threshold: 50 + high_threshold: 150 + aperture_size: 3 + L2_gradient: false + + houghp: + rho: 1.0 + theta: 0.01745329251 # pi / 180 + threshold: 50 + min_line_length: 10.0 # pixels + max_line_gap: 5.0 # pixels + + mode: "standard" # {"standard", "visualize", "debug"} diff --git a/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp b/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp index 0bd6142..3ffbe64 100644 --- a/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp +++ b/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp @@ -3,6 +3,7 @@ #include #include +#include #include #include "typedefs.hpp" @@ -10,14 +11,15 @@ namespace vortex::line_detection { class HoughPLineDetector { public: - explicit HoughPLineDetector(const EdgeDetectionConfig& edge_config, - const LineDetectionConfig& line_config); + explicit HoughPLineDetector(const CannyConfig& edge_config, + const HoughPConfig& line_config); - Result detect(const cv::Mat& input_image, Mode mode = Mode::standard) const; + Result detect(const cv::Mat& input_image, + DetectorMode mode = DetectorMode::standard) const; private: - EdgeDetectionConfig edge_config_; - LineDetectionConfig line_config_; + CannyConfig canny_config_; + HoughPConfig houghp_config_; void detect_edges(const cv::Mat& input_image, cv::Mat& edge_image) const; @@ -26,15 +28,6 @@ class HoughPLineDetector { static std::vector to_line_segments( const std::vector& cv_lines); - - // static void make_overlay_color(const cv::Mat& input_bgr, - // const cv::Mat& edge_image, - // const std::vector& cv_lines, - // cv::Mat& out_bgr); - - // static void make_overlay_canny(const cv::Mat& edge_image, - // const std::vector& cv_lines, - // cv::Mat& out_bgr); }; } // namespace vortex::line_detection diff --git a/line_detection_sonar/include/line_detection_sonar/lib/typedefs.hpp b/line_detection_sonar/include/line_detection_sonar/lib/typedefs.hpp index 6d7e73b..3741f8c 100644 --- a/line_detection_sonar/include/line_detection_sonar/lib/typedefs.hpp +++ b/line_detection_sonar/include/line_detection_sonar/lib/typedefs.hpp @@ -3,6 +3,7 @@ #include #include +#include #include namespace vortex::line_detection { @@ -11,7 +12,7 @@ namespace vortex::line_detection { * @brief Config for the OpenCV Canny edge algorithm * used to detect edges in an 8-bit single channel image. */ -struct EdgeDetectionConfig { +struct CannyConfig { int low_threshold{50}; // Low threshold for the hysteresis procedure. int high_threshold{150}; // High threshold for the hysteresis procedure. int aperture_size{3}; // Aperture size for the Sobel operator. @@ -23,7 +24,7 @@ struct EdgeDetectionConfig { * @brief Config for the OpenCV HoughLinesP algorithm * used to detect lines segments in an 8-bit single channel image. */ -struct LineDetectionConfig { +struct HoughPConfig { double rho{1.0}; // Distance resolution of the accumulator in pixels. double theta{CV_PI / 180.0}; // Theta angle resolution // of the accumulator in radians. @@ -42,7 +43,7 @@ struct LineDetectionConfig { * - debug: return line segments + edge map + * overlay color and edge image for debugging. */ -enum class Mode { standard, visualize, debug }; +enum class DetectorMode { standard, visualize, debug }; /** * @brief Empty output payload for Mode::standard. diff --git a/line_detection_sonar/include/line_detection_sonar/lib/utils.hpp b/line_detection_sonar/include/line_detection_sonar/lib/utils.hpp index 16b5258..5f307e2 100644 --- a/line_detection_sonar/include/line_detection_sonar/lib/utils.hpp +++ b/line_detection_sonar/include/line_detection_sonar/lib/utils.hpp @@ -2,7 +2,8 @@ #define LINE_DETECTION_SONAR__LIB__UTILS_HPP_ #include -#include "utils.h" +#include +#include "line_detection_sonar/lib/utils.h" namespace vortex::line_detection { diff --git a/line_detection_sonar/include/line_detection_sonar/ros/line_detection_sonar_ros.hpp b/line_detection_sonar/include/line_detection_sonar/ros/line_detection_sonar_ros.hpp index e69de29..58ea494 100644 --- a/line_detection_sonar/include/line_detection_sonar/ros/line_detection_sonar_ros.hpp +++ b/line_detection_sonar/include/line_detection_sonar/ros/line_detection_sonar_ros.hpp @@ -0,0 +1,91 @@ +#ifndef LINE_DETECTION_SONAR__ROS__LINE_DETECTION_SONAR_ROS_HPP_ +#define LINE_DETECTION_SONAR__ROS__LINE_DETECTION_SONAR_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "line_detection_sonar/lib/houghp_line_detection.hpp" + +namespace vortex::line_detection { + +class LineDetectionSonarNode : public rclcpp::Node { + public: + explicit LineDetectionSonarNode(const rclcpp::NodeOptions& options); + + ~LineDetectionSonarNode() {} + + private: + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Publisher::SharedPtr + line_segments_pub_; + rclcpp::Publisher::SharedPtr color_overlay_pub_; + rclcpp::Publisher::SharedPtr canny_debug_pub_; + rclcpp::Publisher::SharedPtr canny_overlay_pub_; + + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); + + void declare_parameters(); + + void setup_publishers_and_subscribers(); + + void set_detector(); + + void set_mode(); + + void mode_conditional_publishing(const Result& result, + const std_msgs::msg::Header& header); + + /** + * @brief Initialize the parameter handler and a parameter event callback. + * + */ + void initialize_parameter_handler(); + /** + * @brief Callback function for parameter events. + * Checks for parameter changes that matches the nodes' namespace and + * invokes the relevant initializer functions to update member variables. + * + * @param event The parameter event. + */ + void on_parameter_event(const rcl_interfaces::msg::ParameterEvent& event); + + /** + * @brief Manages parameter events for the node. + * + * This handle is used to set up a mechanism to listen for and react to + * changes in parameters. Parameters can be used to configure the node's + * operational behavior dynamically, allowing adjustments without altering + * the code. The `param_handler_` is responsible for registering callbacks + * that are triggered on parameter changes, providing a centralized + * management system within the node for such events. + */ + std::shared_ptr param_handler_; + + /** + * @brief Handle to the registration of the parameter event callback. + * + * Represents a token or reference to the specific callback registration + * made with the parameter event handler (`param_handler_`). This handle + * allows for management of the lifecycle of the callback, such as removing + * the callback if it's no longer needed. It ensures that the node can + * respond to parameter changes with the registered callback in an efficient + * and controlled manner. + */ + rclcpp::ParameterEventCallbackHandle::SharedPtr param_cb_handle_; + + std::unique_ptr detector_{}; + + DetectorMode mode_{DetectorMode::standard}; +}; + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_SONAR__ROS__LINE_DETECTION_SONAR_ROS_HPP_ diff --git a/line_detection_sonar/launch/line_detection_sonar.launch.py b/line_detection_sonar/launch/line_detection_sonar.launch.py new file mode 100644 index 0000000..9977991 --- /dev/null +++ b/line_detection_sonar/launch/line_detection_sonar.launch.py @@ -0,0 +1,28 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('line_detection_sonar'), + 'config', + 'line_detection_sonar.yaml', + ) + + return LaunchDescription( + [ + Node( + package='line_detection_sonar', + executable='line_detection_sonar_node', + name='line_detection_sonar', + output='screen', + parameters=[ + config, + # {'use_sim_time': True} # If testing with rosbags sim_time might be preferred if bag is looped + ], + ), + ] + ) diff --git a/line_detection_sonar/package.xml b/line_detection_sonar/package.xml index 9721aaa..56a0eab 100644 --- a/line_detection_sonar/package.xml +++ b/line_detection_sonar/package.xml @@ -16,6 +16,7 @@ vortex_utils_ros vortex_msgs opencv + cv_bridge ament_cmake diff --git a/line_detection_sonar/src/lib/houghp_line_detection.cpp b/line_detection_sonar/src/lib/houghp_line_detection.cpp index 12bf99e..159f1a2 100644 --- a/line_detection_sonar/src/lib/houghp_line_detection.cpp +++ b/line_detection_sonar/src/lib/houghp_line_detection.cpp @@ -7,11 +7,12 @@ namespace vortex::line_detection { -HoughPLineDetector::HoughPLineDetector(const EdgeDetectionConfig& edge_config, - const LineDetectionConfig& line_config) - : edge_config_(edge_config), line_config_(line_config) {} +HoughPLineDetector::HoughPLineDetector(const CannyConfig& edge_config, + const HoughPConfig& line_config) + : canny_config_(edge_config), houghp_config_(line_config) {} -Result HoughPLineDetector::detect(const cv::Mat& input_image, Mode mode) const { +Result HoughPLineDetector::detect(const cv::Mat& input_image, + DetectorMode mode) const { if (input_image.empty()) { throw std::runtime_error( "HoughPLineDetector::detect: input_image is empty"); @@ -28,18 +29,18 @@ Result HoughPLineDetector::detect(const cv::Mat& input_image, Mode mode) const { r.line_segments = to_line_segments(cv_lines); switch (mode) { - case Mode::standard: { + case DetectorMode::standard: { r.output = NoOutput{}; break; } - case Mode::visualize: { + case DetectorMode::visualize: { VisualizeOutput v; vortex::line_detection::make_overlay_color( input_image, gray8, cv_lines, v.overlay_color); r.output = std::move(v); break; } - case Mode::debug: { + case DetectorMode::debug: { DebugOutput d; d.canny = edge_image; vortex::line_detection::make_overlay_canny(edge_image, cv_lines, @@ -55,20 +56,22 @@ Result HoughPLineDetector::detect(const cv::Mat& input_image, Mode mode) const { void HoughPLineDetector::detect_edges(const cv::Mat& input_image, cv::Mat& edge_image) const { - cv::Canny(input_image, edge_image, edge_config_.low_threshold, - edge_config_.high_threshold, edge_config_.aperture_size, - edge_config_.L2_gradient); + cv::Canny(input_image, edge_image, canny_config_.low_threshold, + canny_config_.high_threshold, canny_config_.aperture_size, + canny_config_.L2_gradient); } void HoughPLineDetector::detect_line_segments( const cv::Mat& edge_image, std::vector& cv_lines) const { - cv::HoughLinesP(edge_image, cv_lines, line_config_.rho, line_config_.theta, - line_config_.threshold); + cv::HoughLinesP(edge_image, cv_lines, houghp_config_.rho, + houghp_config_.theta, houghp_config_.threshold, + houghp_config_.min_line_length, + houghp_config_.max_line_gap); } -std::vector to_line_segments( - const std::vector& cv_lines) { +std::vector +HoughPLineDetector::to_line_segments(const std::vector& cv_lines) { std::vector line_segments; line_segments.reserve(cv_lines.size()); for (const auto& cv_line : cv_lines) { diff --git a/line_detection_sonar/src/ros/line_detection_sonar_ros.cpp b/line_detection_sonar/src/ros/line_detection_sonar_ros.cpp index e69de29..543839c 100644 --- a/line_detection_sonar/src/ros/line_detection_sonar_ros.cpp +++ b/line_detection_sonar/src/ros/line_detection_sonar_ros.cpp @@ -0,0 +1,135 @@ +#include "line_detection_sonar/ros/line_detection_sonar_ros.hpp" +#include +#include +#include +#include + +namespace vortex::line_detection { + +LineDetectionSonarNode::LineDetectionSonarNode( + const rclcpp::NodeOptions& options) + : rclcpp::Node("line_detection_sonar_node", options) { + declare_parameters(); + set_detector(); + set_mode(); + setup_publishers_and_subscribers(); + initialize_parameter_handler(); +} + +void LineDetectionSonarNode::declare_parameters() { + this->declare_parameter("topic.image_sub_topic"); + this->declare_parameter("topic.line_segments_pub_topic"); + this->declare_parameter("topic.color_overlay_pub_topic"); + this->declare_parameter("topic.canny_debug_pub_topic"); + this->declare_parameter("topic.canny_overlay_pub_topic"); + + this->declare_parameter("canny.low_threshold"); + this->declare_parameter("canny.high_threshold"); + this->declare_parameter("canny.aperture_size"); + this->declare_parameter("canny.L2_gradient"); + + this->declare_parameter("houghp.rho"); + this->declare_parameter("houghp.theta"); + this->declare_parameter("houghp.threshold"); + this->declare_parameter("houghp.min_line_length"); + this->declare_parameter("houghp.max_line_gap"); + + this->declare_parameter("mode"); +} + +void LineDetectionSonarNode::setup_publishers_and_subscribers() { + const std::string image_sub_topic = + this->get_parameter("topic.image_sub_topic").as_string(); + const std::string line_segments_pub_topic = + this->get_parameter("topic.line_segments_pub_topic").as_string(); + const std::string color_overlay_pub_topic = + this->get_parameter("topic.color_overlay_pub_topic").as_string(); + const std::string canny_debug_pub_topic = + this->get_parameter("topic.canny_debug_pub_topic").as_string(); + const std::string canny_overlay_pub_topic = + this->get_parameter("topic.canny_overlay_pub_topic").as_string(); + + auto qos_profile = vortex::utils::qos_profiles::sensor_data_profile(1); + + image_sub_ = this->create_subscription( + image_sub_topic, qos_profile, + std::bind(&LineDetectionSonarNode::image_callback, this, + std::placeholders::_1)); + + line_segments_pub_ = + this->create_publisher( + line_segments_pub_topic, qos_profile); + + color_overlay_pub_ = this->create_publisher( + color_overlay_pub_topic, qos_profile); + + canny_debug_pub_ = this->create_publisher( + canny_debug_pub_topic, qos_profile); + + canny_overlay_pub_ = this->create_publisher( + canny_overlay_pub_topic, qos_profile); +} + +void LineDetectionSonarNode::set_detector() { + CannyConfig edge_config; + edge_config.low_threshold = + this->get_parameter("canny.low_threshold").as_int(); + edge_config.high_threshold = + this->get_parameter("canny.high_threshold").as_int(); + edge_config.aperture_size = + this->get_parameter("canny.aperture_size").as_int(); + edge_config.L2_gradient = + this->get_parameter("canny.L2_gradient").as_bool(); + + HoughPConfig line_config; + line_config.rho = this->get_parameter("houghp.rho").as_double(); + line_config.theta = this->get_parameter("houghp.theta").as_double(); + line_config.threshold = this->get_parameter("houghp.threshold").as_int(); + line_config.min_line_length = + this->get_parameter("houghp.min_line_length").as_double(); + line_config.max_line_gap = + this->get_parameter("houghp.max_line_gap").as_double(); + + detector_ = std::make_unique(edge_config, line_config); +} + +void LineDetectionSonarNode::image_callback( + const sensor_msgs::msg::Image::SharedPtr msg) { + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); + } catch (const cv_bridge::Exception& e) { + spdlog::error("cv_bridge exception: {}", e.what()); + return; + } + + const cv::Mat& input_image = cv_ptr->image; + + Result result; + try { + result = detector_->detect(input_image, mode_); + } catch (const std::exception& e) { + spdlog::error("Line detection failed: {}", e.what()); + return; + } + + vortex_msgs::msg::LineSegment2DArray line_segments_msg; + line_segments_msg.header = msg->header; + line_segments_msg.lines.reserve(result.line_segments.size()); + + for (const auto& segment : result.line_segments) { + vortex_msgs::msg::LineSegment2D seg_msg; + seg_msg.p0.x = segment.p0.x; + seg_msg.p0.y = segment.p0.y; + seg_msg.p1.x = segment.p1.x; + seg_msg.p1.y = segment.p1.y; + line_segments_msg.lines.push_back(seg_msg); + } + line_segments_pub_->publish(line_segments_msg); + + mode_conditional_publishing(result, msg->header); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(LineDetectionSonarNode) + +} // namespace vortex::line_detection diff --git a/line_detection_sonar/src/ros/line_detection_sonar_ros_utils.cpp b/line_detection_sonar/src/ros/line_detection_sonar_ros_utils.cpp new file mode 100644 index 0000000..e08d88b --- /dev/null +++ b/line_detection_sonar/src/ros/line_detection_sonar_ros_utils.cpp @@ -0,0 +1,139 @@ +#include +#include "line_detection_sonar/ros/line_detection_sonar_ros.hpp" + +namespace vortex::line_detection { + +static bool starts_with(const std::string& s, const std::string& prefix) { + return s.size() >= prefix.size() && + s.compare(0, prefix.size(), prefix) == 0; +} + +static DetectorMode parse_mode(const std::string& s) { + if (s == "standard") + return DetectorMode::standard; + if (s == "visualize") + return DetectorMode::visualize; + if (s == "debug") + return DetectorMode::debug; + throw std::runtime_error("Invalid mode: " + s); +} + +void LineDetectionSonarNode::initialize_parameter_handler() { + param_handler_ = std::make_shared(this); + + auto parameter_event_callback = + [this](const rcl_interfaces::msg::ParameterEvent& event) -> void { + this->on_parameter_event(event); + }; + + param_cb_handle_ = + param_handler_->add_parameter_event_callback(parameter_event_callback); +} + +void LineDetectionSonarNode::on_parameter_event( + const rcl_interfaces::msg::ParameterEvent& event) { + const auto node_name = this->get_fully_qualified_name(); + if (event.node != node_name) + return; + + bool detector_params_changed = false; + bool mode_changed = false; + + for (const auto& p : event.changed_parameters) { + const auto& name = p.name; + + if (starts_with(name, "canny.") || starts_with(name, "houghp.")) { + detector_params_changed = true; + } else if (name == "mode") { + mode_changed = true; + } else { + continue; + } + + spdlog::info("Parameter changed: {}", name); + } + + if (mode_changed) { + set_mode(); + } + + if (detector_params_changed) { + set_detector(); + spdlog::info("Recreated HoughPLineDetector with updated params"); + } +} + +void LineDetectionSonarNode::mode_conditional_publishing( + const Result& result, + const std_msgs::msg::Header& header) { + if (mode_ == DetectorMode::standard) { + return; + } + + if (mode_ == DetectorMode::visualize) { + const auto* vis = std::get_if(&result.output); + if (!vis) { + spdlog::warn( + "Mode is visualize but Result::output is not VisualizeOutput"); + return; + } + + cv_bridge::CvImage overlay_msg; + overlay_msg.header = header; + overlay_msg.encoding = sensor_msgs::image_encodings::BGR8; + overlay_msg.image = vis->overlay_color; // expected CV_8UC3 + color_overlay_pub_->publish(*overlay_msg.toImageMsg()); + return; + } + + if (mode_ == DetectorMode::debug) { + const auto* dbg = std::get_if(&result.output); + if (!dbg) { + spdlog::warn("Mode is debug but Result::output is not DebugOutput"); + return; + } + + // overlay_canny: CV_8UC3 -> bgr8 + { + cv_bridge::CvImage m; + m.header = header; + m.encoding = sensor_msgs::image_encodings::BGR8; + m.image = dbg->overlay_canny; + canny_overlay_pub_->publish(*m.toImageMsg()); + } + + // canny: CV_8UC1 -> mono8 + { + cv_bridge::CvImage m; + m.header = header; + m.encoding = sensor_msgs::image_encodings::MONO8; + m.image = dbg->canny; + canny_debug_pub_->publish(*m.toImageMsg()); + } + + // overlay_color: CV_8UC3 -> bgr8 + { + cv_bridge::CvImage m; + m.header = header; + m.encoding = sensor_msgs::image_encodings::BGR8; + m.image = dbg->overlay_color; + color_overlay_pub_->publish(*m.toImageMsg()); + } + + return; + } +} + +void LineDetectionSonarNode::set_mode() { + const auto mode_str = this->get_parameter("mode").as_string(); + + try { + mode_ = parse_mode(mode_str); + spdlog::info("Set detector mode to '{}'", mode_str); + } catch (const std::exception& e) { + spdlog::warn("Invalid mode '{}': {}. Keeping previous mode.", mode_str, + e.what()); + } +} + +} // namespace vortex::line_detection From baa55a0629c346d83b3319e77ecce85e7955205c Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 11 Feb 2026 20:31:30 +0100 Subject: [PATCH 05/13] refactor rename --- .../CMakeLists.txt | 7 +++++-- line_detection_houghp/README.md | 1 + .../config/line_detection_houghp.yaml | 12 +++++------ .../lib/line_detection_houghp.hpp | 11 +++++----- .../line_detection_houghp}/lib/typedefs.hpp | 6 +++--- .../line_detection_houghp}/lib/utils.h | 6 +++--- .../line_detection_houghp}/lib/utils.hpp | 8 +++---- .../ros/line_detection_houghp_ros.hpp | 16 +++++++------- .../launch/line_detection_houghp.launch.py | 10 ++++----- .../package.xml | 5 +++-- .../src/lib/houghp_line_detection.cpp | 19 ++++++++--------- .../src/ros/line_detection_houghp_ros.cpp | 21 ++++++++++--------- .../ros/line_detection_houghp_ros_utils.cpp | 10 ++++----- line_detection_sonar/README.md | 0 14 files changed, 69 insertions(+), 63 deletions(-) rename {line_detection_sonar => line_detection_houghp}/CMakeLists.txt (92%) create mode 100644 line_detection_houghp/README.md rename line_detection_sonar/config/line_detection_sonar.yaml => line_detection_houghp/config/line_detection_houghp.yaml (69%) rename line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp => line_detection_houghp/include/line_detection_houghp/lib/line_detection_houghp.hpp (71%) rename {line_detection_sonar/include/line_detection_sonar => line_detection_houghp/include/line_detection_houghp}/lib/typedefs.hpp (95%) rename {line_detection_sonar/include/line_detection_sonar => line_detection_houghp/include/line_detection_houghp}/lib/utils.h (95%) rename {line_detection_sonar/include/line_detection_sonar => line_detection_houghp/include/line_detection_houghp}/lib/utils.hpp (95%) rename line_detection_sonar/include/line_detection_sonar/ros/line_detection_sonar_ros.hpp => line_detection_houghp/include/line_detection_houghp/ros/line_detection_houghp_ros.hpp (86%) rename line_detection_sonar/launch/line_detection_sonar.launch.py => line_detection_houghp/launch/line_detection_houghp.launch.py (68%) rename {line_detection_sonar => line_detection_houghp}/package.xml (79%) rename {line_detection_sonar => line_detection_houghp}/src/lib/houghp_line_detection.cpp (82%) rename line_detection_sonar/src/ros/line_detection_sonar_ros.cpp => line_detection_houghp/src/ros/line_detection_houghp_ros.cpp (87%) rename line_detection_sonar/src/ros/line_detection_sonar_ros_utils.cpp => line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp (93%) delete mode 100644 line_detection_sonar/README.md diff --git a/line_detection_sonar/CMakeLists.txt b/line_detection_houghp/CMakeLists.txt similarity index 92% rename from line_detection_sonar/CMakeLists.txt rename to line_detection_houghp/CMakeLists.txt index b5ecb95..bdbc490 100644 --- a/line_detection_sonar/CMakeLists.txt +++ b/line_detection_houghp/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(line_detection_sonar) +project(line_detection_houghp) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 20) @@ -12,11 +12,13 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(OpenCV 4.5.4 REQUIRED) find_package(vortex_utils REQUIRED) find_package(vortex_utils_ros REQUIRED) find_package(vortex_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(cv_bridge REQUIRED) +find_package(image_filtering REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) @@ -37,6 +39,7 @@ target_include_directories(${CORE_LIB_NAME} ament_target_dependencies(${CORE_LIB_NAME} vortex_utils + image_filtering ) target_link_libraries(${CORE_LIB_NAME} @@ -65,7 +68,7 @@ target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME}) rclcpp_components_register_node( ${COMPONENT_LIB_NAME} - PLUGIN "LineDetectionSonarNode" + PLUGIN "LineDetectionHoughPNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/line_detection_houghp/README.md b/line_detection_houghp/README.md new file mode 100644 index 0000000..adea80c --- /dev/null +++ b/line_detection_houghp/README.md @@ -0,0 +1 @@ +Readme \ No newline at end of file diff --git a/line_detection_sonar/config/line_detection_sonar.yaml b/line_detection_houghp/config/line_detection_houghp.yaml similarity index 69% rename from line_detection_sonar/config/line_detection_sonar.yaml rename to line_detection_houghp/config/line_detection_houghp.yaml index 200d2e5..5e6273d 100644 --- a/line_detection_sonar/config/line_detection_sonar.yaml +++ b/line_detection_houghp/config/line_detection_houghp.yaml @@ -8,16 +8,16 @@ canny_overlay_pub_topic: line_detection/canny_overlay canny: - low_threshold: 50 - high_threshold: 150 + low_threshold: 100 + high_threshold: 200 aperture_size: 3 L2_gradient: false houghp: rho: 1.0 theta: 0.01745329251 # pi / 180 - threshold: 50 - min_line_length: 10.0 # pixels - max_line_gap: 5.0 # pixels + threshold: 100 + min_line_length: 20.0 # pixels + max_line_gap: 10.0 # pixels - mode: "standard" # {"standard", "visualize", "debug"} + mode: "debug" # {"standard", "visualize", "debug"} diff --git a/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp b/line_detection_houghp/include/line_detection_houghp/lib/line_detection_houghp.hpp similarity index 71% rename from line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp rename to line_detection_houghp/include/line_detection_houghp/lib/line_detection_houghp.hpp index 3ffbe64..9835f6a 100644 --- a/line_detection_sonar/include/line_detection_sonar/lib/houghp_line_detection.hpp +++ b/line_detection_houghp/include/line_detection_houghp/lib/line_detection_houghp.hpp @@ -1,17 +1,18 @@ -#ifndef LINE_DETECTION_SONAR__LIB__HOUGHP_LINE_DETECTION_HPP_ -#define LINE_DETECTION_SONAR__LIB__HOUGHP_LINE_DETECTION_HPP_ +#ifndef LINE_DETECTION_HOUGHP__LIB__HOUGHP_LINE_DETECTION_HPP_ +#define LINE_DETECTION_HOUGHP__LIB__HOUGHP_LINE_DETECTION_HPP_ #include #include #include #include +#include #include "typedefs.hpp" namespace vortex::line_detection { -class HoughPLineDetector { +class LineDetectorHoughP { public: - explicit HoughPLineDetector(const CannyConfig& edge_config, + explicit LineDetectorHoughP(const CannyConfig& edge_config, const HoughPConfig& line_config); Result detect(const cv::Mat& input_image, @@ -32,4 +33,4 @@ class HoughPLineDetector { } // namespace vortex::line_detection -#endif // LINE_DETECTION_SONAR__LIB__HOUGHP_LINE_DETECTION_HPP_ +#endif // LINE_DETECTION_HOUGHP__LIB__HOUGHP_LINE_DETECTION_HPP_ diff --git a/line_detection_sonar/include/line_detection_sonar/lib/typedefs.hpp b/line_detection_houghp/include/line_detection_houghp/lib/typedefs.hpp similarity index 95% rename from line_detection_sonar/include/line_detection_sonar/lib/typedefs.hpp rename to line_detection_houghp/include/line_detection_houghp/lib/typedefs.hpp index 3741f8c..0af584a 100644 --- a/line_detection_sonar/include/line_detection_sonar/lib/typedefs.hpp +++ b/line_detection_houghp/include/line_detection_houghp/lib/typedefs.hpp @@ -1,5 +1,5 @@ -#ifndef LINE_DETECTION_SONAR__LIB__TYPEDEFS_HPP_ -#define LINE_DETECTION_SONAR__LIB__TYPEDEFS_HPP_ +#ifndef LINE_DETECTION_HOUGHP__LIB__TYPEDEFS_HPP_ +#define LINE_DETECTION_HOUGHP__LIB__TYPEDEFS_HPP_ #include #include @@ -104,4 +104,4 @@ struct Result { } // namespace vortex::line_detection -#endif // LINE_DETECTION_SONAR__LIB__TYPEDEFS_HPP_ +#endif // LINE_DETECTION_HOUGHP__LIB__TYPEDEFS_HPP_ diff --git a/line_detection_sonar/include/line_detection_sonar/lib/utils.h b/line_detection_houghp/include/line_detection_houghp/lib/utils.h similarity index 95% rename from line_detection_sonar/include/line_detection_sonar/lib/utils.h rename to line_detection_houghp/include/line_detection_houghp/lib/utils.h index 8f2bdad..414800a 100644 --- a/line_detection_sonar/include/line_detection_sonar/lib/utils.h +++ b/line_detection_houghp/include/line_detection_houghp/lib/utils.h @@ -1,5 +1,5 @@ -#ifndef LINE_DETECTION_SONAR__LIB__UTILS_H_ -#define LINE_DETECTION_SONAR__LIB__UTILS_H_ +#ifndef LINE_DETECTION_HOUGHP__LIB__UTILS_H_ +#define LINE_DETECTION_HOUGHP__LIB__UTILS_H_ #include @@ -78,4 +78,4 @@ inline cv::Mat to_bgr8(const cv::Mat& input, const cv::Mat& gray8 /*CV_8UC1*/); } // namespace vortex::line_detection -#endif // LINE_DETECTION_SONAR__LIB__UTILS_H_ +#endif // LINE_DETECTION_HOUGHP__LIB__UTILS_H_ diff --git a/line_detection_sonar/include/line_detection_sonar/lib/utils.hpp b/line_detection_houghp/include/line_detection_houghp/lib/utils.hpp similarity index 95% rename from line_detection_sonar/include/line_detection_sonar/lib/utils.hpp rename to line_detection_houghp/include/line_detection_houghp/lib/utils.hpp index 5f307e2..d35e0dc 100644 --- a/line_detection_sonar/include/line_detection_sonar/lib/utils.hpp +++ b/line_detection_houghp/include/line_detection_houghp/lib/utils.hpp @@ -1,9 +1,9 @@ -#ifndef LINE_DETECTION_SONAR__LIB__UTILS_HPP_ -#define LINE_DETECTION_SONAR__LIB__UTILS_HPP_ +#ifndef LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ +#define LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ #include #include -#include "line_detection_sonar/lib/utils.h" +#include "line_detection_houghp/lib/utils.h" namespace vortex::line_detection { @@ -143,4 +143,4 @@ inline void make_overlay_canny(const cv::Mat& edges, } // namespace vortex::line_detection -#endif // LINE_DETECTION_SONAR__LIB__UTILS_HPP_ +#endif // LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ diff --git a/line_detection_sonar/include/line_detection_sonar/ros/line_detection_sonar_ros.hpp b/line_detection_houghp/include/line_detection_houghp/ros/line_detection_houghp_ros.hpp similarity index 86% rename from line_detection_sonar/include/line_detection_sonar/ros/line_detection_sonar_ros.hpp rename to line_detection_houghp/include/line_detection_houghp/ros/line_detection_houghp_ros.hpp index 58ea494..7de0e4b 100644 --- a/line_detection_sonar/include/line_detection_sonar/ros/line_detection_sonar_ros.hpp +++ b/line_detection_houghp/include/line_detection_houghp/ros/line_detection_houghp_ros.hpp @@ -1,5 +1,5 @@ -#ifndef LINE_DETECTION_SONAR__ROS__LINE_DETECTION_SONAR_ROS_HPP_ -#define LINE_DETECTION_SONAR__ROS__LINE_DETECTION_SONAR_ROS_HPP_ +#ifndef LINE_DETECTION_HOUGHP__ROS__LINE_DETECTION_HOUGHP_ROS_HPP_ +#define LINE_DETECTION_HOUGHP__ROS__LINE_DETECTION_HOUGHP_ROS_HPP_ #include #include @@ -12,15 +12,15 @@ #include #include #include -#include "line_detection_sonar/lib/houghp_line_detection.hpp" +#include "line_detection_houghp/lib/line_detection_houghp.hpp" namespace vortex::line_detection { -class LineDetectionSonarNode : public rclcpp::Node { +class LineDetectionHoughPNode : public rclcpp::Node { public: - explicit LineDetectionSonarNode(const rclcpp::NodeOptions& options); + explicit LineDetectionHoughPNode(const rclcpp::NodeOptions& options); - ~LineDetectionSonarNode() {} + ~LineDetectionHoughPNode() {} private: rclcpp::Subscription::SharedPtr image_sub_; @@ -81,11 +81,11 @@ class LineDetectionSonarNode : public rclcpp::Node { */ rclcpp::ParameterEventCallbackHandle::SharedPtr param_cb_handle_; - std::unique_ptr detector_{}; + std::unique_ptr detector_{}; DetectorMode mode_{DetectorMode::standard}; }; } // namespace vortex::line_detection -#endif // LINE_DETECTION_SONAR__ROS__LINE_DETECTION_SONAR_ROS_HPP_ +#endif // LINE_DETECTION_HOUGHP__ROS__LINE_DETECTION_HOUGHP_ROS_HPP_ diff --git a/line_detection_sonar/launch/line_detection_sonar.launch.py b/line_detection_houghp/launch/line_detection_houghp.launch.py similarity index 68% rename from line_detection_sonar/launch/line_detection_sonar.launch.py rename to line_detection_houghp/launch/line_detection_houghp.launch.py index 9977991..bb8b412 100644 --- a/line_detection_sonar/launch/line_detection_sonar.launch.py +++ b/line_detection_houghp/launch/line_detection_houghp.launch.py @@ -7,17 +7,17 @@ def generate_launch_description(): config = os.path.join( - get_package_share_directory('line_detection_sonar'), + get_package_share_directory('line_detection_houghp'), 'config', - 'line_detection_sonar.yaml', + 'line_detection_houghp.yaml', ) return LaunchDescription( [ Node( - package='line_detection_sonar', - executable='line_detection_sonar_node', - name='line_detection_sonar', + package='line_detection_houghp', + executable='line_detection_houghp_node', + name='line_detection_houghp', output='screen', parameters=[ config, diff --git a/line_detection_sonar/package.xml b/line_detection_houghp/package.xml similarity index 79% rename from line_detection_sonar/package.xml rename to line_detection_houghp/package.xml index 56a0eab..03446d7 100644 --- a/line_detection_sonar/package.xml +++ b/line_detection_houghp/package.xml @@ -1,9 +1,9 @@ - line_detection_sonar + line_detection_houghp 0.0.0 - Package for detecting lines on images produces by an FLS sonar. + Package for detecting lines using the HoughP OpenCV function. jorgen MIT @@ -15,6 +15,7 @@ vortex_utils vortex_utils_ros vortex_msgs + image_filtering opencv cv_bridge diff --git a/line_detection_sonar/src/lib/houghp_line_detection.cpp b/line_detection_houghp/src/lib/houghp_line_detection.cpp similarity index 82% rename from line_detection_sonar/src/lib/houghp_line_detection.cpp rename to line_detection_houghp/src/lib/houghp_line_detection.cpp index 159f1a2..06211cf 100644 --- a/line_detection_sonar/src/lib/houghp_line_detection.cpp +++ b/line_detection_houghp/src/lib/houghp_line_detection.cpp @@ -1,21 +1,20 @@ -#include "line_detection_sonar/lib/houghp_line_detection.hpp" +#include "line_detection_houghp/lib/line_detection_houghp.hpp" #include -#include #include -#include "line_detection_sonar/lib/typedefs.hpp" -#include "line_detection_sonar/lib/utils.hpp" +#include "line_detection_houghp/lib/typedefs.hpp" +#include "line_detection_houghp/lib/utils.hpp" namespace vortex::line_detection { -HoughPLineDetector::HoughPLineDetector(const CannyConfig& edge_config, +LineDetectorHoughP::LineDetectorHoughP(const CannyConfig& edge_config, const HoughPConfig& line_config) : canny_config_(edge_config), houghp_config_(line_config) {} -Result HoughPLineDetector::detect(const cv::Mat& input_image, +Result LineDetectorHoughP::detect(const cv::Mat& input_image, DetectorMode mode) const { if (input_image.empty()) { throw std::runtime_error( - "HoughPLineDetector::detect: input_image is empty"); + "LineDetectorHoughP::detect: input_image is empty"); } cv::Mat gray8 = vortex::line_detection::to_gray8(input_image); cv::Mat edge_image; @@ -54,14 +53,14 @@ Result HoughPLineDetector::detect(const cv::Mat& input_image, return r; } -void HoughPLineDetector::detect_edges(const cv::Mat& input_image, +void LineDetectorHoughP::detect_edges(const cv::Mat& input_image, cv::Mat& edge_image) const { cv::Canny(input_image, edge_image, canny_config_.low_threshold, canny_config_.high_threshold, canny_config_.aperture_size, canny_config_.L2_gradient); } -void HoughPLineDetector::detect_line_segments( +void LineDetectorHoughP::detect_line_segments( const cv::Mat& edge_image, std::vector& cv_lines) const { cv::HoughLinesP(edge_image, cv_lines, houghp_config_.rho, @@ -71,7 +70,7 @@ void HoughPLineDetector::detect_line_segments( } std::vector -HoughPLineDetector::to_line_segments(const std::vector& cv_lines) { +LineDetectorHoughP::to_line_segments(const std::vector& cv_lines) { std::vector line_segments; line_segments.reserve(cv_lines.size()); for (const auto& cv_line : cv_lines) { diff --git a/line_detection_sonar/src/ros/line_detection_sonar_ros.cpp b/line_detection_houghp/src/ros/line_detection_houghp_ros.cpp similarity index 87% rename from line_detection_sonar/src/ros/line_detection_sonar_ros.cpp rename to line_detection_houghp/src/ros/line_detection_houghp_ros.cpp index 543839c..ed967fe 100644 --- a/line_detection_sonar/src/ros/line_detection_sonar_ros.cpp +++ b/line_detection_houghp/src/ros/line_detection_houghp_ros.cpp @@ -1,14 +1,15 @@ -#include "line_detection_sonar/ros/line_detection_sonar_ros.hpp" +#include "line_detection_houghp/ros/line_detection_houghp_ros.hpp" #include #include #include #include +#include "line_detection_houghp/lib/line_detection_houghp.hpp" namespace vortex::line_detection { -LineDetectionSonarNode::LineDetectionSonarNode( +LineDetectionHoughPNode::LineDetectionHoughPNode( const rclcpp::NodeOptions& options) - : rclcpp::Node("line_detection_sonar_node", options) { + : rclcpp::Node("line_detection_houghp_node", options) { declare_parameters(); set_detector(); set_mode(); @@ -16,7 +17,7 @@ LineDetectionSonarNode::LineDetectionSonarNode( initialize_parameter_handler(); } -void LineDetectionSonarNode::declare_parameters() { +void LineDetectionHoughPNode::declare_parameters() { this->declare_parameter("topic.image_sub_topic"); this->declare_parameter("topic.line_segments_pub_topic"); this->declare_parameter("topic.color_overlay_pub_topic"); @@ -37,7 +38,7 @@ void LineDetectionSonarNode::declare_parameters() { this->declare_parameter("mode"); } -void LineDetectionSonarNode::setup_publishers_and_subscribers() { +void LineDetectionHoughPNode::setup_publishers_and_subscribers() { const std::string image_sub_topic = this->get_parameter("topic.image_sub_topic").as_string(); const std::string line_segments_pub_topic = @@ -53,7 +54,7 @@ void LineDetectionSonarNode::setup_publishers_and_subscribers() { image_sub_ = this->create_subscription( image_sub_topic, qos_profile, - std::bind(&LineDetectionSonarNode::image_callback, this, + std::bind(&LineDetectionHoughPNode::image_callback, this, std::placeholders::_1)); line_segments_pub_ = @@ -70,7 +71,7 @@ void LineDetectionSonarNode::setup_publishers_and_subscribers() { canny_overlay_pub_topic, qos_profile); } -void LineDetectionSonarNode::set_detector() { +void LineDetectionHoughPNode::set_detector() { CannyConfig edge_config; edge_config.low_threshold = this->get_parameter("canny.low_threshold").as_int(); @@ -90,10 +91,10 @@ void LineDetectionSonarNode::set_detector() { line_config.max_line_gap = this->get_parameter("houghp.max_line_gap").as_double(); - detector_ = std::make_unique(edge_config, line_config); + detector_ = std::make_unique(edge_config, line_config); } -void LineDetectionSonarNode::image_callback( +void LineDetectionHoughPNode::image_callback( const sensor_msgs::msg::Image::SharedPtr msg) { cv_bridge::CvImagePtr cv_ptr; try { @@ -130,6 +131,6 @@ void LineDetectionSonarNode::image_callback( mode_conditional_publishing(result, msg->header); } -RCLCPP_COMPONENTS_REGISTER_NODE(LineDetectionSonarNode) +RCLCPP_COMPONENTS_REGISTER_NODE(LineDetectionHoughPNode) } // namespace vortex::line_detection diff --git a/line_detection_sonar/src/ros/line_detection_sonar_ros_utils.cpp b/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp similarity index 93% rename from line_detection_sonar/src/ros/line_detection_sonar_ros_utils.cpp rename to line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp index e08d88b..065ec56 100644 --- a/line_detection_sonar/src/ros/line_detection_sonar_ros_utils.cpp +++ b/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp @@ -1,5 +1,5 @@ #include -#include "line_detection_sonar/ros/line_detection_sonar_ros.hpp" +#include "line_detection_houghp/ros/line_detection_houghp_ros.hpp" namespace vortex::line_detection { @@ -18,7 +18,7 @@ static DetectorMode parse_mode(const std::string& s) { throw std::runtime_error("Invalid mode: " + s); } -void LineDetectionSonarNode::initialize_parameter_handler() { +void LineDetectionHoughPNode::initialize_parameter_handler() { param_handler_ = std::make_shared(this); auto parameter_event_callback = @@ -30,7 +30,7 @@ void LineDetectionSonarNode::initialize_parameter_handler() { param_handler_->add_parameter_event_callback(parameter_event_callback); } -void LineDetectionSonarNode::on_parameter_event( +void LineDetectionHoughPNode::on_parameter_event( const rcl_interfaces::msg::ParameterEvent& event) { const auto node_name = this->get_fully_qualified_name(); if (event.node != node_name) @@ -63,7 +63,7 @@ void LineDetectionSonarNode::on_parameter_event( } } -void LineDetectionSonarNode::mode_conditional_publishing( +void LineDetectionHoughPNode::mode_conditional_publishing( const Result& result, const std_msgs::msg::Header& header) { if (mode_ == DetectorMode::standard) { @@ -124,7 +124,7 @@ void LineDetectionSonarNode::mode_conditional_publishing( } } -void LineDetectionSonarNode::set_mode() { +void LineDetectionHoughPNode::set_mode() { const auto mode_str = this->get_parameter("mode").as_string(); try { diff --git a/line_detection_sonar/README.md b/line_detection_sonar/README.md deleted file mode 100644 index e69de29..0000000 From 4c76b8ac27c13788b62732d97c7386a2ca9941a9 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 11 Feb 2026 21:26:51 +0100 Subject: [PATCH 06/13] LineDetectionHoughP readme --- line_detection_houghp/README.md | 109 +++++++++++++++++++++++++++++++- 1 file changed, 108 insertions(+), 1 deletion(-) diff --git a/line_detection_houghp/README.md b/line_detection_houghp/README.md index adea80c..1978a65 100644 --- a/line_detection_houghp/README.md +++ b/line_detection_houghp/README.md @@ -1 +1,108 @@ -Readme \ No newline at end of file +# Line Detection HoughP + +## Overview + +The `line_detection_houghp` package provides line detection functionality using the Probabilistic Hough Transform (HoughP) for images. It is designed to detect linear features such as pipelines, walls, and other structural elements in underwater environments. + +## Architecture + +The package follows the standard Vortex separation of concerns: + +``` +line_detection_houghp/ +├── include/ +│ └── line_detection_houghp/ +│ ├── lib/ # Non-ROS library headers +│ │ └── houghp_line_detection.hpp +│ └── ros/ # ROS node headers +│ └── line_detection_ros.hpp +├── src/ +│ ├── lib/ # Non-ROS library implementations +│ │ └── houghp_line_detection.cpp +│ └── ros/ # ROS node implementations +│ └── line_detection_ros.cpp +├── launch/ +│ └── line_detection.launch.py +├── config/ +│ └── line_detection_params.yaml +├── test/ +│ └── test_houghp_line_detection.cpp +├── CMakeLists.txt +├── package.xml +└── README.md +``` + +**lib/** contains pure C++/OpenCV logic with no ROS dependencies. This makes it testable and reusable outside of ROS. + +**ros/** contains the ROS 2 node that wraps the library, handling subscriptions, publishers, parameters, and message conversions. + +## Topics + +### Subscriptions + +| Topic | Type | Description | +|-------|------|-------------| +| Input image | `sensor_msgs/msg/Image` | Input image for line detection (8-bit, single or multi-channel) | + +### Publications + +| Topic | Type | Modes | Description | +|-------|------|-------|-------------| +| Detected lines | `vortex_msgs/msg/LineSegment2DArray` | `standard` `visualize` `debug` | Detected line segments (Always published). | +| Color overlay | `sensor_msgs/msg/Image` (BGR, 8UC3) | `visualize` `debug` | Input color image with detected line segments drawn on top | +| Canny edge map | `sensor_msgs/msg/Image` (mono, 8UC1) | `debug` | Raw Canny edge map | +| Canny overlay | `sensor_msgs/msg/Image` (BGR, 8UC3) | `debug` | Edge map converted to BGR with detected line segments drawn on top | + + +## Parameters + +### Canny Edge Detection + +The Canny edge detector is used as a pre-processing step to extract edges before the Hough transform. It uses hysteresis thresholding with two thresholds to identify strong and weak edges. + +| Parameter | Type | Description | +|-----------|------|-------------| +| `canny_low_threshold` | `int` | Low threshold for the hysteresis procedure. Edges with gradient magnitude below this are discarded. | +| `canny_high_threshold` | `int` | High threshold for the hysteresis procedure. Edges with gradient magnitude above this are always accepted. Edges between low and high are accepted only if connected to a strong edge. | +| `canny_aperture_size` | `int` | Aperture size for the Sobel operator used internally by Canny. Must be odd (3, 5, or 7). Larger values detect smoother edges. | +| `canny_L2_gradient` | `bool` | If `true`, uses the more accurate L2 norm (√(Gx² + Gy²)) for gradient magnitude instead of the default L1 norm (\|Gx\| + \|Gy\|). | + +### Probabilistic Hough Transform + +| Parameter | Type | Description | +|-----------|------|-------------| +| `hough_rho` | `double` | Distance resolution of the accumulator in pixels | +| `hough_theta` | `double` | Angle resolution of the accumulator in radians | +| `hough_threshold` | `int` | Accumulator threshold — minimum number of votes to accept a line | +| `min_line_length` | `double` | Minimum line length to accept (pixels). Lines shorter than this are rejected. | +| `max_line_gap` | `double` | Maximum allowed gap between points on the same line to link them into a single segment (pixels) | + + + +> **Note:** Adjust parameters in the config YAML or via `ros2 param set` at runtime. + +## Build + +```bash +# Build this package and its dependencies +colcon build --packages-up-to line_detection_houghp --symlink-install +source install/setup.bash +``` + +## Run + +```bash +# Launch the node with default parameters +ros2 launch line_detection_houghp line_detection.launch.py +``` + + +## Theory + +The [Probabilistic Hough Transform](https://docs.opencv.org/4.x/dd/d1a/group__imgproc__feature.html#ga8618180a5948286384e3b7ca02f6feeb) (`cv::HoughLinesP`) is a variant of the Standard Hough Transform that returns line segments rather than infinite lines. It works by: + +1. Randomly sampling edge points from a binary image. +2. Accumulating votes in (ρ, θ) parameter space. +3. Extracting line segments that exceed the vote threshold and meet minimum length / maximum gap constraints. + +Detected line segments are converted to polar (Hesse normal) form using `vortex::utils::types::LineSegment2D::polar_parametrization()` for downstream consumers. \ No newline at end of file From 5d9db8349cbd7de5e26cd8dba115517ee8e1bb27 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 11 Feb 2026 21:47:51 +0100 Subject: [PATCH 07/13] dependencies.repos file --- dependencies.repos | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 dependencies.repos diff --git a/dependencies.repos b/dependencies.repos new file mode 100644 index 0000000..116cec4 --- /dev/null +++ b/dependencies.repos @@ -0,0 +1,7 @@ +repositories: + vortex-msgs: + type: git + url: https://github.com/vortexntnu/vortex-msgs.git + vortex-utils: + type: git + url: https://github.com/vortexntnu/vortex-utils.git From 192901dc9e8f89986e29ff9fae91ea1dd047da03 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 12 Feb 2026 13:10:31 +0100 Subject: [PATCH 08/13] removed unsued image_filtering dependency --- line_detection_houghp/CMakeLists.txt | 2 -- line_detection_houghp/README.md | 2 +- .../line_detection_houghp/lib/line_detection_houghp.hpp | 7 +++---- line_detection_houghp/package.xml | 1 - 4 files changed, 4 insertions(+), 8 deletions(-) diff --git a/line_detection_houghp/CMakeLists.txt b/line_detection_houghp/CMakeLists.txt index bdbc490..b81dd78 100644 --- a/line_detection_houghp/CMakeLists.txt +++ b/line_detection_houghp/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(vortex_utils_ros REQUIRED) find_package(vortex_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(cv_bridge REQUIRED) -find_package(image_filtering REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) @@ -39,7 +38,6 @@ target_include_directories(${CORE_LIB_NAME} ament_target_dependencies(${CORE_LIB_NAME} vortex_utils - image_filtering ) target_link_libraries(${CORE_LIB_NAME} diff --git a/line_detection_houghp/README.md b/line_detection_houghp/README.md index 1978a65..b210a5a 100644 --- a/line_detection_houghp/README.md +++ b/line_detection_houghp/README.md @@ -105,4 +105,4 @@ The [Probabilistic Hough Transform](https://docs.opencv.org/4.x/dd/d1a/group__im 2. Accumulating votes in (ρ, θ) parameter space. 3. Extracting line segments that exceed the vote threshold and meet minimum length / maximum gap constraints. -Detected line segments are converted to polar (Hesse normal) form using `vortex::utils::types::LineSegment2D::polar_parametrization()` for downstream consumers. \ No newline at end of file +Detected line segments are converted to polar (Hesse normal) form using `vortex::utils::types::LineSegment2D::polar_parametrization()` for downstream consumers. diff --git a/line_detection_houghp/include/line_detection_houghp/lib/line_detection_houghp.hpp b/line_detection_houghp/include/line_detection_houghp/lib/line_detection_houghp.hpp index 9835f6a..53c125b 100644 --- a/line_detection_houghp/include/line_detection_houghp/lib/line_detection_houghp.hpp +++ b/line_detection_houghp/include/line_detection_houghp/lib/line_detection_houghp.hpp @@ -1,11 +1,10 @@ -#ifndef LINE_DETECTION_HOUGHP__LIB__HOUGHP_LINE_DETECTION_HPP_ -#define LINE_DETECTION_HOUGHP__LIB__HOUGHP_LINE_DETECTION_HPP_ +#ifndef LINE_DETECTION_HOUGHP__LIB__LINE_DETECTION_HOUGHP_HPP_ +#define LINE_DETECTION_HOUGHP__LIB__LINE_DETECTION_HOUGHP_HPP_ #include #include #include #include -#include #include "typedefs.hpp" namespace vortex::line_detection { @@ -33,4 +32,4 @@ class LineDetectorHoughP { } // namespace vortex::line_detection -#endif // LINE_DETECTION_HOUGHP__LIB__HOUGHP_LINE_DETECTION_HPP_ +#endif // LINE_DETECTION_HOUGHP__LIB__LINE_DETECTION_HOUGHP_HPP_ diff --git a/line_detection_houghp/package.xml b/line_detection_houghp/package.xml index 03446d7..d09b3e0 100644 --- a/line_detection_houghp/package.xml +++ b/line_detection_houghp/package.xml @@ -15,7 +15,6 @@ vortex_utils vortex_utils_ros vortex_msgs - image_filtering opencv cv_bridge From ba6de2f13a4d95409b3182480a39d539df20981f Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 12 Feb 2026 13:11:44 +0100 Subject: [PATCH 09/13] refactord line_detection_houghp utils --- .../include/line_detection_houghp/lib/utils.h | 81 ------ .../line_detection_houghp/lib/utils.hpp | 234 ++++++++---------- .../src/lib/houghp_line_detection.cpp | 14 +- line_detection_houghp/src/lib/utils.cpp | 146 +++++++++++ 4 files changed, 253 insertions(+), 222 deletions(-) delete mode 100644 line_detection_houghp/include/line_detection_houghp/lib/utils.h create mode 100644 line_detection_houghp/src/lib/utils.cpp diff --git a/line_detection_houghp/include/line_detection_houghp/lib/utils.h b/line_detection_houghp/include/line_detection_houghp/lib/utils.h deleted file mode 100644 index 414800a..0000000 --- a/line_detection_houghp/include/line_detection_houghp/lib/utils.h +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef LINE_DETECTION_HOUGHP__LIB__UTILS_H_ -#define LINE_DETECTION_HOUGHP__LIB__UTILS_H_ - -#include - -namespace vortex::line_detection { - -/** - * @brief Convert an image to 8-bit, single-channel grayscale (CV_8UC1). - * - * This helper is intended to produce a suitable input for edge detection (e.g., - * Canny) from a variety of common OpenCV image formats. - * - * Conversion rules: - * - If @p input is already CV_8UC1, the function returns a **shallow copy** (no - * pixel data copied). - * - If @p input has 3 channels, it is converted using @c cv::COLOR_BGR2GRAY - * (assumes BGR ordering). - * - If @p input has 4 channels, it is converted using @c cv::COLOR_BGRA2GRAY - * (assumes BGRA ordering). - * - * - If the resulting grayscale image is 16-bit unsigned (CV_16U), it is scaled - * to 8-bit using - * @c convertTo with @c alpha = 1/256 (i.e., approximately a right shift by 8 - * bits). - * - For other depths (signed integers or floating point), the image is min-max - * normalized to [0,255] using @c cv::normalize(..., cv::NORM_MINMAX) and - * written as CV_8U. - * - * @param input Input image. Supported channel counts: 1, 3 (BGR), or 4 (BGRA). - * Supported depths: any OpenCV depth; non-8U depths are converted - * as described above. - * - * @return A grayscale image of type CV_8UC1. May share data with @p input if no - * conversion is needed. - * - * @throws std::runtime_error If @p input is empty or has an unsupported channel - * count. - */ -inline cv::Mat to_gray8(const cv::Mat& input); - -/** - * @brief Convert an image to 8-bit, 3-channel BGR (CV_8UC3). - * - * Conversion rules: - * - If @p input is already CV_8UC3, the function returns a **shallow copy** (no - * pixel data copied). - * - If @p input is single-channel, it is expanded to BGR using @c - * cv::COLOR_GRAY2BGR. In this case, the already-computed @p gray8 (CV_8UC1) - * must be provided to avoid recomputing grayscale conversion / scaling. - * - If @p input has 4 channels, it is converted using @c cv::COLOR_BGRA2BGR - * (assumes BGRA ordering). - * - If the resulting BGR image is 16-bit unsigned (CV_16U), it is scaled to - * 8-bit using - * @c convertTo with @c alpha = 1/256 (i.e., approximately a right shift by 8 - * bits). - * - For other depths (signed integers or floating point), the image is min-max - * normalized to [0,255] using @c cv::normalize(..., cv::NORM_MINMAX) and - * written as CV_8U. - * - * @note The normalization step computes min/max per call. This is usually fine - * for visualization, but it may produce different brightness/contrast per frame - * when viewing a stream. - * - * @param input Input image. Supported channel counts: 1, 3 (assumed BGR), or 4 - * (assumed BGRA). - * @param gray8 A grayscale image of type CV_8UC1 corresponding to @p input (or - * derived from it). Required when @p input is single-channel; ignored for 3- or - * 4-channel inputs. - * - * @return A BGR image of type CV_8UC3. May share data with @p input if no - * conversion is needed. - * - * @throws std::runtime_error If @p input is empty, has an unsupported channel - * count, or if @p input is single-channel and @p gray8 is empty or not CV_8UC1. - */ -inline cv::Mat to_bgr8(const cv::Mat& input, const cv::Mat& gray8 /*CV_8UC1*/); - -} // namespace vortex::line_detection - -#endif // LINE_DETECTION_HOUGHP__LIB__UTILS_H_ diff --git a/line_detection_houghp/include/line_detection_houghp/lib/utils.hpp b/line_detection_houghp/include/line_detection_houghp/lib/utils.hpp index d35e0dc..76b3bd0 100644 --- a/line_detection_houghp/include/line_detection_houghp/lib/utils.hpp +++ b/line_detection_houghp/include/line_detection_houghp/lib/utils.hpp @@ -3,143 +3,109 @@ #include #include -#include "line_detection_houghp/lib/utils.h" namespace vortex::line_detection { -inline cv::Mat to_gray8(const cv::Mat& input) { - if (input.empty()) { - throw std::runtime_error("to_gray8: input is empty"); - } - - if (input.type() == CV_8UC1) { - return input; - } - - cv::Mat gray; - const int ch = input.channels(); - - // Convert to grayscale - if (ch == 1) { - gray = input; - } else if (ch == 3) { - cv::cvtColor(input, gray, cv::COLOR_BGR2GRAY); // assumes BGR - } else if (ch == 4) { - cv::cvtColor(input, gray, cv::COLOR_BGRA2GRAY); // assumes BGRA - } else { - throw std::runtime_error("to_gray8: unsupported channel count"); - } - - // Now ensure 8-bit depth. - if (gray.depth() == CV_8U) { - return gray; - } - - if (gray.depth() == CV_16U) { - cv::Mat out; - gray.convertTo(out, CV_8U, 1.0 / 256.0); // quick/common 16U->8U - return out; - } - - // Generic fallback for signed ints / floats etc. - cv::Mat out; - cv::normalize(gray, out, 0, 255, cv::NORM_MINMAX, CV_8U); - return out; -} - -inline cv::Mat to_bgr8(const cv::Mat& input, const cv::Mat& gray8 /*CV_8UC1*/) { - if (input.empty()) { - throw std::runtime_error("to_bgr8_for_overlay: input is empty"); - } - - if (input.type() == CV_8UC3) { - return input; - } - - const int ch = input.channels(); - - if (ch == 1) { - if (gray8.empty() || gray8.type() != CV_8UC1) { - throw std::runtime_error( - "to_bgr8_for_overlay: gray8 must be CV_8UC1"); - } - cv::Mat bgr; - cv::cvtColor(gray8, bgr, cv::COLOR_GRAY2BGR); - return bgr; - } - - if (ch == 4) { - cv::Mat bgr; - cv::cvtColor(input, bgr, cv::COLOR_BGRA2BGR); // assumes BGRA - - if (bgr.depth() == CV_8U) { - return bgr; - } - if (bgr.depth() == CV_16U) { - cv::Mat out; - bgr.convertTo(out, CV_8U, 1.0 / 256.0); - return out; - } - cv::Mat out; - cv::normalize(bgr, out, 0, 255, cv::NORM_MINMAX, CV_8U); - return out; - } - - if (ch == 3) { - if (input.depth() == CV_8U) { - return input; - } - if (input.depth() == CV_16U) { - cv::Mat out; - input.convertTo(out, CV_8U, 1.0 / 256.0); - return out; - } - cv::Mat out; - cv::normalize(input, out, 0, 255, cv::NORM_MINMAX, CV_8U); - return out; - } - - throw std::runtime_error("to_bgr8_for_overlay: unsupported channel count"); -} - -constexpr int kLineThickness = 2; -constexpr int kLineType = cv::LINE_AA; -const cv::Scalar kLineColor(0, 0, 255); // BGR red - -inline void make_overlay_color(const cv::Mat& input_bgr, - const cv::Mat& gray8, - const std::vector& lines, - cv::Mat& out_bgr) { - if (input_bgr.empty()) { - throw std::runtime_error("make_overlay_color: input_bgr is empty"); - } - out_bgr = vortex::line_detection::to_bgr8(input_bgr, gray8); - - for (const auto& l : lines) { - const cv::Point p0(l[0], l[1]); - const cv::Point p1(l[2], l[3]); - cv::line(out_bgr, p0, p1, kLineColor, kLineThickness, kLineType); - } -} - -inline void make_overlay_canny(const cv::Mat& edges, - const std::vector& lines, - cv::Mat& out_bgr) { - if (edges.empty()) { - throw std::runtime_error("make_overlay_canny: edges is empty"); - } - if (edges.type() != CV_8UC1) { - throw std::runtime_error("make_overlay_canny: edges must be CV_8UC1"); - } - - cv::cvtColor(edges, out_bgr, cv::COLOR_GRAY2BGR); - - for (const auto& l : lines) { - const cv::Point p0(l[0], l[1]); - const cv::Point p1(l[2], l[3]); - cv::line(out_bgr, p0, p1, kLineColor, kLineThickness, kLineType); - } -} +/** + * @brief Convert an image to 8-bit, single-channel grayscale (CV_8UC1). + * + * This helper is intended to produce a suitable input for edge detection (e.g., + * Canny) from a variety of common OpenCV image formats. + * + * Conversion rules: + * - If @p input is already CV_8UC1, the function returns a **shallow copy** (no + * pixel data copied). + * - If @p input has 3 channels, it is converted using @c cv::COLOR_BGR2GRAY + * (assumes BGR ordering). + * - If @p input has 4 channels, it is converted using @c cv::COLOR_BGRA2GRAY + * (assumes BGRA ordering). + * + * - If the resulting grayscale image is 16-bit unsigned (CV_16U), it is scaled + * to 8-bit using + * @c convertTo with @c alpha = 1/256 (i.e., approximately a right shift by 8 + * bits). + * - For other depths (signed integers or floating point), the image is min-max + * normalized to [0,255] using @c cv::normalize(..., cv::NORM_MINMAX) and + * written as CV_8U. + * + * @param input Input image. Supported channel counts: 1, 3 (BGR), or 4 (BGRA). + * Supported depths: any OpenCV depth; non-8U depths are converted + * as described above. + * + * @return A grayscale image of type CV_8UC1. May share data with @p input if no + * conversion is needed. + * + * @throws std::runtime_error If @p input is empty or has an unsupported channel + * count. + */ +cv::Mat to_gray8(const cv::Mat& input); + +/** + * @brief Convert an image to 8-bit, 3-channel BGR (CV_8UC3). + * + * Conversion rules: + * - If @p input is already CV_8UC3, the function returns a **shallow copy** (no + * pixel data copied). + * - If @p input is single-channel, it is expanded to BGR using @c + * cv::COLOR_GRAY2BGR. In this case, the already-computed @p gray8 (CV_8UC1) + * must be provided to avoid recomputing grayscale conversion / scaling. + * - If @p input has 4 channels, it is converted using @c cv::COLOR_BGRA2BGR + * (assumes BGRA ordering). + * - If the resulting BGR image is 16-bit unsigned (CV_16U), it is scaled to + * 8-bit using + * @c convertTo with @c alpha = 1/256 (i.e., approximately a right shift by 8 + * bits). + * - For other depths (signed integers or floating point), the image is min-max + * normalized to [0,255] using @c cv::normalize(..., cv::NORM_MINMAX) and + * written as CV_8U. + * + * @note The normalization step computes min/max per call. This is usually fine + * for visualization, but it may produce different brightness/contrast per frame + * when viewing a stream. + * + * @param input Input image. Supported channel counts: 1, 3 (assumed BGR), or 4 + * (assumed BGRA). + * @param gray8 A grayscale image of type CV_8UC1 corresponding to @p input (or + * derived from it). Required when @p input is single-channel; ignored for 3- or + * 4-channel inputs. + * + * @return A BGR image of type CV_8UC3. May share data with @p input if no + * conversion is needed. + * + * @throws std::runtime_error If @p input is empty, has an unsupported channel + * count, or if @p input is single-channel and @p gray8 is empty or not CV_8UC1. + */ +cv::Mat to_bgr8(const cv::Mat& input, const cv::Mat& gray8 /*CV_8UC1*/); + +/** + * @brief Create a BGR visualization by overlaying detected lines on the input + * color image. + * @param input_bgr Input color image (typically CV_8UC3). If not already in + * BGR format, it should be converted to BGR8 using @c to_bgr8 before + * calling this function. + * @param gray8 Grayscale version of the input image (CV_8UC1). This is used as + * an intermediate step to ensure proper handling of different input formats and + * depths. + * @param lines Detected line segments, where each line is represented as a + * cv::Vec4i (x1, y1, x2, y2). + * @return A BGR image with line segments overlaid on the input image. This is + * typically CV_8UC3. + */ +cv::Mat make_overlay_color(const cv::Mat& input_bgr, + const cv::Mat& gray8, + const std::vector& lines); + +/** + * @brief Create a BGR visualization by overlaying detected lines on the Canny + * edge map. + * @param edges Canny edge map (CV_8UC1). + * @param lines Detected line segments, where each line is represented as a + * cv::Vec4i (x1, y1, x2, y2). + * @return A BGR image with line segments overlaid on the edge map. This is + * typically CV_8UC3. + */ +cv::Mat make_overlay_canny(const cv::Mat& edges, + const std::vector& lines); } // namespace vortex::line_detection diff --git a/line_detection_houghp/src/lib/houghp_line_detection.cpp b/line_detection_houghp/src/lib/houghp_line_detection.cpp index 06211cf..7fd07cd 100644 --- a/line_detection_houghp/src/lib/houghp_line_detection.cpp +++ b/line_detection_houghp/src/lib/houghp_line_detection.cpp @@ -1,6 +1,6 @@ -#include "line_detection_houghp/lib/line_detection_houghp.hpp" #include #include +#include "line_detection_houghp/lib/line_detection_houghp.hpp" #include "line_detection_houghp/lib/typedefs.hpp" #include "line_detection_houghp/lib/utils.hpp" @@ -34,18 +34,18 @@ Result LineDetectorHoughP::detect(const cv::Mat& input_image, } case DetectorMode::visualize: { VisualizeOutput v; - vortex::line_detection::make_overlay_color( - input_image, gray8, cv_lines, v.overlay_color); + v.overlay_color = vortex::line_detection::make_overlay_color( + input_image, gray8, cv_lines); r.output = std::move(v); break; } case DetectorMode::debug: { DebugOutput d; d.canny = edge_image; - vortex::line_detection::make_overlay_canny(edge_image, cv_lines, - d.overlay_canny); - vortex::line_detection::make_overlay_color( - input_image, gray8, cv_lines, d.overlay_color); + d.overlay_canny = vortex::line_detection::make_overlay_canny( + edge_image, cv_lines); + d.overlay_color = vortex::line_detection::make_overlay_color( + input_image, gray8, cv_lines); r.output = std::move(d); break; } diff --git a/line_detection_houghp/src/lib/utils.cpp b/line_detection_houghp/src/lib/utils.cpp new file mode 100644 index 0000000..9be7e63 --- /dev/null +++ b/line_detection_houghp/src/lib/utils.cpp @@ -0,0 +1,146 @@ +#ifndef LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ +#define LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ + +#include "line_detection_houghp/lib/utils.hpp" +#include +#include + +namespace vortex::line_detection { + +cv::Mat to_gray8(const cv::Mat& input) { + if (input.empty()) { + throw std::runtime_error("to_gray8: input is empty"); + } + + if (input.type() == CV_8UC1) { + return input; + } + + cv::Mat gray; + const int ch = input.channels(); + + // Convert to grayscale + if (ch == 1) { + gray = input; + } else if (ch == 3) { + cv::cvtColor(input, gray, cv::COLOR_BGR2GRAY); // assumes BGR + } else if (ch == 4) { + cv::cvtColor(input, gray, cv::COLOR_BGRA2GRAY); // assumes BGRA + } else { + throw std::runtime_error("to_gray8: unsupported channel count"); + } + + // Now ensure 8-bit depth. + if (gray.depth() == CV_8U) { + return gray; + } + + if (gray.depth() == CV_16U) { + cv::Mat out; + gray.convertTo(out, CV_8U, 1.0 / 256.0); // quick/common 16U->8U + return out; + } + + // Generic fallback for signed ints / floats etc. + cv::Mat out; + cv::normalize(gray, out, 0, 255, cv::NORM_MINMAX, CV_8U); + return out; +} + +cv::Mat to_bgr8(const cv::Mat& input, const cv::Mat& gray8 /*CV_8UC1*/) { + if (input.empty()) { + throw std::runtime_error("to_bgr8_for_overlay: input is empty"); + } + + if (input.type() == CV_8UC3) { + return input; + } + + const int ch = input.channels(); + + if (ch == 1) { + if (gray8.empty() || gray8.type() != CV_8UC1) { + throw std::runtime_error( + "to_bgr8_for_overlay: gray8 must be CV_8UC1"); + } + cv::Mat bgr; + cv::cvtColor(gray8, bgr, cv::COLOR_GRAY2BGR); + return bgr; + } + + if (ch == 4) { + cv::Mat bgr; + cv::cvtColor(input, bgr, cv::COLOR_BGRA2BGR); // assumes BGRA + + if (bgr.depth() == CV_8U) { + return bgr; + } + if (bgr.depth() == CV_16U) { + cv::Mat out; + bgr.convertTo(out, CV_8U, 1.0 / 256.0); + return out; + } + cv::Mat out; + cv::normalize(bgr, out, 0, 255, cv::NORM_MINMAX, CV_8U); + return out; + } + + if (ch == 3) { + if (input.depth() == CV_8U) { + return input; + } + if (input.depth() == CV_16U) { + cv::Mat out; + input.convertTo(out, CV_8U, 1.0 / 256.0); + return out; + } + cv::Mat out; + cv::normalize(input, out, 0, 255, cv::NORM_MINMAX, CV_8U); + return out; + } + + throw std::runtime_error("to_bgr8_for_overlay: unsupported channel count"); +} + +constexpr int k_line_thickness = 2; +constexpr int k_line_type = cv::LINE_AA; +const cv::Scalar k_line_color(0, 0, 255); // BGR red + +cv::Mat make_overlay_color(const cv::Mat& input_bgr, + const cv::Mat& gray8, + const std::vector& lines) { + if (input_bgr.empty()) { + throw std::runtime_error("make_overlay_color: input_bgr is empty"); + } + cv::Mat out_bgr = vortex::line_detection::to_bgr8(input_bgr, gray8); + for (const auto& l : lines) { + const cv::Point p0(l[0], l[1]); + const cv::Point p1(l[2], l[3]); + cv::line(out_bgr, p0, p1, k_line_color, k_line_thickness, k_line_type); + } + return out_bgr; +} + +cv::Mat make_overlay_canny(const cv::Mat& edges, + const std::vector& lines) { + if (edges.empty()) { + throw std::runtime_error("make_overlay_canny: edges is empty"); + } + if (edges.type() != CV_8UC1) { + throw std::runtime_error("make_overlay_canny: edges must be CV_8UC1"); + } + + cv::Mat out_bgr; + cv::cvtColor(edges, out_bgr, cv::COLOR_GRAY2BGR); + + for (const auto& l : lines) { + const cv::Point p0(l[0], l[1]); + const cv::Point p1(l[2], l[3]); + cv::line(out_bgr, p0, p1, k_line_color, k_line_thickness, k_line_type); + } + return out_bgr; +} + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ From fa697e29482fd3ad31cd152c4abf0eb381aebc0f Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 12 Feb 2026 14:16:34 +0100 Subject: [PATCH 10/13] mode string parsing --- .../line_detection_houghp/lib/mode_utils.hpp | 38 +++++++++++++++++++ .../ros/line_detection_houghp_ros_utils.cpp | 11 +----- 2 files changed, 39 insertions(+), 10 deletions(-) create mode 100644 line_detection_houghp/include/line_detection_houghp/lib/mode_utils.hpp diff --git a/line_detection_houghp/include/line_detection_houghp/lib/mode_utils.hpp b/line_detection_houghp/include/line_detection_houghp/lib/mode_utils.hpp new file mode 100644 index 0000000..6473666 --- /dev/null +++ b/line_detection_houghp/include/line_detection_houghp/lib/mode_utils.hpp @@ -0,0 +1,38 @@ +#ifndef LINE_DETECTION_HOUGHP__LIB__MODE_UTILS_HPP_ +#define LINE_DETECTION_HOUGHP__LIB__MODE_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include "line_detection_houghp/lib/typedefs.hpp" + +namespace vortex::line_detection { + +inline constexpr std::array, 3> + kDetectorModeMap{{ + {"standard", DetectorMode::standard}, + {"visualize", DetectorMode::visualize}, + {"debug", DetectorMode::debug}, + }}; + +inline DetectorMode parse_mode(const std::string& s) { + for (const auto& [name, mode] : kDetectorModeMap) { + if (s == name) + return mode; + } + throw std::runtime_error("Invalid DetectorMode: " + s); +} + +inline std::string_view to_string(DetectorMode mode) { + for (const auto& [name, m] : kDetectorModeMap) { + if (m == mode) + return name; + } + throw std::runtime_error("Unknown DetectorMode"); +} + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_HOUGHP__LIB__MODE_UTILS_HPP_ diff --git a/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp b/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp index 065ec56..f5077e7 100644 --- a/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp +++ b/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp @@ -1,4 +1,5 @@ #include +#include "line_detection_houghp/lib/mode_utils.hpp" #include "line_detection_houghp/ros/line_detection_houghp_ros.hpp" namespace vortex::line_detection { @@ -8,16 +9,6 @@ static bool starts_with(const std::string& s, const std::string& prefix) { s.compare(0, prefix.size(), prefix) == 0; } -static DetectorMode parse_mode(const std::string& s) { - if (s == "standard") - return DetectorMode::standard; - if (s == "visualize") - return DetectorMode::visualize; - if (s == "debug") - return DetectorMode::debug; - throw std::runtime_error("Invalid mode: " + s); -} - void LineDetectionHoughPNode::initialize_parameter_handler() { param_handler_ = std::make_shared(this); From d890206f2f2561511e68d60ec54b263df4a1bbb9 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 12 Feb 2026 14:17:01 +0100 Subject: [PATCH 11/13] ros node fixes --- line_detection_houghp/CMakeLists.txt | 1 + .../ros/line_detection_houghp_ros.hpp | 6 +- .../launch/line_detection_houghp.launch.py | 4 +- .../src/ros/line_detection_houghp_ros.cpp | 10 +- .../ros/line_detection_houghp_ros_utils.cpp | 101 ++++++++++-------- 5 files changed, 66 insertions(+), 56 deletions(-) diff --git a/line_detection_houghp/CMakeLists.txt b/line_detection_houghp/CMakeLists.txt index b81dd78..0bc663e 100644 --- a/line_detection_houghp/CMakeLists.txt +++ b/line_detection_houghp/CMakeLists.txt @@ -27,6 +27,7 @@ set(CORE_LIB_NAME "${PROJECT_NAME}") add_library(${CORE_LIB_NAME} SHARED src/lib/houghp_line_detection.cpp + src/lib/utils.cpp ) target_include_directories(${CORE_LIB_NAME} diff --git a/line_detection_houghp/include/line_detection_houghp/ros/line_detection_houghp_ros.hpp b/line_detection_houghp/include/line_detection_houghp/ros/line_detection_houghp_ros.hpp index 7de0e4b..dd63986 100644 --- a/line_detection_houghp/include/line_detection_houghp/ros/line_detection_houghp_ros.hpp +++ b/line_detection_houghp/include/line_detection_houghp/ros/line_detection_houghp_ros.hpp @@ -1,7 +1,6 @@ #ifndef LINE_DETECTION_HOUGHP__ROS__LINE_DETECTION_HOUGHP_ROS_HPP_ #define LINE_DETECTION_HOUGHP__ROS__LINE_DETECTION_HOUGHP_ROS_HPP_ -#include #include #include #include @@ -10,7 +9,6 @@ #include #include #include -#include #include #include "line_detection_houghp/lib/line_detection_houghp.hpp" @@ -20,7 +18,7 @@ class LineDetectionHoughPNode : public rclcpp::Node { public: explicit LineDetectionHoughPNode(const rclcpp::NodeOptions& options); - ~LineDetectionHoughPNode() {} + ~LineDetectionHoughPNode() override = default; private: rclcpp::Subscription::SharedPtr image_sub_; @@ -30,7 +28,7 @@ class LineDetectionHoughPNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr canny_debug_pub_; rclcpp::Publisher::SharedPtr canny_overlay_pub_; - void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); + void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg); void declare_parameters(); diff --git a/line_detection_houghp/launch/line_detection_houghp.launch.py b/line_detection_houghp/launch/line_detection_houghp.launch.py index bb8b412..c003968 100644 --- a/line_detection_houghp/launch/line_detection_houghp.launch.py +++ b/line_detection_houghp/launch/line_detection_houghp.launch.py @@ -21,7 +21,9 @@ def generate_launch_description(): output='screen', parameters=[ config, - # {'use_sim_time': True} # If testing with rosbags sim_time might be preferred if bag is looped + { + 'use_sim_time': False + }, # If testing with rosbags sim_time might be preferred if bag is looped ], ), ] diff --git a/line_detection_houghp/src/ros/line_detection_houghp_ros.cpp b/line_detection_houghp/src/ros/line_detection_houghp_ros.cpp index ed967fe..936d734 100644 --- a/line_detection_houghp/src/ros/line_detection_houghp_ros.cpp +++ b/line_detection_houghp/src/ros/line_detection_houghp_ros.cpp @@ -50,12 +50,14 @@ void LineDetectionHoughPNode::setup_publishers_and_subscribers() { const std::string canny_overlay_pub_topic = this->get_parameter("topic.canny_overlay_pub_topic").as_string(); - auto qos_profile = vortex::utils::qos_profiles::sensor_data_profile(1); + const auto qos_profile = + vortex::utils::qos_profiles::sensor_data_profile(1); image_sub_ = this->create_subscription( image_sub_topic, qos_profile, - std::bind(&LineDetectionHoughPNode::image_callback, this, - std::placeholders::_1)); + [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + this->image_callback(msg); + }); line_segments_pub_ = this->create_publisher( @@ -95,7 +97,7 @@ void LineDetectionHoughPNode::set_detector() { } void LineDetectionHoughPNode::image_callback( - const sensor_msgs::msg::Image::SharedPtr msg) { + const sensor_msgs::msg::Image::ConstSharedPtr& msg) { cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); diff --git a/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp b/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp index f5077e7..9a0d88c 100644 --- a/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp +++ b/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp @@ -1,3 +1,4 @@ +#include #include #include "line_detection_houghp/lib/mode_utils.hpp" #include "line_detection_houghp/ros/line_detection_houghp_ros.hpp" @@ -57,61 +58,67 @@ void LineDetectionHoughPNode::on_parameter_event( void LineDetectionHoughPNode::mode_conditional_publishing( const Result& result, const std_msgs::msg::Header& header) { - if (mode_ == DetectorMode::standard) { - return; - } - - if (mode_ == DetectorMode::visualize) { - const auto* vis = std::get_if(&result.output); - if (!vis) { - spdlog::warn( - "Mode is visualize but Result::output is not VisualizeOutput"); + switch (mode_) { + case DetectorMode::standard: return; - } - - cv_bridge::CvImage overlay_msg; - overlay_msg.header = header; - overlay_msg.encoding = sensor_msgs::image_encodings::BGR8; - overlay_msg.image = vis->overlay_color; // expected CV_8UC3 - color_overlay_pub_->publish(*overlay_msg.toImageMsg()); - return; - } - if (mode_ == DetectorMode::debug) { - const auto* dbg = std::get_if(&result.output); - if (!dbg) { - spdlog::warn("Mode is debug but Result::output is not DebugOutput"); + case DetectorMode::visualize: { + const auto* vis = std::get_if(&result.output); + if (!vis) { + spdlog::warn( + "Mode is visualize but Result::output is not " + "VisualizeOutput"); + return; + } + + cv_bridge::CvImage overlay_msg; + overlay_msg.header = header; + overlay_msg.encoding = sensor_msgs::image_encodings::BGR8; + overlay_msg.image = vis->overlay_color; // expected CV_8UC3 + color_overlay_pub_->publish(*overlay_msg.toImageMsg()); return; } - // overlay_canny: CV_8UC3 -> bgr8 - { - cv_bridge::CvImage m; - m.header = header; - m.encoding = sensor_msgs::image_encodings::BGR8; - m.image = dbg->overlay_canny; - canny_overlay_pub_->publish(*m.toImageMsg()); - } - - // canny: CV_8UC1 -> mono8 - { - cv_bridge::CvImage m; - m.header = header; - m.encoding = sensor_msgs::image_encodings::MONO8; - m.image = dbg->canny; - canny_debug_pub_->publish(*m.toImageMsg()); - } + case DetectorMode::debug: { + const auto* dbg = std::get_if(&result.output); + if (!dbg) { + spdlog::warn( + "Mode is debug but Result::output is not DebugOutput"); + return; + } + + // overlay_canny: CV_8UC3 -> bgr8 + { + cv_bridge::CvImage m; + m.header = header; + m.encoding = sensor_msgs::image_encodings::BGR8; + m.image = dbg->overlay_canny; + canny_overlay_pub_->publish(*m.toImageMsg()); + } + + // canny: CV_8UC1 -> mono8 + { + cv_bridge::CvImage m; + m.header = header; + m.encoding = sensor_msgs::image_encodings::MONO8; + m.image = dbg->canny; + canny_debug_pub_->publish(*m.toImageMsg()); + } + + // overlay_color: CV_8UC3 -> bgr8 + { + cv_bridge::CvImage m; + m.header = header; + m.encoding = sensor_msgs::image_encodings::BGR8; + m.image = dbg->overlay_color; + color_overlay_pub_->publish(*m.toImageMsg()); + } - // overlay_color: CV_8UC3 -> bgr8 - { - cv_bridge::CvImage m; - m.header = header; - m.encoding = sensor_msgs::image_encodings::BGR8; - m.image = dbg->overlay_color; - color_overlay_pub_->publish(*m.toImageMsg()); + return; } - return; + default: + return; } } From 56071f55dad8a6812a8ef2d16889cc02d2445988 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 12 Feb 2026 14:20:47 +0100 Subject: [PATCH 12/13] correct opencv package.xml dep --- line_detection_houghp/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/line_detection_houghp/package.xml b/line_detection_houghp/package.xml index d09b3e0..4f00beb 100644 --- a/line_detection_houghp/package.xml +++ b/line_detection_houghp/package.xml @@ -15,7 +15,7 @@ vortex_utils vortex_utils_ros vortex_msgs - opencv + libopencv-dev cv_bridge From b0576c39a7cb982374d3331f0d16699925e8b338 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 14 Feb 2026 12:31:11 +0100 Subject: [PATCH 13/13] ci repos and gcc13 fix --- .github/workflows/code-coverage.yml | 3 +++ .github/workflows/industrial-ci.yml | 2 ++ scripts/ci_install_dependencies.sh | 19 +++++++++++++++++++ 3 files changed, 24 insertions(+) create mode 100644 scripts/ci_install_dependencies.sh diff --git a/.github/workflows/code-coverage.yml b/.github/workflows/code-coverage.yml index 7b9b6db..d2c9091 100644 --- a/.github/workflows/code-coverage.yml +++ b/.github/workflows/code-coverage.yml @@ -8,5 +8,8 @@ on: jobs: code-coverage: uses: vortexntnu/vortex-ci/.github/workflows/reusable-code-coverage.yml@main + with: + vcs-repo-file-url: './dependencies.repos' + before_install_target_dependencies: './scripts/ci_install_dependencies.sh' secrets: CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} # Set in the repository secrets diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml index 23b5750..3950afc 100644 --- a/.github/workflows/industrial-ci.yml +++ b/.github/workflows/industrial-ci.yml @@ -13,4 +13,6 @@ jobs: industrial-ci: uses: vortexntnu/vortex-ci/.github/workflows/reusable-industrial-ci.yml@main with: + upstream_workspace: './dependencies.repos' + before_install_upstream_dependencies: './scripts/ci_install_dependencies.sh' ros_repo: '["testing", "main"]' diff --git a/scripts/ci_install_dependencies.sh b/scripts/ci_install_dependencies.sh new file mode 100644 index 0000000..e37c97d --- /dev/null +++ b/scripts/ci_install_dependencies.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash +# Installs dependencies not handled by rosdep. + +set -e # Exit on any error + +# Note: These are required by vortex-vkf +# Add Ubuntu Toolchain PPA to get gcc-13/g++-13 +sudo apt-get update -qq +sudo apt-get install -y --no-install-recommends software-properties-common +sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y +sudo apt-get update -qq + +# Install and switch to GCC 13 +sudo apt-get install -y --no-install-recommends gcc-13 g++-13 lcov +sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-13 100 +sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 100 +sudo update-alternatives --install /usr/bin/gcov gcov /usr/bin/gcov-13 100 + +echo "Done installing additional dependencies."