335 lines
13 KiB
Python
335 lines
13 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
双模式激光识别系统仲裁器
|
||
|
||
核心功能：
|
||
1. 双轨并行处理模式A和模式B的检测结果
|
||
2. 智能仲裁决策，选择最优模式
|
||
3. 边界情况处理和冲突解决
|
||
4. 性能优化策略
|
||
"""
|
||
import numpy as np
|
||
import time
|
||
|
||
class ModeArbitrator:
|
||
def __init__(self):
|
||
"""初始化仲裁器"""
|
||
# 工作模式
|
||
self.work_mode = 'auto' # auto, forced_single, forced_lamp, dual_display
|
||
|
||
# 算法使能状态
|
||
self.enable_mode_a = True
|
||
self.enable_mode_b = True
|
||
|
||
# 历史状态
|
||
self.last_mode = None # 上一帧使用的模式
|
||
self.last_a_score = 0
|
||
self.last_b_score = 0
|
||
self.consecutive_mode_count = 0 # 连续使用同一模式的帧数
|
||
|
||
# 模式失效检测
|
||
self.mode_a_failure_count = 0
|
||
self.mode_b_failure_count = 0
|
||
self.max_failure_count = 30 # 连续无检测的最大帧数
|
||
|
||
# 性能优化
|
||
self.use_alternating_frames = False # 是否使用交替帧策略
|
||
self.current_frame_parity = 0 # 帧奇偶性
|
||
|
||
# 调试信息
|
||
self.debug_info = {
|
||
'mode_a_score': 0,
|
||
'mode_b_score': 0,
|
||
'decision_reason': '',
|
||
'processing_time': 0
|
||
}
|
||
|
||
def set_work_mode(self, mode):
|
||
"""设置工作模式"""
|
||
self.work_mode = mode
|
||
|
||
def set_algorithm_enable(self, enable_a, enable_b):
|
||
"""设置算法使能状态"""
|
||
self.enable_mode_a = enable_a
|
||
self.enable_mode_b = enable_b
|
||
|
||
def set_performance_optimization(self, use_alternating):
|
||
"""设置性能优化策略"""
|
||
self.use_alternating_frames = use_alternating
|
||
|
||
def calculate_mode_a_score(self, results, frame=None):
|
||
"""计算模式A的场景适配分"""
|
||
if not results:
|
||
return 0
|
||
|
||
# 高置信度检测物数量（1-3个为佳）
|
||
count_score = 0
|
||
if 1 <= len(results) <= 3:
|
||
count_score = 1.0
|
||
elif len(results) > 3:
|
||
count_score = max(0, 1.0 - (len(results) - 3) * 0.2)
|
||
else:
|
||
count_score = 0.5
|
||
|
||
# 单个目标圆度和面积
|
||
circularity_scores = []
|
||
area_scores = []
|
||
|
||
for result in results:
|
||
# 圆度得分
|
||
circularity = result[5] # result格式: (cx, cy, w, h, area, circularity)
|
||
circ_score = min(circularity / 0.8, 1.0) if circularity > 0.5 else 0
|
||
circularity_scores.append(circ_score)
|
||
|
||
# 面积得分（适中为佳）
|
||
area = result[4]
|
||
if 100 <= area <= 2000:
|
||
area_score = 1.0
|
||
elif area < 100:
|
||
area_score = max(0, area / 100)
|
||
else:
|
||
area_score = max(0, 1.0 - (area - 2000) / 3000)
|
||
area_scores.append(area_score)
|
||
|
||
# 平均圆度和面积得分
|
||
avg_circularity_score = np.mean(circularity_scores) if circularity_scores else 0
|
||
avg_area_score = np.mean(area_scores) if area_scores else 0
|
||
|
||
# 离散度系数（目标越分散分越低）
|
||
dispersion_coefficient = 1.0
|
||
if len(results) > 1:
|
||
# 计算所有点之间的平均距离
|
||
distances = []
|
||
for i in range(len(results)):
|
||
for j in range(i + 1, len(results)):
|
||
dx = results[i][0] - results[j][0]
|
||
dy = results[i][1] - results[j][1]
|
||
distances.append(np.sqrt(dx*dx + dy*dy))
|
||
if distances:
|
||
avg_distance = np.mean(distances)
|
||
# 距离越大，离散度越高，得分越低
|
||
dispersion_coefficient = max(0.5, 1.0 - min(avg_distance / 300, 0.5))
|
||
|
||
# 总分计算
|
||
total_score = (
|
||
len(results) * 0.3 +
|
||
avg_circularity_score * 0.4 +
|
||
avg_area_score * 0.2 +
|
||
count_score * 0.1
|
||
) * dispersion_coefficient
|
||
|
||
return total_score * 100 # 转换为0-100的分数
|
||
|
||
def calculate_mode_b_score(self, results, frame=None):
|
||
"""计算模式B的场景适配分"""
|
||
if not results:
|
||
return 0
|
||
|
||
# 取置信度最高的结果
|
||
best_result = max(results, key=lambda x: x['confidence'])
|
||
|
||
# 拟合圆置信度
|
||
confidence_score = min(best_result['confidence'] / 70, 1.0) # 70分为满分
|
||
|
||
# 检测到的LED颗粒数
|
||
led_count = best_result['led_count']
|
||
led_score = min(np.log(max(led_count, 10)) / np.log(50), 1.0) # 10-50个LED
|
||
|
||
# 圆度合理性
|
||
circularity = best_result['circularity']
|
||
circularity_score = 0
|
||
if 0.4 <= circularity <= 0.8:
|
||
circularity_score = 1.0
|
||
elif circularity < 0.4:
|
||
circularity_score = max(0, circularity / 0.4)
|
||
else:
|
||
circularity_score = max(0, 1.0 - (circularity - 0.8) / 0.2)
|
||
|
||
# 总分计算
|
||
total_score = (
|
||
confidence_score * 0.5 +
|
||
led_score * 0.3 +
|
||
circularity_score * 0.2
|
||
) * 100 # 转换为0-100的分数
|
||
|
||
return total_score
|
||
|
||
def arbitrate(self, mode_a_results, mode_b_results, frame=None):
|
||
"""
|
||
智能仲裁决策
|
||
|
||
Args:
|
||
mode_a_results: 模式A的检测结果
|
||
mode_b_results: 模式B的检测结果
|
||
frame: 原始帧（可选，用于预筛选）
|
||
|
||
Returns:
|
||
selected_mode: 选中的模式 ('mode_a', 'mode_b', None)
|
||
selected_results: 选中的结果
|
||
debug_info: 调试信息
|
||
"""
|
||
start_time = time.time()
|
||
|
||
# 重置调试信息
|
||
self.debug_info = {
|
||
'mode_a_score': 0,
|
||
'mode_b_score': 0,
|
||
'decision_reason': '',
|
||
'processing_time': 0
|
||
}
|
||
|
||
# 检查使能状态
|
||
if not self.enable_mode_a and not self.enable_mode_b:
|
||
self.debug_info['decision_reason'] = 'Both algorithms disabled'
|
||
self.debug_info['processing_time'] = time.time() - start_time
|
||
return None, None, self.debug_info
|
||
|
||
# 强制模式
|
||
if self.work_mode == 'forced_single' and self.enable_mode_a:
|
||
self.debug_info['decision_reason'] = 'Forced single point mode'
|
||
self.debug_info['processing_time'] = time.time() - start_time
|
||
return 'mode_a', mode_a_results, self.debug_info
|
||
|
||
if self.work_mode == 'forced_lamp' and self.enable_mode_b:
|
||
self.debug_info['decision_reason'] = 'Forced lamp disk mode'
|
||
self.debug_info['processing_time'] = time.time() - start_time
|
||
return 'mode_b', mode_b_results, self.debug_info
|
||
|
||
# 双模显示
|
||
if self.work_mode == 'dual_display':
|
||
self.debug_info['decision_reason'] = 'Dual display mode'
|
||
self.debug_info['processing_time'] = time.time() - start_time
|
||
return 'dual', (mode_a_results, mode_b_results), self.debug_info
|
||
|
||
# 性能优化：交替帧策略
|
||
if self.use_alternating_frames:
|
||
self.current_frame_parity = 1 - self.current_frame_parity
|
||
if self.current_frame_parity == 0 and self.enable_mode_a:
|
||
self.debug_info['decision_reason'] = 'Alternating frame: mode A'
|
||
self.debug_info['processing_time'] = time.time() - start_time
|
||
return 'mode_a', mode_a_results, self.debug_info
|
||
elif self.current_frame_parity == 1 and self.enable_mode_b:
|
||
self.debug_info['decision_reason'] = 'Alternating frame: mode B'
|
||
self.debug_info['processing_time'] = time.time() - start_time
|
||
return 'mode_b', mode_b_results, self.debug_info
|
||
|
||
# 计算得分
|
||
a_score = 0
|
||
b_score = 0
|
||
|
||
if self.enable_mode_a:
|
||
a_score = self.calculate_mode_a_score(mode_a_results, frame)
|
||
self.debug_info['mode_a_score'] = a_score
|
||
|
||
# 模式失效检测
|
||
if not mode_a_results:
|
||
self.mode_a_failure_count += 1
|
||
else:
|
||
self.mode_a_failure_count = 0
|
||
|
||
if self.enable_mode_b:
|
||
b_score = self.calculate_mode_b_score(mode_b_results, frame)
|
||
self.debug_info['mode_b_score'] = b_score
|
||
|
||
# 模式失效检测
|
||
if not mode_b_results:
|
||
self.mode_b_failure_count += 1
|
||
else:
|
||
self.mode_b_failure_count = 0
|
||
|
||
# 自动仲裁逻辑
|
||
selected_mode = None
|
||
selected_results = None
|
||
|
||
# 单模式使能情况
|
||
if self.enable_mode_a and not self.enable_mode_b:
|
||
selected_mode = 'mode_a'
|
||
selected_results = mode_a_results
|
||
self.debug_info['decision_reason'] = 'Only mode A enabled'
|
||
elif self.enable_mode_b and not self.enable_mode_a:
|
||
selected_mode = 'mode_b'
|
||
selected_results = mode_b_results
|
||
self.debug_info['decision_reason'] = 'Only mode B enabled'
|
||
else:
|
||
# 双模式仲裁
|
||
if b_score > a_score * 1.2:
|
||
selected_mode = 'mode_b'
|
||
selected_results = mode_b_results
|
||
self.debug_info['decision_reason'] = f'Lamp mode score ({b_score:.1f}) > single mode ({a_score:.1f}) * 1.2'
|
||
elif a_score > b_score * 1.5:
|
||
selected_mode = 'mode_a'
|
||
selected_results = mode_a_results
|
||
self.debug_info['decision_reason'] = f'Single mode score ({a_score:.1f}) > lamp mode ({b_score:.1f}) * 1.5'
|
||
else:
|
||
# 保持上一帧决策（迟滞效应）
|
||
if self.last_mode:
|
||
selected_mode = self.last_mode
|
||
selected_results = mode_a_results if self.last_mode == 'mode_a' else mode_b_results
|
||
self.debug_info['decision_reason'] = f'Hysteresis:保持{"单点" if self.last_mode == "mode_a" else "灯盘"}模式'
|
||
else:
|
||
# 首次决策
|
||
if a_score > b_score:
|
||
selected_mode = 'mode_a'
|
||
selected_results = mode_a_results
|
||
self.debug_info['decision_reason'] = 'First frame: single mode'
|
||
else:
|
||
selected_mode = 'mode_b'
|
||
selected_results = mode_b_results
|
||
self.debug_info['decision_reason'] = 'First frame: lamp mode'
|
||
|
||
# 更新历史状态
|
||
if selected_mode == self.last_mode:
|
||
self.consecutive_mode_count += 1
|
||
else:
|
||
self.consecutive_mode_count = 1
|
||
|
||
self.last_mode = selected_mode
|
||
self.last_a_score = a_score
|
||
self.last_b_score = b_score
|
||
|
||
# 处理模式失效
|
||
if selected_mode == 'mode_a' and self.mode_a_failure_count >= self.max_failure_count:
|
||
if self.enable_mode_b:
|
||
selected_mode = 'mode_b'
|
||
selected_results = mode_b_results
|
||
self.debug_info['decision_reason'] = 'Mode A failed, switching to mode B'
|
||
elif selected_mode == 'mode_b' and self.mode_b_failure_count >= self.max_failure_count:
|
||
if self.enable_mode_a:
|
||
selected_mode = 'mode_a'
|
||
selected_results = mode_a_results
|
||
self.debug_info['decision_reason'] = 'Mode B failed, switching to mode A'
|
||
|
||
# 计算处理时间
|
||
self.debug_info['processing_time'] = time.time() - start_time
|
||
|
||
return selected_mode, selected_results, self.debug_info
|
||
|
||
def reset(self):
|
||
"""重置仲裁器状态"""
|
||
self.last_mode = None
|
||
self.last_a_score = 0
|
||
self.last_b_score = 0
|
||
self.consecutive_mode_count = 0
|
||
self.mode_a_failure_count = 0
|
||
self.mode_b_failure_count = 0
|
||
self.current_frame_parity = 0
|
||
self.debug_info = {
|
||
'mode_a_score': 0,
|
||
'mode_b_score': 0,
|
||
'decision_reason': '',
|
||
'processing_time': 0
|
||
}
|
||
|
||
def get_debug_info(self):
|
||
"""获取调试信息"""
|
||
return self.debug_info
|
||
|
||
def get_mode_failure_status(self):
|
||
"""获取模式失效状态"""
|
||
return {
|
||
'mode_a_failed': self.mode_a_failure_count >= self.max_failure_count,
|
||
'mode_b_failed': self.mode_b_failure_count >= self.max_failure_count,
|
||
'mode_a_failure_count': self.mode_a_failure_count,
|
||
'mode_b_failure_count': self.mode_b_failure_count
|
||
}
|