421 lines
17 KiB
Python
421 lines
17 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
ArUco标记检测模块
|
||
提供基于ArUco标记的靶心标定和激光点定位功能
|
||
"""
|
||
import cv2
|
||
import numpy as np
|
||
import math
|
||
import config
|
||
from logger_manager import logger_manager
|
||
|
||
|
||
class ArUcoDetector:
|
||
"""ArUco标记检测器"""
|
||
|
||
def __init__(self):
|
||
self.logger = logger_manager.logger
|
||
# 创建ArUco字典和检测器参数
|
||
self.aruco_dict = cv2.aruco.getPredefinedDictionary(config.ARUCO_DICT_TYPE)
|
||
self.detector_params = cv2.aruco.DetectorParameters()
|
||
|
||
# 设置检测参数
|
||
self.detector_params.minMarkerPerimeterRate = config.ARUCO_MIN_MARKER_PERIMETER_RATE
|
||
self.detector_params.cornerRefinementMethod = config.ARUCO_CORNER_REFINEMENT_METHOD
|
||
|
||
# 创建检测器
|
||
self.detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.detector_params)
|
||
|
||
# 预定义靶纸上的标记位置（物理坐标，毫米）
|
||
self.marker_positions_mm = config.ARUCO_MARKER_POSITIONS_MM
|
||
self.marker_ids = config.ARUCO_MARKER_IDS
|
||
self.marker_size_mm = config.ARUCO_MARKER_SIZE_MM
|
||
self.target_paper_size_mm = config.TARGET_PAPER_SIZE_MM
|
||
|
||
# 靶心偏移（相对于靶纸中心）
|
||
self.target_center_offset_mm = config.TARGET_CENTER_OFFSET_MM
|
||
|
||
if self.logger:
|
||
self.logger.info(f"[ARUCO] ArUco检测器初始化完成，字典类型: {config.ARUCO_DICT_TYPE}")
|
||
|
||
def detect_markers(self, frame):
|
||
"""
|
||
检测图像中的ArUco标记
|
||
|
||
Args:
|
||
frame: MaixPy图像帧对象
|
||
|
||
Returns:
|
||
(corners, ids, rejected) - 检测到的标记角点、ID列表、被拒绝的候选
|
||
如果检测失败返回 (None, None, None)
|
||
"""
|
||
try:
|
||
# 转换为OpenCV格式
|
||
from maix import image
|
||
img_cv = image.image2cv(frame, False, False)
|
||
|
||
# 转换为灰度图（ArUco检测需要）
|
||
if len(img_cv.shape) == 3:
|
||
gray = cv2.cvtColor(img_cv, cv2.COLOR_RGB2GRAY)
|
||
else:
|
||
gray = img_cv
|
||
|
||
# 检测标记
|
||
corners, ids, rejected = self.detector.detectMarkers(gray)
|
||
|
||
if self.logger and ids is not None:
|
||
self.logger.debug(f"[ARUCO] 检测到 {len(ids)} 个标记: {ids.flatten().tolist()}")
|
||
|
||
return corners, ids, rejected
|
||
|
||
except Exception as e:
|
||
if self.logger:
|
||
self.logger.error(f"[ARUCO] 标记检测失败: {e}")
|
||
return None, None, None
|
||
|
||
def get_target_center_from_markers(self, corners, ids):
|
||
"""
|
||
从检测到的ArUco标记计算靶心位置
|
||
|
||
Args:
|
||
corners: 标记角点列表
|
||
ids: 标记ID列表
|
||
|
||
Returns:
|
||
(center_x, center_y, radius, ellipse_params) 或 (None, None, None, None)
|
||
center_x, center_y: 靶心像素坐标
|
||
radius: 估计的靶心半径（像素）
|
||
ellipse_params: 椭圆参数用于透视校正
|
||
"""
|
||
if ids is None or len(ids) < 3:
|
||
if self.logger:
|
||
self.logger.debug(f"[ARUCO] 检测到的标记数量不足: {len(ids) if ids is not None else 0} < 3")
|
||
return None, None, None, None
|
||
|
||
try:
|
||
# 将ID转换为列表便于查找
|
||
detected_ids = ids.flatten().tolist()
|
||
|
||
# 收集检测到的标记中心点和对应的物理坐标
|
||
image_points = [] # 图像坐标 (像素)
|
||
object_points = [] # 物理坐标 (毫米)
|
||
marker_centers = {} # 存储每个标记的中心
|
||
|
||
for i, marker_id in enumerate(detected_ids):
|
||
if marker_id not in self.marker_ids:
|
||
continue
|
||
|
||
# 计算标记中心（四个角的平均值）
|
||
corner = corners[i][0] # shape: (4, 2)
|
||
center_x = np.mean(corner[:, 0])
|
||
center_y = np.mean(corner[:, 1])
|
||
marker_centers[marker_id] = (center_x, center_y)
|
||
|
||
# 添加到点列表
|
||
image_points.append([center_x, center_y])
|
||
object_points.append(self.marker_positions_mm[marker_id])
|
||
|
||
if len(image_points) < 3:
|
||
if self.logger:
|
||
self.logger.debug(f"[ARUCO] 有效标记数量不足: {len(image_points)} < 3")
|
||
return None, None, None, None
|
||
|
||
# 转换为numpy数组
|
||
image_points = np.array(image_points, dtype=np.float32)
|
||
object_points = np.array(object_points, dtype=np.float32)
|
||
|
||
# 计算单应性矩阵（Homography）
|
||
# 这建立了物理坐标到图像坐标的映射
|
||
H, status = cv2.findHomography(object_points, image_points, cv2.RANSAC, 5.0)
|
||
|
||
if H is None:
|
||
if self.logger:
|
||
self.logger.warning("[ARUCO] 无法计算单应性矩阵")
|
||
return None, None, None, None
|
||
|
||
# 计算靶心在图像中的位置
|
||
# 靶心物理坐标 = 靶纸中心 + 偏移
|
||
target_center_mm = np.array([[self.target_center_offset_mm[0],
|
||
self.target_center_offset_mm[1]]], dtype=np.float32)
|
||
target_center_mm = target_center_mm.reshape(-1, 1, 2)
|
||
|
||
# 使用单应性矩阵投影到图像坐标
|
||
target_center_img = cv2.perspectiveTransform(target_center_mm, H)
|
||
center_x = target_center_img[0][0][0]
|
||
center_y = target_center_img[0][0][1]
|
||
|
||
# 计算靶心半径（像素）
|
||
# 使用已知物理距离和像素距离的比例
|
||
# 选择两个标记计算比例尺
|
||
if len(marker_centers) >= 2:
|
||
# 使用对角线上的标记计算比例尺
|
||
if 0 in marker_centers and 2 in marker_centers:
|
||
p1_img = np.array(marker_centers[0])
|
||
p2_img = np.array(marker_centers[2])
|
||
p1_mm = np.array(self.marker_positions_mm[0])
|
||
p2_mm = np.array(self.marker_positions_mm[2])
|
||
elif 1 in marker_centers and 3 in marker_centers:
|
||
p1_img = np.array(marker_centers[1])
|
||
p2_img = np.array(marker_centers[3])
|
||
p1_mm = np.array(self.marker_positions_mm[1])
|
||
p2_mm = np.array(self.marker_positions_mm[3])
|
||
else:
|
||
# 使用任意两个标记
|
||
keys = list(marker_centers.keys())
|
||
p1_img = np.array(marker_centers[keys[0]])
|
||
p2_img = np.array(marker_centers[keys[1]])
|
||
p1_mm = np.array(self.marker_positions_mm[keys[0]])
|
||
p2_mm = np.array(self.marker_positions_mm[keys[1]])
|
||
|
||
pixel_distance = np.linalg.norm(p1_img - p2_img)
|
||
mm_distance = np.linalg.norm(p1_mm - p2_mm)
|
||
|
||
if mm_distance > 0:
|
||
pixels_per_mm = pixel_distance / mm_distance
|
||
# 标准靶心半径：10环半径约1.22cm = 12.2mm
|
||
# 但这里我们返回一个估计值，实际环数计算在laser_manager中
|
||
radius_mm = 122.0 # 整个靶纸的半径约200mm，但靶心区域较小
|
||
radius = int(radius_mm * pixels_per_mm)
|
||
else:
|
||
radius = 100 # 默认值
|
||
else:
|
||
radius = 100 # 默认值
|
||
|
||
# 计算椭圆参数（用于透视校正）
|
||
# 从单应性矩阵可以推导出透视变形
|
||
ellipse_params = self._compute_ellipse_params(H, center_x, center_y)
|
||
|
||
if self.logger:
|
||
self.logger.info(f"[ARUCO] 靶心计算成功: 中心=({center_x:.1f}, {center_y:.1f}), "
|
||
f"半径={radius}px, 检测到{len(marker_centers)}个标记")
|
||
|
||
return (int(center_x), int(center_y)), radius, "aruco", ellipse_params
|
||
|
||
except Exception as e:
|
||
if self.logger:
|
||
self.logger.error(f"[ARUCO] 计算靶心失败: {e}")
|
||
import traceback
|
||
self.logger.error(traceback.format_exc())
|
||
return None, None, None, None
|
||
|
||
def _compute_ellipse_params(self, H, center_x, center_y):
|
||
"""
|
||
从单应性矩阵计算椭圆参数，用于透视校正
|
||
|
||
Args:
|
||
H: 单应性矩阵 (3x3)
|
||
center_x, center_y: 靶心图像坐标
|
||
|
||
Returns:
|
||
ellipse_params: ((center_x, center_y), (width, height), angle)
|
||
"""
|
||
try:
|
||
# 在物理坐标系中画一个圆，投影到图像中看变成什么形状
|
||
# 物理圆：半径10mm
|
||
r_mm = 10.0
|
||
angles = np.linspace(0, 2*np.pi, 16)
|
||
circle_mm = np.array([[self.target_center_offset_mm[0] + r_mm * np.cos(a),
|
||
self.target_center_offset_mm[1] + r_mm * np.sin(a)]
|
||
for a in angles], dtype=np.float32)
|
||
circle_mm = circle_mm.reshape(-1, 1, 2)
|
||
|
||
# 投影到图像
|
||
circle_img = cv2.perspectiveTransform(circle_mm, H)
|
||
circle_img = circle_img.reshape(-1, 2)
|
||
|
||
# 拟合椭圆
|
||
if len(circle_img) >= 5:
|
||
ellipse = cv2.fitEllipse(circle_img.astype(np.float32))
|
||
return ellipse
|
||
else:
|
||
# 从单应性矩阵近似估计
|
||
# 提取缩放和旋转
|
||
# H = K * [R|t] 的近似
|
||
# 这里简化处理：假设没有严重变形
|
||
scale_x = np.linalg.norm(H[0, :2])
|
||
scale_y = np.linalg.norm(H[1, :2])
|
||
avg_scale = (scale_x + scale_y) / 2
|
||
|
||
width = r_mm * 2 * scale_x
|
||
height = r_mm * 2 * scale_y
|
||
angle = np.degrees(np.arctan2(H[1, 0], H[0, 0]))
|
||
|
||
return ((center_x, center_y), (width, height), angle)
|
||
|
||
except Exception as e:
|
||
if self.logger:
|
||
self.logger.debug(f"[ARUCO] 计算椭圆参数失败: {e}")
|
||
return None
|
||
|
||
def transform_laser_point(self, laser_point, corners, ids):
|
||
"""
|
||
将激光点从图像坐标转换到物理坐标（毫米），再计算相对于靶心的偏移
|
||
|
||
Args:
|
||
laser_point: (x, y) 激光点在图像中的坐标
|
||
corners: 检测到的标记角点
|
||
ids: 检测到的标记ID
|
||
|
||
Returns:
|
||
(dx_mm, dy_mm) 激光点相对于靶心的偏移（毫米），或 (None, None)
|
||
"""
|
||
if laser_point is None or ids is None or len(ids) < 3:
|
||
return None, None
|
||
|
||
try:
|
||
# 重新计算单应性矩阵（可以优化为缓存）
|
||
detected_ids = ids.flatten().tolist()
|
||
image_points = []
|
||
object_points = []
|
||
|
||
for i, marker_id in enumerate(detected_ids):
|
||
if marker_id not in self.marker_ids:
|
||
continue
|
||
corner = corners[i][0]
|
||
center_x = np.mean(corner[:, 0])
|
||
center_y = np.mean(corner[:, 1])
|
||
image_points.append([center_x, center_y])
|
||
object_points.append(self.marker_positions_mm[marker_id])
|
||
|
||
if len(image_points) < 3:
|
||
return None, None
|
||
|
||
image_points = np.array(image_points, dtype=np.float32)
|
||
object_points = np.array(object_points, dtype=np.float32)
|
||
|
||
H, _ = cv2.findHomography(object_points, image_points, cv2.RANSAC, 5.0)
|
||
if H is None:
|
||
return None, None
|
||
|
||
# 求逆矩阵，将图像坐标转换到物理坐标
|
||
H_inv = np.linalg.inv(H)
|
||
|
||
laser_img = np.array([[laser_point[0], laser_point[1]]], dtype=np.float32)
|
||
laser_img = laser_img.reshape(-1, 1, 2)
|
||
|
||
laser_mm = cv2.perspectiveTransform(laser_img, H_inv)
|
||
laser_x_mm = laser_mm[0][0][0]
|
||
laser_y_mm = laser_mm[0][0][1]
|
||
|
||
# 计算相对于靶心的偏移
|
||
# 注意：Y轴方向可能需要翻转（图像Y向下，物理Y通常向上）
|
||
dx_mm = laser_x_mm - self.target_center_offset_mm[0]
|
||
dy_mm = -(laser_y_mm - self.target_center_offset_mm[1]) # 翻转Y轴
|
||
|
||
if self.logger:
|
||
self.logger.debug(f"[ARUCO] 激光点转换: 图像({laser_point[0]:.1f}, {laser_point[1]:.1f}) -> "
|
||
f"物理({laser_x_mm:.1f}, {laser_y_mm:.1f}) -> "
|
||
f"偏移({dx_mm:.1f}, {dy_mm:.1f})mm")
|
||
|
||
return dx_mm, dy_mm
|
||
|
||
except Exception as e:
|
||
if self.logger:
|
||
self.logger.error(f"[ARUCO] 激光点转换失败: {e}")
|
||
return None, None
|
||
|
||
def draw_debug_info(self, frame, corners, ids, target_center=None, laser_point=None):
|
||
"""
|
||
在图像上绘制调试信息
|
||
|
||
Args:
|
||
frame: MaixPy图像帧
|
||
corners: 标记角点
|
||
ids: 标记ID
|
||
target_center: 计算的靶心位置
|
||
laser_point: 激光点位置
|
||
|
||
Returns:
|
||
绘制后的图像
|
||
"""
|
||
try:
|
||
from maix import image
|
||
img_cv = image.image2cv(frame, False, False).copy()
|
||
|
||
# 绘制检测到的标记
|
||
if ids is not None:
|
||
cv2.aruco.drawDetectedMarkers(img_cv, corners, ids)
|
||
|
||
# 绘制标记ID和中心
|
||
for i, marker_id in enumerate(ids.flatten()):
|
||
corner = corners[i][0]
|
||
center_x = int(np.mean(corner[:, 0]))
|
||
center_y = int(np.mean(corner[:, 1]))
|
||
|
||
# 绘制中心点
|
||
cv2.circle(img_cv, (center_x, center_y), 5, (0, 255, 0), -1)
|
||
|
||
# 绘制ID
|
||
cv2.putText(img_cv, f"ID:{marker_id}",
|
||
(center_x + 10, center_y - 10),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
|
||
|
||
# 绘制靶心
|
||
if target_center:
|
||
cv2.circle(img_cv, target_center, 8, (255, 0, 0), -1)
|
||
cv2.circle(img_cv, target_center, 50, (255, 0, 0), 2)
|
||
cv2.putText(img_cv, "TARGET", (target_center[0] + 15, target_center[1] - 15),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
|
||
|
||
# 绘制激光点
|
||
if laser_point:
|
||
cv2.circle(img_cv, (int(laser_point[0]), int(laser_point[1])), 6, (0, 0, 255), -1)
|
||
cv2.putText(img_cv, "LASER", (int(laser_point[0]) + 10, int(laser_point[1]) - 10),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||
|
||
# 转换回MaixPy图像
|
||
return image.cv2image(img_cv, False, False)
|
||
|
||
except Exception as e:
|
||
if self.logger:
|
||
self.logger.error(f"[ARUCO] 绘制调试信息失败: {e}")
|
||
return frame
|
||
|
||
|
||
# 创建全局单例实例
|
||
aruco_detector = ArUcoDetector()
|
||
|
||
|
||
def detect_target_with_aruco(frame, laser_point=None):
|
||
"""
|
||
使用ArUco标记检测靶心的便捷函数
|
||
|
||
Args:
|
||
frame: MaixPy图像帧
|
||
laser_point: 激光点坐标（可选）
|
||
|
||
Returns:
|
||
(result_img, center, radius, method, best_radius1, ellipse_params)
|
||
与detect_circle_v3保持相同的返回格式
|
||
"""
|
||
detector = aruco_detector
|
||
|
||
# 检测ArUco标记
|
||
corners, ids, rejected = detector.detect_markers(frame)
|
||
|
||
# 计算靶心
|
||
center, radius, method, ellipse_params = detector.get_target_center_from_markers(corners, ids)
|
||
|
||
# 绘制调试信息
|
||
result_img = detector.draw_debug_info(frame, corners, ids, center, laser_point)
|
||
|
||
# 返回与detect_circle_v3相同的格式
|
||
# best_radius1用于距离估算，这里用radius代替
|
||
return result_img, center, radius, method, radius, ellipse_params
|
||
|
||
|
||
def compute_laser_offset_aruco(laser_point, corners, ids):
|
||
"""
|
||
使用ArUco计算激光点相对于靶心的偏移（毫米）
|
||
|
||
Args:
|
||
laser_point: (x, y) 激光点图像坐标
|
||
corners: ArUco标记角点
|
||
ids: ArUco标记ID
|
||
|
||
Returns:
|
||
(dx_mm, dy_mm) 偏移量（毫米），或 (None, None)
|
||
"""
|
||
return aruco_detector.transform_laser_point(laser_point, corners, ids)
|