feat: add /vision/debug_image topic, inner-rect filter, and NaN guards
- Publish binary+contour debug image to /vision/debug_image - Require nested rectangular contour with no deeper children - Add NaN guards after solvePnP and projectPoints to prevent crashes
This commit is contained in:
dev01
@@ -91,6 +91,9 @@ class VisionNode(Node):
|
||||
self._target_publisher = self.create_publisher(
|
||||
Float32MultiArray, '/vision/target', 10
|
||||
)
|
||||
self._debug_publisher = self.create_publisher(
|
||||
Image, '/vision/debug_image', 10
|
||||
)
|
||||
|
||||
# GPU acceleration setup
|
||||
self._use_cuda = False
|
||||
@@ -144,23 +147,48 @@ class VisionNode(Node):
|
||||
kernel = np.ones((3, 3), np.uint8)
|
||||
closed = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel)
|
||||
|
||||
contours, _ = cv2.findContours(
|
||||
closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
contours, hierarchy = cv2.findContours(
|
||||
closed, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
|
||||
|
||||
def _is_rect(ct):
|
||||
ep = 0.02 * cv2.arcLength(ct, True)
|
||||
ap = cv2.approxPolyDP(ct, ep, True)
|
||||
return len(ap) == 4 and cv2.isContourConvex(ap)
|
||||
|
||||
best = None
|
||||
best_area = 0
|
||||
for cnt in contours:
|
||||
best_inner_idx = -1
|
||||
for i, cnt in enumerate(contours):
|
||||
area = cv2.contourArea(cnt)
|
||||
if area < THRESHOLD_AREA:
|
||||
continue
|
||||
epsilon = 0.02 * cv2.arcLength(cnt, True)
|
||||
approx = cv2.approxPolyDP(cnt, epsilon, True)
|
||||
if len(approx) == 4 and cv2.isContourConvex(approx):
|
||||
if area > best_area:
|
||||
best_area = area
|
||||
best = approx.reshape(4, 2)
|
||||
if not _is_rect(cnt):
|
||||
continue
|
||||
# Require an inner rectangular child contour
|
||||
child_idx = hierarchy[0][i][2]
|
||||
has_inner_rect = False
|
||||
while child_idx != -1:
|
||||
if _is_rect(contours[child_idx]) and hierarchy[0][child_idx][2] == -1:
|
||||
has_inner_rect = True
|
||||
break
|
||||
child_idx = hierarchy[0][child_idx][0] # next sibling
|
||||
if not has_inner_rect:
|
||||
continue
|
||||
approx = cv2.approxPolyDP(cnt, 0.02 * cv2.arcLength(cnt, True), True)
|
||||
if area > best_area:
|
||||
best_area = area
|
||||
best = approx.reshape(4, 2)
|
||||
best_inner_idx = child_idx
|
||||
|
||||
return best.astype(np.float32) if best is not None else None
|
||||
# Draw all contours on closed for debugging
|
||||
vis = cv2.cvtColor(closed, cv2.COLOR_GRAY2BGR)
|
||||
cv2.drawContours(vis, contours, -1, (0, 255, 0), 1)
|
||||
if best is not None:
|
||||
cv2.drawContours(vis, [best.astype(np.int32)], -1, (0, 0, 255), 2)
|
||||
if best_inner_idx != -1:
|
||||
cv2.drawContours(vis, [contours[best_inner_idx]], -1, (255, 0, 0), 2)
|
||||
|
||||
return (best.astype(np.float32) if best is not None else None, vis)
|
||||
|
||||
def _order_corners(self, image_corners):
|
||||
"""Order corners as TL, BL, BR, TR and build world points."""
|
||||
@@ -232,7 +260,16 @@ class VisionNode(Node):
|
||||
|
||||
gauss_image = self._preprocess(cv_image)
|
||||
|
||||
corners = self._detect_rectangle(gauss_image)
|
||||
corners, debug_image = self._detect_rectangle(gauss_image)
|
||||
|
||||
# Publish binary debug image at processing resolution
|
||||
if debug_image is not None:
|
||||
debug_msg = self._bridge.cv2_to_imgmsg(
|
||||
debug_image, encoding='bgr8')
|
||||
debug_msg.header.stamp = msg.header.stamp
|
||||
debug_msg.header.frame_id = msg.header.frame_id
|
||||
self._debug_publisher.publish(debug_msg)
|
||||
|
||||
if corners is None:
|
||||
# Publish result image with FPS overlay
|
||||
result_image = cv_image.copy()
|
||||
@@ -257,6 +294,9 @@ class VisionNode(Node):
|
||||
flags=cv2.SOLVEPNP_IPPE
|
||||
)
|
||||
|
||||
if success and (np.isnan(rvec).any() or np.isnan(tvec).any()):
|
||||
success = False
|
||||
|
||||
result_image = cv_image.copy()
|
||||
scale_up = 1.0 / IMAGE_SCALE
|
||||
|
||||
@@ -284,17 +324,18 @@ class VisionNode(Node):
|
||||
[0, 0, axis_len]])
|
||||
imgpts, _ = cv2.projectPoints(
|
||||
axis_3d, rvec, tvec, self._K_scaled, DIST)
|
||||
origin = tuple(
|
||||
(imgpts[0].ravel() * scale_up).astype(int))
|
||||
cv2.line(result_image, origin,
|
||||
tuple((imgpts[1].ravel() * scale_up).astype(int)),
|
||||
(0, 0, 255), 3)
|
||||
cv2.line(result_image, origin,
|
||||
tuple((imgpts[2].ravel() * scale_up).astype(int)),
|
||||
(0, 255, 0), 3)
|
||||
cv2.line(result_image, origin,
|
||||
tuple((imgpts[3].ravel() * scale_up).astype(int)),
|
||||
(255, 0, 0), 3)
|
||||
if not np.isnan(imgpts).any():
|
||||
origin = tuple(
|
||||
(imgpts[0].ravel() * scale_up).astype(int))
|
||||
cv2.line(result_image, origin,
|
||||
tuple((imgpts[1].ravel() * scale_up).astype(int)),
|
||||
(0, 0, 255), 3)
|
||||
cv2.line(result_image, origin,
|
||||
tuple((imgpts[2].ravel() * scale_up).astype(int)),
|
||||
(0, 255, 0), 3)
|
||||
cv2.line(result_image, origin,
|
||||
tuple((imgpts[3].ravel() * scale_up).astype(int)),
|
||||
(255, 0, 0), 3)
|
||||
|
||||
# Compute and publish normalized measurement + reference
|
||||
if laser_world is not None:
|
||||
@@ -312,10 +353,13 @@ class VisionNode(Node):
|
||||
dtype=np.float32)
|
||||
px_proj, _ = cv2.projectPoints(
|
||||
P_world_3d, rvec, tvec, self._K_scaled, DIST)
|
||||
lx = int(px_proj.ravel()[0] * scale_up)
|
||||
ly = int(px_proj.ravel()[1] * scale_up)
|
||||
cv2.circle(result_image, (lx, ly), 10, (0, 0, 255), 2)
|
||||
cv2.circle(result_image, (lx, ly), 2, (0, 0, 255), -1)
|
||||
if not np.isnan(px_proj).any():
|
||||
lx = int(px_proj.ravel()[0] * scale_up)
|
||||
ly = int(px_proj.ravel()[1] * scale_up)
|
||||
cv2.circle(result_image, (lx, ly), 10,
|
||||
(0, 0, 255), 2)
|
||||
cv2.circle(result_image, (lx, ly), 2,
|
||||
(0, 0, 255), -1)
|
||||
else:
|
||||
# PnP failed — still draw detected corners
|
||||
corners_scaled = (ordered_corners * scale_up).astype(np.int32)
|
||||
|
||||
Reference in New Issue
Block a user