Add valve-detection and valve-subtype-resolver to vortex-cv/mission/tacc/visual_inspection

.gitignore

@@ -1,5 +1,5 @@
 # VSCode
-.vscode/*
+.vscode/
 !.vscode/settings.json
 !.vscode/tasks.json
 !.vscode/launch.json

mission/tacc/visual_inspection/valve_detection/CMakeLists.txt

@@ -0,0 +1,92 @@
+cmake_minimum_required(VERSION 3.8)
+project(valve_detection)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+find_package(ament_cmake REQUIRED)
+
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(message_filters REQUIRED)
+
+find_package(sensor_msgs REQUIRED)
+find_package(vision_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(OpenCV REQUIRED)
+
+find_package(PCL REQUIRED)
+find_package(pcl_conversions REQUIRED)
+
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+find_package(vortex_msgs REQUIRED)
+find_package(vortex_utils REQUIRED)
+
+find_package(vortex_utils_ros REQUIRED)
+
+include_directories(include)
+
+set(LIB_NAME "${PROJECT_NAME}_component")
+
+add_library(${LIB_NAME} SHARED
+  src/depth_image_processing.cpp
+  src/detection_utils.cpp
+  src/pcl_extraction.cpp
+  src/pose_estimator.cpp
+  src/ros_utils.cpp
+  src/valve_pose_ros.cpp
+)
+
+target_link_libraries(${LIB_NAME} PUBLIC
+  ${OpenCV_LIBS}
+  ${PCL_LIBRARIES}
+)
+
+target_include_directories(${LIB_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${OpenCV_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+)
+
+ament_target_dependencies(${LIB_NAME} PUBLIC
+  rclcpp
+  rclcpp_components
+  message_filters
+  sensor_msgs
+  vision_msgs
+  geometry_msgs
+  std_msgs
+  cv_bridge
+  OpenCV
+  pcl_conversions
+  tf2
+  tf2_ros
+  vortex_msgs
+  vortex_utils
+  vortex_utils_ros
+)
+
+rclcpp_components_register_node(
+  ${LIB_NAME}
+  PLUGIN "valve_detection::ValvePoseNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+install(TARGETS ${LIB_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/ DESTINATION include)
+install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
+
+ament_package()

mission/tacc/visual_inspection/valve_detection/config/valve_detection_params.yaml

@@ -0,0 +1,99 @@
+/**:
+  ros__parameters:
+    # Inputs
+    depth_image_sub_topic: "/camera/camera/depth/image_rect_raw"
+    detections_sub_topic: "/obb_detections_output"
+    depth_image_info_topic: "/camera/camera/depth/camera_info"
+    color_image_info_topic: "/yolo_obb_encoder/internal/resize/camera_info"
+
+    # Depth-to-color extrinsic
+    # TF frame names (without drone prefix) used to look up the depth-to-color
+    # transform from /tf_static.  The node will wait until this transform is
+    # available before initialising the detector.
+    # NOTE: When launched via valve_intervention.launch.py these are overridden
+    # from cameras.yaml.  Values here are standalone-launch defaults.
+    depth_frame_id: "front_camera_depth_optical"
+    color_frame_id: "front_camera_color_optical"
+
+    # Outputs
+    landmarks_pub_topic: "/valve_landmarks"
+
+    # TF frame used as the frame_id for all published poses and landmarks.
+    # Must match the depth optical frame in the robot's TF tree.
+    # NOTE: When launched via valve_intervention.launch.py this is set from
+    # cameras.yaml depth_frame_id.
+    output_frame_id: "front_camera_depth_optical"
+
+    # Prepended to all TF frame names (e.g. "nautilus" -> "nautilus/front_camera_depth_optical").
+    drone: "nautilus"
+
+    # YOLO letterbox reference size
+    yolo_img_width: 640
+    yolo_img_height: 640
+
+    # Undistort bounding-box detections using the color camera distortion
+    # coefficients from camera_info.  Samples edge midpoints and center,
+    # undistorts them, and refits the OBB — gives a tighter fit than
+    # corner-based undistortion for round objects like valves.
+    undistort_detections: false
+
+    # Annulus and plane fit
+    # Fraction of the bounding-box half-size used as the annulus ring radius
+    # (unitless, 0–1). At 0.8 the ring samples the outer 80 % of the box,
+    # avoiding the central hub while staying inside the valve rim.
+    # Tuned empirically on recorded valve footage.
+    annulus_radius_ratio: 0.8
+
+    # Maximum point-to-plane distance (metres) for a depth point to be counted
+    # as a RANSAC inlier. 0.01 m (1 cm) tolerates typical RealSense depth noise
+    # at ~0.5–1 m range without accepting gross outliers.
+    plane_ransac_threshold: 0.01
+
+    # Number of RANSAC iterations. 150 gives >99 % probability of finding a
+    # good plane even when the inlier ratio drops to ~30 %.
+    plane_ransac_max_iterations: 150
+
+    # Duplicate-detection suppression
+    # Two bounding boxes are considered the same valve when their
+    # axis-aligned IoU exceeds this threshold OR when the smaller box
+    # overlaps the larger by more than 70 % (intersection-over-minimum).
+    # Maximum 2 landmarks are published per frame.
+    iou_duplicate_threshold: 0.5
+
+    # Pose offset
+    # Shift the estimated position along the plane normal by this amount (metres).
+    # Value taken from the official valve CAD file: distance between the valve
+    # face frame and the inside length of the handle is 0.02 m.
+    valve_handle_offset: 0.02
+
+    debug_visualize: true # enable all debug visualization topics
+
+    # Debug visualization only active when debug_visualize: true
+    debug:
+      # All detected valve poses in one PoseArray – useful for visualising
+      # multiple simultaneous detections in Foxglove.
+      valve_poses_pub_topic: "/valve_poses"
+
+      # Annulus point cloud (points sampled inside the bounding-box ring used
+      # for plane fitting) projected into 3D – useful for verifying the depth
+      # extraction region.
+      depth_cloud_pub_topic: "/valve_depth_cloud"
+
+      # Depth image false-coloured with COLORMAP_TURBO and OBB outlines drawn
+      # on top – useful for checking detection alignment on the depth frame.
+      depth_colormap_pub_topic: "/valve_detection_depth_colormap"
+      # Colormap scaling range in raw 16UC1 depth units (millimetres).
+      # Min ~0 mm clips near-zero noise; max ~1125 mm (≈1.1 m) covers the
+      # expected close-range operating distance for valve interaction.
+      depth_colormap_value_min: 0.1
+      depth_colormap_value_max: 1125.5
+
+      # Points sampled from the annulus region of the bounding box, collected
+      # as a byproduct of pose estimation – useful for verifying the depth
+      # extraction region in 3D.
+      annulus_pub_topic: "/bbx_annulus_pcl"
+
+      # RANSAC plane inlier points, collected as a byproduct of pose estimation
+      # – useful for checking that the fitted plane normal (and therefore the
+      # final pose orientation) is correct.
+      plane_pub_topic: "/annulus_plane_pcl"

mission/tacc/visual_inspection/valve_detection/include/valve_detection/depth_image_processing.hpp

@@ -0,0 +1,47 @@
+#ifndef VALVE_DETECTION__DEPTH_IMAGE_PROCESSING_HPP_
+#define VALVE_DETECTION__DEPTH_IMAGE_PROCESSING_HPP_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core.hpp>
+#include "valve_detection/types.hpp"
+
+namespace valve_detection {
+
+/**
+ * @brief Back-projects a depth pixel to a 3D point in the camera frame.
+ *
+ * Assumes no lens distortion — this is valid for depth images, which are
+ * typically rectified before publication by the camera driver.
+ */
+void project_pixel_to_point(int u,
+                            int v,
+                            float depth,
+                            double fx,
+                            double fy,
+                            double cx,
+                            double cy,
+                            pcl::PointXYZ& out);
+
+/// @brief Undistorts a bounding box by undistorting the edge midpoints and
+/// center, then refitting an oriented bounding box.
+/// TODO: Had problems with this, when it undistorted the orientation would be
+/// off by a constant offset Workaround is to just disable this and undistort
+/// the whole image instead (done upstream in perception_setup)
+BoundingBox undistort_bbox(const BoundingBox& bbox,
+                           const CameraIntrinsics& intr);
+
+// Projects a color image pixel to depth image coordinates.
+// u_c, v_c: pixel coordinates in the color image.
+// Z:        depth of the point in the color camera frame (metres).
+cv::Point2f project_color_pixel_to_depth(float u_c,
+                                         float v_c,
+                                         float Z,
+                                         const ImageProperties& color_props,
+                                         const ImageProperties& depth_props,
+                                         const DepthColorExtrinsic& extr);
+
+}  // namespace valve_detection
+
+#endif  // VALVE_DETECTION__DEPTH_IMAGE_PROCESSING_HPP_

mission/tacc/visual_inspection/valve_detection/include/valve_detection/detection_utils.hpp

@@ -0,0 +1,22 @@
+#ifndef VALVE_DETECTION__DETECTION_UTILS_HPP_
+#define VALVE_DETECTION__DETECTION_UTILS_HPP_
+
+#include <utility>
+#include <vector>
+#include "valve_detection/types.hpp"
+
+namespace valve_detection {
+
+/**
+ * @brief Greedy NMS: returns indices of kept detections (max 2).
+ *
+ * scored_boxes: (score, bbox) pairs. Two boxes are duplicates when IoMin
+ * (intersection / min-area) or IoU exceeds iou_duplicate_threshold.
+ */
+std::vector<size_t> filter_duplicate_detections(
+    const std::vector<std::pair<float, BoundingBox>>& scored_boxes,
+    float iou_duplicate_threshold);
+
+}  // namespace valve_detection
+
+#endif  // VALVE_DETECTION__DETECTION_UTILS_HPP_

mission/tacc/visual_inspection/valve_detection/include/valve_detection/pcl_extraction.hpp

@@ -0,0 +1,48 @@
+#ifndef VALVE_DETECTION__PCL_EXTRACTION_HPP_
+#define VALVE_DETECTION__PCL_EXTRACTION_HPP_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <opencv2/core.hpp>
+#include "valve_detection/types.hpp"
+
+namespace valve_detection {
+
+// Iterates depth pixels, back-projects each with depth intrinsics, applies
+// the extrinsic transform, then checks whether the resulting color-frame
+// projection falls inside the elliptic annulus defined by color_bbox.
+// Output points are in the color camera frame.
+void extract_annulus_pcl_aligned(
+    const cv::Mat& depth_image,     // CV_32FC1 metres, depth frame
+    const BoundingBox& color_bbox,  // annulus defined in color pixels
+    const ImageProperties& color_props,
+    const ImageProperties& depth_props,
+    const DepthColorExtrinsic& extrinsic,
+    float annulus_radius_ratio,
+    pcl::PointCloud<pcl::PointXYZ>::Ptr& out);
+
+// Extracts all valid depth points whose color-frame projection falls inside
+// the oriented bounding box. Output points are in the color camera frame.
+void extract_bbox_pcl_aligned(
+    const cv::Mat& depth_image,     // CV_32FC1 metres, depth frame
+    const BoundingBox& color_bbox,  // OBB defined in color pixels
+    const ImageProperties& color_props,
+    const ImageProperties& depth_props,
+    const DepthColorExtrinsic& extrinsic,
+    pcl::PointCloud<pcl::PointXYZ>::Ptr& out);
+
+// Projects the 4 corners of the color OBB into depth image space once, fits
+// an OBB to those projected corners, then tests depth pixels directly against
+// that depth-image OBB — no per-pixel matrix multiply needed. Output points
+// are in the depth camera frame.
+void extract_bbox_pcl_depth(
+    const cv::Mat& depth_image,     // CV_32FC1 metres, depth frame
+    const BoundingBox& color_bbox,  // OBB defined in color pixels
+    const ImageProperties& color_props,
+    const ImageProperties& depth_props,
+    const DepthColorExtrinsic& extrinsic,
+    pcl::PointCloud<pcl::PointXYZ>::Ptr& out);
+
+}  // namespace valve_detection
+
+#endif  // VALVE_DETECTION__PCL_EXTRACTION_HPP_