ZZH000829/archery
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

pref: laser find center point

Browse Source
This commit is contained in:
linyimin
2026-06-02 16:03:18 +08:00
parent aa16676c74
commit 26ed3c1523
4 changed files with 111 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
app.yaml
View File

@@ -1,6 +1,6 @@
id: t11
name: t11
version: 2.14.1
version: 2.15.1
author: t11
icon: ''
desc: t11
@@ -14,6 +14,7 @@ files:
- cameraParameters.xml
- config.py
- hardware.py
- laser_detector.py
- laser_manager.py
- logger_manager.py
- main.py

4
config.py
View File

@@ -134,7 +134,7 @@ IMAGE_CENTER_Y = 240 # 图像中心 Y 坐标
# ==================== 三角形四角标记：单应性偏移 + PnP 估距 ====================
# 依赖 cameraParameters.xml（相机内参）与 triangle_positions.json（四角物方坐标，厘米或毫米见 JSON 约定）。
# 部署时请把这两个文件放到 APP_DIR（与 main 同应用目录），或改下面路径为设备上的实际绝对路径。
USE_TRIANGLE_OFFSET = True # False 时仅走黄心圆/椭圆 + 半径估距，不使用三角形路径
USE_TRIANGLE_OFFSET = False # False 时仅走黄心圆/椭圆 + 半径估距，不使用三角形路径
CAMERA_CALIB_XML = APP_DIR + "/cameraParameters.xml"
TRIANGLE_POSITIONS_JSON = APP_DIR + "/triangle_positions.json"
# 检测到的三角形边长在图像中的像素范围，分辨率或靶纸占比变化时可微调
@@ -309,7 +309,7 @@ LASER_THICKNESS = 1
LASER_LENGTH = 2
# ==================== 图像保存配置 ====================
SAVE_IMAGE_ENABLED = True # 是否保存图像（True=保存，False=不保存）
SAVE_IMAGE_ENABLED = False # 是否保存图像（True=保存，False=不保存）
PHOTO_DIR = "/root/phot" # 照片存储目录
MAX_IMAGES = 1000
# Stage2 调试目录（默认 PHOTO_DIR/stage2_roi）内 JPEG 最多保留张数；None 表示与 MAX_IMAGES 相同

168
laser_detector.py
View File

@@ -14,9 +14,34 @@ except ImportError:
WIDTH = 640
HEIGHT = 480
THRESHOLD = 100
RED_RATIO = 1.3
RED_RATIO = 1
SEARCH_RADIUS = 60
STABLE_COUNT = 5
STABLE_COUNT = 2
# Temporal smoothing
_EMA_ALPHA = 0.4
_GATE_PX = 10
_FRAME_INTERVAL_MS = 50
_prev_smoothed = None
def _red_weighted_centroid(r_ch, g_ch, b_ch, mask, x0, y0):
y_ids, x_ids = np.where(mask)
if len(y_ids) == 0:
return None
r_vals = r_ch[y_ids, x_ids].astype(np.float64)
g_vals = g_ch[y_ids, x_ids].astype(np.float64)
b_vals = b_ch[y_ids, x_ids].astype(np.float64)
w = r_vals - np.maximum(g_vals, b_vals)
w = np.clip(w, 0, None)
w = w * w
total_w = w.sum()
if total_w < 1e-6:
return None
cx = (x_ids.astype(np.float64) * w).sum() / total_w + x0
cy = (y_ids.astype(np.float64) * w).sum() / total_w + y0
return (float(cx), float(cy))
def find_ellipse(img_cv, cx, cy, roi_r, th, ratio):
@@ -43,27 +68,15 @@ def find_ellipse(img_cv, cx, cy, roi_r, th, ratio):
for pt in cnt:
pt[0][0] += x1
pt[0][1] += y1
if len(cnt) >= 5:
ellipse_valid = len(cnt) >= 5
if ellipse_valid:
(ex, ey), (ew, eh), ang = cv2.fitEllipse(cnt)
mask_ellipse = np.zeros((HEIGHT, WIDTH), dtype=np.uint8)
cv2.ellipse(mask_ellipse, (int(ex), int(ey)), (int(ew / 2), int(eh / 2)), ang, 0, 360, 255, -1)
brightness = img_cv[:, :, 0].astype(np.int32) + img_cv[:, :, 1].astype(np.int32) + img_cv[:, :, 2].astype(
np.int32)
masked = np.where(mask_ellipse > 0, brightness, 0)
vals = masked[masked > 0]
if len(vals) > 0:
bth = np.percentile(vals, 90)
bmask = (masked >= bth).astype(np.uint8) * 255
bcontours, _ = cv2.findContours(bmask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if bcontours:
blargest = max(bcontours, key=cv2.contourArea)
if cv2.contourArea(blargest) >= 3 and len(blargest) >= 5:
(ix, iy), _, _ = cv2.fitEllipse(blargest)
return (float(ix), float(iy))
M = cv2.moments(blargest)
if M["m00"] > 0:
return (float(M["m10"] / M["m00"]), float(M["m01"] / M["m00"]))
return (float(ex), float(ey))
return _red_weighted_centroid(
img_cv[:, :, 0], img_cv[:, :, 1], img_cv[:, :, 2],
mask_ellipse > 0, 0, 0
)
M = cv2.moments(cnt)
if M["m00"] > 0:
return (float(M["m10"] / M["m00"]), float(M["m01"] / M["m00"]))
@@ -76,55 +89,71 @@ def find_brightest_bytes(frame, cx, cy, roi_r, th, ratio):
y1 = max(0, cy - roi_r)
y2 = min(HEIGHT, cy + roi_r)
data = frame.to_bytes()
best_score = 0
best_pos = None
for y in range(y1, y2, 2):
for x in range(x1, x2, 2):
rs, gs, bs = [], [], []
xs, ys = [], []
step = 2
for y in range(y1, y2, step):
for x in range(x1, x2, step):
idx = (y * WIDTH + x) * 3
r = data[idx];
g = data[idx + 1];
r = data[idx]
g = data[idx + 1]
b = data[idx + 2]
if (r > th and r > g * ratio and r > b * ratio) or \
(r > 200 and g > 200 and b > 200 and r >= g and r >= b and (r - g) > 10 and (r - b) > 10):
score = r + g + b
dx = x - cx;
dy = y - cy
score *= max(0.5, 1.0 - ((dx * dx + dy * dy) ** 0.5 / roi_r) * 0.5)
if score > best_score:
best_score = score
best_pos = (x, y)
if best_pos is None:
rs.append(r)
gs.append(g)
bs.append(b)
xs.append(x)
ys.append(y)
if not rs:
return None
fx, fy = best_pos
x1f = max(0, fx - 3);
x2f = min(WIDTH, fx + 4)
y1f = max(0, fy - 3);
y2f = min(HEIGHT, fy + 4)
best_bright = 0
final_pos = best_pos
for y in range(y1f, y2f):
for x in range(x1f, x2f):
idx = (y * WIDTH + x) * 3
r = data[idx];
g = data[idx + 1];
b = data[idx + 2]
if (r > th and r > g * ratio and r > b * ratio) or \
(r > 200 and g > 200 and b > 200 and r >= g and r >= b and (r - g) > 10 and (r - b) > 10):
rgb_sum = r + g + b
if rgb_sum > best_bright:
best_bright = rgb_sum
final_pos = (float(x), float(y))
return final_pos
rs = np.array(rs, dtype=np.float64)
gs = np.array(gs, dtype=np.float64)
bs = np.array(bs, dtype=np.float64)
xs = np.array(xs, dtype=np.float64)
ys = np.array(ys, dtype=np.float64)
w = rs - np.maximum(gs, bs)
w = np.clip(w, 0, None)
w = w * w
total_w = w.sum()
if total_w < 1e-6:
return None
cx_f = (xs * w).sum() / total_w
cy_f = (ys * w).sum() / total_w
return (float(cx_f), float(cy_f))
def _ema_filter(pos, alpha=_EMA_ALPHA):
global _prev_smoothed
if _prev_smoothed is None:
_prev_smoothed = pos
return pos
sx = alpha * pos[0] + (1 - alpha) * _prev_smoothed[0]
sy = alpha * pos[1] + (1 - alpha) * _prev_smoothed[1]
_prev_smoothed = (sx, sy)
return _prev_smoothed
def _gated(pos, gate_px=_GATE_PX):
global _prev_smoothed
if _prev_smoothed is None:
return True
dx = pos[0] - _prev_smoothed[0]
dy = pos[1] - _prev_smoothed[1]
return (dx * dx + dy * dy) <= gate_px * gate_px
def get_stable_laser_point(timeout_ms=15000, stable_count=STABLE_COUNT):
global _prev_smoothed
_prev_smoothed = None
try:
last_pos = None
last_raw = None
stable = 0
start = time.ticks_ms()
cx, cy = WIDTH // 2, HEIGHT // 2
while True:
if abs(time.ticks_diff(time.ticks_ms(), start)) > timeout_ms:
_prev_smoothed = None
return None
frame = camera_manager.read_frame()
if frame is None:
@@ -132,21 +161,34 @@ def get_stable_laser_point(timeout_ms=15000, stable_count=STABLE_COUNT):
continue
pos_bright = find_brightest_bytes(frame, cx, cy, SEARCH_RADIUS, THRESHOLD, RED_RATIO)
pos = pos_bright
if logger_manager.logger:
logger_manager.logger.info(f"pos:{pos},stable:{stable}")
if _USE_CV:
img_cv = image.image2cv(frame, False, False)
pos_ellipse = find_ellipse(img_cv, cx, cy, SEARCH_RADIUS, THRESHOLD, RED_RATIO)
if pos_ellipse is not None:
pos = pos_ellipse
if pos is not None:
if last_pos and abs(pos[0] - last_pos[0]) < 1 and abs(pos[1] - last_pos[1]) < 1:
stable += 1
if not _gated(pos):
if logger_manager.logger:
logger_manager.logger.info(f"pos:{pos} gated,stable:{stable}")
time.sleep_ms(_FRAME_INTERVAL_MS)
continue
filtered = _ema_filter(pos)
if last_raw is not None:
dx = abs(filtered[0] - last_raw[0])
dy = abs(filtered[1] - last_raw[1])
if dx <= 2 and dy <= 2:
stable += 1
else:
stable = 1
else:
stable = 1
last_pos = pos
last_raw = filtered
if logger_manager.logger:
logger_manager.logger.info(f"pos:{pos},filtered:{filtered},stable:{stable}")
if stable >= stable_count:
return (int(pos[0]), int(pos[1]))
time.sleep_ms(500)
result = (int(filtered[0]), int(filtered[1]))
_prev_smoothed = None
return result
time.sleep_ms(_FRAME_INTERVAL_MS)
finally:
pass
_prev_smoothed = None

4
laser_manager.py
View File

@@ -54,8 +54,8 @@ class LaserManager:
@property
def laser_point(self):
"""当前激光点（如果启用硬编码，则返回硬编码值）"""
if config.HARDCODE_LASER_POINT:
return config.HARDCODE_LASER_POINT_VALUE
# if config.HARDCODE_LASER_POINT:
# return config.HARDCODE_LASER_POINT_VALUE
return self._laser_point
def get_last_frame_with_ellipse(self):