From df02657f1d6a0692cfafa3c9a0b02f8f1cd63d1b Mon Sep 17 00:00:00 2001 From: dev01 Date: Thu, 4 Jun 2026 11:00:47 +0800 Subject: [PATCH] feat: add PnP pose estimation, laser world coordinate solving, and new 0xA5 protocol - Replace hardcoded arrays with JSON string format for calibration and laser parameters, matching scripts/calibrate/laser_pnp.py - Apply IMAGE_SCALE to resize factor instead of hardcoded 0.5 - Add rectangle corner ordering (TL/BL/BR/TR) and PnP solving with SOLVEPNP_IPPE - Compute laser world coordinates on Z=0 plane, normalize by RECT_L/RECT_W as measurement values; reference values set to rectangle center (0.5, 0.5) - Publish Float32MultiArray [measure_x, ref_x, measure_y, ref_y] on /vision/target - Remove /vision/binary_image publisher - Add ENABLE_CUDA toggle and CUDA detection log with pipeline breakdown --- .../algorithm_pkg/vision_node.py | 109 ++++++++++-------- 1 file changed, 63 insertions(+), 46 deletions(-) diff --git a/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py b/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py index 0488b30..92ca37f 100755 --- a/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py +++ b/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import json import numpy as np import cv2 @@ -8,17 +9,28 @@ from sensor_msgs.msg import Image from std_msgs.msg import Float32MultiArray from cv_bridge import CvBridge -# Camera calibration (from calibrate.py) -K = np.array([ +# Camera calibration (paste from calibrate.py JSON output) +CALIB_JSON = """ +{ + "mtx": [ [1247.0565667653996, 0.0, 753.8809622002652], [0.0, 1256.6168638051222, 528.9954978706462], [0.0, 0.0, 1.0] -]) -DIST = np.array([[0.0, 0.0, 0.0, 0.0, 0.0]]) + ], + "dist": [[0.0, 0.0, 0.0, 0.0, 0.0]], + "img_size": [1440, 1080], + "square_size": 15.0, + "error": 0.021690239575344137 +} +""" -# Laser line parameters (from laser_calibrate.py) -D_CAM = np.array([-0.00989344622939825, 0.015559548512101173, 0.9998300075531006]) -O_CAM = np.array([-26.1895751953125, -3.379796028137207, 0.0]) +# Laser line parameters (paste from laser_calibrate.py JSON output) +LASER_JSON = """ +{ + "direction": [-0.00989344622939825, 0.015559548512101173, 0.9998300075531006], + "origin": [-26.1895751953125, -3.379796028137207, 0.0] +} +""" # Rectangle physical dimensions (mm) RECT_L = 305.0 @@ -26,9 +38,12 @@ RECT_W = 215.0 if RECT_W > RECT_L: RECT_L, RECT_W = RECT_W, RECT_L +# CUDA switch — set to False to force CPU pipeline +ENABLE_CUDA = False + # Image preprocessing IMAGE_SCALE = 0.5 -IMAGE_AREA = 1080 * 1440 / 4 +IMAGE_AREA = 1080 * 1440 * IMAGE_SCALE * IMAGE_SCALE MIN_AREA_RATIO = 0.01 THRESHOLD_AREA = IMAGE_AREA * MIN_AREA_RATIO @@ -37,6 +52,24 @@ REF_X = (RECT_L / 2.0) / RECT_L # 0.5 REF_Y = (RECT_W / 2.0) / RECT_W # 0.5 +def _parse_calib(): + data = json.loads(CALIB_JSON) + mtx = np.array(data["mtx"]) + dist = np.array(data["dist"]) + return mtx, dist + + +def _parse_laser(): + data = json.loads(LASER_JSON) + d = np.array(data["direction"], dtype=np.float64) + o = np.array(data["origin"], dtype=np.float64) + return d, o + + +K, DIST = _parse_calib() +D_CAM, O_CAM = _parse_laser() + + class VisionNode(Node): def __init__(self): super().__init__('vision_node') @@ -55,38 +88,38 @@ class VisionNode(Node): self._result_publisher = self.create_publisher( Image, '/vision/result_image', 10 ) - self._binary_publisher = self.create_publisher( - Image, '/vision/binary_image', 10 - ) self._target_publisher = self.create_publisher( Float32MultiArray, '/vision/target', 10 ) # GPU acceleration setup self._use_cuda = False - try: - if cv2.cuda.getCudaEnabledDeviceCount() > 0: - self._use_cuda = True - self._gpu_frame = cv2.cuda_GpuMat() - self._gpu_blurred = cv2.cuda_GpuMat() - self._gaussian_filter = cv2.cuda.createGaussianFilter( - cv2.CV_8UC1, cv2.CV_8UC1, (5, 5), 1) - except AttributeError: - pass + if ENABLE_CUDA: + try: + if cv2.cuda.getCudaEnabledDeviceCount() > 0: + self._use_cuda = True + self._gpu_frame = cv2.cuda_GpuMat() + self._gpu_blurred = cv2.cuda_GpuMat() + self._gaussian_filter = cv2.cuda.createGaussianFilter( + cv2.CV_8UC1, cv2.CV_8UC1, (5, 5), 1) + self.get_logger().info( + 'CUDA GPU detected — ' + 'resize + cvtColor + GaussianBlur on GPU, ' + 'threshold + morphology + PnP on CPU') + except AttributeError: + pass self.get_logger().info( - 'VisionNode ready - %s, subscribing to /camera/image_raw' - % ('CUDA GPU acceleration enabled' - if self._use_cuda else 'CPU pipeline') - ) + 'VisionNode ready, subscribing to /camera/image_raw') self.start_time = 0 def _preprocess(self, cv_image): - """Return blurred grayscale at half resolution.""" + """Return blurred grayscale at IMAGE_SCALE resolution.""" if self._use_cuda: return self._gpu_preprocess(cv_image) - scaled = cv2.resize(cv_image, None, fx=0.5, fy=0.5, + scaled = cv2.resize(cv_image, None, + fx=IMAGE_SCALE, fy=IMAGE_SCALE, interpolation=cv2.INTER_AREA) gray = cv2.cvtColor(scaled, cv2.COLOR_BGR2GRAY) return cv2.GaussianBlur(gray, (5, 5), 1) @@ -94,7 +127,7 @@ class VisionNode(Node): def _gpu_preprocess(self, cv_image): self._gpu_frame.upload(cv_image) gpu_scaled = cv2.cuda.resize( - self._gpu_frame, None, fx=0.5, fy=0.5, + self._gpu_frame, None, fx=IMAGE_SCALE, fy=IMAGE_SCALE, interpolation=cv2.INTER_AREA) gpu_gray = cv2.cuda.cvtColor(gpu_scaled, cv2.COLOR_BGR2GRAY) self._gpu_blurred.create(gpu_gray.size(), gpu_gray.type()) @@ -104,7 +137,7 @@ class VisionNode(Node): def _detect_rectangle(self, blurred): """Otsu threshold + morphology close + contour fitting. - Returns ordered corners (4, 2) float32 or None. + Returns corners (4,2) float32 or None. """ _, binary = cv2.threshold( blurred, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU) @@ -127,7 +160,7 @@ class VisionNode(Node): best_area = area best = approx.reshape(4, 2) - return best.astype(np.float32) if best is not None else None, closed + return best.astype(np.float32) if best is not None else None def _order_corners(self, image_corners): """Order corners as TL, BL, BR, TR and build world points.""" @@ -199,16 +232,8 @@ class VisionNode(Node): gauss_image = self._preprocess(cv_image) - corners, binary = self._detect_rectangle(gauss_image) + corners = self._detect_rectangle(gauss_image) if corners is None: - # Publish binary image even when no rectangle found - binary_color = cv2.cvtColor(binary, cv2.COLOR_GRAY2BGR) - binary_msg = self._bridge.cv2_to_imgmsg( - binary_color, encoding='bgr8') - binary_msg.header.stamp = msg.header.stamp - binary_msg.header.frame_id = msg.header.frame_id - self._binary_publisher.publish(binary_msg) - # Publish result image with FPS overlay result_image = cv_image.copy() fps = cv2.getTickFrequency() / max( @@ -315,14 +340,6 @@ class VisionNode(Node): result_msg.header.frame_id = msg.header.frame_id self._result_publisher.publish(result_msg) - # Publish binary image - binary_color = cv2.cvtColor(binary, cv2.COLOR_GRAY2BGR) - binary_msg = self._bridge.cv2_to_imgmsg( - binary_color, encoding='bgr8') - binary_msg.header.stamp = msg.header.stamp - binary_msg.header.frame_id = msg.header.frame_id - self._binary_publisher.publish(binary_msg) - except Exception as e: self.get_logger().error(f'Vision processing error: {e}')