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

mission/tacc/visual_inspection/valve_detection/CMakeLists.txt

@@ -0,0 +1,82 @@
+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(vortex_msgs REQUIRED)
+find_package(vortex_utils REQUIRED)
+
+include_directories(include)
+
+set(LIB_NAME "${PROJECT_NAME}_component")
+
+add_library(${LIB_NAME} SHARED
+  src/depth_image_processing.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
+  vortex_msgs
+  vortex_utils
+)
+
+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,134 @@
+/**:
+  ros__parameters:
+    # ── Inputs ────────────────────────────────────────────────────────────────
+    # depth_image_sub_topic: "/realsense/D555_409122300281_Depth"
+    depth_image_sub_topic: "/camera/camera/depth/image_rect_raw"
+    detections_sub_topic: "/yolo_obb_object_detection/detections"
+
+    # Depth camera-info topic (optional – used to load depth intrinsics if published).
+    # Using the realsense depth camera_info since the depth image is from the realsense bag.
+    depth_image_info_topic: "/camera/camera/depth/camera_info"
+
+    # Color camera-info topic (optional – overrides the fallback params below if published).
+    color_image_info_topic: "/moby/front_camera/camera_info"
+
+    # Color camera intrinsics – fallback values used when color_image_info_topic is not published.
+    # Source: /camera/camera/color/camera_info
+    color_fx: 452.121521
+    color_fy: 451.737976
+    color_cx: 438.254059
+    color_cy: 248.703217
+    color_image_width: 896
+    color_image_height: 504
+
+    # Distortion coefficients [k1, k2, p1, p2, k3] (plumb_bob model)
+    color_d1: -0.0548105500638485
+    color_d2:  0.0597584918141365
+    color_d3:  0.000486430013552308
+    color_d4:  0.00031599000794813
+    color_d5: -0.0192314591258764
+
+    # Depth camera intrinsics (fallback when depth camera_info is not published)
+    # Source: /camera/camera/depth/camera_info
+    depth_fx: 449.449707
+    depth_fy: 449.449707
+    depth_cx: 444.819946
+    depth_cy: 248.226578
+    depth_image_width: 896
+    depth_image_height: 504
+
+    # ── Depth-to-color extrinsic ──────────────────────────────────────────────
+    # Translation (metres) of the depth origin expressed in the color camera
+    # frame. Rotation is assumed to be identity (both optical frames share the
+    # same orientation for the RealSense D555).
+    # Source: ros2 topic echo /realsense/extrinsics/depth_to_color
+    depth_to_color_tx: -0.059
+    depth_to_color_ty:  0.0
+    depth_to_color_tz:  0.0
+
+    # ── Outputs ───────────────────────────────────────────────────────────────
+    # Primary output consumed by downstream nodes (e.g. EKF, mission planner).
+    landmarks_pub_topic: "/valve_landmarks"
+
+    # ── YOLO letterbox reference size ─────────────────────────────────────────
+    yolo_img_width: 640
+    yolo_img_height: 640
+
+    # ── 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. 50 gives >99 % probability of finding a
+    # good plane when the inlier ratio is ≥ 50 %, while keeping CPU load low.
+    plane_ransac_max_iterations: 50
+
+    # ── 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.065 m.
+    valve_handle_offset: 0.065
+
+    # ── Behaviour toggles ─────────────────────────────────────────────────────
+    debug_visualize: false       # 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"
+
+    # ── Landmark fields ───────────────────────────────────────────────────────
+    landmark_type: 5        # VALVE
+    # subtype is intentionally unset here (0 = UNKNOWN); resolved downstream
+    # by the valve_subtype_resolver node running on the PI.
+
+    # ── Output frame ──────────────────────────────────────────────────────────
+    # 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.
+    # Depth is from the realsense bag, pretending to be moby/depth_camera/image_depth.
+    # See: auv_setup/description/moby.urdf.xacro
+    output_frame_id: "front_camera_depth_optical"
+    input_color_frame_id: "front_camera_color_optical"
+
+    # Prepended to all TF frame names (e.g. "moby" → "moby/front_camera_depth_optical").
+    # Override at launch time to switch between robots: drone:=orca
+    drone: "moby"

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

@@ -0,0 +1,84 @@
+#pragma once
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <opencv2/core.hpp>
+#include <utility>
+#include <vector>
+#include "valve_detection/types.hpp"
+
+namespace valve_detection {
+
+void project_pixel_to_point(const int u,
+                            const int v,
+                            const float depth,
+                            const double fx,
+                            const double fy,
+                            const double cx,
+                            const double cy,
+                            pcl::PointXYZ& out);
+
+void extract_annulus_pcl(
+    const cv::Mat& depth_image,  // CV_32FC1 meters
+    const BoundingBox& bbox,     // in ORIGINAL image pixels
+    const ImageProperties& img_props,
+    const float annulus_radius_ratio,  // inner radius = outer*ratio
+    pcl::PointCloud<pcl::PointXYZ>::Ptr& out);
+
+// 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 meters, depth frame
+    const BoundingBox& color_bbox,  // annulus defined in color pixels
+    const ImageProperties& color_props,
+    const ImageProperties& depth_props,
+    const DepthColorExtrinsic& extrinsic,
+    const 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 meters, 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 stored in the depth camera frame.
+void extract_bbox_pcl_depth(
+    const cv::Mat& depth_image,     // CV_32FC1 meters, 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);
+
+// Project 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(const float u_c,
+                                         const float v_c,
+                                         const float Z,
+                                         const ImageProperties& color_props,
+                                         const ImageProperties& depth_props,
+                                         const DepthColorExtrinsic& extr);
+
+// Returns undistorted copy of bbox (center_x/y corrected for lens distortion).
+BoundingBox undistort_bbox(const BoundingBox& bbox,
+                           const CameraIntrinsics& intr);
+
+// 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

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

@@ -0,0 +1,90 @@
+#pragma once
+
+#include "valve_detection/depth_image_processing.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 <pcl/segmentation/sac_segmentation.h>
+#include <Eigen/Dense>
+#include <cmath>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+
+namespace valve_detection {
+
+class PoseEstimator {
+   public:
+    PoseEstimator(int yolo_img_width,
+                  int yolo_img_height,
+                  float annulus_radius_ratio,
+                  float plane_ransac_threshold,
+                  int plane_ransac_max_iterations,
+                  float valve_handle_offset);
+
+    void set_color_image_properties(const ImageProperties& props);
+    void set_depth_image_properties(const ImageProperties& props);
+    void set_depth_color_extrinsic(const DepthColorExtrinsic& extr);
+
+    void calculate_letterbox_padding();
+    BoundingBox transform_bounding_box(const BoundingBox& bbox) const;
+
+    PoseResult compute_pose_from_depth(
+        const cv::Mat& depth_image,   // CV_32FC1 meters
+        const BoundingBox& bbox_org,  // in original image pixels
+        pcl::PointCloud<pcl::PointXYZ>::Ptr annulus_dbg,
+        pcl::PointCloud<pcl::PointXYZ>::Ptr plane_dbg,
+        bool debug_visualize) const;
+
+   private:
+    bool segment_plane(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
+                       pcl::ModelCoefficients::Ptr& coeff,
+                       pcl::PointIndices::Ptr& inliers) const;
+
+    Eigen::Vector3f get_ray_direction(const BoundingBox& bbox) const;
+    Eigen::Vector3f compute_plane_normal(
+        const pcl::ModelCoefficients::Ptr& coeff,
+        const Eigen::Vector3f& ray_direction) const;
+    Eigen::Vector3f find_ray_plane_intersection(
+        const pcl::ModelCoefficients::Ptr& coeff,
+        const Eigen::Vector3f& ray_direction,
+        const Eigen::Vector3f& ray_origin = Eigen::Vector3f::Zero()) const;
+    Eigen::Vector3f shift_point_along_normal(
+        const Eigen::Vector3f& intersection_point,
+        const Eigen::Vector3f& plane_normal) const;
+    Eigen::Matrix3f create_rotation_matrix(
+        const pcl::ModelCoefficients::Ptr& coefficients,
+        const Eigen::Vector3f& plane_normal,
+        float angle) const;
+    // Variant that works entirely in depth frame: color rays are rotated by
+    // R_dc = R^T before intersecting the depth-frame plane.
+    Eigen::Matrix3f create_rotation_matrix_depth(
+        const pcl::ModelCoefficients::Ptr& coefficients,
+        const Eigen::Vector3f& plane_normal,
+        float angle,
+        const Eigen::Vector3f& ray_origin,
+        const Eigen::Matrix3f& R_dc) const;
+
+    ImageProperties color_image_properties_{};
+    ImageProperties depth_image_properties_{};
+    DepthColorExtrinsic depth_color_extrinsic_{};
+    bool has_depth_props_{false};
+
+    int yolo_img_width_;
+    int yolo_img_height_;
+    float annulus_radius_ratio_;
+    float plane_ransac_threshold_;
+    int plane_ransac_max_iterations_;
+    float valve_handle_offset_;
+
+    double letterbox_scale_factor_{1.0};
+    double letterbox_pad_x_{0};
+    double letterbox_pad_y_{0};
+
+    mutable Eigen::Vector3f filter_direction_{1, 0, 0};
+};
+
+}  // namespace valve_detection