From 16a341653e27267450e69dc0122d0e5df3be8fd8 Mon Sep 17 00:00:00 2001 From: vortex-orin Date: Fri, 14 Feb 2025 19:09:45 +0100 Subject: [PATCH 01/22] now its stable and fixed so the angle is calculated from the line, and x vector chosen to be the vector 90 degrees on z vector and rotated by angle about z vector --- src/valve_detection.cpp | 47 ++++++++++++++++++++++++++++++++--------- 1 file changed, 37 insertions(+), 10 deletions(-) diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp index 9448161..48e0334 100644 --- a/src/valve_detection.cpp +++ b/src/valve_detection.cpp @@ -348,18 +348,37 @@ void ValveDetectionNode::process_and_publish_image( double angle = std::atan2(longest_line[3] - longest_line[1], longest_line[2] - longest_line[0]); - // ANGLE TO QUATERNION WITH RESPECT TO PLANE NORMAL. + Eigen::Vector3f plane_normal(coefficients->values[0], coefficients->values[1], coefficients->values[2]); + plane_normal.normalize(); - // Publish detected line - geometry_msgs::msg::PoseStamped line_pose_msg; - line_pose_msg.header.stamp = depth_image->header.stamp; - line_pose_msg.header.frame_id = "zed_left_camera_frame"; - line_pose_msg.pose.position.x = centroid.x(); - line_pose_msg.pose.position.y = centroid.y(); - line_pose_msg.pose.position.z = centroid.z(); + // Find an arbitrary vector perpendicular to the plane normal + Eigen::Vector3f perp_vector = (std::abs(plane_normal.z()) < 0.99) + ? Eigen::Vector3f(0, 0, 1).cross(plane_normal).normalized() + : Eigen::Vector3f(1, 0, 0).cross(plane_normal).normalized(); + + // Rotate perp_vector by `angle` around `plane_normal` to get x-axis + Eigen::AngleAxisf rotation(angle, plane_normal); + Eigen::Vector3f x_axis = rotation * perp_vector; // Rotated vector in-plane + + // Filter the x-axis direction to maintain consistency + if (filter_direction_.dot(x_axis) < 0) { + x_axis = -x_axis; // Flip to maintain consistency + } + + // Update filter_direction_ + filter_direction_ = x_axis; + + // Compute y-axis (perpendicular to both x and z) + Eigen::Vector3f y_axis = plane_normal.cross(x_axis).normalized(); + + // Construct rotation matrix + Eigen::Matrix3f rotation_matrix; + rotation_matrix.col(0) = x_axis; + rotation_matrix.col(1) = y_axis; + rotation_matrix.col(2) = plane_normal; // z-axis is the plane normal - Eigen::Quaternionf quaternion; - quaternion.setFromTwoVectors(Eigen::Vector3f(1, 0, 0), line_direction.normalized()); + // Convert rotation matrix to quaternion + Eigen::Quaternionf quaternion(rotation_matrix); if (std::isnan(quaternion.x()) || std::isnan(quaternion.y()) || std::isnan(quaternion.z()) || std::isnan(quaternion.w())) { @@ -367,6 +386,14 @@ void ValveDetectionNode::process_and_publish_image( return; } + geometry_msgs::msg::PoseStamped line_pose_msg; + line_pose_msg.header.stamp = color_image->header.stamp; + line_pose_msg.header.frame_id = "zed_left_camera_frame"; + + line_pose_msg.pose.position.x = centroid[0]; + line_pose_msg.pose.position.y = centroid[1]; + line_pose_msg.pose.position.z = centroid[2]; + line_pose_msg.pose.orientation.x = quaternion.x(); line_pose_msg.pose.orientation.y = quaternion.y(); line_pose_msg.pose.orientation.z = quaternion.z(); From 69a8a4205023744290742f29302a80ca808fe07c Mon Sep 17 00:00:00 2001 From: ole Date: Fri, 7 Mar 2025 17:06:08 +0100 Subject: [PATCH 02/22] added docstrings --- include/valve_detection/valve_detection.hpp | 58 +++++ src/valve_detection.cpp | 275 +------------------- 2 files changed, 62 insertions(+), 271 deletions(-) diff --git a/include/valve_detection/valve_detection.hpp b/include/valve_detection/valve_detection.hpp index 78f830c..8818100 100644 --- a/include/valve_detection/valve_detection.hpp +++ b/include/valve_detection/valve_detection.hpp @@ -91,6 +91,11 @@ class ValveDetectionNode : public rclcpp::Node double height_scalar_; double width_scalar_; + /** + * @brief Callback function for synchronized depth image, color image, and 2D detections. + * + * This function is triggered when synchronized messages for a depth image, a color image, and a 2D detection array + */ void synchronized_callback(const sensor_msgs::msg::Image::ConstSharedPtr & depth_image, const sensor_msgs::msg::Image::ConstSharedPtr & color_image, const vision_msgs::msg::Detection2DArray::ConstSharedPtr & detections); @@ -100,11 +105,64 @@ class ValveDetectionNode : public rclcpp::Node // Utility functions + /** + * @brief Computes the height and width scalars based on the camera information. + * + * This function calculates the scaling factors (`height_scalar_` and `width_scalar_`) + * by comparing the dimensions of two camera frames: + * - The YOLO color camera frame (`camera_info_yolo_color_`). + * - The reference camera frame (`camera_info_`). + * + */ void compute_height_width_scalars(); + + /** + * @brief Main logic executed when two images are synchronized. + * + * This function processes synchronized depth and color images along with 2D detections to calculate the angle of the valve handle + * and its orientation in 3D space. It uses RANSAC line detection to determine the handle's angle and RANSAC plane detection + * to find the normal vector of the plane in which the valve handle lies. + * + * The function performs the following steps: + * 1. Converts the depth and color images from ROS messages to OpenCV matrices. + * 2. Selects the most centered detection from the 2D detection array. + * 3. Projects the detected bounding box into 3D space using the depth image and camera intrinsics. + * 4. Uses RANSAC plane detection to find the plane normal vector and filters points near the plane. + * 5. Detects the valve handle's orientation using Hough line detection on the color image. + * 6. Computes the 3D pose of the valve handle and publishes it as a `geometry_msgs::msg::PoseStamped` message. + * + * @param[in] depth_image A shared pointer to the depth image message. + * @param[in] color_image A shared pointer to the color image message. + * @param[in] detections A shared pointer to the 2D detection array message. + * + * @pre The `camera_info_` and `camera_info_yolo_color_` member variables must be initialized with valid camera intrinsic parameters. + * @pre The input images and detections must be synchronized (i.e., they correspond to the same timestamp or frame). + * @post If successful, the function publishes the following: + * - A point cloud of the detected valve handle region. + * - The normal vector of the detected plane. + * - The 3D pose of the valve handle. + * @post If unsuccessful, the function logs warnings or errors and returns early. + * + * @note The function assumes that the input images and detections are synchronized. + * @note The function uses RANSAC for robust plane and line detection, which is suitable for noisy sensor data. + * @note The function handles edge cases such as invalid depth values, empty detections, and failed RANSAC fits. + */ void process_and_publish_image(const sensor_msgs::msg::Image::ConstSharedPtr & depth_image, const sensor_msgs::msg::Image::ConstSharedPtr & color_image, const vision_msgs::msg::Detection2DArray::ConstSharedPtr & detections); + + /** + * @brief Projects a 2D pixel coordinate and depth value into a 3D point in the camera frame. + * + * This function converts a pixel coordinate `(u, v)` and its corresponding depth value into a 3D point + */ void project_pixel_to_3d(int u, int v, float depth, pcl::PointXYZ &point); + + /** + * @brief Projects a 3D point in the camera frame to 2D pixel coordinates. + * + * This function converts a 3D point `(x, y, z)` in the camera frame into 2D pixel coordinates `(u, v)` + */ void project_3d_to_pixel(float x, float y, float z, int &u, int &v); }; diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp index 48e0334..0df9a5f 100644 --- a/src/valve_detection.cpp +++ b/src/valve_detection.cpp @@ -130,7 +130,6 @@ void ValveDetectionNode::synchronized_callback( } - void ValveDetectionNode::process_and_publish_image( const sensor_msgs::msg::Image::ConstSharedPtr & depth_image, const sensor_msgs::msg::Image::ConstSharedPtr & color_image, @@ -153,8 +152,8 @@ void ValveDetectionNode::process_and_publish_image( // The most centred of the two most likely variables are chosen as a boundingbox. // Calculate distance to center (cx, cy) - double cx = camera_info_->k[2]; // k[2] = cx - double cy = camera_info_->k[5]; // k[5] = cy + double cx = camera_info_->k[2]; + double cy = camera_info_->k[5]; if (detections->detections.size() > 1) { float dist_first = std::sqrt(std::pow(detections->detections[0].bbox.center.position.x - cx, 2) + @@ -190,11 +189,7 @@ void ValveDetectionNode::process_and_publish_image( float depth_value = cv_depth_image.at(y, x); pcl::PointXYZ point; project_pixel_to_3d(x, y, depth_value, point); - // if (y == center_y && x == center_x) - // { - // RCLCPP_INFO(this->get_logger(), "center 2d point: (%d, %d, %f)", x, y, depth_value); - // RCLCPP_INFO(this->get_logger(), "Center 3d point: (%.3f, %.3f, %.3f)", point.x, point.y, point.z); - // } + if (depth_value > 0) // Check for valid depth { cloud->points.push_back(point); @@ -421,266 +416,4 @@ void ValveDetectionNode::process_and_publish_image( } } -// void ValveDetectionNode::process_and_publish_image(const sensor_msgs::msg::Image::ConstSharedPtr & image,const vision_msgs::msg::Detection2DArray::ConstSharedPtr & detections) -// { -// std::string target_frame = "zed_left_camera_frame"; -// if (!camera_info_received_ || !camera_info_received_yolo_color_) -// { -// return; -// } - -// try -// { -// cv::Mat cv_image = cv_bridge::toCvCopy(image, "32FC1")->image; - -// if (!detections->detections.empty()) -// { -// const auto* first_detection = &detections->detections[0]; -// // The most centred of the two most likely variables are chosen as a boundingbox. -// // Calculate distance to center (cx, cy) -// double cx = camera_info_->k[2]; // k[2] = cx -// double cy = camera_info_->k[5]; // k[5] = cy -// if (detections->detections.size() > 1) -// { -// float dist_first = std::sqrt(std::pow(detections->detections[0].bbox.center.position.x - cx, 2) + -// std::pow(detections->detections[0].bbox.center.position.y - cy, 2)); - -// float dist_second = std::sqrt(std::pow(detections->detections[1].bbox.center.position.x - cx, 2) + -// std::pow(detections->detections[1].bbox.center.position.y - cy, 2)); - -// // Select the detection that is closer to the center. -// if (dist_first > dist_second) { -// first_detection = &detections->detections[1]; -// } -// } - -// int center_x = static_cast(first_detection->bbox.center.position.x / this->width_scalar_); -// int center_y = static_cast((first_detection->bbox.center.position.y -140) / this->height_scalar_); -// int width = static_cast(first_detection->bbox.size_x / this->width_scalar_); -// int height = static_cast(first_detection->bbox.size_y / this->height_scalar_); - - - -// pcl::PointCloud::Ptr cloud(new pcl::PointCloud); -// bool points_added = false; - -// for (int y = center_y - (height / 2); y <= center_y + (height / 2); ++y) -// { -// for (int x = center_x - (width / 2); x <= center_x + (width / 2); ++x) -// { -// int dx = x - center_x; -// int dy = y - center_y; -// if (dx * dx / (float)((width / 2) * (width / 2)) + dy * dy / (float)((height / 2) * (height / 2)) <= 1.0) -// { -// if (y >= 0 && y < cv_image.rows && x >= 0 && x < cv_image.cols) -// { -// float depth_value = cv_image.at(y, x); -// pcl::PointXYZ point; -// project_pixel_to_3d(x, y, depth_value, point); -// // if (y == center_y && x == center_x) -// // { -// // RCLCPP_INFO(this->get_logger(), "center 2d point: (%d, %d, %f)", x, y, depth_value); -// // RCLCPP_INFO(this->get_logger(), "Center 3d point: (%.3f, %.3f, %.3f)", point.x, point.y, point.z); -// // } -// if (depth_value > 0) // Check for valid depth -// { -// cloud->points.push_back(point); -// points_added = true; -// } -// } -// } -// } -// } - - -// if (points_added && cloud->points.size() > 0) -// { - - - - -// sensor_msgs::msg::PointCloud2 cloud_msg; -// pcl::toROSMsg(*cloud, cloud_msg); -// cloud_msg.header.stamp = image->header.stamp; -// cloud_msg.header.frame_id = target_frame; - -// pointcloud_pub_->publish(cloud_msg); - -// pcl::SACSegmentation seg; -// seg.setOptimizeCoefficients(true); -// seg.setModelType(pcl::SACMODEL_PLANE); -// seg.setMethodType(pcl::SAC_RANSAC); -// seg.setDistanceThreshold(0.01); - -// pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); -// pcl::PointIndices::Ptr inliers(new pcl::PointIndices); -// seg.setInputCloud(cloud); -// seg.segment(*inliers, *coefficients); - -// if (inliers->indices.size() > 0) -// { -// geometry_msgs::msg::Vector3 normal_msg; -// normal_msg.x = coefficients->values[0]; -// normal_msg.y = coefficients->values[1]; -// normal_msg.z = coefficients->values[2]; -// plane_normal_pub_->publish(normal_msg); - -// RCLCPP_INFO(this->get_logger(), "RANSAC successful: Plane normal (%.3f, %.3f, %.3f)", -// coefficients->values[0], coefficients->values[1], coefficients->values[2]); - -// pcl::PointCloud::Ptr near_plane_cloud(new pcl::PointCloud); - -// // Select points within a distance threshold of the plane model -// pcl::ExtractIndices extract; -// extract.setInputCloud(cloud); -// extract.setIndices(inliers); -// extract.setNegative(false); // Select the inliers -// extract.filter(*near_plane_cloud); - - -// // Now we can use `selectWithinDistance` to select inliers that are within the distance threshold -// pcl::PointIndices::Ptr selected_inliers(new pcl::PointIndices()); -// float threshold_distance = 0.02; // Example threshold (3 cm) - -// // Logic for selecting points close to the threshold_distance of the plane. -// for (size_t i = 0; i < cloud->points.size(); ++i) { -// const auto& point = cloud->points[i]; -// float distance = std::abs(coefficients->values[0] * point.x + -// coefficients->values[1] * point.y + -// coefficients->values[2] * point.z + -// coefficients->values[3]) / -// std::sqrt(coefficients->values[0] * coefficients->values[0] + -// coefficients->values[1] * coefficients->values[1] + -// coefficients->values[2] * coefficients->values[2]); -// if (distance < threshold_distance) { -// selected_inliers->indices.push_back(i); -// } -// } -// RCLCPP_INFO(this->get_logger(), "Selected inlier size: %zu", selected_inliers->indices.size()); - -// // Extract the selected inliers -// pcl::PointCloud::Ptr final_inliers_cloud(new pcl::PointCloud); -// pcl::PointCloud::Ptr near_plane_cloud_copy(new pcl::PointCloud); - -// for (const auto& idx : selected_inliers->indices) -// { -// // Convert 3D point (x, y) to image space (u, v) -// int u, v; -// const auto& point = cloud->points[idx]; -// project_3d_to_pixel(point.x, point.y, point.z, u, v); - - -// // Check if the point (u, v) is inside the smaller ellipse -// int dx = u - center_x; -// int dy = v - center_y; - -// // Check if point is inside the smaller ellipse in image space -// float a = static_cast(width) / 6.0f; // Semi-major axis -// float b = static_cast(height) / 6.0f; // Semi-minor axis - -// if ((dx * dx) / (a * a) + (dy * dy) / (b * b) <= 1.0f) { -// // Point is inside the ellipse, add it to the near_plane_cloud -// final_inliers_cloud->points.push_back(cloud->points[idx]); -// } - - -// } - -// near_plane_cloud_copy = near_plane_cloud; -// near_plane_cloud = final_inliers_cloud; - -// RCLCPP_INFO(this->get_logger(), "near_plane_points size: %zu", near_plane_cloud->points.size()); - -// if (near_plane_cloud->points.size() > 0) -// { -// sensor_msgs::msg::PointCloud2 near_plane_cloud_msg; -// pcl::toROSMsg(*near_plane_cloud, near_plane_cloud_msg); -// near_plane_cloud_msg.header.stamp = image->header.stamp; -// near_plane_cloud_msg.header.frame_id = target_frame; -// near_plane_cloud_pub_->publish(near_plane_cloud_msg); // Publish near plane cloud - - -// Eigen::Vector4f centroid; -// pcl::compute3DCentroid(*near_plane_cloud, centroid); - -// Eigen::Matrix3f covariance; -// pcl::computeCovarianceMatrixNormalized(*near_plane_cloud, centroid, covariance); - -// Eigen::SelfAdjointEigenSolver eigen_solver(covariance, Eigen::ComputeEigenvectors); - -// Eigen::Vector3f line_direction = eigen_solver.eigenvectors().col(2); - -// // Filter the line direction to maintain consistency -// if (filter_direction_.dot(line_direction) < 0) { -// line_direction = -line_direction; // Flip to maintain consistency -// } - -// filter_direction_ = line_direction; - -// geometry_msgs::msg::PoseStamped line_pose_msg; -// line_pose_msg.header.stamp = image->header.stamp; -// line_pose_msg.header.frame_id = target_frame; - -// line_pose_msg.pose.position.x = centroid[0]; -// line_pose_msg.pose.position.y = centroid[1]; -// line_pose_msg.pose.position.z = centroid[2]; - -// Eigen::Quaternionf quaternion; -// quaternion.setFromTwoVectors(Eigen::Vector3f(1, 0, 0), line_direction); - -// line_pose_msg.pose.orientation.x = quaternion.x(); -// line_pose_msg.pose.orientation.y = quaternion.y(); -// line_pose_msg.pose.orientation.z = quaternion.z(); -// line_pose_msg.pose.orientation.w = quaternion.w(); - -// line_pose_pub_->publish(line_pose_msg); -// RCLCPP_INFO(this->get_logger(), "Line fitted and published."); - -// // Publish line points as a PointCloud2 message -// pcl::PointCloud::Ptr line_cloud(new pcl::PointCloud); -// const int num_points = 100; // Number of points along the line - -// for (int i = -num_points / 2; i <= num_points / 2; ++i) -// { -// pcl::PointXYZ line_point; -// float scale = i * 0.01; // Adjust the scale for length - -// line_point.x = centroid[0] + scale * line_direction[0]; -// line_point.y = centroid[1] + scale * line_direction[1]; -// line_point.z = centroid[2] + scale * line_direction[2]; -// line_cloud->points.push_back(line_point); -// } - -// // Convert to ROS message and publish -// sensor_msgs::msg::PointCloud2 line_cloud_msg; -// pcl::toROSMsg(*line_cloud, line_cloud_msg); -// line_cloud_msg.header.stamp = image->header.stamp; -// line_cloud_msg.header.frame_id = target_frame; -// line_points_pub_->publish(line_cloud_msg); - -// // RCLCPP_INFO(this->get_logger(), "Line points published as PointCloud2."); -// } -// } -// else -// { -// RCLCPP_WARN(this->get_logger(), "No plane inliers found."); -// } -// } -// else -// { -// RCLCPP_WARN(this->get_logger(), "No valid points found for RANSAC."); -// } -// } - -// auto processed_msg = cv_bridge::CvImage(image->header, "32FC1", cv_image).toImageMsg(); -// processed_image_pub_->publish(*processed_msg); -// } -// catch (cv_bridge::Exception &e) -// { -// RCLCPP_ERROR(this->get_logger(), "CvBridge Error: %s", e.what()); -// } -// } - - -RCLCPP_COMPONENTS_REGISTER_NODE(ValveDetectionNode) - +RCLCPP_COMPONENTS_REGISTER_NODE(ValveDetectionNode) \ No newline at end of file From d95465b9344d9a2171d563fc914590336391d440 Mon Sep 17 00:00:00 2001 From: vortex-orin Date: Fri, 7 Mar 2025 18:45:07 +0100 Subject: [PATCH 03/22] fixed so the image is bublished always, with less lag by placing the publisher outside the sync checks --- package.xml | 3 +-- src/valve_detection.cpp | 39 ++++++++++++++++++++++++++++++++++++++- 2 files changed, 39 insertions(+), 3 deletions(-) diff --git a/package.xml b/package.xml index 7247b06..ea94fd4 100644 --- a/package.xml +++ b/package.xml @@ -16,9 +16,8 @@ vision_msgs geometry_msgs std_msgs - PCL pcl_conversions - opencv + ament_lint_auto ament_lint_common diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp index 0df9a5f..55910ba 100644 --- a/src/valve_detection.cpp +++ b/src/valve_detection.cpp @@ -402,13 +402,50 @@ void ValveDetectionNode::process_and_publish_image( { RCLCPP_WARN(this->get_logger(), "No lines detected in bounding box."); } + if (!cv_color_image.empty()) { + // Draw the bounding box on the color image + int x1 = std::max(center_x - width / 2, 0); + int y1 = std::max(center_y - height / 2, 0); + int x2 = std::min(center_x + width / 2, cv_color_image.cols - 1); + int y2 = std::min(center_y + height / 2, cv_color_image.rows - 1); + + cv::rectangle(cv_color_image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); + + // Draw the detected line on the color image + if (!lines.empty()) { + cv::Vec4i longest_line = lines[0]; + double max_length = 0; + + for (const auto &line : lines) { + double length = std::hypot(line[2] - line[0], line[3] - line[1]); + if (length > max_length) { + max_length = length; + longest_line = line; + } + } + + // Adjust line coordinates to the full image size + cv::Point pt1(longest_line[0] + x1, longest_line[1] + y1); + cv::Point pt2(longest_line[2] + x1, longest_line[3] + y1); + + // Draw the line on the color image + cv::line(cv_color_image, pt1, pt2, cv::Scalar(0, 0, 255), 2); + } + } + } else { RCLCPP_WARN(this->get_logger(), "No plane inliers found."); } - } } + } + // Convert the edited OpenCV image back to a ROS message + sensor_msgs::msg::Image::SharedPtr edited_image_msg = cv_bridge::CvImage(color_image->header, "bgr8", cv_color_image).toImageMsg(); + + // Publish the edited image + processed_image_pub_->publish(*edited_image_msg); + } catch (cv_bridge::Exception &e) { From b54fb3b63bdc0b613258d8690328d6e2323ff8e9 Mon Sep 17 00:00:00 2001 From: vortex-orin Date: Sun, 16 Mar 2025 12:20:12 +0100 Subject: [PATCH 04/22] fixed offset in line plot. TODO: roi is square, should be sircle --- config/valve_detection_params.yaml | 3 +- include/valve_detection/valve_detection.hpp | 2 + src/valve_detection.cpp | 42 ++++++++++++++------- 3 files changed, 33 insertions(+), 14 deletions(-) diff --git a/config/valve_detection_params.yaml b/config/valve_detection_params.yaml index aba82f4..d5de105 100644 --- a/config/valve_detection_params.yaml +++ b/config/valve_detection_params.yaml @@ -3,5 +3,6 @@ depth_image_sub_topic: "/zed_node/depth/depth_registered" color_image_sub_topic: "/image_rect" detections_sub_topic: "/detections_output" - + + line_detection_area: 4 diff --git a/include/valve_detection/valve_detection.hpp b/include/valve_detection/valve_detection.hpp index 8818100..089ad03 100644 --- a/include/valve_detection/valve_detection.hpp +++ b/include/valve_detection/valve_detection.hpp @@ -91,6 +91,8 @@ class ValveDetectionNode : public rclcpp::Node double height_scalar_; double width_scalar_; + int line_detection_area_; + /** * @brief Callback function for synchronized depth image, color image, and 2D detections. * diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp index 55910ba..449d8c7 100644 --- a/src/valve_detection.cpp +++ b/src/valve_detection.cpp @@ -11,6 +11,8 @@ ValveDetectionNode::ValveDetectionNode(const rclcpp::NodeOptions & options) auto color_image_sub_topic = this->declare_parameter("color_image_sub_topic"); auto detections_sub_topic = this->declare_parameter("detections_sub_topic"); + line_detection_area_ = this->declare_parameter("line_detection_area", 5); + rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(10)).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); depth_image_sub_.subscribe(this, depth_image_sub_topic, qos.get_rmw_qos_profile()); color_image_sub_.subscribe(this, color_image_sub_topic, qos.get_rmw_qos_profile()); @@ -32,7 +34,7 @@ ValveDetectionNode::ValveDetectionNode(const rclcpp::NodeOptions & options) processed_image_pub_ = this->create_publisher("yolov8_valve_detection_image", 10); plane_normal_pub_ = this->create_publisher("plane_normal", 10); pointcloud_pub_ = this->create_publisher("point_cloud", 10); - line_pose_pub_ = this->create_publisher("line_pose", 10); + line_pose_pub_ = this->create_publisher("valve_pose", 10); line_points_pub_ = this->create_publisher("line_points", 10); near_plane_cloud_pub_ = this->create_publisher("near_plane_cloud", 10); } @@ -306,10 +308,12 @@ void ValveDetectionNode::process_and_publish_image( return; } - int x1 = std::max(center_x - width / 2, 0); - int y1 = std::max(center_y - height / 2, 0); - int x2 = std::min(center_x + width / 2, cv_color_image.cols - 1); - int y2 = std::min(center_y + height / 2, cv_color_image.rows - 1); + int x1 = std::max(center_x - width * line_detection_area_/10 , 0); + int y1 = std::max(center_y - height * line_detection_area_/10 , 0); + int x2 = std::min(center_x + width * line_detection_area_/10 , cv_color_image.cols - 1); + int y2 = std::min(center_y + height * line_detection_area_/10 , cv_color_image.rows - 1); + // Draw roi for debugging + cv::rectangle(cv_color_image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); // Extract ROI (region of interest) from the color image cv::Mat roi = cv_color_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); @@ -324,7 +328,7 @@ void ValveDetectionNode::process_and_publish_image( // Detect lines using Hough Transform std::vector lines; - cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 50, 50, 10); + cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 20, 40, 10); if (!lines.empty()) { @@ -415,7 +419,8 @@ void ValveDetectionNode::process_and_publish_image( if (!lines.empty()) { cv::Vec4i longest_line = lines[0]; double max_length = 0; - + + // Find the longest line in the ROI for (const auto &line : lines) { double length = std::hypot(line[2] - line[0], line[3] - line[1]); if (length > max_length) { @@ -423,13 +428,24 @@ void ValveDetectionNode::process_and_publish_image( longest_line = line; } } - - // Adjust line coordinates to the full image size - cv::Point pt1(longest_line[0] + x1, longest_line[1] + y1); - cv::Point pt2(longest_line[2] + x1, longest_line[3] + y1); - - // Draw the line on the color image + + // Adjust line coordinates to the full image size using the same x1 ... as roi calculation + int x1 = std::max(center_x - width * line_detection_area_/10 , 0); + int y1 = std::max(center_y - height * line_detection_area_/10 , 0); + int x2 = std::min(center_x + width * line_detection_area_/10 , cv_color_image.cols - 1); + int y2 = std::min(center_y + height * line_detection_area_/10 , cv_color_image.rows - 1); + cv::Point pt1(longest_line[0] + x1, longest_line[1] + y1); // Add ROI offset + cv::Point pt2(longest_line[2] + x1, longest_line[3] + y1); // Add ROI offset + + // Draw the line on the full color image cv::line(cv_color_image, pt1, pt2, cv::Scalar(0, 0, 255), 2); + + // Debugging output + RCLCPP_INFO(this->get_logger(), "ROI Coordinates: x1=%d, y1=%d, x2=%d, y2=%d", x1, y1, x2, y2); + RCLCPP_INFO(this->get_logger(), "Longest Line in ROI: (%d, %d) -> (%d, %d)", + longest_line[0], longest_line[1], longest_line[2], longest_line[3]); + RCLCPP_INFO(this->get_logger(), "Adjusted Line in Full Image: (%d, %d) -> (%d, %d)", + pt1.x, pt1.y, pt2.x, pt2.y); } } From e8babaf75e9b9481d44b228c2846fdbc896793b9 Mon Sep 17 00:00:00 2001 From: vortex-orin Date: Sun, 16 Mar 2025 12:57:37 +0100 Subject: [PATCH 05/22] sphere roi added, could have bad offset, has not ben tested :/ --- src/valve_detection.cpp | 25 ++++++++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp index 449d8c7..f51fb80 100644 --- a/src/valve_detection.cpp +++ b/src/valve_detection.cpp @@ -312,12 +312,31 @@ void ValveDetectionNode::process_and_publish_image( int y1 = std::max(center_y - height * line_detection_area_/10 , 0); int x2 = std::min(center_x + width * line_detection_area_/10 , cv_color_image.cols - 1); int y2 = std::min(center_y + height * line_detection_area_/10 , cv_color_image.rows - 1); - // Draw roi for debugging - cv::rectangle(cv_color_image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); + // // Draw roi for debugging + // cv::rectangle(cv_color_image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); - // Extract ROI (region of interest) from the color image + // // Extract ROI (region of interest) from the color image + // cv::Mat roi = cv_color_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); + + + // Define the ellipse parameters + cv::Point center((x1 + x2) / 2, (y1 + y2) / 2); // Center of the ellipse + cv::Size axes((x2 - x1) / 2, (y2 - y1) / 2); // Semi-major and semi-minor axes + + // Create a mask for the elliptical ROI + cv::Mat mask = cv::Mat::zeros(cv_color_image.size(), CV_8UC1); + cv::ellipse(mask, center, axes, 0, 0, 360, cv::Scalar(255), -1); // Draw a filled ellipse + + // Apply the mask to the ROI cv::Mat roi = cv_color_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); + cv::Mat masked_roi; + cv::bitwise_and(roi, roi, masked_roi, mask(cv::Rect(x1, y1, x2 - x1, y2 - y1))); + + // For debugging: Draw the ellipse on the original image + cv::ellipse(cv_color_image, center, axes, 0, 0, 360, cv::Scalar(0, 255, 0), 2); + + // Convert to grayscale for edge detection cv::Mat gray; cv::cvtColor(roi, gray, cv::COLOR_BGR2GRAY); From 90099b0067dcba0174eb093d709a7f33d64bc1f3 Mon Sep 17 00:00:00 2001 From: vortex-orin Date: Thu, 3 Apr 2025 19:38:37 +0200 Subject: [PATCH 06/22] commit stuff --- src/valve_detection.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp index f51fb80..f953b05 100644 --- a/src/valve_detection.cpp +++ b/src/valve_detection.cpp @@ -308,10 +308,10 @@ void ValveDetectionNode::process_and_publish_image( return; } - int x1 = std::max(center_x - width * line_detection_area_/10 , 0); - int y1 = std::max(center_y - height * line_detection_area_/10 , 0); - int x2 = std::min(center_x + width * line_detection_area_/10 , cv_color_image.cols - 1); - int y2 = std::min(center_y + height * line_detection_area_/10 , cv_color_image.rows - 1); + int x1 = std::max(center_x - width * line_detection_area_/100 , 0); + int y1 = std::max(center_y - height * line_detection_area_/100 , 0); + int x2 = std::min(center_x + width * line_detection_area_/100 , cv_color_image.cols - 1); + int y2 = std::min(center_y + height * line_detection_area_/100 , cv_color_image.rows - 1); // // Draw roi for debugging // cv::rectangle(cv_color_image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); @@ -449,10 +449,10 @@ void ValveDetectionNode::process_and_publish_image( } // Adjust line coordinates to the full image size using the same x1 ... as roi calculation - int x1 = std::max(center_x - width * line_detection_area_/10 , 0); - int y1 = std::max(center_y - height * line_detection_area_/10 , 0); - int x2 = std::min(center_x + width * line_detection_area_/10 , cv_color_image.cols - 1); - int y2 = std::min(center_y + height * line_detection_area_/10 , cv_color_image.rows - 1); + int x1 = std::max(center_x - width * line_detection_area_/100 , 0); + int y1 = std::max(center_y - height * line_detection_area_/100 , 0); + int x2 = std::min(center_x + width * line_detection_area_/100 , cv_color_image.cols - 1); + int y2 = std::min(center_y + height * line_detection_area_/100 , cv_color_image.rows - 1); cv::Point pt1(longest_line[0] + x1, longest_line[1] + y1); // Add ROI offset cv::Point pt2(longest_line[2] + x1, longest_line[3] + y1); // Add ROI offset From 1e93a624dc5d74e4b1e1d2c82b4044075730eea3 Mon Sep 17 00:00:00 2001 From: vortex-orin Date: Fri, 4 Apr 2025 18:12:27 +0200 Subject: [PATCH 07/22] canny edge params update --- config/valve_detection_params.yaml | 2 +- src/valve_detection.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/config/valve_detection_params.yaml b/config/valve_detection_params.yaml index d5de105..3cfa44e 100644 --- a/config/valve_detection_params.yaml +++ b/config/valve_detection_params.yaml @@ -4,5 +4,5 @@ color_image_sub_topic: "/image_rect" detections_sub_topic: "/detections_output" - line_detection_area: 4 + line_detection_area: 25 diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp index f953b05..e5d461f 100644 --- a/src/valve_detection.cpp +++ b/src/valve_detection.cpp @@ -343,7 +343,7 @@ void ValveDetectionNode::process_and_publish_image( // Apply Canny edge detection cv::Mat edges; - cv::Canny(gray, edges, 50, 150, 3); + cv::Canny(gray, edges, 50, 250, 3); // Detect lines using Hough Transform std::vector lines; From d50261d3f57f276cd8205e32f6843282cec5275b Mon Sep 17 00:00:00 2001 From: vortex-orin Date: Sun, 6 Apr 2025 13:21:11 +0200 Subject: [PATCH 08/22] visualisation of canny edge detection, tuned to work at the office. Works well in good ligting, but when it is covered by a shadow the edge detection does not detect shit roflcopter --- include/valve_detection/valve_detection.hpp | 1 + src/valve_detection.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/include/valve_detection/valve_detection.hpp b/include/valve_detection/valve_detection.hpp index 089ad03..f2124eb 100644 --- a/include/valve_detection/valve_detection.hpp +++ b/include/valve_detection/valve_detection.hpp @@ -72,6 +72,7 @@ class ValveDetectionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr line_pose_pub_; rclcpp::Publisher::SharedPtr line_points_pub_; rclcpp::Publisher::SharedPtr near_plane_cloud_pub_; + rclcpp::Publisher::SharedPtr canny_debug_image_pub_; // Stored messages sensor_msgs::msg::CameraInfo::SharedPtr camera_info_; diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp index e5d461f..e23cdcd 100644 --- a/src/valve_detection.cpp +++ b/src/valve_detection.cpp @@ -37,6 +37,8 @@ ValveDetectionNode::ValveDetectionNode(const rclcpp::NodeOptions & options) line_pose_pub_ = this->create_publisher("valve_pose", 10); line_points_pub_ = this->create_publisher("line_points", 10); near_plane_cloud_pub_ = this->create_publisher("near_plane_cloud", 10); + canny_debug_image_pub_ = this->create_publisher("canny_valve_detection_image", 10); + } Eigen::Vector3f ValveDetectionNode::filter_direction_ = Eigen::Vector3f(1, 0, 0); @@ -343,11 +345,12 @@ void ValveDetectionNode::process_and_publish_image( // Apply Canny edge detection cv::Mat edges; - cv::Canny(gray, edges, 50, 250, 3); + cv::Canny(gray, edges, 100, 250, 3); + canny_debug_image_pub_->publish(*cv_bridge::CvImage(color_image->header, "mono8", edges).toImageMsg()); // Detect lines using Hough Transform std::vector lines; - cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 20, 40, 10); + cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 20, 40, 5); if (!lines.empty()) { From fe49ebe8ca4bbce7ec5b674d8082466630793434 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sun, 6 Apr 2025 12:16:45 +0000 Subject: [PATCH 09/22] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- config/valve_detection_params.yaml | 2 +- include/valve_detection/valve_detection.hpp | 99 +-- src/valve_detection.cpp | 674 +++++++++++--------- 3 files changed, 450 insertions(+), 325 deletions(-) diff --git a/config/valve_detection_params.yaml b/config/valve_detection_params.yaml index 87e22bd..41186aa 100644 --- a/config/valve_detection_params.yaml +++ b/config/valve_detection_params.yaml @@ -3,5 +3,5 @@ depth_image_sub_topic: "/zed_node/depth/depth_registered" color_image_sub_topic: "/image_rect" detections_sub_topic: "/detections_output" - + line_detection_area: 25 diff --git a/include/valve_detection/valve_detection.hpp b/include/valve_detection/valve_detection.hpp index aa062b1..06b324f 100644 --- a/include/valve_detection/valve_detection.hpp +++ b/include/valve_detection/valve_detection.hpp @@ -72,10 +72,14 @@ class ValveDetectionNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr processed_image_pub_; rclcpp::Publisher::SharedPtr plane_normal_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; - rclcpp::Publisher::SharedPtr line_pose_pub_; - rclcpp::Publisher::SharedPtr line_points_pub_; - rclcpp::Publisher::SharedPtr near_plane_cloud_pub_; - rclcpp::Publisher::SharedPtr canny_debug_image_pub_; + rclcpp::Publisher::SharedPtr + line_pose_pub_; + rclcpp::Publisher::SharedPtr + line_points_pub_; + rclcpp::Publisher::SharedPtr + near_plane_cloud_pub_; + rclcpp::Publisher::SharedPtr + canny_debug_image_pub_; // Stored messages sensor_msgs::msg::CameraInfo::SharedPtr camera_info_; @@ -98,13 +102,16 @@ class ValveDetectionNode : public rclcpp::Node { int line_detection_area_; /** - * @brief Callback function for synchronized depth image, color image, and 2D detections. + * @brief Callback function for synchronized depth image, color image, and + * 2D detections. * - * This function is triggered when synchronized messages for a depth image, a color image, and a 2D detection array + * This function is triggered when synchronized messages for a depth image, + * a color image, and a 2D detection array */ - void synchronized_callback(const sensor_msgs::msg::Image::ConstSharedPtr & depth_image, - const sensor_msgs::msg::Image::ConstSharedPtr & color_image, - const vision_msgs::msg::Detection2DArray::ConstSharedPtr & detections); + void synchronized_callback( + const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, + const sensor_msgs::msg::Image::ConstSharedPtr& color_image, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); // Callback functions void camera_info_callback( const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg); @@ -113,64 +120,82 @@ class ValveDetectionNode : public rclcpp::Node { // Utility functions /** - * @brief Computes the height and width scalars based on the camera information. + * @brief Computes the height and width scalars based on the camera + * information. * - * This function calculates the scaling factors (`height_scalar_` and `width_scalar_`) - * by comparing the dimensions of two camera frames: + * This function calculates the scaling factors (`height_scalar_` and + * `width_scalar_`) by comparing the dimensions of two camera frames: * - The YOLO color camera frame (`camera_info_yolo_color_`). * - The reference camera frame (`camera_info_`). * - */ + */ void compute_height_width_scalars(); /** * @brief Main logic executed when two images are synchronized. * - * This function processes synchronized depth and color images along with 2D detections to calculate the angle of the valve handle - * and its orientation in 3D space. It uses RANSAC line detection to determine the handle's angle and RANSAC plane detection - * to find the normal vector of the plane in which the valve handle lies. + * This function processes synchronized depth and color images along with 2D + * detections to calculate the angle of the valve handle and its orientation + * in 3D space. It uses RANSAC line detection to determine the handle's + * angle and RANSAC plane detection to find the normal vector of the plane + * in which the valve handle lies. * * The function performs the following steps: - * 1. Converts the depth and color images from ROS messages to OpenCV matrices. + * 1. Converts the depth and color images from ROS messages to OpenCV + * matrices. * 2. Selects the most centered detection from the 2D detection array. - * 3. Projects the detected bounding box into 3D space using the depth image and camera intrinsics. - * 4. Uses RANSAC plane detection to find the plane normal vector and filters points near the plane. - * 5. Detects the valve handle's orientation using Hough line detection on the color image. - * 6. Computes the 3D pose of the valve handle and publishes it as a `geometry_msgs::msg::PoseStamped` message. + * 3. Projects the detected bounding box into 3D space using the depth image + * and camera intrinsics. + * 4. Uses RANSAC plane detection to find the plane normal vector and + * filters points near the plane. + * 5. Detects the valve handle's orientation using Hough line detection on + * the color image. + * 6. Computes the 3D pose of the valve handle and publishes it as a + * `geometry_msgs::msg::PoseStamped` message. * * @param[in] depth_image A shared pointer to the depth image message. * @param[in] color_image A shared pointer to the color image message. * @param[in] detections A shared pointer to the 2D detection array message. * - * @pre The `camera_info_` and `camera_info_yolo_color_` member variables must be initialized with valid camera intrinsic parameters. - * @pre The input images and detections must be synchronized (i.e., they correspond to the same timestamp or frame). + * @pre The `camera_info_` and `camera_info_yolo_color_` member variables + * must be initialized with valid camera intrinsic parameters. + * @pre The input images and detections must be synchronized (i.e., they + * correspond to the same timestamp or frame). * @post If successful, the function publishes the following: * - A point cloud of the detected valve handle region. * - The normal vector of the detected plane. * - The 3D pose of the valve handle. - * @post If unsuccessful, the function logs warnings or errors and returns early. + * @post If unsuccessful, the function logs warnings or errors and returns + * early. * - * @note The function assumes that the input images and detections are synchronized. - * @note The function uses RANSAC for robust plane and line detection, which is suitable for noisy sensor data. - * @note The function handles edge cases such as invalid depth values, empty detections, and failed RANSAC fits. + * @note The function assumes that the input images and detections are + * synchronized. + * @note The function uses RANSAC for robust plane and line detection, which + * is suitable for noisy sensor data. + * @note The function handles edge cases such as invalid depth values, empty + * detections, and failed RANSAC fits. */ - void process_and_publish_image(const sensor_msgs::msg::Image::ConstSharedPtr & depth_image, - const sensor_msgs::msg::Image::ConstSharedPtr & color_image, - const vision_msgs::msg::Detection2DArray::ConstSharedPtr & detections); + void process_and_publish_image( + const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, + const sensor_msgs::msg::Image::ConstSharedPtr& color_image, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); /** - * @brief Projects a 2D pixel coordinate and depth value into a 3D point in the camera frame. + * @brief Projects a 2D pixel coordinate and depth value into a 3D point in + * the camera frame. * - * This function converts a pixel coordinate `(u, v)` and its corresponding depth value into a 3D point - */ - void project_pixel_to_3d(int u, int v, float depth, pcl::PointXYZ &point); + * This function converts a pixel coordinate `(u, v)` and its corresponding + * depth value into a 3D point + */ + void project_pixel_to_3d(int u, int v, float depth, pcl::PointXYZ& point); /** * @brief Projects a 3D point in the camera frame to 2D pixel coordinates. * - * This function converts a 3D point `(x, y, z)` in the camera frame into 2D pixel coordinates `(u, v)` - */ - void project_3d_to_pixel(float x, float y, float z, int &u, int &v); + * This function converts a 3D point `(x, y, z)` in the camera frame into 2D + * pixel coordinates `(u, v)` + */ + void project_3d_to_pixel(float x, float y, float z, int& u, int& v); }; #endif // DETECTION_IMAGE_PROCESSOR_HPP diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp index c30fcba..b8142b2 100644 --- a/src/valve_detection.cpp +++ b/src/valve_detection.cpp @@ -4,22 +4,31 @@ using std::placeholders::_1; using std::placeholders::_2; using std::placeholders::_3; -ValveDetectionNode::ValveDetectionNode(const rclcpp::NodeOptions & options) - : Node("valve_detection_node", options) -{ - auto depth_image_sub_topic = this->declare_parameter("depth_image_sub_topic"); - auto color_image_sub_topic = this->declare_parameter("color_image_sub_topic"); - auto detections_sub_topic = this->declare_parameter("detections_sub_topic"); - - line_detection_area_ = this->declare_parameter("line_detection_area", 5); - - rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(10)).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); - depth_image_sub_.subscribe(this, depth_image_sub_topic, qos.get_rmw_qos_profile()); - color_image_sub_.subscribe(this, color_image_sub_topic, qos.get_rmw_qos_profile()); - detections_sub_.subscribe(this, detections_sub_topic, qos.get_rmw_qos_profile()); - - sync_ = std::make_shared>(MySyncPolicy(10), depth_image_sub_, color_image_sub_, detections_sub_); - sync_->registerCallback(std::bind(&ValveDetectionNode::synchronized_callback, this, _1, _2, _3)); +ValveDetectionNode::ValveDetectionNode(const rclcpp::NodeOptions& options) + : Node("valve_detection_node", options) { + auto depth_image_sub_topic = + this->declare_parameter("depth_image_sub_topic"); + auto color_image_sub_topic = + this->declare_parameter("color_image_sub_topic"); + auto detections_sub_topic = + this->declare_parameter("detections_sub_topic"); + + line_detection_area_ = + this->declare_parameter("line_detection_area", 5); + + rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(10)) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + depth_image_sub_.subscribe(this, depth_image_sub_topic, + qos.get_rmw_qos_profile()); + color_image_sub_.subscribe(this, color_image_sub_topic, + qos.get_rmw_qos_profile()); + detections_sub_.subscribe(this, detections_sub_topic, + qos.get_rmw_qos_profile()); + + sync_ = std::make_shared>( + MySyncPolicy(10), depth_image_sub_, color_image_sub_, detections_sub_); + sync_->registerCallback(std::bind( + &ValveDetectionNode::synchronized_callback, this, _1, _2, _3)); // Create subscriptions with synchronized callbacks camera_info_subscription_yolo_color_ = @@ -29,18 +38,27 @@ ValveDetectionNode::ValveDetectionNode(const rclcpp::NodeOptions & options) this, std::placeholders::_1)); // Create subscriptions with synchronized callbacks - camera_info_subscription_ = this->create_subscription( - "/zed_node/depth/camera_info", 10, - std::bind(&ValveDetectionNode::camera_info_callback, this, std::placeholders::_1)); - - processed_image_pub_ = this->create_publisher("yolov8_valve_detection_image", 10); - plane_normal_pub_ = this->create_publisher("plane_normal", 10); - pointcloud_pub_ = this->create_publisher("point_cloud", 10); - line_pose_pub_ = this->create_publisher("valve_pose", 10); - line_points_pub_ = this->create_publisher("line_points", 10); - near_plane_cloud_pub_ = this->create_publisher("near_plane_cloud", 10); - canny_debug_image_pub_ = this->create_publisher("canny_valve_detection_image", 10); - + camera_info_subscription_ = + this->create_subscription( + "/zed_node/depth/camera_info", 10, + std::bind(&ValveDetectionNode::camera_info_callback, this, + std::placeholders::_1)); + + processed_image_pub_ = this->create_publisher( + "yolov8_valve_detection_image", 10); + plane_normal_pub_ = + this->create_publisher("plane_normal", 10); + pointcloud_pub_ = this->create_publisher( + "point_cloud", 10); + line_pose_pub_ = this->create_publisher( + "valve_pose", 10); + line_points_pub_ = this->create_publisher( + "line_points", 10); + near_plane_cloud_pub_ = + this->create_publisher( + "near_plane_cloud", 10); + canny_debug_image_pub_ = this->create_publisher( + "canny_valve_detection_image", 10); } Eigen::Vector3f ValveDetectionNode::filter_direction_ = @@ -141,36 +159,43 @@ void ValveDetectionNode::synchronized_callback( } void ValveDetectionNode::process_and_publish_image( - const sensor_msgs::msg::Image::ConstSharedPtr & depth_image, - const sensor_msgs::msg::Image::ConstSharedPtr & color_image, - const vision_msgs::msg::Detection2DArray::ConstSharedPtr & detections) -{ - if (!camera_info_received_ || !camera_info_received_yolo_color_) - { + const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, + const sensor_msgs::msg::Image::ConstSharedPtr& color_image, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { + if (!camera_info_received_ || !camera_info_received_yolo_color_) { return; } - try - { + try { // Convert depth and color images - cv::Mat cv_depth_image = cv_bridge::toCvCopy(depth_image, "32FC1")->image; - cv::Mat cv_color_image = cv_bridge::toCvCopy(color_image, "bgr8")->image; - - if (!detections->detections.empty()) - { - const auto *first_detection = &detections->detections[0]; - - // The most centred of the two most likely variables are chosen as a boundingbox. - // Calculate distance to center (cx, cy) - double cx = camera_info_->k[2]; - double cy = camera_info_->k[5]; - if (detections->detections.size() > 1) - { - float dist_first = std::sqrt(std::pow(detections->detections[0].bbox.center.position.x - cx, 2) + - std::pow(detections->detections[0].bbox.center.position.y - cy, 2)); - - float dist_second = std::sqrt(std::pow(detections->detections[1].bbox.center.position.x - cx, 2) + - std::pow(detections->detections[1].bbox.center.position.y - cy, 2)); + cv::Mat cv_depth_image = + cv_bridge::toCvCopy(depth_image, "32FC1")->image; + cv::Mat cv_color_image = + cv_bridge::toCvCopy(color_image, "bgr8")->image; + + if (!detections->detections.empty()) { + const auto* first_detection = &detections->detections[0]; + + // The most centred of the two most likely variables are chosen as a + // boundingbox. Calculate distance to center (cx, cy) + double cx = camera_info_->k[2]; + double cy = camera_info_->k[5]; + if (detections->detections.size() > 1) { + float dist_first = std::sqrt( + std::pow( + detections->detections[0].bbox.center.position.x - cx, + 2) + + std::pow( + detections->detections[0].bbox.center.position.y - cy, + 2)); + + float dist_second = std::sqrt( + std::pow( + detections->detections[1].bbox.center.position.x - cx, + 2) + + std::pow( + detections->detections[1].bbox.center.position.y - cy, + 2)); // Select the detection that is closer to the center. if (dist_first > dist_second) { @@ -178,29 +203,36 @@ void ValveDetectionNode::process_and_publish_image( } } - int center_x = static_cast(first_detection->bbox.center.position.x / this->width_scalar_); - int center_y = static_cast((first_detection->bbox.center.position.y - 140) / this->height_scalar_); - int width = static_cast(first_detection->bbox.size_x / this->width_scalar_); - int height = static_cast(first_detection->bbox.size_y / this->height_scalar_); - - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + int center_x = static_cast( + first_detection->bbox.center.position.x / this->width_scalar_); + int center_y = static_cast( + (first_detection->bbox.center.position.y - 140) / + this->height_scalar_); + int width = static_cast(first_detection->bbox.size_x / + this->width_scalar_); + int height = static_cast(first_detection->bbox.size_y / + this->height_scalar_); + + pcl::PointCloud::Ptr cloud( + new pcl::PointCloud); bool points_added = false; - for (int y = center_y - (height / 2); y <= center_y + (height / 2); ++y) - { - for (int x = center_x - (width / 2); x <= center_x + (width / 2); ++x) - { + for (int y = center_y - (height / 2); y <= center_y + (height / 2); + ++y) { + for (int x = center_x - (width / 2); + x <= center_x + (width / 2); ++x) { int dx = x - center_x; int dy = y - center_y; - if (dx * dx / (float)((width / 2) * (width / 2)) + dy * dy / (float)((height / 2) * (height / 2)) <= 1.0) - { - if (y >= 0 && y < cv_depth_image.rows && x >= 0 && x < cv_depth_image.cols) - { + if (dx * dx / (float)((width / 2) * (width / 2)) + + dy * dy / (float)((height / 2) * (height / 2)) <= + 1.0) { + if (y >= 0 && y < cv_depth_image.rows && x >= 0 && + x < cv_depth_image.cols) { float depth_value = cv_depth_image.at(y, x); pcl::PointXYZ point; project_pixel_to_3d(x, y, depth_value, point); - if (depth_value > 0) // Check for valid depth + if (depth_value > 0) // Check for valid depth { cloud->points.push_back(point); points_added = true; @@ -210,12 +242,7 @@ void ValveDetectionNode::process_and_publish_image( } } - if (points_added && cloud->points.size() > 0) - { - - - - + if (points_added && cloud->points.size() > 0) { sensor_msgs::msg::PointCloud2 cloud_msg; pcl::toROSMsg(*cloud, cloud_msg); cloud_msg.header.stamp = depth_image->header.stamp; @@ -229,270 +256,343 @@ void ValveDetectionNode::process_and_publish_image( seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.01); - pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::ModelCoefficients::Ptr coefficients( + new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); - if (inliers->indices.size() > 0) - { + if (inliers->indices.size() > 0) { geometry_msgs::msg::Vector3 normal_msg; normal_msg.x = coefficients->values[0]; normal_msg.y = coefficients->values[1]; normal_msg.z = coefficients->values[2]; plane_normal_pub_->publish(normal_msg); - RCLCPP_INFO(this->get_logger(), "RANSAC successful: Plane normal (%.3f, %.3f, %.3f)", - coefficients->values[0], coefficients->values[1], coefficients->values[2]); + RCLCPP_INFO( + this->get_logger(), + "RANSAC successful: Plane normal (%.3f, %.3f, %.3f)", + coefficients->values[0], coefficients->values[1], + coefficients->values[2]); - pcl::PointCloud::Ptr near_plane_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr near_plane_cloud( + new pcl::PointCloud); - // Select points within a distance threshold of the plane model + // Select points within a distance threshold of the plane + // model pcl::ExtractIndices extract; extract.setInputCloud(cloud); extract.setIndices(inliers); extract.setNegative(false); // Select the inliers extract.filter(*near_plane_cloud); + // Now we can use `selectWithinDistance` to select inliers + // that are within the distance threshold + pcl::PointIndices::Ptr selected_inliers( + new pcl::PointIndices()); + float threshold_distance = + 0.02; // Example threshold (3 cm) - // Now we can use `selectWithinDistance` to select inliers that are within the distance threshold - pcl::PointIndices::Ptr selected_inliers(new pcl::PointIndices()); - float threshold_distance = 0.02; // Example threshold (3 cm) - - // Logic for selecting points close to the threshold_distance of the plane. + // Logic for selecting points close to the + // threshold_distance of the plane. for (size_t i = 0; i < cloud->points.size(); ++i) { const auto& point = cloud->points[i]; - float distance = std::abs(coefficients->values[0] * point.x + - coefficients->values[1] * point.y + - coefficients->values[2] * point.z + - coefficients->values[3]) / - std::sqrt(coefficients->values[0] * coefficients->values[0] + - coefficients->values[1] * coefficients->values[1] + - coefficients->values[2] * coefficients->values[2]); + float distance = + std::abs(coefficients->values[0] * point.x + + coefficients->values[1] * point.y + + coefficients->values[2] * point.z + + coefficients->values[3]) / + std::sqrt(coefficients->values[0] * + coefficients->values[0] + + coefficients->values[1] * + coefficients->values[1] + + coefficients->values[2] * + coefficients->values[2]); if (distance < threshold_distance) { selected_inliers->indices.push_back(i); } } - RCLCPP_INFO(this->get_logger(), "Selected inlier size: %zu", selected_inliers->indices.size()); + RCLCPP_INFO(this->get_logger(), "Selected inlier size: %zu", + selected_inliers->indices.size()); // Extract the selected inliers - pcl::PointCloud::Ptr final_inliers_cloud(new pcl::PointCloud); - - for (const auto& idx : selected_inliers->indices) - { - // Convert 3D point (x, y) to image space (u, v) - int u, v; - const auto& point = cloud->points[idx]; - project_3d_to_pixel(point.x, point.y, point.z, u, v); - - - // Check if the point (u, v) is inside the smaller ellipse - int dx = u - center_x; - int dy = v - center_y; - - // Check if point is inside the smaller ellipse in image space - float a = static_cast(width) / 6.0f; // Semi-major axis - float b = static_cast(height) / 6.0f; // Semi-minor axis - - if ((dx * dx) / (a * a) + (dy * dy) / (b * b) <= 1.0f) { - // Point is inside the ellipse, add it to the near_plane_cloud - final_inliers_cloud->points.push_back(cloud->points[idx]); - } - - + pcl::PointCloud::Ptr final_inliers_cloud( + new pcl::PointCloud); + + for (const auto& idx : selected_inliers->indices) { + // Convert 3D point (x, y) to image space (u, v) + int u, v; + const auto& point = cloud->points[idx]; + project_3d_to_pixel(point.x, point.y, point.z, u, v); + + // Check if the point (u, v) is inside the smaller + // ellipse + int dx = u - center_x; + int dy = v - center_y; + + // Check if point is inside the smaller ellipse in image + // space + float a = static_cast(width) / + 6.0f; // Semi-major axis + float b = static_cast(height) / + 6.0f; // Semi-minor axis + + if ((dx * dx) / (a * a) + (dy * dy) / (b * b) <= 1.0f) { + // Point is inside the ellipse, add it to the + // near_plane_cloud + final_inliers_cloud->points.push_back( + cloud->points[idx]); + } } near_plane_cloud = final_inliers_cloud; - // Calculate centroid position from near_plane_cloud - Eigen::Vector3f centroid(0, 0, 0); - if (!near_plane_cloud->empty()) { - for (const auto& point : near_plane_cloud->points) { - centroid += Eigen::Vector3f(point.x, point.y, point.z); + // Calculate centroid position from near_plane_cloud + Eigen::Vector3f centroid(0, 0, 0); + if (!near_plane_cloud->empty()) { + for (const auto& point : near_plane_cloud->points) { + centroid += + Eigen::Vector3f(point.x, point.y, point.z); + } + centroid /= near_plane_cloud->points.size(); + } else { + RCLCPP_WARN(this->get_logger(), + "No valid near plane points found."); + return; } - centroid /= near_plane_cloud->points.size(); - } else { - RCLCPP_WARN(this->get_logger(), "No valid near plane points found."); - return; - } - - int x1 = std::max(center_x - width * line_detection_area_/100 , 0); - int y1 = std::max(center_y - height * line_detection_area_/100 , 0); - int x2 = std::min(center_x + width * line_detection_area_/100 , cv_color_image.cols - 1); - int y2 = std::min(center_y + height * line_detection_area_/100 , cv_color_image.rows - 1); - // // Draw roi for debugging - // cv::rectangle(cv_color_image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); - - // // Extract ROI (region of interest) from the color image - // cv::Mat roi = cv_color_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); - - - // Define the ellipse parameters - cv::Point center((x1 + x2) / 2, (y1 + y2) / 2); // Center of the ellipse - cv::Size axes((x2 - x1) / 2, (y2 - y1) / 2); // Semi-major and semi-minor axes - - // Create a mask for the elliptical ROI - cv::Mat mask = cv::Mat::zeros(cv_color_image.size(), CV_8UC1); - cv::ellipse(mask, center, axes, 0, 0, 360, cv::Scalar(255), -1); // Draw a filled ellipse - - // Apply the mask to the ROI - cv::Mat roi = cv_color_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); - cv::Mat masked_roi; - cv::bitwise_and(roi, roi, masked_roi, mask(cv::Rect(x1, y1, x2 - x1, y2 - y1))); - - // For debugging: Draw the ellipse on the original image - cv::ellipse(cv_color_image, center, axes, 0, 0, 360, cv::Scalar(0, 255, 0), 2); + int x1 = std::max( + center_x - width * line_detection_area_ / 100, 0); + int y1 = std::max( + center_y - height * line_detection_area_ / 100, 0); + int x2 = + std::min(center_x + width * line_detection_area_ / 100, + cv_color_image.cols - 1); + int y2 = + std::min(center_y + height * line_detection_area_ / 100, + cv_color_image.rows - 1); + // // Draw roi for debugging + // cv::rectangle(cv_color_image, cv::Point(x1, y1), + // cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); + + // // Extract ROI (region of interest) from the color image + // cv::Mat roi = cv_color_image(cv::Rect(x1, y1, x2 - x1, y2 + // - y1)); + + // Define the ellipse parameters + cv::Point center((x1 + x2) / 2, + (y1 + y2) / 2); // Center of the ellipse + cv::Size axes( + (x2 - x1) / 2, + (y2 - y1) / 2); // Semi-major and semi-minor axes + + // Create a mask for the elliptical ROI + cv::Mat mask = + cv::Mat::zeros(cv_color_image.size(), CV_8UC1); + cv::ellipse(mask, center, axes, 0, 0, 360, cv::Scalar(255), + -1); // Draw a filled ellipse + + // Apply the mask to the ROI + cv::Mat roi = + cv_color_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); + cv::Mat masked_roi; + cv::bitwise_and(roi, roi, masked_roi, + mask(cv::Rect(x1, y1, x2 - x1, y2 - y1))); + + // For debugging: Draw the ellipse on the original image + cv::ellipse(cv_color_image, center, axes, 0, 0, 360, + cv::Scalar(0, 255, 0), 2); + + // Convert to grayscale for edge detection + cv::Mat gray; + cv::cvtColor(roi, gray, cv::COLOR_BGR2GRAY); + + // Apply Canny edge detection + cv::Mat edges; + cv::Canny(gray, edges, 100, 250, 3); + canny_debug_image_pub_->publish( + *cv_bridge::CvImage(color_image->header, "mono8", edges) + .toImageMsg()); + + // Detect lines using Hough Transform + std::vector lines; + cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 20, 40, 5); - - // Convert to grayscale for edge detection - cv::Mat gray; - cv::cvtColor(roi, gray, cv::COLOR_BGR2GRAY); - - // Apply Canny edge detection - cv::Mat edges; - cv::Canny(gray, edges, 100, 250, 3); - canny_debug_image_pub_->publish(*cv_bridge::CvImage(color_image->header, "mono8", edges).toImageMsg()); - - // Detect lines using Hough Transform - std::vector lines; - cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 20, 40, 5); - - if (!lines.empty()) - { - cv::Vec4i longest_line = lines[0]; - double max_length = 0; + if (!lines.empty()) { + cv::Vec4i longest_line = lines[0]; + double max_length = 0; - for (const auto &line : lines) - { - double length = std::hypot(line[2] - line[0], line[3] - line[1]); - if (length > max_length) - { - max_length = length; - longest_line = line; + for (const auto& line : lines) { + double length = std::hypot(line[2] - line[0], + line[3] - line[1]); + if (length > max_length) { + max_length = length; + longest_line = line; + } } - } - - double angle = std::atan2(longest_line[3] - longest_line[1], longest_line[2] - longest_line[0]); - Eigen::Vector3f plane_normal(coefficients->values[0], coefficients->values[1], coefficients->values[2]); - plane_normal.normalize(); - - // Find an arbitrary vector perpendicular to the plane normal - Eigen::Vector3f perp_vector = (std::abs(plane_normal.z()) < 0.99) - ? Eigen::Vector3f(0, 0, 1).cross(plane_normal).normalized() - : Eigen::Vector3f(1, 0, 0).cross(plane_normal).normalized(); + double angle = + std::atan2(longest_line[3] - longest_line[1], + longest_line[2] - longest_line[0]); + + Eigen::Vector3f plane_normal(coefficients->values[0], + coefficients->values[1], + coefficients->values[2]); + plane_normal.normalize(); + + // Find an arbitrary vector perpendicular to the plane + // normal + Eigen::Vector3f perp_vector = + (std::abs(plane_normal.z()) < 0.99) + ? Eigen::Vector3f(0, 0, 1) + .cross(plane_normal) + .normalized() + : Eigen::Vector3f(1, 0, 0) + .cross(plane_normal) + .normalized(); + + // Rotate perp_vector by `angle` around `plane_normal` + // to get x-axis + Eigen::AngleAxisf rotation(angle, plane_normal); + Eigen::Vector3f x_axis = + rotation * perp_vector; // Rotated vector in-plane + + // Filter the x-axis direction to maintain consistency + if (filter_direction_.dot(x_axis) < 0) { + x_axis = -x_axis; // Flip to maintain consistency + } - // Rotate perp_vector by `angle` around `plane_normal` to get x-axis - Eigen::AngleAxisf rotation(angle, plane_normal); - Eigen::Vector3f x_axis = rotation * perp_vector; // Rotated vector in-plane + // Update filter_direction_ + filter_direction_ = x_axis; + + // Compute y-axis (perpendicular to both x and z) + Eigen::Vector3f y_axis = + plane_normal.cross(x_axis).normalized(); + + // Construct rotation matrix + Eigen::Matrix3f rotation_matrix; + rotation_matrix.col(0) = x_axis; + rotation_matrix.col(1) = y_axis; + rotation_matrix.col(2) = + plane_normal; // z-axis is the plane normal + + // Convert rotation matrix to quaternion + Eigen::Quaternionf quaternion(rotation_matrix); + + if (std::isnan(quaternion.x()) || + std::isnan(quaternion.y()) || + std::isnan(quaternion.z()) || + std::isnan(quaternion.w())) { + RCLCPP_WARN(this->get_logger(), + "Invalid quaternion computed, skipping " + "pose publishing."); + return; + } - // Filter the x-axis direction to maintain consistency - if (filter_direction_.dot(x_axis) < 0) { - x_axis = -x_axis; // Flip to maintain consistency - } + geometry_msgs::msg::PoseStamped line_pose_msg; + line_pose_msg.header.stamp = color_image->header.stamp; + line_pose_msg.header.frame_id = "zed_left_camera_frame"; - // Update filter_direction_ - filter_direction_ = x_axis; - - // Compute y-axis (perpendicular to both x and z) - Eigen::Vector3f y_axis = plane_normal.cross(x_axis).normalized(); + line_pose_msg.pose.position.x = centroid[0]; + line_pose_msg.pose.position.y = centroid[1]; + line_pose_msg.pose.position.z = centroid[2]; - // Construct rotation matrix - Eigen::Matrix3f rotation_matrix; - rotation_matrix.col(0) = x_axis; - rotation_matrix.col(1) = y_axis; - rotation_matrix.col(2) = plane_normal; // z-axis is the plane normal + line_pose_msg.pose.orientation.x = quaternion.x(); + line_pose_msg.pose.orientation.y = quaternion.y(); + line_pose_msg.pose.orientation.z = quaternion.z(); + line_pose_msg.pose.orientation.w = quaternion.w(); - // Convert rotation matrix to quaternion - Eigen::Quaternionf quaternion(rotation_matrix); + line_pose_pub_->publish(line_pose_msg); - if (std::isnan(quaternion.x()) || std::isnan(quaternion.y()) || - std::isnan(quaternion.z()) || std::isnan(quaternion.w())) { - RCLCPP_WARN(this->get_logger(), "Invalid quaternion computed, skipping pose publishing."); - return; + RCLCPP_INFO(this->get_logger(), + "Detected line published."); + } else { + RCLCPP_WARN(this->get_logger(), + "No lines detected in bounding box."); } - - geometry_msgs::msg::PoseStamped line_pose_msg; - line_pose_msg.header.stamp = color_image->header.stamp; - line_pose_msg.header.frame_id = "zed_left_camera_frame"; - - line_pose_msg.pose.position.x = centroid[0]; - line_pose_msg.pose.position.y = centroid[1]; - line_pose_msg.pose.position.z = centroid[2]; - - line_pose_msg.pose.orientation.x = quaternion.x(); - line_pose_msg.pose.orientation.y = quaternion.y(); - line_pose_msg.pose.orientation.z = quaternion.z(); - line_pose_msg.pose.orientation.w = quaternion.w(); - - line_pose_pub_->publish(line_pose_msg); - - RCLCPP_INFO(this->get_logger(), "Detected line published."); - } - else - { - RCLCPP_WARN(this->get_logger(), "No lines detected in bounding box."); - } - if (!cv_color_image.empty()) { - // Draw the bounding box on the color image - int x1 = std::max(center_x - width / 2, 0); - int y1 = std::max(center_y - height / 2, 0); - int x2 = std::min(center_x + width / 2, cv_color_image.cols - 1); - int y2 = std::min(center_y + height / 2, cv_color_image.rows - 1); - - cv::rectangle(cv_color_image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); - - // Draw the detected line on the color image - if (!lines.empty()) { - cv::Vec4i longest_line = lines[0]; - double max_length = 0; - - // Find the longest line in the ROI - for (const auto &line : lines) { - double length = std::hypot(line[2] - line[0], line[3] - line[1]); - if (length > max_length) { - max_length = length; - longest_line = line; + if (!cv_color_image.empty()) { + // Draw the bounding box on the color image + int x1 = std::max(center_x - width / 2, 0); + int y1 = std::max(center_y - height / 2, 0); + int x2 = std::min(center_x + width / 2, + cv_color_image.cols - 1); + int y2 = std::min(center_y + height / 2, + cv_color_image.rows - 1); + + cv::rectangle(cv_color_image, cv::Point(x1, y1), + cv::Point(x2, y2), cv::Scalar(0, 255, 0), + 2); + + // Draw the detected line on the color image + if (!lines.empty()) { + cv::Vec4i longest_line = lines[0]; + double max_length = 0; + + // Find the longest line in the ROI + for (const auto& line : lines) { + double length = std::hypot(line[2] - line[0], + line[3] - line[1]); + if (length > max_length) { + max_length = length; + longest_line = line; + } } + + // Adjust line coordinates to the full image size + // using the same x1 ... as roi calculation + int x1 = std::max( + center_x - width * line_detection_area_ / 100, + 0); + int y1 = std::max( + center_y - height * line_detection_area_ / 100, + 0); + int x2 = std::min( + center_x + width * line_detection_area_ / 100, + cv_color_image.cols - 1); + int y2 = std::min( + center_y + height * line_detection_area_ / 100, + cv_color_image.rows - 1); + cv::Point pt1( + longest_line[0] + x1, + longest_line[1] + y1); // Add ROI offset + cv::Point pt2( + longest_line[2] + x1, + longest_line[3] + y1); // Add ROI offset + + // Draw the line on the full color image + cv::line(cv_color_image, pt1, pt2, + cv::Scalar(0, 0, 255), 2); + + // Debugging output + RCLCPP_INFO( + this->get_logger(), + "ROI Coordinates: x1=%d, y1=%d, x2=%d, y2=%d", + x1, y1, x2, y2); + RCLCPP_INFO( + this->get_logger(), + "Longest Line in ROI: (%d, %d) -> (%d, %d)", + longest_line[0], longest_line[1], + longest_line[2], longest_line[3]); + RCLCPP_INFO(this->get_logger(), + "Adjusted Line in Full Image: (%d, %d) " + "-> (%d, %d)", + pt1.x, pt1.y, pt2.x, pt2.y); } - - // Adjust line coordinates to the full image size using the same x1 ... as roi calculation - int x1 = std::max(center_x - width * line_detection_area_/100 , 0); - int y1 = std::max(center_y - height * line_detection_area_/100 , 0); - int x2 = std::min(center_x + width * line_detection_area_/100 , cv_color_image.cols - 1); - int y2 = std::min(center_y + height * line_detection_area_/100 , cv_color_image.rows - 1); - cv::Point pt1(longest_line[0] + x1, longest_line[1] + y1); // Add ROI offset - cv::Point pt2(longest_line[2] + x1, longest_line[3] + y1); // Add ROI offset - - // Draw the line on the full color image - cv::line(cv_color_image, pt1, pt2, cv::Scalar(0, 0, 255), 2); - - // Debugging output - RCLCPP_INFO(this->get_logger(), "ROI Coordinates: x1=%d, y1=%d, x2=%d, y2=%d", x1, y1, x2, y2); - RCLCPP_INFO(this->get_logger(), "Longest Line in ROI: (%d, %d) -> (%d, %d)", - longest_line[0], longest_line[1], longest_line[2], longest_line[3]); - RCLCPP_INFO(this->get_logger(), "Adjusted Line in Full Image: (%d, %d) -> (%d, %d)", - pt1.x, pt1.y, pt2.x, pt2.y); } - + } else { + RCLCPP_WARN(this->get_logger(), "No plane inliers found."); } } - else - { - RCLCPP_WARN(this->get_logger(), "No plane inliers found."); - } } - } - // Convert the edited OpenCV image back to a ROS message - sensor_msgs::msg::Image::SharedPtr edited_image_msg = cv_bridge::CvImage(color_image->header, "bgr8", cv_color_image).toImageMsg(); + // Convert the edited OpenCV image back to a ROS message + sensor_msgs::msg::Image::SharedPtr edited_image_msg = + cv_bridge::CvImage(color_image->header, "bgr8", cv_color_image) + .toImageMsg(); - // Publish the edited image - processed_image_pub_->publish(*edited_image_msg); + // Publish the edited image + processed_image_pub_->publish(*edited_image_msg); - } - catch (cv_bridge::Exception &e) - { + } catch (cv_bridge::Exception& e) { RCLCPP_ERROR(this->get_logger(), "CvBridge Error: %s", e.what()); } } From 164af43d944599322fddfb76f2d7f15a00db5197 Mon Sep 17 00:00:00 2001 From: vortex-orin Date: Sun, 13 Apr 2025 14:16:32 +0200 Subject: [PATCH 10/22] fixed compile error --- src/valve_detection.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp index b8142b2..b14388b 100644 --- a/src/valve_detection.cpp +++ b/src/valve_detection.cpp @@ -154,8 +154,8 @@ void ValveDetectionNode::synchronized_callback( RCLCPP_INFO(this->get_logger(), "Synchronized image and detection messages received."); - // process_and_publish_image(depth_image, color_image, detections); - process_and_publish_image(depth_image, detections); + process_and_publish_image(depth_image, color_image, detections); + // process_and_publish_image(depth_image, detections); } void ValveDetectionNode::process_and_publish_image( @@ -409,14 +409,14 @@ void ValveDetectionNode::process_and_publish_image( // Apply Canny edge detection cv::Mat edges; - cv::Canny(gray, edges, 100, 250, 3); + cv::Canny(gray, edges, 50, 200, 3); canny_debug_image_pub_->publish( *cv_bridge::CvImage(color_image->header, "mono8", edges) .toImageMsg()); // Detect lines using Hough Transform std::vector lines; - cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 20, 40, 5); + cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 20, 50, 5); if (!lines.empty()) { cv::Vec4i longest_line = lines[0]; From 08de1b7dcf35807333ccc79f7c33f0c1a31d8b2c Mon Sep 17 00:00:00 2001 From: ole Date: Sun, 13 Apr 2025 15:40:54 +0200 Subject: [PATCH 11/22] tuned canny --- config/valve_detection_params.yaml | 2 +- launch/valve_detection.launch.py | 3 ++- src/valve_detection.cpp | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/config/valve_detection_params.yaml b/config/valve_detection_params.yaml index 41186aa..d0d4299 100644 --- a/config/valve_detection_params.yaml +++ b/config/valve_detection_params.yaml @@ -4,4 +4,4 @@ color_image_sub_topic: "/image_rect" detections_sub_topic: "/detections_output" - line_detection_area: 25 + line_detection_area: 22 diff --git a/launch/valve_detection.launch.py b/launch/valve_detection.launch.py index 1bacfd0..0ea4591 100644 --- a/launch/valve_detection.launch.py +++ b/launch/valve_detection.launch.py @@ -16,7 +16,8 @@ def generate_launch_description(): get_package_share_directory('valve_detection'), 'config', 'valve_detection_params.yaml', - ) + ), + {'use_sim_time': True}, ], output='screen', ) diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp index b14388b..482751f 100644 --- a/src/valve_detection.cpp +++ b/src/valve_detection.cpp @@ -409,14 +409,14 @@ void ValveDetectionNode::process_and_publish_image( // Apply Canny edge detection cv::Mat edges; - cv::Canny(gray, edges, 50, 200, 3); + cv::Canny(gray, edges, 50, 100, 3); canny_debug_image_pub_->publish( *cv_bridge::CvImage(color_image->header, "mono8", edges) .toImageMsg()); // Detect lines using Hough Transform std::vector lines; - cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 20, 50, 5); + cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 20, 45, 5); if (!lines.empty()) { cv::Vec4i longest_line = lines[0]; From 410219db319916d41ff645f485171bf959594bf3 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 13 Apr 2025 22:14:28 +0200 Subject: [PATCH 12/22] subscribe zed color image directly --- config/valve_detection_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/valve_detection_params.yaml b/config/valve_detection_params.yaml index d0d4299..185d440 100644 --- a/config/valve_detection_params.yaml +++ b/config/valve_detection_params.yaml @@ -1,7 +1,7 @@ /**: 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" detections_sub_topic: "/detections_output" line_detection_area: 22 From 04e1d7a1dfebe9eedc8e723aa96b52f2088470f7 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 20 Aug 2025 22:13:07 +0200 Subject: [PATCH 13/22] base and depth classes --- CMakeLists.txt | 1 - config/depth_image_angle_params.yaml | 27 +++ include/valve_detection/types.hpp | 38 ++++ include/valve_detection/valve_pose_base.hpp | 103 +++++++++ include/valve_detection/valve_pose_depth.hpp | 35 +++ .../valve_pose_depth_ros.hpp | 76 +++++++ src/valve_pose_base.cpp | 181 +++++++++++++++ src/valve_pose_depth.cpp | 82 +++++++ src/valve_pose_depth_ros.cpp | 209 ++++++++++++++++++ src/valve_pose_pcl.cpp | 0 10 files changed, 751 insertions(+), 1 deletion(-) create mode 100644 config/depth_image_angle_params.yaml create mode 100644 include/valve_detection/types.hpp create mode 100644 include/valve_detection/valve_pose_base.hpp create mode 100644 include/valve_detection/valve_pose_depth.hpp create mode 100644 include/valve_detection_ros/valve_pose_depth_ros.hpp create mode 100644 src/valve_pose_base.cpp create mode 100644 src/valve_pose_depth.cpp create mode 100644 src/valve_pose_depth_ros.cpp create mode 100644 src/valve_pose_pcl.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 587d063..de59d4f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,7 +49,6 @@ ament_target_dependencies(${LIB_NAME} PUBLIC vision_msgs geometry_msgs std_msgs - PCL pcl_conversions ) diff --git a/config/depth_image_angle_params.yaml b/config/depth_image_angle_params.yaml new file mode 100644 index 0000000..bc3d5f3 --- /dev/null +++ b/config/depth_image_angle_params.yaml @@ -0,0 +1,27 @@ +/**: + ros__parameters: + depth_image_sub_topic: "/zed_node/depth/depth_registered" + color_image_sub_topic: "/zed_node/left/image_rect_color" + detections_sub_topic: "/detections_output" + + color_image_info_topic: "/zed_node/left/camera_info" + depth_image_info_topic: "/zed_node/depth/camera_info" + + valve_pose_pub_topic: "/valve_pose" + + 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 + + calculate_angle: true + + visualize_detections: true + debug_visualize: false + annulus_pub_topic: "/bbx_annulus_pcl" + plane_normal_pub_topic: "/plane_normal" diff --git a/include/valve_detection/types.hpp b/include/valve_detection/types.hpp new file mode 100644 index 0000000..b42082c --- /dev/null +++ b/include/valve_detection/types.hpp @@ -0,0 +1,38 @@ +#ifndef TYPES_HPP +#define TYPES_HPP + +namespace valve_detection { + +struct CameraIntrinsics { + double fx; + double fy; + double cx; + double cy; +}; + +struct ImageDimensions { + int width; + int height; +}; + +struct ImageProperties { + CameraIntrinsics intr; + ImageDimensions dim; +}; + +struct BoundingBox { + float center_x; + float center_y; + float size_x; + float size_y; + float theta; // Orientation in radians positive counter-clockwise +} + +enum class AngleDetection { + ENABLED, + DISABLED +}; + +} // namespace valve_detection + +#endif // TYPES_HPP \ No newline at end of file diff --git a/include/valve_detection/valve_pose_base.hpp b/include/valve_detection/valve_pose_base.hpp new file mode 100644 index 0000000..f40e1a1 --- /dev/null +++ b/include/valve_detection/valve_pose_base.hpp @@ -0,0 +1,103 @@ +#ifndef VALVE_POSE_BASE_HPP +#define VALVE_POSE_BASE_HPP + +#include "valve_detection/types.hpp" +#include + +namespace valve_detection { + +class ValvePoseBase { +public: + ValvePoseBase(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); + virtual ~ValvePoseBase() = default; + + void project_point_to_pixel( + float x, float y, float z, int& u, int& v) const; + + void calculate_letterbox_properties(); + + BoundingBox transform_bounding_box( + const BoundingBox& bbox) const; + + void set_color_image_properties( + const ImageProperties& properties) { + color_image_properties_ = properties; + } + + /** + * @brief Segments the dominant plane from a point cloud using RANSAC. + * @param cloud The input point cloud. + * @param coefficients The output model coefficients (ax + by + cz + d = 0). + * @return True if a plane with inliers was found, false otherwise. + */ + bool segment_plane(const pcl::PointCloud::Ptr& cloud, + pcl::ModelCoefficients::Ptr& coefficients, + pcl::PointIndices::Ptr& inliers) const; + + Eigen::Vector3f get_ray_direction(const BoundingBox& bbox) const; + + /** + * @brief Creates a consistent, normalized Eigen::Vector3f from PCL plane coefficients + * and a reference ray. + * + * This function extracts the normal vector (A, B, C) from the plane equation + * Ax + By + Cz + D = 0, normalizes it, and flips its direction if necessary + * to ensure it always points in the same general direction as the reference ray. + * + * @param coefficients A shared pointer to the pcl::ModelCoefficients object. + * @param ray_direction A reference to the Eigen::Vector3f representing the ray. + * @return A normalized Eigen::Vector3f representing the consistent plane normal. + */ + Eigen::Vector3f create_consistent_plane_normal_with_ray( + const pcl::ModelCoefficients::Ptr& coefficients, + const Eigen::Vector3f& ray_direction) const; + + + /** + * @brief Finds the intersection point of a ray and a plane. + * + * The ray is defined by its origin (assumed to be the camera's optical center, + * i.e., [0, 0, 0]) and a direction vector. The plane is defined by its coefficients + * Ax + By + Cz + D = 0. + * + * @param coefficients A shared pointer to the pcl::ModelCoefficients object. + * @param ray_direction The normalized Eigen::Vector3f ray direction. + * @return An Eigen::Vector3f representing the 3D intersection point. + */ + Eigen::Vector3f find_ray_plane_intersection( + const pcl::ModelCoefficients::Ptr& coefficients, + const Eigen::Vector3f& ray_direction) const; + + /** + * @brief Shifts a point along a plane normal by a given distance. + * + * This function is useful for moving a point from the detected plane's + * surface to a new location, such as the center of a valve knob. + * The shift is performed along the vector of the consistent plane normal. + * + * @param intersection_point The 3D intersection point on the plane. + * @param plane_normal The consistent, normalized plane normal vector. + * @return The new Eigen::Vector3f point after the shift. + */ + Eigen::Vector3f shift_point_along_normal( + const Eigen::Vector3f& intersection_point, + const Eigen::Vector3f& plane_normal) const; + +protected: + ImageProperties color_image_properties_; + 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_; + float letterbox_scale_factor_; + int letterbox_pad_x_; + int letterbox_pad_y_; +} +} // namespace valve_detection + +#endif // VALVE_POSE_BASE_HPP \ No newline at end of file diff --git a/include/valve_detection/valve_pose_depth.hpp b/include/valve_detection/valve_pose_depth.hpp new file mode 100644 index 0000000..f826a4c --- /dev/null +++ b/include/valve_detection/valve_pose_depth.hpp @@ -0,0 +1,35 @@ +#ifndef VALVE_POSE_DEPTH_HPP +#define VALVE_POSE_DEPTH_HPP + +#include "valve_detection/valve_pose_base.hpp" +#include +#include +#include + +namespace valve_detection { + +class ValvePoseDepth : public ValvePoseBase { +public: + ValvePoseDepth(int yolo_img_width, int yolo_img_height_, float annulus_radius_ratio, + float plane_ransac_threshold, int plane_ransac_max_iterations); + virtual ~ValvePoseDepth() = default; + + void project_pixel_to_point(int u, + int v, + float depth, + pcl::PointXYZ& point) const; + + void extract_annulus_pcl( + const cv::Mat& depth_image, + const BoundingBox& bbox, + pcl::PointCloud::Ptr& cloud) const; + +private: + ImageProperties depth_image_properties_; + double height_scalar_; + double width_scalar_; +}; + +} // namespace valve_detection + +#endif // VALVE_POSE_DEPTH_HPP \ No newline at end of file diff --git a/include/valve_detection_ros/valve_pose_depth_ros.hpp b/include/valve_detection_ros/valve_pose_depth_ros.hpp new file mode 100644 index 0000000..8e5ce9f --- /dev/null +++ b/include/valve_detection_ros/valve_pose_depth_ros.hpp @@ -0,0 +1,76 @@ +#ifndef VALVE_POSE_DEPTH_ROS_HPP +#define VALVE_POSE_DEPTH_ROS_HPP + +#include "valve_detection/valve_pose_depth.hpp" +#include +#include +#include +#include + +#include +#include +#include + + +namespace valve_detection { + +class ValvePoseDepthNode { +public: + explicit ValvePoseDepthNode(const rclcpp::NodeOptions& options); + virtual ~ValvePoseDepthNode() = default; + + +private: + void color_image_info_callback( + const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg); + + BoundingBox to_bounding_box(const vision_msgs::msg::BoundingBox2D& bbox) const; + + void publish_annulus_pcl(const pcl::PointCloud::Ptr& cloud) const; + + /** + * @brief Callback function for synchronized depth image, color image, and + * 2D detections. + * + * This function is triggered when synchronized messages for a depth image, + * a color image, and a 2D detection array are received. + */ + void synchronized_callback( + const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, + const sensor_msgs::msg::Image::ConstSharedPtr& color_image, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); + + + rclcpp::Subscription::SharedPtr color_image_info_sub_; + bool color_image_info_received_ = false; + + message_filters::Subscriber depth_image_sub_; + message_filters::Subscriber color_image_sub_; + message_filters::Subscriber + detections_sub_; + + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::msg::Image, + sensor_msgs::msg::Image, + vision_msgs::msg::Detection2DArray> + MySyncPolicy; + + std::shared_ptr> sync_; + + rclcpp::Publisher::SharedPtr + valve_pose_pub_; + + std::unique_ptr valve_detector_; + + std::string color_image_frame_id_; + + bool visualize_detections_ = true; + bool debug_visualize_ = false; + bool calculate_angle_ = true; + rclcpp::Publisher::SharedPtr annulus_pcl_pub_; + rclcpp::Publisher::SharedPtr plane_normal_pub_; +} + +} // namespace valve_detection + +#endif // VALVE_POSE_DEPTH_ROS_HPP \ No newline at end of file diff --git a/src/valve_pose_base.cpp b/src/valve_pose_base.cpp new file mode 100644 index 0000000..b780d0d --- /dev/null +++ b/src/valve_pose_base.cpp @@ -0,0 +1,181 @@ +#include "valve_detection/valve_pose_base.hpp" + +namespace valve_detection { + +ValvePoseBase::ValvePoseBase(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) + : yolo_img_width_(yolo_img_width_), yolo_img_height_(yolo_img_height_), + annulus_radius_ratio_(annulus_radius_ratio), + plane_ransac_threshold_(plane_ransac_threshold), + plane_ransac_max_iterations_(plane_ransac_max_iterations), + valve_handle_offset_(valve_handle_offset) { +} + +void ValvePoseBase::project_point_to_pixel(float x, float y, float z, int& u, int& v) const { + if (z <= 0) { + u = v = -1; // Invalid pixel coordinates + return; + } + + double fx = color_image_properties_.intr.fx; + double fy = color_image_properties_.intr.fy; + double cx = color_image_properties_.intr.cx; + double cy = color_image_properties_.intr.cy; + + u = static_cast((x * fx / z) + cx); + v = static_cast((y * fy / z) + cy); +} + +void ValvePoseBase::calculate_letterbox_padding() { + int org_image_width = color_image_properties_.dim.x; + int org_image_height = color_image_properties_.dim.y; + + letterbox_scale_factor_ = std::min( + static_cast(yolo_input_width) / original_width, + static_cast(yolo_input_height) / original_height); + + double resized_width = original_width * letterbox_scale_factor_; + double resized_height = original_height * letterbox_scale_factor_; + + letterbox_pad_x_ = (yolo_input_width - resized_width) / 2.0; + letterbox_pad_y_ = (yolo_input_height - resized_height) / 2.0; +} + +BoundingBox ValvePoseBase::transform_bounding_box(const BoundingBox& bbox) const { + BoundingBox transformed_bbox = bbox; + + transformed_bbox.center.position.x = (bbox.center_x - letterbox_pad_x_) / letterbox_scale_factor_; + transformed_bbox.center.position.y = (bbox.center_y - letterbox_pad_y_) / letterbox_scale_factor_; + transformed_bbox.size_x /= letterbox_scale_factor_; + transformed_bbox.size_y /= letterbox_scale_factor_; + + return transformed_bbox; +} + +bool ValvePoseBase::segment_plane(const pcl::PointCloud::Ptr& cloud, + pcl::ModelCoefficients::Ptr& coefficients, + pcl::PointIndices::Ptr& inliers) const { + + if (cloud->points.empty()) { + return false; + } + + pcl::SACSegmentation seg; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(plane_ransac_threshold_); + seg.setMaxIterations(plane_ransac_max_iterations_); + seg.setInputCloud(cloud); + seg.segment(*inliers, *coefficients); + + return !inliers->indices.empty(); +} + +Eigen::Vector3f ValvePoseBase::get_ray_direction(const BoundingBox& bbox) const { + float u = bbox.center_x; + float v = bbox.center_y; + + float fx = color_image_properties_.intr.fx; + float fy = color_image_properties_.intr.fy; + float cx = color_image_properties_.intr.cx; + float cy = color_image_properties_.intr.cy; + + float xc = (u - cx) / fx; + float yc = (v - cy) / fy; + + Eigen::Vector3f ray_direction; + ray_direction << xc, yc, 1.0f; + + return ray_direction.normalized(); +} + +Eigen::Vector3f ValvePoseBase::compute_plane_normal( + const pcl::ModelCoefficients::Ptr& coefficients, + const Eigen::Vector3f& ray_direction) { + + Eigen::Vector3f normal_vector; + + if (!coefficients || coefficients->values.size() < 3) { + return Eigen::Vector3f::Zero(); + } + + normal_vector << coefficients->values[0], coefficients->values[1], coefficients->values[2]; + + normal_vector.normalize(); + + if (normal_vector.dot(ray_direction) > 0.0) { + normal_vector = -normal_vector; + } + + return normal_vector; +} + +/** + * @brief Finds the intersection point of a ray and a plane. + * + * The ray is defined by its origin (The camera's optical center, + * i.e., [0, 0, 0]) and a direction vector. The plane is defined by its coefficients + * Ax + By + Cz + D = 0. + * + * @param coefficients A shared pointer to the pcl::ModelCoefficients object. + * @param ray_direction The normalized Eigen::Vector3f ray direction. + * @return An Eigen::Vector3f representing the 3D intersection point. + */ + Eigen::Vector3f ValvePoseBase::find_ray_plane_intersection( + const pcl::ModelCoefficients::Ptr& coefficients, + const Eigen::Vector3f& ray_direction) const { + + // Plane equation: Ax + By + Cz + D = 0 + // Ray equation: P = O + t * d, where O is origin [0,0,0], P is point, t is scalar, d is direction. + // Substituting the ray equation into the plane equation: + // A*(O.x + t*d.x) + B*(O.y + t*d.y) + C*(O.z + t*d.z) + D = 0 + // Since O is [0,0,0], this simplifies to: + // A*t*d.x + B*t*d.y + C*t*d.z + D = 0 + // t * (A*d.x + B*d.y + C*d.z) = -D + // t = -D / (A*d.x + B*d.y + C*d.z) + + if (!coefficients || coefficients->values.size() < 4) { + return Eigen::Vector3f::Zero(); + } + + Eigen::Vector3f plane_normal; + plane_normal << coefficients->values[0], coefficients->values[1], coefficients->values[2]; + float D = coefficients->values[3]; + + float normal_dot_ray = plane_normal.dot(ray_direction); + + if (std::abs(normal_dot_ray) < 1e-6) { + // The ray is parallel or contained within the plane. + return Eigen::Vector3f::Zero(); + } + + float t = -D / normal_dot_ray; + + Eigen::Vector3f intersection_point = t * ray_direction; + + return intersection_point; +} + +/** + * @brief Shifts a point along a plane normal by a given distance. + * + * This function is useful for moving a point from the detected plane's + * surface to a new location, such as the center of a valve knob. + * The shift is performed along the vector of the consistent plane normal. + * + * @param intersection_point The 3D intersection point on the plane. + * @param plane_normal The consistent, normalized plane normal vector. + * @return The new Eigen::Vector3f point after the shift. + */ + Eigen::Vector3f ValvePoseBase::shift_point_along_normal( + const Eigen::Vector3f& intersection_point, + const Eigen::Vector3f& plane_normal) const { + + return intersection_point + (plane_normal * valve_handle_offset_); +} + + +} // namespace valve_detection \ No newline at end of file diff --git a/src/valve_pose_depth.cpp b/src/valve_pose_depth.cpp new file mode 100644 index 0000000..96de2f3 --- /dev/null +++ b/src/valve_pose_depth.cpp @@ -0,0 +1,82 @@ +#include "valve_detection/valve_pose_depth.hpp" + +namespace valve_detection { + +ValvePoseDepth::ValvePoseDepth(int yolo_img_width, int yolo_img_height_, + float plane_ransac_threshold, + int plane_ransac_max_iterations, + float annulus_radius_ratio, + float valve_handle_offset) + : ValvePoseBase(yolo_img_width, yolo_img_height_, + plane_ransac_threshold, plane_ransac_max_iterations, + annulus_radius_ratio, valve_handle_offset){} + +void ValvePoseDepth::project_pixel_to_point(int u, int v, float depth, pcl::PointXYZ& point) { + if (depth <= 0 || depth == std::numeric_limits::infinity() || std::isnan(depth)) { + point.x = point.y = point.z = std::numeric_limits::quiet_NaN(); + return; + } + + double fx = depth_image_properties_.intr.fx; + double fy = depth_image_properties_.intr.fy; + double cx = depth_image_properties_.intr.cx; + double cy = depth_image_properties_.intr.cy; + + point.x = (u - cx) * depth / fx; + point.y = (v - cy) * depth / fy; + point.z = depth; +} + +void ValvePoseDepth::extract_annulus_pcl( + const cv::Mat& depth_image, + const BoundingBox& bbox, + pcl::PointCloud::Ptr& cloud) const { + + float center_x = bbox.center_x; + float center_y = bbox.center_y; + float outer_radius_x = bbox.size_x / 2.0f; + float outer_radius_y = bbox.size_y / 2.0f; + + float inner_radius_x = outer_radius_x * annulus_radius_ratio_; + float inner_radius_y = outer_radius_y * annulus_radius_ratio_; + + cloud->clear(); + + int upper_bound_size = static_cast(M_PI * ((outer_radius_x * outer_radius_y - inner_radius_x * inner_radius_y) + 1)); + cloud->reserve(upper_bound_size); + + for (int y = static_cast(-outer_radius_y); y <= static_cast(outer_radius_y); ++y) { + for (int x = static_cast(-outer_radius_x); x <= static_cast(outer_radius_x); ++x) { + float dx_scaled_outer = x / outer_radius_x; + float dy_scaled_outer = y / outer_radius_y; + bool is_inside_outer_ellipse = (dx_scaled_outer * dx_scaled_outer + dy_scaled_outer * dy_scaled_outer) <= 1.0f; + + float dx_scaled_inner = x / inner_radius_x; + float dy_scaled_inner = y / inner_radius_y; + bool is_outside_inner_ellipse = (dx_scaled_inner * dx_scaled_inner + dy_scaled_inner * dy_scaled_inner) > 1.0f; + + if (is_inside_outer_ellipse && is_outside_inner_ellipse) { + int u = static_cast(center_x + x); + int v = static_cast(center_y + y); + + if (u >= 0 && u < depth_image.cols && v >= 0 && v < depth_image.rows) { + float depth_value = depth_image.at(v, u); + pcl::PointXYZ point; + project_pixel_to_point(u, v, depth_value, point); + + if (!std::isnan(point.x) && !std::isnan(point.y) && !std::isnan(point.z)) { + cloud->points.push_back(point); + } + } + } + } + } + + cloud->width = static_cast(cloud->points.size()); + cloud->height = 1; + cloud->is_dense = false; +} + + +} // namespace valve_detection + \ No newline at end of file diff --git a/src/valve_pose_depth_ros.cpp b/src/valve_pose_depth_ros.cpp new file mode 100644 index 0000000..8c8388c --- /dev/null +++ b/src/valve_pose_depth_ros.cpp @@ -0,0 +1,209 @@ +#include "valve_detection/valve_pose_depth_ros.hpp" + +ValvePoseDepthNode::ValvePoseDepthNode(const rclcpp::NodeOptions& options) +: Node("valve_detection_node", options) { + std::string color_image_sub_topic = + this->declare_parameter("color_image_sub_topic"); + std::string depth_image_sub_topic = + this->declare_parameter("depth_image_sub_topic"); + std::string detections_sub_topic = + this->declare_parameter("detections_sub_topic"); + visualize_detections_ = + this->declare_parameter("visualize_detections"); + debug_visualize_ = + this->declare_parameter("debug_visualize"); + calculate_angle_ = + this->declare_parameter("calculate_angle"); + + int yolo_img_width = + this->declare_parameter("yolo_img_width"); + int yolo_img_height = + this->declare_parameter("yolo_img_height"); + + float annulus_radius_ratio = + this->declare_parameter("annulus_radius_ratio"); + + float plane_ransac_threshold = + this->declare_parameter("plane_ransac_threshold"); + int plane_ransac_max_iterations = + this->declare_parameter("plane_ransac_max_iterations"); + + float valve_handle_offset = + this->declare_parameter("valve_handle_offset"); + + valve_detector_ = std::make_shared(yolo_img_width, yolo_img_height, annulus_radius_ratio, + plane_ransac_threshold, plane_ransac_max_iterations, valve_handle_offset); + + + rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(10)) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + depth_image_sub_.subscribe(this, depth_image_sub_topic, + qos.get_rmw_qos_profile()); + color_image_sub_.subscribe(this, color_image_sub_topic, + qos.get_rmw_qos_profile()); + detections_sub_.subscribe(this, detections_sub_topic, + qos.get_rmw_qos_profile()); + + sync_ = std::make_shared>( + MySyncPolicy(10), depth_image_sub_, color_image_sub_, detections_sub_); + sync_->registerCallback(std::bind( + &ValvePoseDepthNode::synchronized_callback, this, _1, _2, _3)); + + std::string color_image_info_topic = + this->declare_parameter("color_image_info_topic"); + + qos.history = rclcpp::KeepLast(1); + color_image_info_sub_ = this->create_subscription( + color_image_info_topic, 10, + std::bind(&ValvePoseDepthNode::color_image_info_callback, + this, std::placeholders::_1)); + + std::string valve_pose_pub_topic = + this->declare_parameter("valve_pose_pub_topic"); + + valve_pose_pub_ = this->create_publisher( + valve_pose_pub_topic, qos); + + if (debug_visualize_) { + std::string annulus_pub_topic = + this->declare_parameter("annulus_pub_topic"); + annulus_pcl_pub_ = this->create_publisher( + annulus_pub_topic, qos); + } +} + +void ValvePoseDepthNode::color_image_info_callback( + const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg) { + if (!color_image_info_received_) { + ImageProperties img_props; + img_props.intr.fx = camera_info_msg->k[0]; + img_props.intr.fy = camera_info_msg->k[4]; + img_props.intr.cx = camera_info_msg->k[2]; + img_props.intr.cy = camera_info_msg->k[5]; + img_props.dim.x = camera_info_msg->width; + img_props.dim.y = camera_info_msg->height; + + valve_detector_->set_color_image_properties(img_props); + valve_detector_->calculate_letterbox_padding(); + + color_image_frame_id_ = camera_info_msg->header.frame_id; + color_image_info_received_ = true; + color_image_info_sub_.reset(); + } +} + +BoundingBox ValvePoseDepthNode::to_bounding_box( + const vision_msgs::msg::BoundingBox2D& bbox) const { + BoundingBox box; + box.center_x = bbox.center.x; + box.center_y = bbox.center.y; + box.size_x = bbox.size_x; + box.size_y = bbox.size_y; + box.theta = bbox.center.theta; + return box; +} + +void ValvePoseDepthNode::publish_annulus_pcl( + const pcl::PointCloud::Ptr& cloud) const { + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(*cloud, cloud_msg); + cloud_msg.header.frame_id = color_image_frame_id_; + cloud_msg.header.stamp = this->now(); + cloud_msg.is_dense = false; + cloud_msg.width = static_cast(cloud->points.size()); + cloud_msg.height = 1; + + annulus_pcl_pub_->publish(cloud_msg); +} + +void ValvePoseDepthNode::synchronized_callback( + const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, + const sensor_msgs::msg::Image::ConstSharedPtr& color_image, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { + if (!color_image_info_received_) { + return; + } + cv_bridge::CvImageConstPtr cv_ptr = + cv_bridge::toCvShare(color_image, "bgr8"); + + cv:brudge::CvImageConstPtr cv_depth_ptr = + cv_bridge::toCvShare(depth_image, "32FC1"); + + cv::Mat cv_color_image = cv_ptr->image; + + if (visualize_detections_) { + cv_color_image = cv_color_image.clone(); + } + std::vector boxes = std::vector().reserve(detections->detections.size()); + for (const auto& detection : detections->detections) { + boxes.push_back(to_bounding_box(detection.bbox)); + } + + for (const auto& box : boxes) { + BoundingBox org_image_box = + valve_detector_->transform_bounding_box(box); + + pcl::PointCloud::Ptr cloud( + new pcl::PointCloud); + + valve_detector_->extract_annulus_pcl( + cv_depth_ptr->image, + org_image_box, + cloud); + + if (cloud->empty()) { + continue; + } + if (debug_visualize_) { + publish_annulus_pcl(cloud); + } + + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + if (!valve_detector_->segment_plane(cloud, coefficients, inliers)) { + continue; + } + + Eigen::Vector3f bb_centre_ray = + valve_detector_->get_ray_direction(org_image_box); + + Eigen::Vector3f ray_plane_intersection = + valve_detector_->find_ray_plane_intersection(coefficients, bb_centre_ray); + + if (ray_plane_intersection.isZero()) { + continue; + } + + Eigen::Vector3f plane_normal = + valve_detector_->compute_plane_normal( + coefficients, ray_direction); + + if (plane_normal.isZero()) { + continue; + } + + Eigen::Vector3f shifted_position = + valve_detector_->shift_point_along_normal( + ray_plane_intersection, plane_normal); + + float angle = org_image_box.theta; + if (calculate_angle_) { + // angle = calculate_angle() + } + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.stamp = color_image->header.stamp; + pose_msg.header.frame_id = color_image_frame_id_; + + pose_msg.pose.position.x = shifted_position[0]; + pose_msg.pose.position.y = shifted_position[1]; + pose_msg.pose.position.z = shifted_position[2]; + + } + + + + + +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ValvePoseDepthNode) \ No newline at end of file diff --git a/src/valve_pose_pcl.cpp b/src/valve_pose_pcl.cpp new file mode 100644 index 0000000..e69de29 From ef8c4bc8d5608c1b6d3008416397237814b6f0d1 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 25 Aug 2025 20:36:12 +0200 Subject: [PATCH 14/22] small refactor --- CMakeLists.txt | 12 +- config/depth_image_angle_params.yaml | 27 - config/valve_detection_params.yaml | 35 +- include/valve_detection/angle_detector.hpp | 52 ++ .../depth_image_processing.hpp | 61 ++ .../valve_detection/pointcloud_processing.hpp | 39 ++ include/valve_detection/types.hpp | 87 ++- include/valve_detection/valve_detection.hpp | 201 ------ include/valve_detection/valve_detector.hpp | 293 +++++++++ include/valve_detection/valve_pose_base.hpp | 103 --- include/valve_detection/valve_pose_depth.hpp | 35 - .../valve_pose_depth_ros.hpp | 76 --- .../valve_detection_ros/valve_pose_ros.hpp | 310 +++++++++ launch/valve_detection.launch.py | 33 +- package.xml | 1 + src/angle_detector.cpp | 81 +++ src/depth_image_processing.cpp | 88 +++ src/pointcloud_processing.cpp | 59 ++ src/valve_detection.cpp | 600 ------------------ src/valve_detector.cpp | 241 +++++++ src/valve_pose_base.cpp | 181 ------ src/valve_pose_depth.cpp | 82 --- src/valve_pose_depth_ros.cpp | 209 ------ src/valve_pose_pcl.cpp | 0 src/valve_pose_ros.cpp | 289 +++++++++ 25 files changed, 1646 insertions(+), 1549 deletions(-) delete mode 100644 config/depth_image_angle_params.yaml create mode 100644 include/valve_detection/angle_detector.hpp create mode 100644 include/valve_detection/depth_image_processing.hpp create mode 100644 include/valve_detection/pointcloud_processing.hpp delete mode 100644 include/valve_detection/valve_detection.hpp create mode 100644 include/valve_detection/valve_detector.hpp delete mode 100644 include/valve_detection/valve_pose_base.hpp delete mode 100644 include/valve_detection/valve_pose_depth.hpp delete mode 100644 include/valve_detection_ros/valve_pose_depth_ros.hpp create mode 100644 include/valve_detection_ros/valve_pose_ros.hpp create mode 100644 src/angle_detector.cpp create mode 100644 src/depth_image_processing.cpp create mode 100644 src/pointcloud_processing.cpp delete mode 100644 src/valve_detection.cpp create mode 100644 src/valve_detector.cpp delete mode 100644 src/valve_pose_base.cpp delete mode 100644 src/valve_pose_depth.cpp delete mode 100644 src/valve_pose_depth_ros.cpp delete mode 100644 src/valve_pose_pcl.cpp create mode 100644 src/valve_pose_ros.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index de59d4f..1c5783a 100644 --- a/CMakeLists.txt +++ b/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 @@ -50,11 +55,14 @@ ament_target_dependencies(${LIB_NAME} PUBLIC geometry_msgs std_msgs pcl_conversions + tf2 + tf2_ros + tf2_geometry_msgs ) rclcpp_components_register_node( ${LIB_NAME} - PLUGIN "ValveDetectionNode" + PLUGIN "valve_detection::ValvePoseNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/config/depth_image_angle_params.yaml b/config/depth_image_angle_params.yaml deleted file mode 100644 index bc3d5f3..0000000 --- a/config/depth_image_angle_params.yaml +++ /dev/null @@ -1,27 +0,0 @@ -/**: - ros__parameters: - depth_image_sub_topic: "/zed_node/depth/depth_registered" - color_image_sub_topic: "/zed_node/left/image_rect_color" - detections_sub_topic: "/detections_output" - - color_image_info_topic: "/zed_node/left/camera_info" - depth_image_info_topic: "/zed_node/depth/camera_info" - - valve_pose_pub_topic: "/valve_pose" - - 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 - - calculate_angle: true - - visualize_detections: true - debug_visualize: false - annulus_pub_topic: "/bbx_annulus_pcl" - plane_normal_pub_topic: "/plane_normal" diff --git a/config/valve_detection_params.yaml b/config/valve_detection_params.yaml index 185d440..2b9f093 100644 --- a/config/valve_detection_params.yaml +++ b/config/valve_detection_params.yaml @@ -2,6 +2,39 @@ ros__parameters: depth_image_sub_topic: "/zed_node/depth/depth_registered" color_image_sub_topic: "/zed_node/left/image_rect_color" + pcl_sub_topic: "/zed_node/depth/point_cloud" detections_sub_topic: "/detections_output" - line_detection_area: 22 + color_image_info_topic: "/zed_node/left/camera_info" + depth_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" + pcl_visualize: false + annulus_pub_topic: "/bbx_annulus_pcl" + plane_pub_topic: "/annulus_plane_pcl" + + calculate_angle: true + angle.detection_area_ratio: 0.05 # percentage of bb size + angle.canny_low_threshold: 50 + angle.canny_high_threshold: 150 + 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 diff --git a/include/valve_detection/angle_detector.hpp b/include/valve_detection/angle_detector.hpp new file mode 100644 index 0000000..188d563 --- /dev/null +++ b/include/valve_detection/angle_detector.hpp @@ -0,0 +1,52 @@ +#ifndef VALVE_POSE_ANGLE_HPP +#define VALVE_POSE_ANGLE_HPP + +#include +#include +#include +#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 Calculates the angle of the longest line within a detected valve. + * @param color_image The input color image. + * @param bbox The bounding box of the detected valve. + * @return The angle in radians, or NaN if no lines are detected. + */ + double calculate_angle(const cv::Mat& color_image, + const BoundingBox& bbox) 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& 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 diff --git a/include/valve_detection/depth_image_processing.hpp b/include/valve_detection/depth_image_processing.hpp new file mode 100644 index 0000000..2ddea69 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +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::Ptr& cloud); + +} // namespace valve_detection + +#endif // VALVE_POSE_DEPTH_HPP diff --git a/include/valve_detection/pointcloud_processing.hpp b/include/valve_detection/pointcloud_processing.hpp new file mode 100644 index 0000000..588496f --- /dev/null +++ b/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 +#include +#include +#include + +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::Ptr& input_cloud, + const BoundingBox& bbox, + const ImageProperties& image_properties, + float annulus_radius_ratio, + pcl::PointCloud::Ptr& cloud); + +} // namespace valve_detection + +#endif // VALVE_POSE_PCL_HPP diff --git a/include/valve_detection/types.hpp b/include/valve_detection/types.hpp index b42082c..248452b 100644 --- a/include/valve_detection/types.hpp +++ b/include/valve_detection/types.hpp @@ -1,38 +1,85 @@ #ifndef TYPES_HPP #define TYPES_HPP +#include + namespace valve_detection { +/** + * @struct CameraIntrinsics + * @brief Stores intrinsic parameters of a pinhole camera model. + */ struct CameraIntrinsics { - double fx; - double fy; - double cx; - double cy; + double fx; ///< Focal length in pixels along x-axis + double fy; ///< Focal length in pixels along y-axis + double cx; ///< Principal point x-coordinate (in pixels) + double cy; ///< Principal point y-coordinate (in pixels) }; +/** + * @struct ImageDimensions + * @brief Represents the width and height of an image. + */ struct ImageDimensions { - int width; - int height; + int width; ///< Image width in pixels + int height; ///< Image height in pixels }; +/** + * @struct ImageProperties + * @brief Combines camera intrinsics and image dimensions. + */ struct ImageProperties { - CameraIntrinsics intr; - ImageDimensions dim; + CameraIntrinsics intr; ///< Camera intrinsic parameters + ImageDimensions dim; ///< Image width and height }; +/** + * @struct BoundingBox + * @brief Represents a 2D bounding box with center, size, and orientation. + */ struct BoundingBox { - float center_x; - float center_y; - float size_x; - float size_y; - float theta; // Orientation in radians positive counter-clockwise -} - -enum class AngleDetection { - ENABLED, - DISABLED + float center_x; ///< X-coordinate of the bounding box center + float center_y; ///< Y-coordinate of the bounding box center + float size_x; ///< Width of the bounding box + float size_y; ///< Height of the bounding box + float theta; ///< Orientation in radians (positive counter-clockwise) +}; + +/** + * @struct Pose + * @brief Represents a 3D pose with position and orientation. + */ +struct Pose { + Eigen::Vector3f position; ///< 3D position (x, y, z) + Eigen::Quaternionf orientation; ///< Orientation as a quaternion +}; + +/** + * @struct AngleDetectorParams + * @brief Parameters for Canny edge detection and Hough line transform used in + * angle detection. + */ +struct AngleDetectorParams { + float line_detection_area; ///< Fraction of the bounding box to use as ROI + ///< (0.0–1.0) + + // Canny edge detection parameters + int canny_low_threshold; ///< Low threshold for Canny edge detector + int canny_high_threshold; ///< High threshold for Canny edge detector + int canny_aperture_size; ///< Aperture size for the Sobel operator in Canny + + // Hough line transform parameters + double hough_rho_res; ///< Distance resolution in pixels + double hough_theta_res; ///< Angle resolution in radians + int hough_threshold; ///< Minimum number of votes (intersections) to detect + ///< a line + double + hough_min_line_length; ///< Minimum line length to accept (in pixels) + double hough_max_line_gap; ///< Maximum allowed gap between line segments + ///< to treat them as a single line }; -} // namespace valve_detection +} // namespace valve_detection -#endif // TYPES_HPP \ No newline at end of file +#endif // TYPES_HPP diff --git a/include/valve_detection/valve_detection.hpp b/include/valve_detection/valve_detection.hpp deleted file mode 100644 index 06b324f..0000000 --- a/include/valve_detection/valve_detection.hpp +++ /dev/null @@ -1,201 +0,0 @@ -#ifndef DETECTION_IMAGE_PROCESSOR_HPP -#define DETECTION_IMAGE_PROCESSOR_HPP - -#include -#include -#include -#include -#include -#include -#include - -#include -#include "cv_bridge/cv_bridge.h" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/vector3.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/camera_info.hpp" -#include "sensor_msgs/msg/image.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "std_msgs/msg/header.hpp" -#include "vision_msgs/msg/detection2_d_array.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -class ValveDetectionNode : public rclcpp::Node { - public: - explicit ValveDetectionNode(const rclcpp::NodeOptions& options); - ~ValveDetectionNode() {}; - - private: - message_filters::Subscriber depth_image_sub_; - message_filters::Subscriber color_image_sub_; - message_filters::Subscriber - detections_sub_; - - typedef message_filters::sync_policies::ExactTime< - sensor_msgs::msg::Image, - sensor_msgs::msg::Image, - vision_msgs::msg::Detection2DArray> - MySyncPolicy; - - std::shared_ptr> sync_; - - // Subscriptions - rclcpp::Subscription::SharedPtr - camera_info_subscription_; - rclcpp::Subscription::SharedPtr - camera_info_subscription_yolo_color_; - rclcpp::Subscription::SharedPtr - detections_subscription_; - rclcpp::Subscription::SharedPtr - image_subscription_; - - // Publishers - rclcpp::Publisher::SharedPtr processed_image_pub_; - rclcpp::Publisher::SharedPtr plane_normal_pub_; - rclcpp::Publisher::SharedPtr pointcloud_pub_; - rclcpp::Publisher::SharedPtr - line_pose_pub_; - rclcpp::Publisher::SharedPtr - line_points_pub_; - rclcpp::Publisher::SharedPtr - near_plane_cloud_pub_; - rclcpp::Publisher::SharedPtr - canny_debug_image_pub_; - - // Stored messages - sensor_msgs::msg::CameraInfo::SharedPtr camera_info_; - bool camera_info_received_ = false; - sensor_msgs::msg::CameraInfo::SharedPtr camera_info_yolo_color_; - bool camera_info_received_yolo_color_ = false; - // vision_msgs::msg::Detection2DArray::SharedPtr latest_detections_; - // sensor_msgs::msg::Image::SharedPtr latest_image_; - - // Mutex for thread safety - std::mutex mutex_; - - // Used for line fitting direction filtering - static Eigen::Vector3f filter_direction_; - - // Scalars for image resizing - double height_scalar_; - double width_scalar_; - - int line_detection_area_; - - /** - * @brief Callback function for synchronized depth image, color image, and - * 2D detections. - * - * This function is triggered when synchronized messages for a depth image, - * a color image, and a 2D detection array - */ - void synchronized_callback( - const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, - const sensor_msgs::msg::Image::ConstSharedPtr& color_image, - const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); - // Callback functions - void camera_info_callback( - const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg); - void camera_info_callback_yolo_colored( - const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg); - - // Utility functions - /** - * @brief Computes the height and width scalars based on the camera - * information. - * - * This function calculates the scaling factors (`height_scalar_` and - * `width_scalar_`) by comparing the dimensions of two camera frames: - * - The YOLO color camera frame (`camera_info_yolo_color_`). - * - The reference camera frame (`camera_info_`). - * - */ - void compute_height_width_scalars(); - - /** - * @brief Main logic executed when two images are synchronized. - * - * This function processes synchronized depth and color images along with 2D - * detections to calculate the angle of the valve handle and its orientation - * in 3D space. It uses RANSAC line detection to determine the handle's - * angle and RANSAC plane detection to find the normal vector of the plane - * in which the valve handle lies. - * - * The function performs the following steps: - * 1. Converts the depth and color images from ROS messages to OpenCV - * matrices. - * 2. Selects the most centered detection from the 2D detection array. - * 3. Projects the detected bounding box into 3D space using the depth image - * and camera intrinsics. - * 4. Uses RANSAC plane detection to find the plane normal vector and - * filters points near the plane. - * 5. Detects the valve handle's orientation using Hough line detection on - * the color image. - * 6. Computes the 3D pose of the valve handle and publishes it as a - * `geometry_msgs::msg::PoseStamped` message. - * - * @param[in] depth_image A shared pointer to the depth image message. - * @param[in] color_image A shared pointer to the color image message. - * @param[in] detections A shared pointer to the 2D detection array message. - * - * @pre The `camera_info_` and `camera_info_yolo_color_` member variables - * must be initialized with valid camera intrinsic parameters. - * @pre The input images and detections must be synchronized (i.e., they - * correspond to the same timestamp or frame). - * @post If successful, the function publishes the following: - * - A point cloud of the detected valve handle region. - * - The normal vector of the detected plane. - * - The 3D pose of the valve handle. - * @post If unsuccessful, the function logs warnings or errors and returns - * early. - * - * @note The function assumes that the input images and detections are - * synchronized. - * @note The function uses RANSAC for robust plane and line detection, which - * is suitable for noisy sensor data. - * @note The function handles edge cases such as invalid depth values, empty - * detections, and failed RANSAC fits. - */ - void process_and_publish_image( - const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, - const sensor_msgs::msg::Image::ConstSharedPtr& color_image, - const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); - - /** - * @brief Projects a 2D pixel coordinate and depth value into a 3D point in - * the camera frame. - * - * This function converts a pixel coordinate `(u, v)` and its corresponding - * depth value into a 3D point - */ - void project_pixel_to_3d(int u, int v, float depth, pcl::PointXYZ& point); - - /** - * @brief Projects a 3D point in the camera frame to 2D pixel coordinates. - * - * This function converts a 3D point `(x, y, z)` in the camera frame into 2D - * pixel coordinates `(u, v)` - */ - void project_3d_to_pixel(float x, float y, float z, int& u, int& v); -}; - -#endif // DETECTION_IMAGE_PROCESSOR_HPP diff --git a/include/valve_detection/valve_detector.hpp b/include/valve_detection/valve_detector.hpp new file mode 100644 index 0000000..3d83432 --- /dev/null +++ b/include/valve_detection/valve_detector.hpp @@ -0,0 +1,293 @@ +#ifndef VALVE_POSE_BASE_HPP +#define VALVE_POSE_BASE_HPP + +#include +#include +#include +#include +#include +#include +#include +#include "valve_detection/angle_detector.hpp" +#include "valve_detection/depth_image_processing.hpp" +#include "valve_detection/pointcloud_processing.hpp" +#include "valve_detection/types.hpp" + +namespace valve_detection { + +/** + * @class ValveDetector + * @brief Detects valve poses from color and depth images or point clouds. + * + * This class extracts valve annulus point clouds, segments planes, + * computes ray-plane intersections, estimates valve poses, and can calculate + * the valve rotation angle using the AngleDetector. + */ +class ValveDetector { + public: + /** + * @brief Constructs a ValveDetector with YOLO image dimensions and + * detection parameters. + * @param yolo_img_width Width of the YOLO input image. + * @param yolo_img_height Height of the YOLO input image. + * @param annulus_radius_ratio Ratio of bounding box used to define the + * annulus. + * @param plane_ransac_threshold Distance threshold for plane segmentation. + * @param plane_ransac_max_iterations Max iterations for RANSAC plane + * segmentation. + * @param valve_handle_offset Distance to shift the valve handle along the + * plane normal. + */ + ValveDetector(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); + + virtual ~ValveDetector() = default; + + /** + * @brief Computes valve poses from depth input and bounding boxes. + * + * This template function supports depth input as either a PCL point cloud + * or an OpenCV depth image. It extracts the annulus points, segments + * planes, computes ray-plane intersections, shifts points along normals, + * and optionally calculates valve rotation angles. + * + * @tparam T Depth input type (cv::Mat or + * pcl::PointCloud::Ptr). + * @param depth_input The input depth data. + * @param color_image Corresponding color image for angle calculation. + * @param boxes Bounding boxes of detected valves. + * @param poses Output vector of computed valve poses. + * @param all_annulus_cloud Optional point cloud for visualizing all annuli. + * @param all_annulus_plane_cloud Optional point cloud for visualizing + * segmented planes. + * @param pcl_visualize Whether to populate visualization point clouds. + * @param calculate_angle Whether to compute the valve rotation angle. + */ + template + void Compute_valve_poses( + const T& depth_input, + const cv::Mat& color_image, + const std::vector& boxes, + std::vector& poses, + pcl::PointCloud::Ptr& all_annulus_cloud, + pcl::PointCloud::Ptr& all_annulus_plane_cloud, + bool pcl_visualize, + bool calculate_angle) { + if (pcl_visualize) { + all_annulus_cloud->clear(); + all_annulus_plane_cloud->clear(); + } + + for (const auto& box : boxes) { + BoundingBox org_image_box = transform_bounding_box(box); + pcl::PointCloud::Ptr annulus_cloud( + new pcl::PointCloud); + + if constexpr (std::is_same< + T, pcl::PointCloud::Ptr>::value) { + extract_annulus_pcl(depth_input, org_image_box, + color_image_properties_, + annulus_radius_ratio_, annulus_cloud); + } else if constexpr (std::is_same::value) { + extract_annulus_pcl(depth_input, org_image_box, + color_image_properties_, + annulus_radius_ratio_, annulus_cloud); + } + + if (annulus_cloud->empty()) + continue; + + if (pcl_visualize) { + *all_annulus_cloud += *annulus_cloud; + } + + pcl::ModelCoefficients::Ptr coefficients( + new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + + if (!segment_plane(annulus_cloud, coefficients, inliers)) + continue; + + if (pcl_visualize) { + pcl::PointCloud::Ptr annulus_plane_cloud( + new pcl::PointCloud); + pcl::copyPointCloud(*annulus_cloud, inliers->indices, + *annulus_plane_cloud); + *all_annulus_plane_cloud += *annulus_plane_cloud; + } + + Eigen::Vector3f bb_centre_ray = get_ray_direction(org_image_box); + Eigen::Vector3f ray_plane_intersection = + find_ray_plane_intersection(coefficients, bb_centre_ray); + if (ray_plane_intersection.isZero()) + continue; + + Eigen::Vector3f plane_normal = + compute_plane_normal(coefficients, bb_centre_ray); + if (plane_normal.isZero()) + continue; + + Eigen::Vector3f shifted_position = + shift_point_along_normal(ray_plane_intersection, plane_normal); + + float angle = org_image_box.theta; + if (calculate_angle) { + angle = angle_detector_->calculate_angle(color_image, + org_image_box); + } + + Eigen::Matrix3f rotation_matrix = + create_rotation_matrix(plane_normal, angle); + Eigen::Quaternionf rotation_quat; + rmat_to_quat(rotation_matrix, rotation_quat); + + Pose pose; + pose.position = shifted_position; + pose.orientation = rotation_quat; + poses.emplace_back(pose); + } + } + + /** + * @brief Projects a 3D point to pixel coordinates using the camera + * intrinsics. + * @param x X coordinate in 3D space. + * @param y Y coordinate in 3D space. + * @param z Z coordinate in 3D space. + * @param[out] u Output pixel x-coordinate. + * @param[out] v Output pixel y-coordinate. + */ + void project_point_to_pixel(float x, + float y, + float z, + int& u, + int& v) const; + + /** + * @brief Calculates the padding and scaling for letterbox resizing. + */ + void calculate_letterbox_padding(); + + /** + * @brief Transforms bounding box coordinates from letterboxed YOLO input to + * original image coordinates. + * @param bbox Bounding box in YOLO image coordinates. + * @return Bounding box transformed to original image coordinates. + */ + BoundingBox transform_bounding_box(const BoundingBox& bbox) const; + + /** + * @brief Sets the color image properties (intrinsics and dimensions). + */ + void set_color_image_properties(const ImageProperties& properties) { + color_image_properties_ = properties; + } + + /** + * @brief Segments the dominant plane from a point cloud using RANSAC. + * @param cloud Input point cloud. + * @param coefficients Output model coefficients (Ax + By + Cz + D = 0). + * @param inliers Output inlier indices of the plane. + * @return True if a plane with inliers was found, false otherwise. + */ + bool segment_plane(const pcl::PointCloud::Ptr& cloud, + pcl::ModelCoefficients::Ptr& coefficients, + pcl::PointIndices::Ptr& inliers) const; + + /** + * @brief Computes the ray direction vector through the bounding box center + * in camera frame. + * @param bbox Bounding box for the valve. + * @return Normalized Eigen::Vector3f representing the ray direction. + */ + Eigen::Vector3f get_ray_direction(const BoundingBox& bbox) const; + + /** + * @brief Computes the plane normal from plane coefficients, ensuring it + * points against the ray. + * @param coefficients Plane coefficients (Ax + By + Cz + D = 0). + * @param ray_direction Ray direction vector from camera. + * @return Normalized Eigen::Vector3f plane normal, or zero if invalid. + */ + Eigen::Vector3f compute_plane_normal( + const pcl::ModelCoefficients::Ptr& coefficients, + const Eigen::Vector3f& ray_direction) const; + + /** + * @brief Finds the intersection point of a ray and a plane. + * @param coefficients Plane coefficients. + * @param ray_direction Normalized ray direction. + * @return Intersection point in 3D space, or zero vector if no + * intersection. + */ + Eigen::Vector3f find_ray_plane_intersection( + const pcl::ModelCoefficients::Ptr& coefficients, + const Eigen::Vector3f& ray_direction) const; + + /** + * @brief Shifts a point along a plane normal by the valve handle offset. + * @param intersection_point Point on the plane. + * @param plane_normal Normalized plane normal vector. + * @return Shifted 3D point representing valve handle position. + */ + Eigen::Vector3f shift_point_along_normal( + const Eigen::Vector3f& intersection_point, + const Eigen::Vector3f& plane_normal) const; + + /** + * @brief Creates a rotation matrix with the plane normal as z-axis and + * valve angle applied. + * @param plane_normal Normalized plane normal vector. + * @param angle Valve rotation angle in radians. + * @return Rotation matrix (Eigen::Matrix3f) for the valve pose. + */ + Eigen::Matrix3f create_rotation_matrix(const Eigen::Vector3f& plane_normal, + float angle); + + /** + * @brief Initializes the angle detector with given parameters. + * @param params Parameters for Canny and Hough-based angle detection. + */ + void init_angle_detector(const AngleDetectorParams& params); + + /** + * @brief Converts a 3x3 rotation matrix to a normalized quaternion. + * @param rotation_matrix Input rotation matrix. + * @param quat Output quaternion. + */ + void rmat_to_quat(const Eigen::Matrix3f& rotation_matrix, + Eigen::Quaternionf& quat) const; + + /** + * @brief Draws bounding boxes and 3D axes for valve poses on an image. + * @param image Input color image. + * @param boxes Detected valve bounding boxes. + * @param poses Computed valve poses. + * @return Image with drawn detections. + */ + cv::Mat draw_detections(const cv::Mat& image, + const std::vector& boxes, + const std::vector& poses) const; + + protected: + ImageProperties color_image_properties_; + 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_; + float letterbox_scale_factor_; + int letterbox_pad_x_; + int letterbox_pad_y_; + Eigen::Vector3f filter_direction_ = Eigen::Vector3f(1, 0, 0); + std::unique_ptr angle_detector_; +}; + +} // namespace valve_detection + +#endif // VALVE_POSE_BASE_HPP diff --git a/include/valve_detection/valve_pose_base.hpp b/include/valve_detection/valve_pose_base.hpp deleted file mode 100644 index f40e1a1..0000000 --- a/include/valve_detection/valve_pose_base.hpp +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef VALVE_POSE_BASE_HPP -#define VALVE_POSE_BASE_HPP - -#include "valve_detection/types.hpp" -#include - -namespace valve_detection { - -class ValvePoseBase { -public: - ValvePoseBase(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); - virtual ~ValvePoseBase() = default; - - void project_point_to_pixel( - float x, float y, float z, int& u, int& v) const; - - void calculate_letterbox_properties(); - - BoundingBox transform_bounding_box( - const BoundingBox& bbox) const; - - void set_color_image_properties( - const ImageProperties& properties) { - color_image_properties_ = properties; - } - - /** - * @brief Segments the dominant plane from a point cloud using RANSAC. - * @param cloud The input point cloud. - * @param coefficients The output model coefficients (ax + by + cz + d = 0). - * @return True if a plane with inliers was found, false otherwise. - */ - bool segment_plane(const pcl::PointCloud::Ptr& cloud, - pcl::ModelCoefficients::Ptr& coefficients, - pcl::PointIndices::Ptr& inliers) const; - - Eigen::Vector3f get_ray_direction(const BoundingBox& bbox) const; - - /** - * @brief Creates a consistent, normalized Eigen::Vector3f from PCL plane coefficients - * and a reference ray. - * - * This function extracts the normal vector (A, B, C) from the plane equation - * Ax + By + Cz + D = 0, normalizes it, and flips its direction if necessary - * to ensure it always points in the same general direction as the reference ray. - * - * @param coefficients A shared pointer to the pcl::ModelCoefficients object. - * @param ray_direction A reference to the Eigen::Vector3f representing the ray. - * @return A normalized Eigen::Vector3f representing the consistent plane normal. - */ - Eigen::Vector3f create_consistent_plane_normal_with_ray( - const pcl::ModelCoefficients::Ptr& coefficients, - const Eigen::Vector3f& ray_direction) const; - - - /** - * @brief Finds the intersection point of a ray and a plane. - * - * The ray is defined by its origin (assumed to be the camera's optical center, - * i.e., [0, 0, 0]) and a direction vector. The plane is defined by its coefficients - * Ax + By + Cz + D = 0. - * - * @param coefficients A shared pointer to the pcl::ModelCoefficients object. - * @param ray_direction The normalized Eigen::Vector3f ray direction. - * @return An Eigen::Vector3f representing the 3D intersection point. - */ - Eigen::Vector3f find_ray_plane_intersection( - const pcl::ModelCoefficients::Ptr& coefficients, - const Eigen::Vector3f& ray_direction) const; - - /** - * @brief Shifts a point along a plane normal by a given distance. - * - * This function is useful for moving a point from the detected plane's - * surface to a new location, such as the center of a valve knob. - * The shift is performed along the vector of the consistent plane normal. - * - * @param intersection_point The 3D intersection point on the plane. - * @param plane_normal The consistent, normalized plane normal vector. - * @return The new Eigen::Vector3f point after the shift. - */ - Eigen::Vector3f shift_point_along_normal( - const Eigen::Vector3f& intersection_point, - const Eigen::Vector3f& plane_normal) const; - -protected: - ImageProperties color_image_properties_; - 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_; - float letterbox_scale_factor_; - int letterbox_pad_x_; - int letterbox_pad_y_; -} -} // namespace valve_detection - -#endif // VALVE_POSE_BASE_HPP \ No newline at end of file diff --git a/include/valve_detection/valve_pose_depth.hpp b/include/valve_detection/valve_pose_depth.hpp deleted file mode 100644 index f826a4c..0000000 --- a/include/valve_detection/valve_pose_depth.hpp +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef VALVE_POSE_DEPTH_HPP -#define VALVE_POSE_DEPTH_HPP - -#include "valve_detection/valve_pose_base.hpp" -#include -#include -#include - -namespace valve_detection { - -class ValvePoseDepth : public ValvePoseBase { -public: - ValvePoseDepth(int yolo_img_width, int yolo_img_height_, float annulus_radius_ratio, - float plane_ransac_threshold, int plane_ransac_max_iterations); - virtual ~ValvePoseDepth() = default; - - void project_pixel_to_point(int u, - int v, - float depth, - pcl::PointXYZ& point) const; - - void extract_annulus_pcl( - const cv::Mat& depth_image, - const BoundingBox& bbox, - pcl::PointCloud::Ptr& cloud) const; - -private: - ImageProperties depth_image_properties_; - double height_scalar_; - double width_scalar_; -}; - -} // namespace valve_detection - -#endif // VALVE_POSE_DEPTH_HPP \ No newline at end of file diff --git a/include/valve_detection_ros/valve_pose_depth_ros.hpp b/include/valve_detection_ros/valve_pose_depth_ros.hpp deleted file mode 100644 index 8e5ce9f..0000000 --- a/include/valve_detection_ros/valve_pose_depth_ros.hpp +++ /dev/null @@ -1,76 +0,0 @@ -#ifndef VALVE_POSE_DEPTH_ROS_HPP -#define VALVE_POSE_DEPTH_ROS_HPP - -#include "valve_detection/valve_pose_depth.hpp" -#include -#include -#include -#include - -#include -#include -#include - - -namespace valve_detection { - -class ValvePoseDepthNode { -public: - explicit ValvePoseDepthNode(const rclcpp::NodeOptions& options); - virtual ~ValvePoseDepthNode() = default; - - -private: - void color_image_info_callback( - const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg); - - BoundingBox to_bounding_box(const vision_msgs::msg::BoundingBox2D& bbox) const; - - void publish_annulus_pcl(const pcl::PointCloud::Ptr& cloud) const; - - /** - * @brief Callback function for synchronized depth image, color image, and - * 2D detections. - * - * This function is triggered when synchronized messages for a depth image, - * a color image, and a 2D detection array are received. - */ - void synchronized_callback( - const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, - const sensor_msgs::msg::Image::ConstSharedPtr& color_image, - const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); - - - rclcpp::Subscription::SharedPtr color_image_info_sub_; - bool color_image_info_received_ = false; - - message_filters::Subscriber depth_image_sub_; - message_filters::Subscriber color_image_sub_; - message_filters::Subscriber - detections_sub_; - - typedef message_filters::sync_policies::ExactTime< - sensor_msgs::msg::Image, - sensor_msgs::msg::Image, - vision_msgs::msg::Detection2DArray> - MySyncPolicy; - - std::shared_ptr> sync_; - - rclcpp::Publisher::SharedPtr - valve_pose_pub_; - - std::unique_ptr valve_detector_; - - std::string color_image_frame_id_; - - bool visualize_detections_ = true; - bool debug_visualize_ = false; - bool calculate_angle_ = true; - rclcpp::Publisher::SharedPtr annulus_pcl_pub_; - rclcpp::Publisher::SharedPtr plane_normal_pub_; -} - -} // namespace valve_detection - -#endif // VALVE_POSE_DEPTH_ROS_HPP \ No newline at end of file diff --git a/include/valve_detection_ros/valve_pose_ros.hpp b/include/valve_detection_ros/valve_pose_ros.hpp new file mode 100644 index 0000000..51d645a --- /dev/null +++ b/include/valve_detection_ros/valve_pose_ros.hpp @@ -0,0 +1,310 @@ +#ifndef VALVE_POSE_DEPTH_ROS_HPP +#define VALVE_POSE_DEPTH_ROS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include "valve_detection/valve_detector.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace valve_detection { + +/** + * @class ValvePoseNode + * @brief ROS 2 node for computing valve poses from depth images or point clouds + * and 2D detections. + * + * This node subscribes to depth images or point clouds, color images + * (optional), and 2D detection messages. It synchronizes the inputs, computes + * valve poses using ValveDetector, and publishes results as PoseArray and + * optionally as visualized point clouds or images. + */ +class ValvePoseNode : public rclcpp::Node { + public: + /** + * @brief Constructs the ValvePoseNode with ROS node options. + * @param options Node options. + */ + explicit ValvePoseNode(const rclcpp::NodeOptions& options); + + virtual ~ValvePoseNode() = default; + + private: + /** + * @brief Callback for camera info subscription to obtain intrinsics and + * image size. + * @param camera_info_msg Shared pointer to the CameraInfo message. + */ + void color_image_info_callback( + const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg); + + /** + * @brief Initializes the ValveDetector with the specified ros parameters. + */ + void init_valve_detector(); + + /** + * @brief Initializes the angle detector inside the ValveDetector if angle + * calculation is enabled. + */ + void init_angle_detector(); + + /** + * @brief Converts a vision_msgs::BoundingBox2D to local BoundingBox type. + * @param bbox The input 2D bounding box message. + * @return Converted BoundingBox. + */ + BoundingBox to_bounding_box( + const vision_msgs::msg::BoundingBox2D& bbox) const; + + /** + * @brief Publishes a point cloud containing annulus points. + * @param cloud The point cloud to publish. + * @param header ROS message header to use for publication. + */ + void publish_annulus_pcl(const pcl::PointCloud::Ptr& cloud, + const std_msgs::msg::Header& header) const; + + /** + * @brief Publishes a point cloud containing segmented plane points. + * @param cloud The point cloud to publish. + * @param header ROS message header to use for publication. + */ + void publish_annulus_plane(const pcl::PointCloud::Ptr& cloud, + const std_msgs::msg::Header& header) const; + + /** + * @brief Publishes computed valve poses as a PoseArray message. + * @param poses Vector of computed valve poses. + * @param header ROS message header to use for publication. + */ + void publish_valve_poses(const std::vector& poses, + const std_msgs::msg::Header& header) const; + + /** + * @brief Configures message_filters synchronizers based on available + * inputs. + */ + void setup_sync(); + + /** + * @brief Callback for Depth Image + Color Image + Detections + * synchronization. + */ + void di_ci_d_callback( + const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, + const sensor_msgs::msg::Image::ConstSharedPtr& color_image, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); + + /** + * @brief Callback for Depth Image + Detections synchronization (no color + * image). + */ + void di_d_callback( + const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); + + /** + * @brief Callback for Point Cloud + Color Image + Detections + * synchronization. + */ + void pc_ci_d_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& pcl, + const sensor_msgs::msg::Image::ConstSharedPtr& color_image, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); + + /** + * @brief Callback for Point Cloud + Detections synchronization (no color + * image). + */ + void pc_d_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& pcl, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); + + /** + * @brief Generic template callback for synchronized inputs. + * + * Supports T = sensor_msgs::msg::Image (depth image) or PointCloud2. + * Handles optional color image and converts messages to OpenCV/PCL formats. + * + * @tparam T Depth input message type. + * @param depth_msg Depth image or point cloud message. + * @param color_image_msg Optional color image message. + * @param detections 2D detection message. + */ + template + void synchronized_callback( + const std::shared_ptr& depth_msg, + const sensor_msgs::msg::Image::ConstSharedPtr& color_image_msg, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { + if (!color_image_info_received_) { + return; + } + if (detections->detections.empty() && !visualize_detections_) { + return; + } + + cv::Mat cv_depth_image; + sensor_msgs::msg::PointCloud2 pcl_tf; + cv::Mat cv_color_image; + + if constexpr (std::is_same::value) { + std::string source_frame = depth_msg->header.frame_id; + std::string target_frame = color_image_frame_id_; + + try { + geometry_msgs::msg::TransformStamped transform = + tf_buffer_->lookupTransform( + target_frame, source_frame, + rclcpp::Time(depth_msg->header.stamp)); + + tf2::doTransform(*depth_msg, pcl_tf, transform); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), + "Failed to transform point cloud: %s", ex.what()); + return; + } + pcl::PointCloud::Ptr cloud( + new pcl::PointCloud); + pcl::fromROSMsg(pcl_tf, *cloud); + + } else if constexpr (std::is_same::value) { + cv_bridge::CvImageConstPtr cv_depth_ptr = + cv_bridge::toCvShare(depth_msg, "32FC1"); + cv_depth_image = cv_depth_ptr->image; + } else { + RCLCPP_ERROR(this->get_logger(), + "Unsupported message type in synchronized_callback."); + return; + } + + if (use_color_image_) { + if (!color_image_msg) { + RCLCPP_ERROR(this->get_logger(), + "Color image message is null."); + return; + } + cv_bridge::CvImageConstPtr cv_ptr = + cv_bridge::toCvShare(color_image_msg, "bgr8"); + + cv_color_image = cv_ptr->image; + } + + std::vector boxes; + boxes.reserve(detections->detections.size()); + for (const auto& detection : detections->detections) { + boxes.push_back(to_bounding_box(detection.bbox)); + } + + std::vector poses; + poses.reserve(boxes.size()); + + pcl::PointCloud::Ptr all_annulus_cloud; + pcl::PointCloud::Ptr all_annulus_plane_cloud; + + if constexpr (std::is_same::value) { + valve_detector_->Compute_valve_poses( + pcl_tf, cv_color_image, boxes, poses, all_annulus_cloud, + all_annulus_plane_cloud, pcl_visualize_, calculate_angle_); + } else if constexpr (std::is_same::value) { + valve_detector_->Compute_valve_poses( + cv_depth_image, cv_color_image, boxes, poses, all_annulus_cloud, + all_annulus_plane_cloud, pcl_visualize_, calculate_angle_); + } + if (pcl_visualize_ && all_annulus_cloud && all_annulus_plane_cloud) { + publish_annulus_pcl(all_annulus_cloud, depth_msg->header); + publish_annulus_plane(all_annulus_plane_cloud, depth_msg->header); + } + if (visualize_detections_) { + cv::Mat visualized_image = + valve_detector_->draw_detections(cv_color_image, boxes, poses); + sensor_msgs::msg::Image::SharedPtr output_msg = + cv_bridge::CvImage(depth_msg->header, "bgr8", visualized_image) + .toImageMsg(); + processed_image_pub_->publish(*output_msg); + } + publish_valve_poses(poses, depth_msg->header); + } + + rclcpp::Subscription::SharedPtr + color_image_info_sub_; + message_filters::Subscriber color_image_sub_; + message_filters::Subscriber pcl_sub_; + message_filters::Subscriber depth_image_sub_; + message_filters::Subscriber + detections_sub_; + + rclcpp::Publisher::SharedPtr + valve_poses_pub_; + rclcpp::Publisher::SharedPtr + annulus_pcl_pub_; + rclcpp::Publisher::SharedPtr + annulus_plane_pub_; + rclcpp::Publisher::SharedPtr processed_image_pub_; + + // Depth image sync policies uses ExactTime with the assumption + // that the depth image and color image are from the same camera + // and are synchronized. + + // Policy for Depth Image + Color Image + Detections + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::msg::Image, + sensor_msgs::msg::Image, + vision_msgs::msg::Detection2DArray> + SyncPolicy_DI_CI_D; + + // Policy for Depth Image + Detections (no color image) + typedef message_filters::sync_policies:: + ExactTime + SyncPolicy_DI_D; + + // Policy for Point Cloud + Color Image + Detections + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::PointCloud2, + sensor_msgs::msg::Image, + vision_msgs::msg::Detection2DArray> + SyncPolicy_PC_CI_D; + + // Policy for Point Cloud + Detections (no color image) + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::PointCloud2, + vision_msgs::msg::Detection2DArray> + SyncPolicy_PC_D; + + std::shared_ptr> + sync_di_ci_d_; + std::shared_ptr> sync_di_d_; + std::shared_ptr> + sync_pc_ci_d_; + std::shared_ptr> sync_pc_d_; + + bool color_image_info_received_ = false; + std::unique_ptr valve_detector_; + std::string color_image_frame_id_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + bool visualize_detections_ = true; + bool pcl_visualize_ = false; + bool calculate_angle_ = true; + bool use_color_image_ = true; + bool use_depth_image_ = true; +}; + +} // namespace valve_detection + +#endif // VALVE_POSE_DEPTH_ROS_HPP diff --git a/launch/valve_detection.launch.py b/launch/valve_detection.launch.py index 0ea4591..28ee6b0 100644 --- a/launch/valve_detection.launch.py +++ b/launch/valve_detection.launch.py @@ -1,24 +1,33 @@ import os from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode from launch import LaunchDescription +config_file = os.path.join( + get_package_share_directory("valve_detection"), + "config", + "valve_detection_params.yaml", +) + def generate_launch_description(): - valve_detection_node = Node( - package='valve_detection', - executable='valve_detection_node', - name='valve_detection_node', - parameters=[ - os.path.join( - get_package_share_directory('valve_detection'), - 'config', - 'valve_detection_params.yaml', + container = ComposableNodeContainer( + name='valve_detection_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='valve_detection', + plugin='valve_detection::ValvePoseNode', + name='valve_pose_node', + parameters=[config_file], ), - {'use_sim_time': True}, ], output='screen', ) - return LaunchDescription([valve_detection_node]) + + return LaunchDescription([container]) diff --git a/package.xml b/package.xml index 19b40b4..7c4730e 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,7 @@ geometry_msgs std_msgs pcl_conversions + tf2_geometry_msgs ament_lint_auto ament_lint_common diff --git a/src/angle_detector.cpp b/src/angle_detector.cpp new file mode 100644 index 0000000..649fc69 --- /dev/null +++ b/src/angle_detector.cpp @@ -0,0 +1,81 @@ +#include "valve_detection/angle_detector.hpp" +#include +#include "valve_detection/types.hpp" + +namespace valve_detection { + +double AngleDetector::calculate_angle(const cv::Mat& color_image, + const BoundingBox& bbox) const { + double width = bbox.size_x; + double height = bbox.size_y; + double center_x = bbox.center_x; + double center_y = bbox.center_y; + + int x1 = std::max( + static_cast(center_x - width * params_.line_detection_area), 0); + int y1 = std::max( + static_cast(center_y - height * params_.line_detection_area), 0); + int x2 = std::min( + static_cast(center_x + width * params_.line_detection_area), + color_image.cols - 1); + int y2 = std::min( + static_cast(center_y + height * params_.line_detection_area), + color_image.rows - 1); + + cv::Mat roi = color_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); + + cv::Mat mask = cv::Mat::zeros(roi.size(), CV_8UC1); + cv::Point center((roi.cols / 2), (roi.rows / 2)); + cv::Size axes(roi.cols / 2, roi.rows / 2); + cv::ellipse(mask, center, axes, 0, 0, 360, cv::Scalar(255), -1); + + cv::Mat masked_roi; + roi.copyTo(masked_roi, mask); + + cv::Mat gray; + cv::cvtColor(masked_roi, gray, cv::COLOR_BGR2GRAY); + + cv::Mat edges = apply_canny_edge_detection(gray); + + std::vector lines; + cv::HoughLinesP(edges, lines, params_.hough_rho_res, + params_.hough_theta_res, params_.hough_threshold, + params_.hough_min_line_length, params_.hough_max_line_gap); + + if (lines.empty()) { + return std::numeric_limits::quiet_NaN(); + } + + cv::Vec4i longest_line = find_longest_line(lines); + return std::atan2(longest_line[3] - longest_line[1], + longest_line[2] - longest_line[0]); +} + +cv::Vec4i AngleDetector::find_longest_line( + const std::vector& lines) const { + if (lines.empty()) { + return cv::Vec4i(0, 0, 0, 0); + } + + cv::Vec4i longest_line = lines[0]; + double max_length = 0; + + for (const auto& line : lines) { + double length = std::hypot(line[2] - line[0], line[3] - line[1]); + if (length > max_length) { + max_length = length; + longest_line = line; + } + } + return longest_line; +} + +cv::Mat AngleDetector::apply_canny_edge_detection( + const cv::Mat& gray_image) const { + cv::Mat edges; + cv::Canny(gray_image, edges, params_.canny_low_threshold, + params_.canny_high_threshold, params_.canny_aperture_size); + return edges; +} + +} // namespace valve_detection diff --git a/src/depth_image_processing.cpp b/src/depth_image_processing.cpp new file mode 100644 index 0000000..c76fee4 --- /dev/null +++ b/src/depth_image_processing.cpp @@ -0,0 +1,88 @@ +#include "valve_detection/depth_image_processing.hpp" + +namespace valve_detection { + +void project_pixel_to_point(int u, + int v, + float depth, + double fx, + double fy, + double cx, + double cy, + pcl::PointXYZ& point) { + if (depth <= 0 || depth == std::numeric_limits::infinity() || + std::isnan(depth)) { + point.x = point.y = point.z = std::numeric_limits::quiet_NaN(); + return; + } + + point.x = (u - cx) * depth / fx; + point.y = (v - cy) * depth / fy; + point.z = depth; +} + +void extract_annulus_pcl(const cv::Mat& depth_image, + const BoundingBox& bbox, + const ImageProperties& image_properties, + float annulus_radius_ratio, + pcl::PointCloud::Ptr& cloud) { + float center_x = bbox.center_x; + float center_y = bbox.center_y; + float outer_radius_x = bbox.size_x / 2.0f; + float outer_radius_y = bbox.size_y / 2.0f; + + float inner_radius_x = outer_radius_x * annulus_radius_ratio; + float inner_radius_y = outer_radius_y * annulus_radius_ratio; + + int upper_bound_size = static_cast( + M_PI * + ((outer_radius_x * outer_radius_y - inner_radius_x * inner_radius_y) + + 1)); + + cloud->clear(); + cloud->reserve(upper_bound_size); + + for (int y = static_cast(-outer_radius_y); + y <= static_cast(outer_radius_y); ++y) { + for (int x = static_cast(-outer_radius_x); + x <= static_cast(outer_radius_x); ++x) { + float dx_scaled_outer = x / outer_radius_x; + float dy_scaled_outer = y / outer_radius_y; + bool is_inside_outer_ellipse = + (dx_scaled_outer * dx_scaled_outer + + dy_scaled_outer * dy_scaled_outer) <= 1.0f; + + float dx_scaled_inner = x / inner_radius_x; + float dy_scaled_inner = y / inner_radius_y; + bool is_outside_inner_ellipse = + (dx_scaled_inner * dx_scaled_inner + + dy_scaled_inner * dy_scaled_inner) > 1.0f; + + if (is_inside_outer_ellipse && is_outside_inner_ellipse) { + int u = static_cast(center_x + x); + int v = static_cast(center_y + y); + + if (u >= 0 && u < depth_image.cols && v >= 0 && + v < depth_image.rows) { + float depth_value = depth_image.at(v, u); + pcl::PointXYZ point; + project_pixel_to_point( + u, v, depth_value, image_properties.intr.fx, + image_properties.intr.fy, image_properties.intr.cx, + image_properties.intr.cy, point); + + if (!std::isnan(point.x) && !std::isnan(point.y) && + !std::isnan(point.z)) { + cloud->points.push_back(point); + } + } + } + } + } + + cloud->width = static_cast(cloud->points.size()); + cloud->height = 1; + cloud->is_dense = false; +} + +} // namespace valve_detection diff --git a/src/pointcloud_processing.cpp b/src/pointcloud_processing.cpp new file mode 100644 index 0000000..707aa69 --- /dev/null +++ b/src/pointcloud_processing.cpp @@ -0,0 +1,59 @@ +#include "valve_detection/pointcloud_processing.hpp" + +namespace valve_detection { + +void extract_annulus_pcl(const pcl::PointCloud::Ptr& input_cloud, + const BoundingBox& bbox, + const ImageProperties& image_properties, + float annulus_radius_ratio, + pcl::PointCloud::Ptr& cloud) { + float center_x = bbox.center_x; + float center_y = bbox.center_y; + float outer_radius_x = bbox.size_x / 2.0f; + float outer_radius_y = bbox.size_y / 2.0f; + + float inner_radius_x = outer_radius_x * annulus_radius_ratio; + float inner_radius_y = outer_radius_y * annulus_radius_ratio; + + double fx = image_properties.intr.fx; + double fy = image_properties.intr.fy; + double cx = image_properties.intr.cx; + double cy = image_properties.intr.cy; + + cloud->clear(); + cloud->points.reserve(input_cloud->points.size()); + + for (const auto& point_in : input_cloud->points) { + if (std::isnan(point_in.z)) { + continue; + } + + int u = static_cast((point_in.x * fx / point_in.z) + cx); + int v = static_cast((point_in.y * fy / point_in.z) + cy); + + if (u >= center_x - outer_radius_x && u <= center_x + outer_radius_x && + v >= center_y - outer_radius_y && v <= center_y + outer_radius_y) { + float dx_scaled_outer = (u - center_x) / outer_radius_x; + float dy_scaled_outer = (v - center_y) / outer_radius_y; + bool is_inside_outer_ellipse = + (dx_scaled_outer * dx_scaled_outer + + dy_scaled_outer * dy_scaled_outer) <= 1.0f; + + float dx_scaled_inner = (u - center_x) / inner_radius_x; + float dy_scaled_inner = (v - center_y) / inner_radius_y; + bool is_outside_inner_ellipse = + (dx_scaled_inner * dx_scaled_inner + + dy_scaled_inner * dy_scaled_inner) > 1.0f; + + if (is_inside_outer_ellipse && is_outside_inner_ellipse) { + cloud->points.push_back(point_in); + } + } + } + + cloud->width = static_cast(cloud->points.size()); + cloud->height = 1; + cloud->is_dense = false; +} + +} // namespace valve_detection diff --git a/src/valve_detection.cpp b/src/valve_detection.cpp deleted file mode 100644 index 482751f..0000000 --- a/src/valve_detection.cpp +++ /dev/null @@ -1,600 +0,0 @@ -#include - -using std::placeholders::_1; -using std::placeholders::_2; -using std::placeholders::_3; - -ValveDetectionNode::ValveDetectionNode(const rclcpp::NodeOptions& options) - : Node("valve_detection_node", options) { - auto depth_image_sub_topic = - this->declare_parameter("depth_image_sub_topic"); - auto color_image_sub_topic = - this->declare_parameter("color_image_sub_topic"); - auto detections_sub_topic = - this->declare_parameter("detections_sub_topic"); - - line_detection_area_ = - this->declare_parameter("line_detection_area", 5); - - rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(10)) - .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); - depth_image_sub_.subscribe(this, depth_image_sub_topic, - qos.get_rmw_qos_profile()); - color_image_sub_.subscribe(this, color_image_sub_topic, - qos.get_rmw_qos_profile()); - detections_sub_.subscribe(this, detections_sub_topic, - qos.get_rmw_qos_profile()); - - sync_ = std::make_shared>( - MySyncPolicy(10), depth_image_sub_, color_image_sub_, detections_sub_); - sync_->registerCallback(std::bind( - &ValveDetectionNode::synchronized_callback, this, _1, _2, _3)); - - // Create subscriptions with synchronized callbacks - camera_info_subscription_yolo_color_ = - this->create_subscription( - "/yolov8_encoder/resize/camera_info", 10, - std::bind(&ValveDetectionNode::camera_info_callback_yolo_colored, - this, std::placeholders::_1)); - - // Create subscriptions with synchronized callbacks - camera_info_subscription_ = - this->create_subscription( - "/zed_node/depth/camera_info", 10, - std::bind(&ValveDetectionNode::camera_info_callback, this, - std::placeholders::_1)); - - processed_image_pub_ = this->create_publisher( - "yolov8_valve_detection_image", 10); - plane_normal_pub_ = - this->create_publisher("plane_normal", 10); - pointcloud_pub_ = this->create_publisher( - "point_cloud", 10); - line_pose_pub_ = this->create_publisher( - "valve_pose", 10); - line_points_pub_ = this->create_publisher( - "line_points", 10); - near_plane_cloud_pub_ = - this->create_publisher( - "near_plane_cloud", 10); - canny_debug_image_pub_ = this->create_publisher( - "canny_valve_detection_image", 10); -} - -Eigen::Vector3f ValveDetectionNode::filter_direction_ = - Eigen::Vector3f(1, 0, 0); - -void ValveDetectionNode::camera_info_callback( - const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg) { - if (!camera_info_received_) { - camera_info_ = camera_info_msg; - camera_info_received_ = true; - RCLCPP_INFO(this->get_logger(), - "Camera resize info received and stored."); - - compute_height_width_scalars(); - // unsubscribe to avoid further callback executions - camera_info_subscription_.reset(); - } -} - -void ValveDetectionNode::camera_info_callback_yolo_colored( - const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg) { - if (!camera_info_received_yolo_color_) { - camera_info_yolo_color_ = camera_info_msg; - camera_info_received_yolo_color_ = true; - RCLCPP_INFO(this->get_logger(), - "Camera yolo color info received and stored."); - - compute_height_width_scalars(); - // unsubscribe to avoid further callback executions - camera_info_subscription_yolo_color_.reset(); - } -} - -void ValveDetectionNode::compute_height_width_scalars() { - if (camera_info_received_ && camera_info_received_yolo_color_) { - height_scalar_ = - static_cast(camera_info_yolo_color_->height - 280) / - camera_info_->height; - width_scalar_ = static_cast(camera_info_yolo_color_->width) / - camera_info_->width; - - RCLCPP_INFO(this->get_logger(), "Height scalar: %.3f", height_scalar_); - RCLCPP_INFO(this->get_logger(), "Width scalar: %.3f", width_scalar_); - } -} - -void ValveDetectionNode::project_pixel_to_3d(int u, - int v, - float depth, - pcl::PointXYZ& point) { - if (!camera_info_ || depth <= 0) { - return; // Skip invalid depth or missing camera info - } - - // Extract intrinsic parameters - double fx = camera_info_->k[0]; // k[0] = fx - double fy = camera_info_->k[4]; // k[4] = fy - double cx = camera_info_->k[2]; // k[2] = cx - double cy = camera_info_->k[5]; // k[5] = cy - - // Calculate 3D point coordinates - // transform from optical to camera frame - point.x = depth; - point.y = -(u - cx) * depth / fx; - point.z = -(v - cy) * depth / fy; -} - -void ValveDetectionNode::project_3d_to_pixel(float x, - float y, - float z, - int& u, - int& v) { - if (!camera_info_ || z <= 0) { - return; // Ensure camera info is available and z is positive - } - - // Extract intrinsic parameters - double fx = camera_info_->k[0]; // Focal length in x direction - double fy = camera_info_->k[4]; // Focal length in y direction - double cx = camera_info_->k[2]; // Principal point in x direction - double cy = camera_info_->k[5]; // Principal point in y direction - - // Project the 3D point to 2D pixel coordinates - // transform from camera to optical frame - u = static_cast((-y * fx) / x + cx); - v = static_cast((-z * fy) / x + cy); -} - -void ValveDetectionNode::synchronized_callback( - const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, - const sensor_msgs::msg::Image::ConstSharedPtr& color_image, - const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { - RCLCPP_INFO(this->get_logger(), - "Synchronized image and detection messages received."); - - process_and_publish_image(depth_image, color_image, detections); - // process_and_publish_image(depth_image, detections); -} - -void ValveDetectionNode::process_and_publish_image( - const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, - const sensor_msgs::msg::Image::ConstSharedPtr& color_image, - const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { - if (!camera_info_received_ || !camera_info_received_yolo_color_) { - return; - } - - try { - // Convert depth and color images - cv::Mat cv_depth_image = - cv_bridge::toCvCopy(depth_image, "32FC1")->image; - cv::Mat cv_color_image = - cv_bridge::toCvCopy(color_image, "bgr8")->image; - - if (!detections->detections.empty()) { - const auto* first_detection = &detections->detections[0]; - - // The most centred of the two most likely variables are chosen as a - // boundingbox. Calculate distance to center (cx, cy) - double cx = camera_info_->k[2]; - double cy = camera_info_->k[5]; - if (detections->detections.size() > 1) { - float dist_first = std::sqrt( - std::pow( - detections->detections[0].bbox.center.position.x - cx, - 2) + - std::pow( - detections->detections[0].bbox.center.position.y - cy, - 2)); - - float dist_second = std::sqrt( - std::pow( - detections->detections[1].bbox.center.position.x - cx, - 2) + - std::pow( - detections->detections[1].bbox.center.position.y - cy, - 2)); - - // Select the detection that is closer to the center. - if (dist_first > dist_second) { - first_detection = &detections->detections[1]; - } - } - - int center_x = static_cast( - first_detection->bbox.center.position.x / this->width_scalar_); - int center_y = static_cast( - (first_detection->bbox.center.position.y - 140) / - this->height_scalar_); - int width = static_cast(first_detection->bbox.size_x / - this->width_scalar_); - int height = static_cast(first_detection->bbox.size_y / - this->height_scalar_); - - pcl::PointCloud::Ptr cloud( - new pcl::PointCloud); - bool points_added = false; - - for (int y = center_y - (height / 2); y <= center_y + (height / 2); - ++y) { - for (int x = center_x - (width / 2); - x <= center_x + (width / 2); ++x) { - int dx = x - center_x; - int dy = y - center_y; - if (dx * dx / (float)((width / 2) * (width / 2)) + - dy * dy / (float)((height / 2) * (height / 2)) <= - 1.0) { - if (y >= 0 && y < cv_depth_image.rows && x >= 0 && - x < cv_depth_image.cols) { - float depth_value = cv_depth_image.at(y, x); - pcl::PointXYZ point; - project_pixel_to_3d(x, y, depth_value, point); - - if (depth_value > 0) // Check for valid depth - { - cloud->points.push_back(point); - points_added = true; - } - } - } - } - } - - if (points_added && cloud->points.size() > 0) { - sensor_msgs::msg::PointCloud2 cloud_msg; - pcl::toROSMsg(*cloud, cloud_msg); - cloud_msg.header.stamp = depth_image->header.stamp; - cloud_msg.header.frame_id = "zed_left_camera_frame"; - - pointcloud_pub_->publish(cloud_msg); - - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setDistanceThreshold(0.01); - - pcl::ModelCoefficients::Ptr coefficients( - new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers(new pcl::PointIndices); - seg.setInputCloud(cloud); - seg.segment(*inliers, *coefficients); - - if (inliers->indices.size() > 0) { - geometry_msgs::msg::Vector3 normal_msg; - normal_msg.x = coefficients->values[0]; - normal_msg.y = coefficients->values[1]; - normal_msg.z = coefficients->values[2]; - plane_normal_pub_->publish(normal_msg); - - RCLCPP_INFO( - this->get_logger(), - "RANSAC successful: Plane normal (%.3f, %.3f, %.3f)", - coefficients->values[0], coefficients->values[1], - coefficients->values[2]); - - pcl::PointCloud::Ptr near_plane_cloud( - new pcl::PointCloud); - - // Select points within a distance threshold of the plane - // model - pcl::ExtractIndices extract; - extract.setInputCloud(cloud); - extract.setIndices(inliers); - extract.setNegative(false); // Select the inliers - extract.filter(*near_plane_cloud); - - // Now we can use `selectWithinDistance` to select inliers - // that are within the distance threshold - pcl::PointIndices::Ptr selected_inliers( - new pcl::PointIndices()); - float threshold_distance = - 0.02; // Example threshold (3 cm) - - // Logic for selecting points close to the - // threshold_distance of the plane. - for (size_t i = 0; i < cloud->points.size(); ++i) { - const auto& point = cloud->points[i]; - float distance = - std::abs(coefficients->values[0] * point.x + - coefficients->values[1] * point.y + - coefficients->values[2] * point.z + - coefficients->values[3]) / - std::sqrt(coefficients->values[0] * - coefficients->values[0] + - coefficients->values[1] * - coefficients->values[1] + - coefficients->values[2] * - coefficients->values[2]); - if (distance < threshold_distance) { - selected_inliers->indices.push_back(i); - } - } - RCLCPP_INFO(this->get_logger(), "Selected inlier size: %zu", - selected_inliers->indices.size()); - - // Extract the selected inliers - pcl::PointCloud::Ptr final_inliers_cloud( - new pcl::PointCloud); - - for (const auto& idx : selected_inliers->indices) { - // Convert 3D point (x, y) to image space (u, v) - int u, v; - const auto& point = cloud->points[idx]; - project_3d_to_pixel(point.x, point.y, point.z, u, v); - - // Check if the point (u, v) is inside the smaller - // ellipse - int dx = u - center_x; - int dy = v - center_y; - - // Check if point is inside the smaller ellipse in image - // space - float a = static_cast(width) / - 6.0f; // Semi-major axis - float b = static_cast(height) / - 6.0f; // Semi-minor axis - - if ((dx * dx) / (a * a) + (dy * dy) / (b * b) <= 1.0f) { - // Point is inside the ellipse, add it to the - // near_plane_cloud - final_inliers_cloud->points.push_back( - cloud->points[idx]); - } - } - - near_plane_cloud = final_inliers_cloud; - - // Calculate centroid position from near_plane_cloud - Eigen::Vector3f centroid(0, 0, 0); - if (!near_plane_cloud->empty()) { - for (const auto& point : near_plane_cloud->points) { - centroid += - Eigen::Vector3f(point.x, point.y, point.z); - } - centroid /= near_plane_cloud->points.size(); - } else { - RCLCPP_WARN(this->get_logger(), - "No valid near plane points found."); - return; - } - - int x1 = std::max( - center_x - width * line_detection_area_ / 100, 0); - int y1 = std::max( - center_y - height * line_detection_area_ / 100, 0); - int x2 = - std::min(center_x + width * line_detection_area_ / 100, - cv_color_image.cols - 1); - int y2 = - std::min(center_y + height * line_detection_area_ / 100, - cv_color_image.rows - 1); - // // Draw roi for debugging - // cv::rectangle(cv_color_image, cv::Point(x1, y1), - // cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); - - // // Extract ROI (region of interest) from the color image - // cv::Mat roi = cv_color_image(cv::Rect(x1, y1, x2 - x1, y2 - // - y1)); - - // Define the ellipse parameters - cv::Point center((x1 + x2) / 2, - (y1 + y2) / 2); // Center of the ellipse - cv::Size axes( - (x2 - x1) / 2, - (y2 - y1) / 2); // Semi-major and semi-minor axes - - // Create a mask for the elliptical ROI - cv::Mat mask = - cv::Mat::zeros(cv_color_image.size(), CV_8UC1); - cv::ellipse(mask, center, axes, 0, 0, 360, cv::Scalar(255), - -1); // Draw a filled ellipse - - // Apply the mask to the ROI - cv::Mat roi = - cv_color_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); - cv::Mat masked_roi; - cv::bitwise_and(roi, roi, masked_roi, - mask(cv::Rect(x1, y1, x2 - x1, y2 - y1))); - - // For debugging: Draw the ellipse on the original image - cv::ellipse(cv_color_image, center, axes, 0, 0, 360, - cv::Scalar(0, 255, 0), 2); - - // Convert to grayscale for edge detection - cv::Mat gray; - cv::cvtColor(roi, gray, cv::COLOR_BGR2GRAY); - - // Apply Canny edge detection - cv::Mat edges; - cv::Canny(gray, edges, 50, 100, 3); - canny_debug_image_pub_->publish( - *cv_bridge::CvImage(color_image->header, "mono8", edges) - .toImageMsg()); - - // Detect lines using Hough Transform - std::vector lines; - cv::HoughLinesP(edges, lines, 1, CV_PI / 180, 20, 45, 5); - - if (!lines.empty()) { - cv::Vec4i longest_line = lines[0]; - double max_length = 0; - - for (const auto& line : lines) { - double length = std::hypot(line[2] - line[0], - line[3] - line[1]); - if (length > max_length) { - max_length = length; - longest_line = line; - } - } - - double angle = - std::atan2(longest_line[3] - longest_line[1], - longest_line[2] - longest_line[0]); - - Eigen::Vector3f plane_normal(coefficients->values[0], - coefficients->values[1], - coefficients->values[2]); - plane_normal.normalize(); - - // Find an arbitrary vector perpendicular to the plane - // normal - Eigen::Vector3f perp_vector = - (std::abs(plane_normal.z()) < 0.99) - ? Eigen::Vector3f(0, 0, 1) - .cross(plane_normal) - .normalized() - : Eigen::Vector3f(1, 0, 0) - .cross(plane_normal) - .normalized(); - - // Rotate perp_vector by `angle` around `plane_normal` - // to get x-axis - Eigen::AngleAxisf rotation(angle, plane_normal); - Eigen::Vector3f x_axis = - rotation * perp_vector; // Rotated vector in-plane - - // Filter the x-axis direction to maintain consistency - if (filter_direction_.dot(x_axis) < 0) { - x_axis = -x_axis; // Flip to maintain consistency - } - - // Update filter_direction_ - filter_direction_ = x_axis; - - // Compute y-axis (perpendicular to both x and z) - Eigen::Vector3f y_axis = - plane_normal.cross(x_axis).normalized(); - - // Construct rotation matrix - Eigen::Matrix3f rotation_matrix; - rotation_matrix.col(0) = x_axis; - rotation_matrix.col(1) = y_axis; - rotation_matrix.col(2) = - plane_normal; // z-axis is the plane normal - - // Convert rotation matrix to quaternion - Eigen::Quaternionf quaternion(rotation_matrix); - - if (std::isnan(quaternion.x()) || - std::isnan(quaternion.y()) || - std::isnan(quaternion.z()) || - std::isnan(quaternion.w())) { - RCLCPP_WARN(this->get_logger(), - "Invalid quaternion computed, skipping " - "pose publishing."); - return; - } - - geometry_msgs::msg::PoseStamped line_pose_msg; - line_pose_msg.header.stamp = color_image->header.stamp; - line_pose_msg.header.frame_id = "zed_left_camera_frame"; - - line_pose_msg.pose.position.x = centroid[0]; - line_pose_msg.pose.position.y = centroid[1]; - line_pose_msg.pose.position.z = centroid[2]; - - line_pose_msg.pose.orientation.x = quaternion.x(); - line_pose_msg.pose.orientation.y = quaternion.y(); - line_pose_msg.pose.orientation.z = quaternion.z(); - line_pose_msg.pose.orientation.w = quaternion.w(); - - line_pose_pub_->publish(line_pose_msg); - - RCLCPP_INFO(this->get_logger(), - "Detected line published."); - } else { - RCLCPP_WARN(this->get_logger(), - "No lines detected in bounding box."); - } - if (!cv_color_image.empty()) { - // Draw the bounding box on the color image - int x1 = std::max(center_x - width / 2, 0); - int y1 = std::max(center_y - height / 2, 0); - int x2 = std::min(center_x + width / 2, - cv_color_image.cols - 1); - int y2 = std::min(center_y + height / 2, - cv_color_image.rows - 1); - - cv::rectangle(cv_color_image, cv::Point(x1, y1), - cv::Point(x2, y2), cv::Scalar(0, 255, 0), - 2); - - // Draw the detected line on the color image - if (!lines.empty()) { - cv::Vec4i longest_line = lines[0]; - double max_length = 0; - - // Find the longest line in the ROI - for (const auto& line : lines) { - double length = std::hypot(line[2] - line[0], - line[3] - line[1]); - if (length > max_length) { - max_length = length; - longest_line = line; - } - } - - // Adjust line coordinates to the full image size - // using the same x1 ... as roi calculation - int x1 = std::max( - center_x - width * line_detection_area_ / 100, - 0); - int y1 = std::max( - center_y - height * line_detection_area_ / 100, - 0); - int x2 = std::min( - center_x + width * line_detection_area_ / 100, - cv_color_image.cols - 1); - int y2 = std::min( - center_y + height * line_detection_area_ / 100, - cv_color_image.rows - 1); - cv::Point pt1( - longest_line[0] + x1, - longest_line[1] + y1); // Add ROI offset - cv::Point pt2( - longest_line[2] + x1, - longest_line[3] + y1); // Add ROI offset - - // Draw the line on the full color image - cv::line(cv_color_image, pt1, pt2, - cv::Scalar(0, 0, 255), 2); - - // Debugging output - RCLCPP_INFO( - this->get_logger(), - "ROI Coordinates: x1=%d, y1=%d, x2=%d, y2=%d", - x1, y1, x2, y2); - RCLCPP_INFO( - this->get_logger(), - "Longest Line in ROI: (%d, %d) -> (%d, %d)", - longest_line[0], longest_line[1], - longest_line[2], longest_line[3]); - RCLCPP_INFO(this->get_logger(), - "Adjusted Line in Full Image: (%d, %d) " - "-> (%d, %d)", - pt1.x, pt1.y, pt2.x, pt2.y); - } - } - } else { - RCLCPP_WARN(this->get_logger(), "No plane inliers found."); - } - } - } - // Convert the edited OpenCV image back to a ROS message - sensor_msgs::msg::Image::SharedPtr edited_image_msg = - cv_bridge::CvImage(color_image->header, "bgr8", cv_color_image) - .toImageMsg(); - - // Publish the edited image - processed_image_pub_->publish(*edited_image_msg); - - } catch (cv_bridge::Exception& e) { - RCLCPP_ERROR(this->get_logger(), "CvBridge Error: %s", e.what()); - } -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ValveDetectionNode) diff --git a/src/valve_detector.cpp b/src/valve_detector.cpp new file mode 100644 index 0000000..b35cd81 --- /dev/null +++ b/src/valve_detector.cpp @@ -0,0 +1,241 @@ +#include "valve_detection/valve_detector.hpp" + +namespace valve_detection { + +ValveDetector::ValveDetector(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) + : yolo_img_width_(yolo_img_width_), + yolo_img_height_(yolo_img_height_), + annulus_radius_ratio_(annulus_radius_ratio), + plane_ransac_threshold_(plane_ransac_threshold), + plane_ransac_max_iterations_(plane_ransac_max_iterations), + valve_handle_offset_(valve_handle_offset) {} + +void ValveDetector::init_angle_detector(const AngleDetectorParams& params) { + angle_detector_ = std::make_unique(params); +} + +cv::Mat ValveDetector::draw_detections(const cv::Mat& image, + const std::vector& boxes, + const std::vector& poses) const { + if (boxes.size() != poses.size()) { + return cv::Mat(); + } + + cv::Mat visualized_image = image.clone(); + + cv::Mat camera_matrix = + (cv::Mat_(3, 3) << color_image_properties_.intr.fx, 0, + color_image_properties_.intr.cx, 0, color_image_properties_.intr.fy, + color_image_properties_.intr.cy, 0, 0, 1); + cv::Mat dist_coeffs = cv::Mat::zeros(4, 1, CV_64F); + + for (size_t i = 0; i < boxes.size(); ++i) { + const auto& box = boxes[i]; + const auto& pose = poses[i]; + + int x1 = box.center_x - box.size_x / 2; + int y1 = box.center_y - box.size_y / 2; + int x2 = box.center_x + box.size_x / 2; + int y2 = box.center_y + box.size_y / 2; + cv::rectangle(visualized_image, cv::Point(x1, y1), cv::Point(x2, y2), + cv::Scalar(0, 255, 0), 2); + + cv::Mat tvec = (cv::Mat_(3, 1) << pose.position[0], + pose.position[1], pose.position[2]); + + Eigen::Matrix3f rotation_matrix = pose.orientation.toRotationMatrix(); + cv::Mat rmat = (cv::Mat_(3, 3) << rotation_matrix(0, 0), + rotation_matrix(0, 1), rotation_matrix(0, 2), + rotation_matrix(1, 0), rotation_matrix(1, 1), + rotation_matrix(1, 2), rotation_matrix(2, 0), + rotation_matrix(2, 1), rotation_matrix(2, 2)); + cv::Mat rvec; + cv::Rodrigues(rmat, rvec); + + cv::drawFrameAxes(visualized_image, camera_matrix, dist_coeffs, rvec, + tvec, 0.1); + } + return visualized_image; +} + +void ValveDetector::rmat_to_quat(const Eigen::Matrix3f& rotation_matrix, + Eigen::Quaternionf& quat) const { + quat = Eigen::Quaternionf(rotation_matrix); + quat.normalize(); +} + +void ValveDetector::project_point_to_pixel(float x, + float y, + float z, + int& u, + int& v) const { + if (z <= 0) { + u = v = -1; // Invalid pixel coordinates + return; + } + + double fx = color_image_properties_.intr.fx; + double fy = color_image_properties_.intr.fy; + double cx = color_image_properties_.intr.cx; + double cy = color_image_properties_.intr.cy; + + u = static_cast((x * fx / z) + cx); + v = static_cast((y * fy / z) + cy); +} + +void ValveDetector::calculate_letterbox_padding() { + int org_image_width = color_image_properties_.dim.width; + int org_image_height = color_image_properties_.dim.height; + + letterbox_scale_factor_ = + std::min(static_cast(yolo_img_width_) / org_image_width, + static_cast(yolo_img_height_) / org_image_height); + + double resized_width = org_image_width * letterbox_scale_factor_; + double resized_height = org_image_height * letterbox_scale_factor_; + + letterbox_pad_x_ = (yolo_img_width_ - resized_width) / 2.0; + letterbox_pad_y_ = (yolo_img_height_ - resized_height) / 2.0; +} + +BoundingBox ValveDetector::transform_bounding_box( + const BoundingBox& bbox) const { + BoundingBox transformed_bbox = bbox; + + transformed_bbox.center_x = + (bbox.center_x - letterbox_pad_x_) / letterbox_scale_factor_; + transformed_bbox.center_y = + (bbox.center_y - letterbox_pad_y_) / letterbox_scale_factor_; + transformed_bbox.size_x /= letterbox_scale_factor_; + transformed_bbox.size_y /= letterbox_scale_factor_; + + return transformed_bbox; +} + +bool ValveDetector::segment_plane( + const pcl::PointCloud::Ptr& cloud, + pcl::ModelCoefficients::Ptr& coefficients, + pcl::PointIndices::Ptr& inliers) const { + if (cloud->points.empty()) { + return false; + } + + pcl::SACSegmentation seg; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(plane_ransac_threshold_); + seg.setMaxIterations(plane_ransac_max_iterations_); + seg.setInputCloud(cloud); + seg.segment(*inliers, *coefficients); + + return !inliers->indices.empty(); +} + +Eigen::Vector3f ValveDetector::get_ray_direction( + const BoundingBox& bbox) const { + float u = bbox.center_x; + float v = bbox.center_y; + + float fx = color_image_properties_.intr.fx; + float fy = color_image_properties_.intr.fy; + float cx = color_image_properties_.intr.cx; + float cy = color_image_properties_.intr.cy; + + float xc = (u - cx) / fx; + float yc = (v - cy) / fy; + + Eigen::Vector3f ray_direction; + ray_direction << xc, yc, 1.0f; + + return ray_direction.normalized(); +} + +Eigen::Vector3f ValveDetector::compute_plane_normal( + const pcl::ModelCoefficients::Ptr& coefficients, + const Eigen::Vector3f& ray_direction) const { + Eigen::Vector3f normal_vector; + + if (!coefficients || coefficients->values.size() < 3) { + return Eigen::Vector3f::Zero(); + } + + normal_vector << coefficients->values[0], coefficients->values[1], + coefficients->values[2]; + + normal_vector.normalize(); + + if (normal_vector.dot(ray_direction) > 0.0) { + normal_vector = -normal_vector; + } + + return normal_vector; +} + +Eigen::Vector3f ValveDetector::find_ray_plane_intersection( + const pcl::ModelCoefficients::Ptr& coefficients, + const Eigen::Vector3f& ray_direction) const { + if (!coefficients || coefficients->values.size() < 4) { + return Eigen::Vector3f::Zero(); + } + + Eigen::Vector3f plane_normal; + plane_normal << coefficients->values[0], coefficients->values[1], + coefficients->values[2]; + float D = coefficients->values[3]; + + float normal_dot_ray = plane_normal.dot(ray_direction); + + if (std::abs(normal_dot_ray) < 1e-6) { + // The ray is parallel or contained within the plane. + return Eigen::Vector3f::Zero(); + } + + float t = -D / normal_dot_ray; + + Eigen::Vector3f intersection_point = t * ray_direction; + + return intersection_point; +} + +Eigen::Vector3f ValveDetector::shift_point_along_normal( + const Eigen::Vector3f& intersection_point, + const Eigen::Vector3f& plane_normal) const { + return intersection_point + (plane_normal * valve_handle_offset_); +} + +Eigen::Matrix3f ValveDetector::create_rotation_matrix( + const Eigen::Vector3f& plane_normal, + float angle) { + Eigen::Vector3f z_axis = plane_normal; + + Eigen::Vector3f temp_vec(1, 0, 0); + if (std::abs(z_axis.dot(temp_vec)) > 0.99f) { + temp_vec = Eigen::Vector3f(0, 1, 0); + } + Eigen::Vector3f base_x_axis = z_axis.cross(temp_vec).normalized(); + + Eigen::AngleAxisf rotation(angle, z_axis); + Eigen::Vector3f x_axis = rotation * base_x_axis; + + if (filter_direction_.dot(x_axis) < 0) { + x_axis = -x_axis; + } + filter_direction_ = x_axis; + + Eigen::Vector3f y_axis = z_axis.cross(x_axis).normalized(); + + Eigen::Matrix3f rotation_matrix; + rotation_matrix.col(0) = x_axis; + rotation_matrix.col(1) = y_axis; + rotation_matrix.col(2) = z_axis; + + return rotation_matrix; +} + +} // namespace valve_detection diff --git a/src/valve_pose_base.cpp b/src/valve_pose_base.cpp deleted file mode 100644 index b780d0d..0000000 --- a/src/valve_pose_base.cpp +++ /dev/null @@ -1,181 +0,0 @@ -#include "valve_detection/valve_pose_base.hpp" - -namespace valve_detection { - -ValvePoseBase::ValvePoseBase(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) - : yolo_img_width_(yolo_img_width_), yolo_img_height_(yolo_img_height_), - annulus_radius_ratio_(annulus_radius_ratio), - plane_ransac_threshold_(plane_ransac_threshold), - plane_ransac_max_iterations_(plane_ransac_max_iterations), - valve_handle_offset_(valve_handle_offset) { -} - -void ValvePoseBase::project_point_to_pixel(float x, float y, float z, int& u, int& v) const { - if (z <= 0) { - u = v = -1; // Invalid pixel coordinates - return; - } - - double fx = color_image_properties_.intr.fx; - double fy = color_image_properties_.intr.fy; - double cx = color_image_properties_.intr.cx; - double cy = color_image_properties_.intr.cy; - - u = static_cast((x * fx / z) + cx); - v = static_cast((y * fy / z) + cy); -} - -void ValvePoseBase::calculate_letterbox_padding() { - int org_image_width = color_image_properties_.dim.x; - int org_image_height = color_image_properties_.dim.y; - - letterbox_scale_factor_ = std::min( - static_cast(yolo_input_width) / original_width, - static_cast(yolo_input_height) / original_height); - - double resized_width = original_width * letterbox_scale_factor_; - double resized_height = original_height * letterbox_scale_factor_; - - letterbox_pad_x_ = (yolo_input_width - resized_width) / 2.0; - letterbox_pad_y_ = (yolo_input_height - resized_height) / 2.0; -} - -BoundingBox ValvePoseBase::transform_bounding_box(const BoundingBox& bbox) const { - BoundingBox transformed_bbox = bbox; - - transformed_bbox.center.position.x = (bbox.center_x - letterbox_pad_x_) / letterbox_scale_factor_; - transformed_bbox.center.position.y = (bbox.center_y - letterbox_pad_y_) / letterbox_scale_factor_; - transformed_bbox.size_x /= letterbox_scale_factor_; - transformed_bbox.size_y /= letterbox_scale_factor_; - - return transformed_bbox; -} - -bool ValvePoseBase::segment_plane(const pcl::PointCloud::Ptr& cloud, - pcl::ModelCoefficients::Ptr& coefficients, - pcl::PointIndices::Ptr& inliers) const { - - if (cloud->points.empty()) { - return false; - } - - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setDistanceThreshold(plane_ransac_threshold_); - seg.setMaxIterations(plane_ransac_max_iterations_); - seg.setInputCloud(cloud); - seg.segment(*inliers, *coefficients); - - return !inliers->indices.empty(); -} - -Eigen::Vector3f ValvePoseBase::get_ray_direction(const BoundingBox& bbox) const { - float u = bbox.center_x; - float v = bbox.center_y; - - float fx = color_image_properties_.intr.fx; - float fy = color_image_properties_.intr.fy; - float cx = color_image_properties_.intr.cx; - float cy = color_image_properties_.intr.cy; - - float xc = (u - cx) / fx; - float yc = (v - cy) / fy; - - Eigen::Vector3f ray_direction; - ray_direction << xc, yc, 1.0f; - - return ray_direction.normalized(); -} - -Eigen::Vector3f ValvePoseBase::compute_plane_normal( - const pcl::ModelCoefficients::Ptr& coefficients, - const Eigen::Vector3f& ray_direction) { - - Eigen::Vector3f normal_vector; - - if (!coefficients || coefficients->values.size() < 3) { - return Eigen::Vector3f::Zero(); - } - - normal_vector << coefficients->values[0], coefficients->values[1], coefficients->values[2]; - - normal_vector.normalize(); - - if (normal_vector.dot(ray_direction) > 0.0) { - normal_vector = -normal_vector; - } - - return normal_vector; -} - -/** - * @brief Finds the intersection point of a ray and a plane. - * - * The ray is defined by its origin (The camera's optical center, - * i.e., [0, 0, 0]) and a direction vector. The plane is defined by its coefficients - * Ax + By + Cz + D = 0. - * - * @param coefficients A shared pointer to the pcl::ModelCoefficients object. - * @param ray_direction The normalized Eigen::Vector3f ray direction. - * @return An Eigen::Vector3f representing the 3D intersection point. - */ - Eigen::Vector3f ValvePoseBase::find_ray_plane_intersection( - const pcl::ModelCoefficients::Ptr& coefficients, - const Eigen::Vector3f& ray_direction) const { - - // Plane equation: Ax + By + Cz + D = 0 - // Ray equation: P = O + t * d, where O is origin [0,0,0], P is point, t is scalar, d is direction. - // Substituting the ray equation into the plane equation: - // A*(O.x + t*d.x) + B*(O.y + t*d.y) + C*(O.z + t*d.z) + D = 0 - // Since O is [0,0,0], this simplifies to: - // A*t*d.x + B*t*d.y + C*t*d.z + D = 0 - // t * (A*d.x + B*d.y + C*d.z) = -D - // t = -D / (A*d.x + B*d.y + C*d.z) - - if (!coefficients || coefficients->values.size() < 4) { - return Eigen::Vector3f::Zero(); - } - - Eigen::Vector3f plane_normal; - plane_normal << coefficients->values[0], coefficients->values[1], coefficients->values[2]; - float D = coefficients->values[3]; - - float normal_dot_ray = plane_normal.dot(ray_direction); - - if (std::abs(normal_dot_ray) < 1e-6) { - // The ray is parallel or contained within the plane. - return Eigen::Vector3f::Zero(); - } - - float t = -D / normal_dot_ray; - - Eigen::Vector3f intersection_point = t * ray_direction; - - return intersection_point; -} - -/** - * @brief Shifts a point along a plane normal by a given distance. - * - * This function is useful for moving a point from the detected plane's - * surface to a new location, such as the center of a valve knob. - * The shift is performed along the vector of the consistent plane normal. - * - * @param intersection_point The 3D intersection point on the plane. - * @param plane_normal The consistent, normalized plane normal vector. - * @return The new Eigen::Vector3f point after the shift. - */ - Eigen::Vector3f ValvePoseBase::shift_point_along_normal( - const Eigen::Vector3f& intersection_point, - const Eigen::Vector3f& plane_normal) const { - - return intersection_point + (plane_normal * valve_handle_offset_); -} - - -} // namespace valve_detection \ No newline at end of file diff --git a/src/valve_pose_depth.cpp b/src/valve_pose_depth.cpp deleted file mode 100644 index 96de2f3..0000000 --- a/src/valve_pose_depth.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#include "valve_detection/valve_pose_depth.hpp" - -namespace valve_detection { - -ValvePoseDepth::ValvePoseDepth(int yolo_img_width, int yolo_img_height_, - float plane_ransac_threshold, - int plane_ransac_max_iterations, - float annulus_radius_ratio, - float valve_handle_offset) - : ValvePoseBase(yolo_img_width, yolo_img_height_, - plane_ransac_threshold, plane_ransac_max_iterations, - annulus_radius_ratio, valve_handle_offset){} - -void ValvePoseDepth::project_pixel_to_point(int u, int v, float depth, pcl::PointXYZ& point) { - if (depth <= 0 || depth == std::numeric_limits::infinity() || std::isnan(depth)) { - point.x = point.y = point.z = std::numeric_limits::quiet_NaN(); - return; - } - - double fx = depth_image_properties_.intr.fx; - double fy = depth_image_properties_.intr.fy; - double cx = depth_image_properties_.intr.cx; - double cy = depth_image_properties_.intr.cy; - - point.x = (u - cx) * depth / fx; - point.y = (v - cy) * depth / fy; - point.z = depth; -} - -void ValvePoseDepth::extract_annulus_pcl( - const cv::Mat& depth_image, - const BoundingBox& bbox, - pcl::PointCloud::Ptr& cloud) const { - - float center_x = bbox.center_x; - float center_y = bbox.center_y; - float outer_radius_x = bbox.size_x / 2.0f; - float outer_radius_y = bbox.size_y / 2.0f; - - float inner_radius_x = outer_radius_x * annulus_radius_ratio_; - float inner_radius_y = outer_radius_y * annulus_radius_ratio_; - - cloud->clear(); - - int upper_bound_size = static_cast(M_PI * ((outer_radius_x * outer_radius_y - inner_radius_x * inner_radius_y) + 1)); - cloud->reserve(upper_bound_size); - - for (int y = static_cast(-outer_radius_y); y <= static_cast(outer_radius_y); ++y) { - for (int x = static_cast(-outer_radius_x); x <= static_cast(outer_radius_x); ++x) { - float dx_scaled_outer = x / outer_radius_x; - float dy_scaled_outer = y / outer_radius_y; - bool is_inside_outer_ellipse = (dx_scaled_outer * dx_scaled_outer + dy_scaled_outer * dy_scaled_outer) <= 1.0f; - - float dx_scaled_inner = x / inner_radius_x; - float dy_scaled_inner = y / inner_radius_y; - bool is_outside_inner_ellipse = (dx_scaled_inner * dx_scaled_inner + dy_scaled_inner * dy_scaled_inner) > 1.0f; - - if (is_inside_outer_ellipse && is_outside_inner_ellipse) { - int u = static_cast(center_x + x); - int v = static_cast(center_y + y); - - if (u >= 0 && u < depth_image.cols && v >= 0 && v < depth_image.rows) { - float depth_value = depth_image.at(v, u); - pcl::PointXYZ point; - project_pixel_to_point(u, v, depth_value, point); - - if (!std::isnan(point.x) && !std::isnan(point.y) && !std::isnan(point.z)) { - cloud->points.push_back(point); - } - } - } - } - } - - cloud->width = static_cast(cloud->points.size()); - cloud->height = 1; - cloud->is_dense = false; -} - - -} // namespace valve_detection - \ No newline at end of file diff --git a/src/valve_pose_depth_ros.cpp b/src/valve_pose_depth_ros.cpp deleted file mode 100644 index 8c8388c..0000000 --- a/src/valve_pose_depth_ros.cpp +++ /dev/null @@ -1,209 +0,0 @@ -#include "valve_detection/valve_pose_depth_ros.hpp" - -ValvePoseDepthNode::ValvePoseDepthNode(const rclcpp::NodeOptions& options) -: Node("valve_detection_node", options) { - std::string color_image_sub_topic = - this->declare_parameter("color_image_sub_topic"); - std::string depth_image_sub_topic = - this->declare_parameter("depth_image_sub_topic"); - std::string detections_sub_topic = - this->declare_parameter("detections_sub_topic"); - visualize_detections_ = - this->declare_parameter("visualize_detections"); - debug_visualize_ = - this->declare_parameter("debug_visualize"); - calculate_angle_ = - this->declare_parameter("calculate_angle"); - - int yolo_img_width = - this->declare_parameter("yolo_img_width"); - int yolo_img_height = - this->declare_parameter("yolo_img_height"); - - float annulus_radius_ratio = - this->declare_parameter("annulus_radius_ratio"); - - float plane_ransac_threshold = - this->declare_parameter("plane_ransac_threshold"); - int plane_ransac_max_iterations = - this->declare_parameter("plane_ransac_max_iterations"); - - float valve_handle_offset = - this->declare_parameter("valve_handle_offset"); - - valve_detector_ = std::make_shared(yolo_img_width, yolo_img_height, annulus_radius_ratio, - plane_ransac_threshold, plane_ransac_max_iterations, valve_handle_offset); - - - rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(10)) - .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); - depth_image_sub_.subscribe(this, depth_image_sub_topic, - qos.get_rmw_qos_profile()); - color_image_sub_.subscribe(this, color_image_sub_topic, - qos.get_rmw_qos_profile()); - detections_sub_.subscribe(this, detections_sub_topic, - qos.get_rmw_qos_profile()); - - sync_ = std::make_shared>( - MySyncPolicy(10), depth_image_sub_, color_image_sub_, detections_sub_); - sync_->registerCallback(std::bind( - &ValvePoseDepthNode::synchronized_callback, this, _1, _2, _3)); - - std::string color_image_info_topic = - this->declare_parameter("color_image_info_topic"); - - qos.history = rclcpp::KeepLast(1); - color_image_info_sub_ = this->create_subscription( - color_image_info_topic, 10, - std::bind(&ValvePoseDepthNode::color_image_info_callback, - this, std::placeholders::_1)); - - std::string valve_pose_pub_topic = - this->declare_parameter("valve_pose_pub_topic"); - - valve_pose_pub_ = this->create_publisher( - valve_pose_pub_topic, qos); - - if (debug_visualize_) { - std::string annulus_pub_topic = - this->declare_parameter("annulus_pub_topic"); - annulus_pcl_pub_ = this->create_publisher( - annulus_pub_topic, qos); - } -} - -void ValvePoseDepthNode::color_image_info_callback( - const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg) { - if (!color_image_info_received_) { - ImageProperties img_props; - img_props.intr.fx = camera_info_msg->k[0]; - img_props.intr.fy = camera_info_msg->k[4]; - img_props.intr.cx = camera_info_msg->k[2]; - img_props.intr.cy = camera_info_msg->k[5]; - img_props.dim.x = camera_info_msg->width; - img_props.dim.y = camera_info_msg->height; - - valve_detector_->set_color_image_properties(img_props); - valve_detector_->calculate_letterbox_padding(); - - color_image_frame_id_ = camera_info_msg->header.frame_id; - color_image_info_received_ = true; - color_image_info_sub_.reset(); - } -} - -BoundingBox ValvePoseDepthNode::to_bounding_box( - const vision_msgs::msg::BoundingBox2D& bbox) const { - BoundingBox box; - box.center_x = bbox.center.x; - box.center_y = bbox.center.y; - box.size_x = bbox.size_x; - box.size_y = bbox.size_y; - box.theta = bbox.center.theta; - return box; -} - -void ValvePoseDepthNode::publish_annulus_pcl( - const pcl::PointCloud::Ptr& cloud) const { - sensor_msgs::msg::PointCloud2 cloud_msg; - pcl::toROSMsg(*cloud, cloud_msg); - cloud_msg.header.frame_id = color_image_frame_id_; - cloud_msg.header.stamp = this->now(); - cloud_msg.is_dense = false; - cloud_msg.width = static_cast(cloud->points.size()); - cloud_msg.height = 1; - - annulus_pcl_pub_->publish(cloud_msg); -} - -void ValvePoseDepthNode::synchronized_callback( - const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, - const sensor_msgs::msg::Image::ConstSharedPtr& color_image, - const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { - if (!color_image_info_received_) { - return; - } - cv_bridge::CvImageConstPtr cv_ptr = - cv_bridge::toCvShare(color_image, "bgr8"); - - cv:brudge::CvImageConstPtr cv_depth_ptr = - cv_bridge::toCvShare(depth_image, "32FC1"); - - cv::Mat cv_color_image = cv_ptr->image; - - if (visualize_detections_) { - cv_color_image = cv_color_image.clone(); - } - std::vector boxes = std::vector().reserve(detections->detections.size()); - for (const auto& detection : detections->detections) { - boxes.push_back(to_bounding_box(detection.bbox)); - } - - for (const auto& box : boxes) { - BoundingBox org_image_box = - valve_detector_->transform_bounding_box(box); - - pcl::PointCloud::Ptr cloud( - new pcl::PointCloud); - - valve_detector_->extract_annulus_pcl( - cv_depth_ptr->image, - org_image_box, - cloud); - - if (cloud->empty()) { - continue; - } - if (debug_visualize_) { - publish_annulus_pcl(cloud); - } - - pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers(new pcl::PointIndices); - if (!valve_detector_->segment_plane(cloud, coefficients, inliers)) { - continue; - } - - Eigen::Vector3f bb_centre_ray = - valve_detector_->get_ray_direction(org_image_box); - - Eigen::Vector3f ray_plane_intersection = - valve_detector_->find_ray_plane_intersection(coefficients, bb_centre_ray); - - if (ray_plane_intersection.isZero()) { - continue; - } - - Eigen::Vector3f plane_normal = - valve_detector_->compute_plane_normal( - coefficients, ray_direction); - - if (plane_normal.isZero()) { - continue; - } - - Eigen::Vector3f shifted_position = - valve_detector_->shift_point_along_normal( - ray_plane_intersection, plane_normal); - - float angle = org_image_box.theta; - if (calculate_angle_) { - // angle = calculate_angle() - } - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.stamp = color_image->header.stamp; - pose_msg.header.frame_id = color_image_frame_id_; - - pose_msg.pose.position.x = shifted_position[0]; - pose_msg.pose.position.y = shifted_position[1]; - pose_msg.pose.position.z = shifted_position[2]; - - } - - - - - -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ValvePoseDepthNode) \ No newline at end of file diff --git a/src/valve_pose_pcl.cpp b/src/valve_pose_pcl.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/valve_pose_ros.cpp b/src/valve_pose_ros.cpp new file mode 100644 index 0000000..82c2e3f --- /dev/null +++ b/src/valve_pose_ros.cpp @@ -0,0 +1,289 @@ +#include "valve_detection_ros/valve_pose_ros.hpp" + +namespace valve_detection { + +using namespace std::placeholders; + +ValvePoseNode::ValvePoseNode(const rclcpp::NodeOptions& options) + : Node("valve_detection_node", options) { + init_valve_detector(); + + setup_sync(); + + if (calculate_angle_) { + init_angle_detector(); + } + + std::string color_image_info_topic = + this->declare_parameter("color_image_info_topic"); + + rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(1)) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + + color_image_info_sub_ = + this->create_subscription( + color_image_info_topic, qos, + std::bind(&ValvePoseNode::color_image_info_callback, this, + std::placeholders::_1)); + + std::string valve_poses_pub_topic = + this->declare_parameter("valve_poses_pub_topic"); + + valve_poses_pub_ = this->create_publisher( + valve_poses_pub_topic, qos); + + pcl_visualize_ = this->declare_parameter("pcl_visualize"); + + if (pcl_visualize_) { + std::string annulus_pub_topic = + this->declare_parameter("annulus_pub_topic"); + annulus_pcl_pub_ = + this->create_publisher( + annulus_pub_topic, qos); + std::string plane_pub_topic = + this->declare_parameter("plane_pub_topic"); + annulus_plane_pub_ = + this->create_publisher( + plane_pub_topic, qos); + } + if (visualize_detections_) { + std::string processed_image_pub_topic = + this->declare_parameter("processed_image_pub_topic"); + processed_image_pub_ = this->create_publisher( + processed_image_pub_topic, qos); + } + if (!use_depth_image_) { + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); + } +} + +void ValvePoseNode::init_valve_detector() { + int yolo_img_width = this->declare_parameter("yolo_img_width"); + int yolo_img_height = this->declare_parameter("yolo_img_height"); + + float annulus_radius_ratio = + this->declare_parameter("annulus_radius_ratio"); + + float plane_ransac_threshold = + this->declare_parameter("plane_ransac_threshold"); + int plane_ransac_max_iterations = + this->declare_parameter("plane_ransac_max_iterations"); + + float valve_handle_offset = + this->declare_parameter("valve_handle_offset"); + + valve_detector_ = std::make_unique( + yolo_img_width, yolo_img_height, annulus_radius_ratio, + plane_ransac_threshold, plane_ransac_max_iterations, + valve_handle_offset); +} + +void ValvePoseNode::setup_sync() { + visualize_detections_ = + this->declare_parameter("visualize_detections"); + calculate_angle_ = this->declare_parameter("calculate_angle"); + use_depth_image_ = this->declare_parameter("use_depth_image"); + + use_color_image_ = (calculate_angle_ || visualize_detections_); + + std::string color_image_sub_topic = + this->declare_parameter("color_image_sub_topic"); + std::string depth_image_sub_topic = + this->declare_parameter("depth_image_sub_topic"); + std::string pcl_sub_topic = + this->declare_parameter("pcl_sub_topic"); + std::string detections_sub_topic = + this->declare_parameter("detections_sub_topic"); + + rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(10)) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + + if (use_color_image_ && use_depth_image_) { + color_image_sub_.subscribe(this, color_image_sub_topic, + qos.get_rmw_qos_profile()); + depth_image_sub_.subscribe(this, depth_image_sub_topic, + qos.get_rmw_qos_profile()); + detections_sub_.subscribe(this, detections_sub_topic, + qos.get_rmw_qos_profile()); + sync_di_ci_d_ = + std::make_shared>( + SyncPolicy_DI_CI_D(10), depth_image_sub_, color_image_sub_, + detections_sub_); + sync_di_ci_d_->registerCallback( + std::bind(&ValvePoseNode::di_ci_d_callback, this, _1, _2, _3)); + + } else if (use_depth_image_) { + depth_image_sub_.subscribe(this, depth_image_sub_topic, + qos.get_rmw_qos_profile()); + detections_sub_.subscribe(this, detections_sub_topic, + qos.get_rmw_qos_profile()); + sync_di_d_ = + std::make_shared>( + SyncPolicy_DI_D(10), depth_image_sub_, detections_sub_); + sync_di_d_->registerCallback( + std::bind(&ValvePoseNode::di_d_callback, this, _1, _2)); + + } else if (use_color_image_ && !use_depth_image_) { + pcl_sub_.subscribe(this, pcl_sub_topic, qos.get_rmw_qos_profile()); + color_image_sub_.subscribe(this, color_image_sub_topic, + qos.get_rmw_qos_profile()); + detections_sub_.subscribe(this, detections_sub_topic, + qos.get_rmw_qos_profile()); + sync_pc_ci_d_ = + std::make_shared>( + SyncPolicy_PC_CI_D(10), pcl_sub_, color_image_sub_, + detections_sub_); + sync_pc_ci_d_->registerCallback( + std::bind(&ValvePoseNode::pc_ci_d_callback, this, _1, _2, _3)); + + } else { + pcl_sub_.subscribe(this, pcl_sub_topic, qos.get_rmw_qos_profile()); + detections_sub_.subscribe(this, detections_sub_topic, + qos.get_rmw_qos_profile()); + sync_pc_d_ = + std::make_shared>( + SyncPolicy_PC_D(10), pcl_sub_, detections_sub_); + sync_pc_d_->registerCallback( + std::bind(&ValvePoseNode::pc_d_callback, this, _1, _2)); + } +} + +void ValvePoseNode::di_ci_d_callback( + const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, + const sensor_msgs::msg::Image::ConstSharedPtr& color_image, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { + synchronized_callback(depth_image, color_image, + detections); +} + +void ValvePoseNode::di_d_callback( + const sensor_msgs::msg::Image::ConstSharedPtr& depth_image, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { + synchronized_callback(depth_image, nullptr, + detections); +} + +void ValvePoseNode::pc_ci_d_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& pcl, + const sensor_msgs::msg::Image::ConstSharedPtr& color_image, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { + synchronized_callback(pcl, color_image, + detections); +} + +void ValvePoseNode::pc_d_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr& pcl, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { + synchronized_callback(pcl, nullptr, + detections); +} + +void ValvePoseNode::init_angle_detector() { + if (calculate_angle_) { + AngleDetectorParams angle_params; + angle_params.line_detection_area = + this->declare_parameter("angle.detection_area_ratio"); + angle_params.canny_low_threshold = + this->declare_parameter("angle.canny_low_threshold"); + angle_params.canny_high_threshold = + this->declare_parameter("angle.canny_high_threshold"); + angle_params.canny_aperture_size = + this->declare_parameter("angle.canny_aperture_size"); + angle_params.hough_rho_res = + this->declare_parameter("angle.hough_rho_res"); + angle_params.hough_theta_res = + this->declare_parameter("angle.hough_theta_res"); + angle_params.hough_threshold = + this->declare_parameter("angle.hough_threshold"); + angle_params.hough_min_line_length = + this->declare_parameter("angle.hough_min_line_length"); + angle_params.hough_max_line_gap = + this->declare_parameter("angle.hough_max_line_gap"); + + valve_detector_->init_angle_detector(angle_params); + } +} + +void ValvePoseNode::color_image_info_callback( + const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg) { + if (!color_image_info_received_) { + ImageProperties img_props; + img_props.intr.fx = camera_info_msg->k[0]; + img_props.intr.fy = camera_info_msg->k[4]; + img_props.intr.cx = camera_info_msg->k[2]; + img_props.intr.cy = camera_info_msg->k[5]; + img_props.dim.width = camera_info_msg->width; + img_props.dim.height = camera_info_msg->height; + + valve_detector_->set_color_image_properties(img_props); + valve_detector_->calculate_letterbox_padding(); + + color_image_frame_id_ = camera_info_msg->header.frame_id; + color_image_info_received_ = true; + color_image_info_sub_.reset(); + } +} + +BoundingBox ValvePoseNode::to_bounding_box( + const vision_msgs::msg::BoundingBox2D& bbox) const { + BoundingBox box; + box.center_x = bbox.center.position.x; + box.center_y = bbox.center.position.y; + box.size_x = bbox.size_x; + box.size_y = bbox.size_y; + box.theta = bbox.center.theta; + return box; +} + +void ValvePoseNode::publish_annulus_pcl( + const pcl::PointCloud::Ptr& cloud, + const std_msgs::msg::Header& header) const { + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(*cloud, cloud_msg); + cloud_msg.header = header; + cloud_msg.is_dense = false; + cloud_msg.width = static_cast(cloud->points.size()); + cloud_msg.height = 1; + + annulus_pcl_pub_->publish(cloud_msg); +} + +void ValvePoseNode::publish_annulus_plane( + const pcl::PointCloud::Ptr& cloud, + const std_msgs::msg::Header& header) const { + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(*cloud, cloud_msg); + cloud_msg.header = header; + cloud_msg.is_dense = false; + cloud_msg.width = static_cast(cloud->points.size()); + cloud_msg.height = 1; + + annulus_plane_pub_->publish(cloud_msg); +} + +void ValvePoseNode::publish_valve_poses( + const std::vector& poses, + const std_msgs::msg::Header& header) const { + geometry_msgs::msg::PoseArray pose_array_msg; + pose_array_msg.header = header; + + for (const auto& pose : poses) { + geometry_msgs::msg::Pose pose_msg; + pose_msg.position.x = pose.position.x(); + pose_msg.position.y = pose.position.y(); + pose_msg.position.z = pose.position.z(); + pose_msg.orientation.x = pose.orientation.x(); + pose_msg.orientation.y = pose.orientation.y(); + pose_msg.orientation.z = pose.orientation.z(); + pose_msg.orientation.w = pose.orientation.w(); + + pose_array_msg.poses.push_back(pose_msg); + } + valve_poses_pub_->publish(pose_array_msg); +} + +} // namespace valve_detection + +RCLCPP_COMPONENTS_REGISTER_NODE(valve_detection::ValvePoseNode) From fa8b0555ab1ef0e2124f2fae6e765789b8bd484d Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 26 Aug 2025 21:47:35 +0200 Subject: [PATCH 15/22] added transform to visualized bb --- config/valve_detection_params.yaml | 5 ++--- src/valve_detector.cpp | 9 +++++++-- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/config/valve_detection_params.yaml b/config/valve_detection_params.yaml index 2b9f093..9374831 100644 --- a/config/valve_detection_params.yaml +++ b/config/valve_detection_params.yaml @@ -5,8 +5,7 @@ pcl_sub_topic: "/zed_node/depth/point_cloud" detections_sub_topic: "/detections_output" - color_image_info_topic: "/zed_node/left/camera_info" - depth_image_info_topic: "/zed_node/depth/camera_info" + color_image_info_topic: "/zed_node/depth/camera_info" valve_poses_pub_topic: "/valve_poses" @@ -24,7 +23,7 @@ visualize_detections: true processed_image_pub_topic: "/valve_detection_image" - pcl_visualize: false + pcl_visualize: true annulus_pub_topic: "/bbx_annulus_pcl" plane_pub_topic: "/annulus_plane_pcl" diff --git a/src/valve_detector.cpp b/src/valve_detector.cpp index b35cd81..5d7effd 100644 --- a/src/valve_detector.cpp +++ b/src/valve_detector.cpp @@ -25,6 +25,11 @@ cv::Mat ValveDetector::draw_detections(const cv::Mat& image, if (boxes.size() != poses.size()) { return cv::Mat(); } + std::vector transformed_boxes; + transformed_boxes.reserve(boxes.size()); + for (const auto& box : boxes) { + transformed_boxes.push_back(transform_bounding_box(box)); + } cv::Mat visualized_image = image.clone(); @@ -34,8 +39,8 @@ cv::Mat ValveDetector::draw_detections(const cv::Mat& image, color_image_properties_.intr.cy, 0, 0, 1); cv::Mat dist_coeffs = cv::Mat::zeros(4, 1, CV_64F); - for (size_t i = 0; i < boxes.size(); ++i) { - const auto& box = boxes[i]; + for (size_t i = 0; i < transformed_boxes.size(); ++i) { + const auto& box = transformed_boxes[i]; const auto& pose = poses[i]; int x1 = box.center_x - box.size_x / 2; From 7d334804f8b15dbea3e6df5cdb43698fce4185fe Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 26 Aug 2025 21:49:12 +0200 Subject: [PATCH 16/22] proper pcl init, fixed nullptr segfault --- include/valve_detection_ros/valve_pose_ros.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/include/valve_detection_ros/valve_pose_ros.hpp b/include/valve_detection_ros/valve_pose_ros.hpp index 51d645a..a33418a 100644 --- a/include/valve_detection_ros/valve_pose_ros.hpp +++ b/include/valve_detection_ros/valve_pose_ros.hpp @@ -213,8 +213,10 @@ class ValvePoseNode : public rclcpp::Node { std::vector poses; poses.reserve(boxes.size()); - pcl::PointCloud::Ptr all_annulus_cloud; - pcl::PointCloud::Ptr all_annulus_plane_cloud; + pcl::PointCloud::Ptr all_annulus_cloud( + new pcl::PointCloud); + pcl::PointCloud::Ptr all_annulus_plane_cloud( + new pcl::PointCloud); if constexpr (std::is_same::value) { valve_detector_->Compute_valve_poses( From fac4a8805ad37a970f06958fec84a12832af8de0 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 27 Aug 2025 22:40:31 +0200 Subject: [PATCH 17/22] moved angle detector as member of ros node for easier debug visualization --- config/valve_detection_params.yaml | 9 +- include/valve_detection/angle_detector.hpp | 32 ++- include/valve_detection/valve_detector.hpp | 37 +--- .../valve_detection_ros/valve_pose_ros.hpp | 32 ++- src/angle_detector.cpp | 184 ++++++++++++++---- src/valve_detector.cpp | 13 +- src/valve_pose_ros.cpp | 10 +- 7 files changed, 217 insertions(+), 100 deletions(-) diff --git a/config/valve_detection_params.yaml b/config/valve_detection_params.yaml index 9374831..f59c6ad 100644 --- a/config/valve_detection_params.yaml +++ b/config/valve_detection_params.yaml @@ -23,14 +23,15 @@ visualize_detections: true processed_image_pub_topic: "/valve_detection_image" - pcl_visualize: true + 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.05 # percentage of bb size - angle.canny_low_threshold: 50 - angle.canny_high_threshold: 150 + 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 diff --git a/include/valve_detection/angle_detector.hpp b/include/valve_detection/angle_detector.hpp index 188d563..e565ade 100644 --- a/include/valve_detection/angle_detector.hpp +++ b/include/valve_detection/angle_detector.hpp @@ -18,13 +18,33 @@ class AngleDetector { : params_(params) {} /** - * @brief Calculates the angle of the longest line within a detected valve. - * @param color_image The input color image. - * @param bbox The bounding box of the detected valve. - * @return The angle in radians, or NaN if no lines are detected. + * @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. */ - double calculate_angle(const cv::Mat& color_image, - const BoundingBox& bbox) const; + void compute_angles(const cv::Mat& color_image, + std::vector& 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& boxes) const; private: /** diff --git a/include/valve_detection/valve_detector.hpp b/include/valve_detection/valve_detector.hpp index 3d83432..b733c8e 100644 --- a/include/valve_detection/valve_detector.hpp +++ b/include/valve_detection/valve_detector.hpp @@ -7,8 +7,7 @@ #include #include #include -#include -#include "valve_detection/angle_detector.hpp" +#include #include "valve_detection/depth_image_processing.hpp" #include "valve_detection/pointcloud_processing.hpp" #include "valve_detection/types.hpp" @@ -64,44 +63,39 @@ class ValveDetector { * @param all_annulus_cloud Optional point cloud for visualizing all annuli. * @param all_annulus_plane_cloud Optional point cloud for visualizing * segmented planes. - * @param pcl_visualize Whether to populate visualization point clouds. + * @param debug_visualize Whether to populate visualization point clouds. * @param calculate_angle Whether to compute the valve rotation angle. */ template void Compute_valve_poses( const T& depth_input, - const cv::Mat& color_image, const std::vector& boxes, std::vector& poses, pcl::PointCloud::Ptr& all_annulus_cloud, pcl::PointCloud::Ptr& all_annulus_plane_cloud, - bool pcl_visualize, - bool calculate_angle) { - if (pcl_visualize) { + bool debug_visualize) { + if (debug_visualize) { all_annulus_cloud->clear(); all_annulus_plane_cloud->clear(); } for (const auto& box : boxes) { - BoundingBox org_image_box = transform_bounding_box(box); pcl::PointCloud::Ptr annulus_cloud( new pcl::PointCloud); if constexpr (std::is_same< T, pcl::PointCloud::Ptr>::value) { - extract_annulus_pcl(depth_input, org_image_box, - color_image_properties_, + extract_annulus_pcl(depth_input, box, color_image_properties_, annulus_radius_ratio_, annulus_cloud); } else if constexpr (std::is_same::value) { - extract_annulus_pcl(depth_input, org_image_box, - color_image_properties_, + extract_annulus_pcl(depth_input, box, color_image_properties_, annulus_radius_ratio_, annulus_cloud); } if (annulus_cloud->empty()) continue; - if (pcl_visualize) { + if (debug_visualize) { *all_annulus_cloud += *annulus_cloud; } @@ -112,7 +106,7 @@ class ValveDetector { if (!segment_plane(annulus_cloud, coefficients, inliers)) continue; - if (pcl_visualize) { + if (debug_visualize) { pcl::PointCloud::Ptr annulus_plane_cloud( new pcl::PointCloud); pcl::copyPointCloud(*annulus_cloud, inliers->indices, @@ -120,7 +114,7 @@ class ValveDetector { *all_annulus_plane_cloud += *annulus_plane_cloud; } - Eigen::Vector3f bb_centre_ray = get_ray_direction(org_image_box); + Eigen::Vector3f bb_centre_ray = get_ray_direction(box); Eigen::Vector3f ray_plane_intersection = find_ray_plane_intersection(coefficients, bb_centre_ray); if (ray_plane_intersection.isZero()) @@ -134,11 +128,7 @@ class ValveDetector { Eigen::Vector3f shifted_position = shift_point_along_normal(ray_plane_intersection, plane_normal); - float angle = org_image_box.theta; - if (calculate_angle) { - angle = angle_detector_->calculate_angle(color_image, - org_image_box); - } + float angle = box.theta; Eigen::Matrix3f rotation_matrix = create_rotation_matrix(plane_normal, angle); @@ -248,12 +238,6 @@ class ValveDetector { Eigen::Matrix3f create_rotation_matrix(const Eigen::Vector3f& plane_normal, float angle); - /** - * @brief Initializes the angle detector with given parameters. - * @param params Parameters for Canny and Hough-based angle detection. - */ - void init_angle_detector(const AngleDetectorParams& params); - /** * @brief Converts a 3x3 rotation matrix to a normalized quaternion. * @param rotation_matrix Input rotation matrix. @@ -285,7 +269,6 @@ class ValveDetector { int letterbox_pad_x_; int letterbox_pad_y_; Eigen::Vector3f filter_direction_ = Eigen::Vector3f(1, 0, 0); - std::unique_ptr angle_detector_; }; } // namespace valve_detection diff --git a/include/valve_detection_ros/valve_pose_ros.hpp b/include/valve_detection_ros/valve_pose_ros.hpp index a33418a..26dee16 100644 --- a/include/valve_detection_ros/valve_pose_ros.hpp +++ b/include/valve_detection_ros/valve_pose_ros.hpp @@ -9,6 +9,7 @@ #include #include #include +#include "valve_detection/angle_detector.hpp" #include "valve_detection/valve_detector.hpp" #include @@ -207,7 +208,10 @@ class ValvePoseNode : public rclcpp::Node { std::vector boxes; boxes.reserve(detections->detections.size()); for (const auto& detection : detections->detections) { - boxes.push_back(to_bounding_box(detection.bbox)); + BoundingBox box = to_bounding_box(detection.bbox); + BoundingBox org_image_box = + valve_detector_->transform_bounding_box(box); + boxes.push_back(org_image_box); } std::vector poses; @@ -218,16 +222,28 @@ class ValvePoseNode : public rclcpp::Node { pcl::PointCloud::Ptr all_annulus_plane_cloud( new pcl::PointCloud); + if (calculate_angle_ && debug_visualize_) { + cv::Mat angle_debug_image = cv_color_image.clone(); + + angle_detector_->compute_angles_debug(angle_debug_image, boxes); + sensor_msgs::msg::Image::SharedPtr output_msg = + cv_bridge::CvImage(depth_msg->header, "bgr8", angle_debug_image) + .toImageMsg(); + angle_image_pub_->publish(*output_msg); + } else if (calculate_angle_) { + angle_detector_->compute_angles(cv_color_image, boxes); + } + if constexpr (std::is_same::value) { valve_detector_->Compute_valve_poses( - pcl_tf, cv_color_image, boxes, poses, all_annulus_cloud, - all_annulus_plane_cloud, pcl_visualize_, calculate_angle_); + pcl_tf, boxes, poses, all_annulus_cloud, + all_annulus_plane_cloud, debug_visualize_); } else if constexpr (std::is_same::value) { valve_detector_->Compute_valve_poses( - cv_depth_image, cv_color_image, boxes, poses, all_annulus_cloud, - all_annulus_plane_cloud, pcl_visualize_, calculate_angle_); + cv_depth_image, boxes, poses, all_annulus_cloud, + all_annulus_plane_cloud, debug_visualize_); } - if (pcl_visualize_ && all_annulus_cloud && all_annulus_plane_cloud) { + if (debug_visualize_ && all_annulus_cloud && all_annulus_plane_cloud) { publish_annulus_pcl(all_annulus_cloud, depth_msg->header); publish_annulus_plane(all_annulus_plane_cloud, depth_msg->header); } @@ -257,6 +273,7 @@ class ValvePoseNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr annulus_plane_pub_; rclcpp::Publisher::SharedPtr processed_image_pub_; + rclcpp::Publisher::SharedPtr angle_image_pub_; // Depth image sync policies uses ExactTime with the assumption // that the depth image and color image are from the same camera @@ -296,12 +313,13 @@ class ValvePoseNode : public rclcpp::Node { bool color_image_info_received_ = false; std::unique_ptr valve_detector_; + std::unique_ptr angle_detector_; std::string color_image_frame_id_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; bool visualize_detections_ = true; - bool pcl_visualize_ = false; + bool debug_visualize_ = false; bool calculate_angle_ = true; bool use_color_image_ = true; bool use_depth_image_ = true; diff --git a/src/angle_detector.cpp b/src/angle_detector.cpp index 649fc69..ec750ef 100644 --- a/src/angle_detector.cpp +++ b/src/angle_detector.cpp @@ -4,51 +4,151 @@ namespace valve_detection { -double AngleDetector::calculate_angle(const cv::Mat& color_image, - const BoundingBox& bbox) const { - double width = bbox.size_x; - double height = bbox.size_y; - double center_x = bbox.center_x; - double center_y = bbox.center_y; - - int x1 = std::max( - static_cast(center_x - width * params_.line_detection_area), 0); - int y1 = std::max( - static_cast(center_y - height * params_.line_detection_area), 0); - int x2 = std::min( - static_cast(center_x + width * params_.line_detection_area), - color_image.cols - 1); - int y2 = std::min( - static_cast(center_y + height * params_.line_detection_area), - color_image.rows - 1); - - cv::Mat roi = color_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); - - cv::Mat mask = cv::Mat::zeros(roi.size(), CV_8UC1); - cv::Point center((roi.cols / 2), (roi.rows / 2)); - cv::Size axes(roi.cols / 2, roi.rows / 2); - cv::ellipse(mask, center, axes, 0, 0, 360, cv::Scalar(255), -1); - - cv::Mat masked_roi; - roi.copyTo(masked_roi, mask); - - cv::Mat gray; - cv::cvtColor(masked_roi, gray, cv::COLOR_BGR2GRAY); - - cv::Mat edges = apply_canny_edge_detection(gray); - - std::vector lines; - cv::HoughLinesP(edges, lines, params_.hough_rho_res, - params_.hough_theta_res, params_.hough_threshold, - params_.hough_min_line_length, params_.hough_max_line_gap); +void AngleDetector::compute_angles(const cv::Mat& color_image, + std::vector& boxes) const { + for (auto& box : boxes) { + double width = box.size_x; + double height = box.size_y; + double center_x = box.center_x; + double center_y = box.center_y; + + int x1 = std::max( + static_cast(center_x - width * params_.line_detection_area), + 0); + int y1 = std::max( + static_cast(center_y - height * params_.line_detection_area), + 0); + int x2 = std::min( + static_cast(center_x + width * params_.line_detection_area), + color_image.cols - 1); + int y2 = std::min( + static_cast(center_y + height * params_.line_detection_area), + color_image.rows - 1); + + if (x1 >= x2 || y1 >= y2) { + box.theta = std::numeric_limits::quiet_NaN(); + continue; + } - if (lines.empty()) { - return std::numeric_limits::quiet_NaN(); + cv::Mat roi = color_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); + + cv::Mat mask = cv::Mat::zeros(roi.size(), CV_8UC1); + cv::Point center((roi.cols / 2), (roi.rows / 2)); + cv::Size axes(roi.cols / 2, roi.rows / 2); + cv::ellipse(mask, center, axes, 0, 0, 360, cv::Scalar(255), -1); + + cv::Mat masked_roi; + roi.copyTo(masked_roi, mask); + + cv::Mat gray; + cv::cvtColor(masked_roi, gray, cv::COLOR_BGR2GRAY); + cv::Mat edges = apply_canny_edge_detection(gray); + + std::vector lines; + cv::HoughLinesP(edges, lines, params_.hough_rho_res, + params_.hough_theta_res, params_.hough_threshold, + params_.hough_min_line_length, + params_.hough_max_line_gap); + + if (lines.empty()) { + box.theta = std::numeric_limits::quiet_NaN(); + continue; + } + + cv::Vec4i longest_line = find_longest_line(lines); + box.theta = std::atan2(longest_line[3] - longest_line[1], + longest_line[2] - longest_line[0]); + } +} + +void AngleDetector::compute_angles_debug( + cv::Mat& image_to_draw_on, + std::vector& boxes) const { + const cv::Scalar ALL_LINES_COLOR(255, 0, 0); + const cv::Scalar LONGEST_LINE_COLOR(0, 255, 0); + const int LINE_THICKNESS = 2; + + cv::Mat output_image = cv::Mat::zeros(image_to_draw_on.size(), CV_8UC3); + + for (auto& box : boxes) { + double width = box.size_x; + double height = box.size_y; + double center_x = box.center_x; + double center_y = box.center_y; + + int x1 = std::max( + static_cast(center_x - width * params_.line_detection_area), + 0); + int y1 = std::max( + static_cast(center_y - height * params_.line_detection_area), + 0); + int x2 = std::min( + static_cast(center_x + width * params_.line_detection_area), + image_to_draw_on.cols - 1); + int y2 = std::min( + static_cast(center_y + height * params_.line_detection_area), + image_to_draw_on.rows - 1); + + if (x1 >= x2 || y1 >= y2) { + box.theta = std::numeric_limits::quiet_NaN(); + continue; + } + + cv::Mat roi = image_to_draw_on(cv::Rect(x1, y1, x2 - x1, y2 - y1)); + + cv::Mat mask = cv::Mat::zeros(roi.size(), CV_8UC1); + cv::Point center((roi.cols / 2), (roi.rows / 2)); + cv::Size axes(roi.cols / 2, roi.rows / 2); + cv::ellipse(mask, center, axes, 0, 0, 360, cv::Scalar(255), -1); + + cv::Mat masked_roi; + roi.copyTo(masked_roi, mask); + + cv::Mat gray; + cv::cvtColor(masked_roi, gray, cv::COLOR_BGR2GRAY); + cv::Mat edges = apply_canny_edge_detection(gray); + + std::vector lines; + cv::HoughLinesP(edges, lines, params_.hough_rho_res, + params_.hough_theta_res, params_.hough_threshold, + params_.hough_min_line_length, + params_.hough_max_line_gap); + + if (lines.empty()) { + box.theta = std::numeric_limits::quiet_NaN(); + + cv::Mat output_roi_gray = + output_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); + cv::Mat edges_color; + cv::cvtColor(edges, edges_color, cv::COLOR_GRAY2BGR); + edges_color.copyTo(output_roi_gray, mask); + + continue; + } + + cv::Mat output_roi = output_image(cv::Rect(x1, y1, x2 - x1, y2 - y1)); + cv::Mat edges_color; + cv::cvtColor(edges, edges_color, cv::COLOR_GRAY2BGR); + edges_color.copyTo(output_roi, mask); + + const cv::Point offset(x1, y1); + for (const auto& line : lines) { + cv::line(output_image, cv::Point(line[0], line[1]) + offset, + cv::Point(line[2], line[3]) + offset, ALL_LINES_COLOR, + LINE_THICKNESS); + } + + cv::Vec4i longest_line = find_longest_line(lines); + cv::line(output_image, + cv::Point(longest_line[0], longest_line[1]) + offset, + cv::Point(longest_line[2], longest_line[3]) + offset, + LONGEST_LINE_COLOR, LINE_THICKNESS); + + box.theta = std::atan2(longest_line[3] - longest_line[1], + longest_line[2] - longest_line[0]); } - cv::Vec4i longest_line = find_longest_line(lines); - return std::atan2(longest_line[3] - longest_line[1], - longest_line[2] - longest_line[0]); + image_to_draw_on = output_image; } cv::Vec4i AngleDetector::find_longest_line( diff --git a/src/valve_detector.cpp b/src/valve_detector.cpp index 5d7effd..566023c 100644 --- a/src/valve_detector.cpp +++ b/src/valve_detector.cpp @@ -15,21 +15,12 @@ ValveDetector::ValveDetector(int yolo_img_width_, plane_ransac_max_iterations_(plane_ransac_max_iterations), valve_handle_offset_(valve_handle_offset) {} -void ValveDetector::init_angle_detector(const AngleDetectorParams& params) { - angle_detector_ = std::make_unique(params); -} - cv::Mat ValveDetector::draw_detections(const cv::Mat& image, const std::vector& boxes, const std::vector& poses) const { if (boxes.size() != poses.size()) { return cv::Mat(); } - std::vector transformed_boxes; - transformed_boxes.reserve(boxes.size()); - for (const auto& box : boxes) { - transformed_boxes.push_back(transform_bounding_box(box)); - } cv::Mat visualized_image = image.clone(); @@ -39,8 +30,8 @@ cv::Mat ValveDetector::draw_detections(const cv::Mat& image, color_image_properties_.intr.cy, 0, 0, 1); cv::Mat dist_coeffs = cv::Mat::zeros(4, 1, CV_64F); - for (size_t i = 0; i < transformed_boxes.size(); ++i) { - const auto& box = transformed_boxes[i]; + for (size_t i = 0; i < boxes.size(); ++i) { + const auto& box = boxes[i]; const auto& pose = poses[i]; int x1 = box.center_x - box.size_x / 2; diff --git a/src/valve_pose_ros.cpp b/src/valve_pose_ros.cpp index 82c2e3f..54b28c9 100644 --- a/src/valve_pose_ros.cpp +++ b/src/valve_pose_ros.cpp @@ -32,9 +32,9 @@ ValvePoseNode::ValvePoseNode(const rclcpp::NodeOptions& options) valve_poses_pub_ = this->create_publisher( valve_poses_pub_topic, qos); - pcl_visualize_ = this->declare_parameter("pcl_visualize"); + debug_visualize_ = this->declare_parameter("debug_visualize"); - if (pcl_visualize_) { + if (debug_visualize_) { std::string annulus_pub_topic = this->declare_parameter("annulus_pub_topic"); annulus_pcl_pub_ = @@ -45,6 +45,10 @@ ValvePoseNode::ValvePoseNode(const rclcpp::NodeOptions& options) annulus_plane_pub_ = this->create_publisher( plane_pub_topic, qos); + angle_image_pub_ = this->create_publisher( + this->declare_parameter( + "angle_detection_image_pub_topic"), + qos); } if (visualize_detections_) { std::string processed_image_pub_topic = @@ -202,7 +206,7 @@ void ValvePoseNode::init_angle_detector() { angle_params.hough_max_line_gap = this->declare_parameter("angle.hough_max_line_gap"); - valve_detector_->init_angle_detector(angle_params); + angle_detector_ = std::make_unique(angle_params); } } From 08e25362a6f1b42cb961adad8fc5d78c30013494 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 17 Oct 2025 21:06:30 +0200 Subject: [PATCH 18/22] fixed orientation calculation --- include/valve_detection/valve_detector.hpp | 9 ++-- src/valve_detector.cpp | 62 ++++++++++++++++++---- 2 files changed, 57 insertions(+), 14 deletions(-) diff --git a/include/valve_detection/valve_detector.hpp b/include/valve_detection/valve_detector.hpp index b733c8e..b60259e 100644 --- a/include/valve_detection/valve_detector.hpp +++ b/include/valve_detection/valve_detector.hpp @@ -131,7 +131,7 @@ class ValveDetector { float angle = box.theta; Eigen::Matrix3f rotation_matrix = - create_rotation_matrix(plane_normal, angle); + create_rotation_matrix(coefficients, plane_normal, angle); Eigen::Quaternionf rotation_quat; rmat_to_quat(rotation_matrix, rotation_quat); @@ -231,12 +231,15 @@ class ValveDetector { /** * @brief Creates a rotation matrix with the plane normal as z-axis and * valve angle applied. + * @param coefficients Plane coefficients. * @param plane_normal Normalized plane normal vector. * @param angle Valve rotation angle in radians. * @return Rotation matrix (Eigen::Matrix3f) for the valve pose. */ - Eigen::Matrix3f create_rotation_matrix(const Eigen::Vector3f& plane_normal, - float angle); + Eigen::Matrix3f create_rotation_matrix( + const pcl::ModelCoefficients::Ptr& coefficients, + const Eigen::Vector3f& plane_normal, + float angle); /** * @brief Converts a 3x3 rotation matrix to a normalized quaternion. diff --git a/src/valve_detector.cpp b/src/valve_detector.cpp index 566023c..924054a 100644 --- a/src/valve_detector.cpp +++ b/src/valve_detector.cpp @@ -206,30 +206,70 @@ Eigen::Vector3f ValveDetector::shift_point_along_normal( } Eigen::Matrix3f ValveDetector::create_rotation_matrix( + const pcl::ModelCoefficients::Ptr& coefficients, const Eigen::Vector3f& plane_normal, float angle) { + if (!coefficients || coefficients->values.size() < 4) { + return Eigen::Matrix3f::Identity(); + } + Eigen::Vector3f z_axis = plane_normal; + float D = coefficients->values[3]; + + float fx = color_image_properties_.intr.fx; + float fy = color_image_properties_.intr.fy; + float cx = color_image_properties_.intr.cx; + float cy = color_image_properties_.intr.cy; + + Eigen::Matrix3f K; + K << fx, 0, cx, 0, fy, cy, 0, 0, 1; + Eigen::Matrix3f Kinv = K.inverse(); + + // Define two image points along the given angle (through principal point) + // --- + float len = 50.0f; // arbitrary pixel length (only direction matters) + Eigen::Vector3f p1(cx, cy, 1.f); + Eigen::Vector3f p2(cx + len * std::cos(angle), cy + len * std::sin(angle), + 1.f); + + // Back project points to rays + Eigen::Vector3f r1 = (Kinv * p1).normalized(); + Eigen::Vector3f r2 = (Kinv * p2).normalized(); + + // Compute intersections of rays with the plane + float denom1 = z_axis.dot(r1); + float denom2 = z_axis.dot(r2); - Eigen::Vector3f temp_vec(1, 0, 0); - if (std::abs(z_axis.dot(temp_vec)) > 0.99f) { - temp_vec = Eigen::Vector3f(0, 1, 0); + if (std::abs(denom1) < 1e-6f || std::abs(denom2) < 1e-6f) { + // rays parallel to plane — return identity fallback + return Eigen::Matrix3f::Identity(); } - Eigen::Vector3f base_x_axis = z_axis.cross(temp_vec).normalized(); - Eigen::AngleAxisf rotation(angle, z_axis); - Eigen::Vector3f x_axis = rotation * base_x_axis; + float t1 = -D / denom1; + float t2 = -D / denom2; - if (filter_direction_.dot(x_axis) < 0) { + Eigen::Vector3f X1 = t1 * r1; + Eigen::Vector3f X2 = t2 * r2; + + // Compute in-plane direction corresponding to the image line angle + Eigen::Vector3f x_axis = (X2 - X1).normalized(); + + // Project onto the plane (for numerical stability) + x_axis = (x_axis - x_axis.dot(z_axis) * z_axis).normalized(); + + // Ensure consistent direction (optional: avoid flipping frame between + // frames) + if (filter_direction_.dot(x_axis) < 0) x_axis = -x_axis; - } filter_direction_ = x_axis; Eigen::Vector3f y_axis = z_axis.cross(x_axis).normalized(); + x_axis = y_axis.cross(z_axis).normalized(); Eigen::Matrix3f rotation_matrix; - rotation_matrix.col(0) = x_axis; - rotation_matrix.col(1) = y_axis; - rotation_matrix.col(2) = z_axis; + rotation_matrix.col(0) = x_axis; // X_obj: direction of the image line + rotation_matrix.col(1) = y_axis; // Y_obj: perpendicular in-plane + rotation_matrix.col(2) = z_axis; // Z_obj: plane normal return rotation_matrix; } From f6c4a0bc85965ae7891abc8a266e3deddf56285e Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 17 Oct 2025 21:09:03 +0200 Subject: [PATCH 19/22] improved visualization --- src/angle_detector.cpp | 20 +++++++++++++++-- src/valve_detector.cpp | 51 ++++++++++++++++++++++++++---------------- src/valve_pose_ros.cpp | 27 +++++++++++++--------- 3 files changed, 67 insertions(+), 31 deletions(-) diff --git a/src/angle_detector.cpp b/src/angle_detector.cpp index ec750ef..203dc1f 100644 --- a/src/angle_detector.cpp +++ b/src/angle_detector.cpp @@ -56,8 +56,24 @@ void AngleDetector::compute_angles(const cv::Mat& color_image, } cv::Vec4i longest_line = find_longest_line(lines); - box.theta = std::atan2(longest_line[3] - longest_line[1], - longest_line[2] - longest_line[0]); + + if (longest_line[0] >= longest_line[2]) { + int temp_x = longest_line[0]; + int temp_y = longest_line[1]; + longest_line[0] = longest_line[2]; + longest_line[1] = longest_line[3]; + } + + float theta = std::atan2(longest_line[3] - longest_line[1], + longest_line[2] - longest_line[0]); + + if (theta < 0) { + theta += CV_PI; + } + if (theta > CV_PI) { + theta -= CV_PI; + } + box.theta = theta; } } diff --git a/src/valve_detector.cpp b/src/valve_detector.cpp index 924054a..3ad858a 100644 --- a/src/valve_detector.cpp +++ b/src/valve_detector.cpp @@ -18,10 +18,6 @@ ValveDetector::ValveDetector(int yolo_img_width_, cv::Mat ValveDetector::draw_detections(const cv::Mat& image, const std::vector& boxes, const std::vector& poses) const { - if (boxes.size() != poses.size()) { - return cv::Mat(); - } - cv::Mat visualized_image = image.clone(); cv::Mat camera_matrix = @@ -32,7 +28,6 @@ cv::Mat ValveDetector::draw_detections(const cv::Mat& image, for (size_t i = 0; i < boxes.size(); ++i) { const auto& box = boxes[i]; - const auto& pose = poses[i]; int x1 = box.center_x - box.size_x / 2; int y1 = box.center_y - box.size_y / 2; @@ -41,20 +36,38 @@ cv::Mat ValveDetector::draw_detections(const cv::Mat& image, cv::rectangle(visualized_image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); - cv::Mat tvec = (cv::Mat_(3, 1) << pose.position[0], - pose.position[1], pose.position[2]); - - Eigen::Matrix3f rotation_matrix = pose.orientation.toRotationMatrix(); - cv::Mat rmat = (cv::Mat_(3, 3) << rotation_matrix(0, 0), - rotation_matrix(0, 1), rotation_matrix(0, 2), - rotation_matrix(1, 0), rotation_matrix(1, 1), - rotation_matrix(1, 2), rotation_matrix(2, 0), - rotation_matrix(2, 1), rotation_matrix(2, 2)); - cv::Mat rvec; - cv::Rodrigues(rmat, rvec); - - cv::drawFrameAxes(visualized_image, camera_matrix, dist_coeffs, rvec, - tvec, 0.1); + std::ostringstream label; + label << "id:" << i; + if (!std::isnan(box.theta)) { + label << "Valve θ=" << std::fixed << std::setprecision(2) + << (box.theta * 180.0 / M_PI) << "°"; + cv::putText(visualized_image, label.str(), cv::Point(x1, y1 - 5), + cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), + 1); + } + // Draw pose axes only if valid pose exists + if (i < poses.size()) { + const Pose& pose = poses[i]; + if (!std::isnan(pose.position[0]) && + !std::isnan(pose.position[1]) && + !std::isnan(pose.position[2]) && !std::isnan(box.theta)) { + cv::Mat tvec = (cv::Mat_(3, 1) << pose.position[0], + pose.position[1], pose.position[2]); + + Eigen::Matrix3f rotation_matrix = + pose.orientation.toRotationMatrix(); + cv::Mat rmat = (cv::Mat_(3, 3) << rotation_matrix(0, 0), + rotation_matrix(0, 1), rotation_matrix(0, 2), + rotation_matrix(1, 0), rotation_matrix(1, 1), + rotation_matrix(1, 2), rotation_matrix(2, 0), + rotation_matrix(2, 1), rotation_matrix(2, 2)); + cv::Mat rvec; + cv::Rodrigues(rmat, rvec); + + cv::drawFrameAxes(visualized_image, camera_matrix, dist_coeffs, + rvec, tvec, 0.1); + } + } } return visualized_image; } diff --git a/src/valve_pose_ros.cpp b/src/valve_pose_ros.cpp index 54b28c9..8a07168 100644 --- a/src/valve_pose_ros.cpp +++ b/src/valve_pose_ros.cpp @@ -274,16 +274,23 @@ void ValvePoseNode::publish_valve_poses( pose_array_msg.header = header; for (const auto& pose : poses) { - geometry_msgs::msg::Pose pose_msg; - pose_msg.position.x = pose.position.x(); - pose_msg.position.y = pose.position.y(); - pose_msg.position.z = pose.position.z(); - pose_msg.orientation.x = pose.orientation.x(); - pose_msg.orientation.y = pose.orientation.y(); - pose_msg.orientation.z = pose.orientation.z(); - pose_msg.orientation.w = pose.orientation.w(); - - pose_array_msg.poses.push_back(pose_msg); + if (!std::isnan(pose.orientation.x()) && + !std::isnan(pose.orientation.y()) && + !std::isnan(pose.orientation.z()) && + !std::isnan(pose.orientation.w()) && + !std::isnan(pose.position.x()) && !std::isnan(pose.position.y()) && + !std::isnan(pose.position.z())) { + geometry_msgs::msg::Pose pose_msg; + pose_msg.position.x = pose.position.x(); + pose_msg.position.y = pose.position.y(); + pose_msg.position.z = pose.position.z(); + pose_msg.orientation.x = pose.orientation.x(); + pose_msg.orientation.y = pose.orientation.y(); + pose_msg.orientation.z = pose.orientation.z(); + pose_msg.orientation.w = pose.orientation.w(); + + pose_array_msg.poses.push_back(pose_msg); + } } valve_poses_pub_->publish(pose_array_msg); } From 01743588d17c12ef6cb763e134566886d22b8128 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 17 Oct 2025 21:19:03 +0200 Subject: [PATCH 20/22] added readme --- README.md | 92 ++++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 91 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 1cb4209..4c3b264 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,94 @@ -# 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 | Fix | +| ---------------------------- | ---------------------------------------------------------------------------- | --- | +| No poses published | Missing plane segmentation (adjust `plane_ransac_threshold` or annulus size) | | | | +| Angle NaN / no handle lines | Tune Hough and Canny thresholds | | + +--- From b25f100f184c0c057f5281716f315ead06403835 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 18 Oct 2025 10:34:33 +0200 Subject: [PATCH 21/22] added todo section in readme --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 4c3b264..546aab5 100644 --- a/README.md +++ b/README.md @@ -92,3 +92,7 @@ A `PoseArray` message is published with one `Pose` per detected valve. If orient | 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. From 239b6f9cd897125a86330cc1cce9660a3b87bac3 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 18 Oct 2025 12:34:04 +0200 Subject: [PATCH 22/22] fixed line endpoint swapping --- README.md | 9 +++++---- src/angle_detector.cpp | 2 ++ 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 546aab5..7b0612c 100644 --- a/README.md +++ b/README.md @@ -86,10 +86,11 @@ A `PoseArray` message is published with one `Pose` per detected valve. If orient ## Common Issues -| Issue | Possible Cause | Fix | -| ---------------------------- | ---------------------------------------------------------------------------- | --- | -| No poses published | Missing plane segmentation (adjust `plane_ransac_threshold` or annulus size) | | | | -| Angle NaN / no handle lines | Tune Hough and Canny thresholds | | +| 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 | + --- diff --git a/src/angle_detector.cpp b/src/angle_detector.cpp index 203dc1f..33bf776 100644 --- a/src/angle_detector.cpp +++ b/src/angle_detector.cpp @@ -62,6 +62,8 @@ void AngleDetector::compute_angles(const cv::Mat& color_image, int temp_y = longest_line[1]; longest_line[0] = longest_line[2]; longest_line[1] = longest_line[3]; + longest_line[2] = temp_x; + longest_line[3] = temp_y; } float theta = std::atan2(longest_line[3] - longest_line[1],