Refactor

CMakeLists.txt

@@ -20,13 +20,18 @@ find_package(std_msgs REQUIRED)
 find_package(OpenCV 4.5.4 REQUIRED)
 find_package(PCL REQUIRED)
 find_package(pcl_conversions REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 
 include_directories(include)
 
 set(LIB_NAME "${PROJECT_NAME}_component")
 
 add_library(${LIB_NAME} SHARED
-  src/valve_detection.cpp
+  src/valve_detector.cpp
+  src/depth_image_processing.cpp
+  src/pointcloud_processing.cpp
+  src/angle_detector.cpp
+  src/valve_pose_ros.cpp
 )
 
 target_link_libraries(${LIB_NAME} PUBLIC
@@ -49,13 +54,15 @@ ament_target_dependencies(${LIB_NAME} PUBLIC
   vision_msgs
   geometry_msgs
   std_msgs
-  PCL
   pcl_conversions
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
 )
 
 rclcpp_components_register_node(
     ${LIB_NAME}
-    PLUGIN "ValveDetectionNode"
+    PLUGIN "valve_detection::ValvePoseNode"
     EXECUTABLE ${PROJECT_NAME}_node
 )
 

README.md

@@ -1,4 +1,99 @@
-# valve-detection
+# Valve Detection
 [![Industrial CI](https://github.com/vortexntnu/valve-detection/actions/workflows/industrial-ci.yml/badge.svg)](https://github.com/vortexntnu/valve-detection/actions/workflows/industrial-ci.yml)
 [![pre-commit.ci status](https://results.pre-commit.ci/badge/github/vortexntnu/valve-detection/main.svg)](https://results.pre-commit.ci/latest/github/vortexntnu/valve-detection/main)
 [![codecov](https://codecov.io/github/vortexntnu/valve-detection/graph/badge.svg?token=6csTwaozlh)](https://codecov.io/github/vortexntnu/valve-detection)
+
+---
+
+### Overview
+
+The **Valve Detection Node** estimates the **3D pose and orientation** of industrial valves from:
+
+* A **depth image** or **point cloud**,
+* An optional **color image**, and
+* **2D detections** (e.g., YOLO bounding boxes).
+
+It fits a **plane** to the valve annulus, computes the **3D intersection** of the valve center ray with that plane, and estimates the **rotation** of the valve handle using image-based line detection.
+
+It can output:
+
+* A `geometry_msgs::msg::PoseArray` of all valve poses,
+* Debug point clouds (annulus points and segmented planes),
+* Annotated color images showing detections, plane fits, and axes,
+* Optional angle detection debug images.
+
+---
+
+
+
+
+
+## Launching the Node
+
+
+```bash
+ros2 launch valve_detection valve_detection.launch.py
+```
+
+## How it works
+
+### 1. Input Synchronization
+
+The node synchronizes:
+
+* **Depth (or point cloud)**
+* **Color image** (optional)
+* **2D detections**
+
+It supports multiple input modes via `use_depth_image` and `use_color_image`.
+
+### 2. Annulus Extraction
+
+For each bounding box, a ring-shaped region (annulus) around the center is extracted from the depth image or point cloud.
+
+### 3. Plane Segmentation
+
+RANSAC is used to fit a plane through the annulus points.
+
+### 4. Ray–Plane Intersection
+
+The center ray from the camera through the bounding box center is intersected with the plane to find the valve’s 3D position.
+
+### 5. Orientation Estimation
+
+* The **plane normal** defines one orientation axis.
+* The **valve handle angle** (from image) defines the in-plane rotation.
+* Intrinsics are used to back-project that angle into 3D.
+* The result is a full 3×3 rotation matrix and quaternion.
+
+### 6. Pose Publishing
+
+A `PoseArray` message is published with one `Pose` per detected valve. If orientation computation failed no pose is published.
+
+---
+
+## Visualization Outputs
+
+| Topic                    | Description                                 |                                  |
+| ------------------------ | ------------------------------------------- | ---------------------------------------- |
+| `/valve_detection_image` | Color image with bounding boxes and 3D axes | |
+| `/valve_angle_image`     | Debug overlay showing detected Hough lines  |                       |
+| `/bbx_annulus_pcl`       | Ring-shaped depth points used for plane fit                   |
+| `/annulus_plane_pcl`     | Segmented plane points                      |
+
+---
+
+
+## Common Issues
+
+| Issue                      | Possible Cause                                                               |
+|----------------------------|-----------------------------------------------------------------------------|
+| No poses published         | Missing plane segmentation (adjust `plane_ransac_threshold` or annulus size) |
+| Angle NaN / no handle lines| Tune Hough and Canny thresholds                                              |
+
+
+---
+
+## Future work
+* Use actual endpoints of line for backprojection to retrieve the perspective-correct plane angle. Now we just use rotation around optical axis.
+TODO: For OBB, estimate line segment from BB size.

config/valve_detection_params.yaml

@@ -1,5 +1,40 @@
 /**:
   ros__parameters:
     depth_image_sub_topic: "/zed_node/depth/depth_registered"
-    color_image_sub_topic: "/image_rect"
+    color_image_sub_topic: "/zed_node/left/image_rect_color"
+    pcl_sub_topic: "/zed_node/depth/point_cloud"
     detections_sub_topic: "/detections_output"
+
+    color_image_info_topic: "/zed_node/depth/camera_info"
+
+    valve_poses_pub_topic: "/valve_poses"
+
+    yolo_img_width: 640
+    yolo_img_height: 640
+
+    annulus_radius_ratio: 0.8
+
+    plane_ransac_threshold: 0.01
+    plane_ransac_max_iterations: 20
+
+    valve_handle_offset: 0.05
+
+    use_depth_image: true # if false, use pcl and tf2 to transform pointcloud to color frame
+
+    visualize_detections: true
+    processed_image_pub_topic: "/valve_detection_image"
+    debug_visualize: true
+    annulus_pub_topic: "/bbx_annulus_pcl"
+    plane_pub_topic: "/annulus_plane_pcl"
+    angle_detection_image_pub_topic: "/valve_angle_image"
+
+    calculate_angle: true
+    angle.detection_area_ratio: 0.22 # percentage of bb size
+    angle.canny_low_threshold: 40
+    angle.canny_high_threshold: 80
+    angle.canny_aperture_size: 3 # must be odd
+    angle.hough_rho_res: 1.0
+    angle.hough_theta_res: 0.0174533 # 1 degree in radians
+    angle.hough_threshold: 20
+    angle.hough_min_line_length: 45.0
+    angle.hough_max_line_gap: 5.0

include/valve_detection/angle_detector.hpp

@@ -0,0 +1,72 @@
+#ifndef VALVE_POSE_ANGLE_HPP
+#define VALVE_POSE_ANGLE_HPP
+
+#include <cmath>
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include "valve_detection/types.hpp"
+
+namespace valve_detection {
+
+class AngleDetector {
+   public:
+    /**
+     * @brief Constructs an AngleDetector with a set of parameters.
+     * @param params The parameters for the detector.
+     */
+    explicit AngleDetector(const AngleDetectorParams& params)
+        : params_(params) {}
+
+    /**
+     * @brief Computes the angle for each bounding box in a vector.
+     *
+     * This function processes each bounding box to define a region of interest
+     * (ROI) in the input image, applies Canny edge detection and Hough line
+     * transform, and then calculates the dominant line angle within each box's
+     * ROI, and updates the 'theta' member of each box in-place.
+     *
+     * @param color_image The input image to be processed.
+     * @param boxes A vector of BoundingBox objects passed by reference. The
+     * 'theta' member of each box will be updated.
+     */
+    void compute_angles(const cv::Mat& color_image,
+                        std::vector<BoundingBox>& boxes) const;
+
+    /**
+     * @brief Processes a vector of bounding boxes to calculate the angle for
+     * each, updating the boxes in-place and drawing all visualizations on a
+     * single image.
+     *
+     * @param image_to_draw_on The input/output image. This cv::Mat will be
+     * modified to include visualizations for all processed boxes.
+     * @param boxes A vector of BoundingBox objects. The theta member of each
+     * box will be updated with the calculated angle (or NaN if no line is
+     * found).
+     */
+    void compute_angles_debug(cv::Mat& image_to_draw_on,
+                              std::vector<BoundingBox>& boxes) const;
+
+   private:
+    /**
+     * @brief Finds the longest line among a set of lines detected in an image.
+     * @param lines A vector of lines, each represented as cv::Vec4i (x1, y1,
+     * x2, y2).
+     * @return The cv::Vec4i representing the longest line.
+     * If the input vector is empty, returns a zero-length line {0, 0, 0, 0}.
+     */
+    cv::Vec4i find_longest_line(const std::vector<cv::Vec4i>& lines) const;
+
+    /**
+     * @brief Applies Canny edge detection to a grayscale image.
+     * @param gray_image The input grayscale image.
+     * @return A binary image (CV_8UC1) with edges detected.
+     */
+    cv::Mat apply_canny_edge_detection(const cv::Mat& gray_image) const;
+
+    /** @brief Parameters for the angle detector. */
+    AngleDetectorParams params_;
+};
+
+}  // namespace valve_detection
+
+#endif  // VALVE_POSE_ANGLE_HPP

include/valve_detection/depth_image_processing.hpp

@@ -0,0 +1,61 @@
+#ifndef VALVE_POSE_DEPTH_HPP
+#define VALVE_POSE_DEPTH_HPP
+
+#include "valve_detection/types.hpp"
+
+#include <pcl/ModelCoefficients.h>
+#include <pcl/PointIndices.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <cmath>
+#include <limits>
+#include <opencv2/core/mat.hpp>
+
+namespace valve_detection {
+
+/**
+ * @brief Projects a 2D pixel with depth into 3D camera coordinates.
+ *
+ * @param u The x-coordinate of the pixel in the image.
+ * @param v The y-coordinate of the pixel in the image.
+ * @param depth The depth value at the pixel (in meters or the same units as
+ * intrinsics).
+ * @param fx Focal length along x-axis.
+ * @param fy Focal length along y-axis.
+ * @param cx Principal point x-coordinate.
+ * @param cy Principal point y-coordinate.
+ * @param[out] point The resulting 3D point in camera coordinates.
+ */
+void project_pixel_to_point(int u,
+                            int v,
+                            float depth,
+                            double fx,
+                            double fy,
+                            double cx,
+                            double cy,
+                            pcl::PointXYZ& point);
+
+/**
+ * @brief Extracts a 3D point cloud representing the annulus (valve rim) from a
+ * depth image.
+ *
+ * The function selects pixels within the annulus region of a bounding box and
+ * projects them into 3D points using the camera intrinsics.
+ *
+ * @param depth_image The input depth image (CV_32FC1 or similar).
+ * @param bbox The bounding box around the valve in the image.
+ * @param image_properties Camera intrinsics and image dimensions.
+ * @param annulus_radius_ratio Fraction of the bounding box considered as the
+ * annulus (0.0–1.0).
+ * @param[out] cloud The resulting point cloud containing the 3D points of the
+ * annulus.
+ */
+void extract_annulus_pcl(const cv::Mat& depth_image,
+                         const BoundingBox& bbox,
+                         const ImageProperties& image_properties,
+                         float annulus_radius_ratio,
+                         pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
+
+}  // namespace valve_detection
+
+#endif  // VALVE_POSE_DEPTH_HPP

include/valve_detection/pointcloud_processing.hpp

@@ -0,0 +1,39 @@
+#ifndef VALVE_POSE_PCL_HPP
+#define VALVE_POSE_PCL_HPP
+
+#include "valve_detection/types.hpp"
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <cmath>
+#include <limits>
+
+namespace valve_detection {
+
+/**
+ * @brief Extracts a 3D point cloud representing the annulus (valve rim) from an
+ * input point cloud.
+ *
+ * This function selects points that correspond to the annulus region of a
+ * bounding box in image space, based on the camera intrinsics and the specified
+ * radius ratio.
+ *
+ * @param input_cloud The input point cloud (typically from a depth camera or
+ * LIDAR).
+ * @param bbox The bounding box around the valve in image coordinates.
+ * @param image_properties Camera intrinsics and image dimensions for
+ * projection.
+ * @param annulus_radius_ratio Fraction of the bounding box considered as the
+ * annulus (0.0–1.0).
+ * @param[out] cloud The resulting point cloud containing the 3D points of the
+ * annulus.
+ */
+void extract_annulus_pcl(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_cloud,
+                         const BoundingBox& bbox,
+                         const ImageProperties& image_properties,
+                         float annulus_radius_ratio,
+                         pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
+
+}  // namespace valve_detection
+
+#endif  // VALVE_POSE_PCL_HPP