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
This commit is contained in:
dev01
@@ -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}')
|
||||
|
||||
|
||||
Reference in New Issue
Block a user