From e399c8c8379bf9f1dffa9d16f09ba4ad49a12d56 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Sat, 31 May 2025 07:30:18 +0000 Subject: [PATCH] Enhance dual track centering functionality by adding an estimated track distance parameter for improved deviation calculations. Update the logic for converting pixel deviation to actual distance, optimizing the centering process. Refactor detection and movement commands for better responsiveness and accuracy. --- base_move/center_on_dual_tracks.py | 109 ++-- utils/detect_dual_track_lines.py | 894 +---------------------------- 2 files changed, 51 insertions(+), 952 deletions(-) diff --git a/base_move/center_on_dual_tracks.py b/base_move/center_on_dual_tracks.py index f2f15d7..4972f42 100644 --- a/base_move/center_on_dual_tracks.py +++ b/base_move/center_on_dual_tracks.py @@ -5,10 +5,11 @@ import os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing -from utils.detect_dual_track_lines import detect_dual_track_lines, auto_detect_dual_track_lines +from utils.detect_dual_track_lines import detect_dual_track_lines def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=False, - mode=11, gait_id=26, step_height=[0.06, 0.06]): + mode=11, gait_id=26, step_height=[0.06, 0.06], + estimated_track_distance=1.0): """ 控制机器狗仅使用Y轴移动调整到双轨道线的中间位置 @@ -21,6 +22,7 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal mode: 控制模式，默认为11 gait_id: 步态ID，默认为26 step_height: 抬腿高度，默认为[0.06, 0.06] + estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米 返回: bool: 是否成功调整到中心位置 @@ -44,12 +46,14 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal if hasattr(ctrl, 'place_marker'): ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True) - # PID控制参数 - 增加比例系数提高响应速度 - kp = 0.005 # 比例系数增加 + # 根据RGB相机参数，计算像素偏差到实际距离的转换 + # 相机水平视场角FOV = 1.46608弧度（约84度） + # 图像宽度 = 1920像素 + camera_hfov = 1.46608 # 水平视场角（弧度） + image_width = 1920 # 图像宽度（像素） - # 帧间滤波参数 - 减少滤波长度以提高响应性 - filter_size = 3 # 滤波队列大小 - deviation_queue = [] # 偏差值队列 + # 计算像素到角度的转换比例 + pixels_per_radian = image_width / camera_hfov # 统计变量 detection_success_count = 0 @@ -57,7 +61,7 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal # 稳定计数器 - 连续几次在中心位置才认为稳定 stable_count = 0 - required_stable_count = 2 # 降低稳定条件，更快响应 + required_stable_count = 2 # 变量记录最后有效的偏差，用于处理检测失败的情况 last_valid_deviation = 0 @@ -67,7 +71,7 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal # 获取当前图像 image = ctrl.image_processor.get_current_image() - # 检测双轨道线，尝试使用自动检测方法，可能更稳定 + # 检测双轨道线 detection_total_count += 1 center_info, left_info, right_info = detect_dual_track_lines(image, observe=observe, save_log=True) @@ -79,68 +83,51 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal current_deviation = center_info["deviation"] last_valid_deviation = current_deviation # 记录有效偏差 - # 添加到队列 - deviation_queue.append(current_deviation) - if len(deviation_queue) > filter_size: - deviation_queue.pop(0) - - # 计算滤波后的偏差值 (去除最大和最小值后的平均) - if len(deviation_queue) >= 3: - filtered_deviations = sorted(deviation_queue)[1:-1] if len(deviation_queue) > 2 else deviation_queue - filtered_deviation = sum(filtered_deviations) / len(filtered_deviations) - else: - filtered_deviation = current_deviation + # 转换像素偏差到实际距离偏差 + # 1. 像素偏差转换为弧度 + deviation_rad = current_deviation / pixels_per_radian + # 2. 根据估计距离计算实际偏差（单位：米） + actual_deviation = math.tan(deviation_rad) * estimated_track_distance if observe: - debug(f"原始偏差: {current_deviation:.1f}px, 滤波后: {filtered_deviation:.1f}px", "偏差") + debug(f"偏差: {current_deviation:.1f}px", "偏差") + debug(f"实际距离偏差: {actual_deviation:.3f}m (距离估计: {estimated_track_distance:.1f}m)", "实际偏差") # 判断是否已经居中 - if abs(filtered_deviation) <= max_deviation: + if abs(current_deviation) <= max_deviation: stable_count += 1 if observe: info(f"已接近中心，稳定计数: {stable_count}/{required_stable_count}", "居中") + info(f"距离偏差: {actual_deviation:.3f}m", "居中") # 即使在稳定过程中，也要保持小幅调整以保持居中 if stable_count < required_stable_count: - # 使用非常小的调整速度 - lateral_velocity = kp * filtered_deviation * 0.5 - max_lateral_velocity = 0.1 # 减小稳定阶段的最大速度 - lateral_velocity = max(-max_lateral_velocity, min(max_lateral_velocity, lateral_velocity)) - - msg.vel_des = [0, lateral_velocity, 0] + # 直接根据实际偏差移动 + msg.vel_des = [0, actual_deviation, 0] msg.life_count += 1 ctrl.Send_cmd(msg) if stable_count >= required_stable_count: # 已经稳定在中心位置 if observe: - success(f"成功居中，最终偏差: {filtered_deviation:.1f}px", "完成") + success(f"成功居中，最终偏差: {current_deviation:.1f}px ({actual_deviation:.3f}m)", "完成") break else: # 不在中心，重置稳定计数 stable_count = 0 - # 计算横向移动速度 (只使用y轴移动) - # 注意：偏差为正表示需要向左移动（y轴正方向），偏差为负表示需要向右移动（y轴负方向） - - # 使用动态kp：偏差越大，移动越快；偏差越小，移动越慢 - dynamic_kp = kp - if abs(filtered_deviation) > 50: - dynamic_kp = kp * 1.5 # 大偏差时增加响应 - elif abs(filtered_deviation) < 20: - dynamic_kp = kp * 0.8 # 小偏差时减小响应，避免过冲 - - lateral_velocity = dynamic_kp * filtered_deviation - - # 限制横向移动速度 - max_lateral_velocity = 0.35 # 增加最大横向速度 (米/秒) - lateral_velocity = max(-max_lateral_velocity, min(max_lateral_velocity, lateral_velocity)) + # 直接根据计算出的实际偏差距离进行移动 + # 为了避免移动过猛，可以限制最大移动距离 + max_move_distance = 0.35 # 最大移动距离（米） + move_distance = actual_deviation + if abs(move_distance) > max_move_distance: + move_distance = max_move_distance * (1 if move_distance > 0 else -1) if observe: - debug(f"横向移动速度: {lateral_velocity:.3f}m/s", "控制") + debug(f"横向移动距离: {move_distance:.3f}m", "控制") # 设置速度命令 - 只使用y轴移动，不前进和转向 - msg.vel_des = [0, lateral_velocity, 0] # [前进速度, 侧向速度, 角速度] + msg.vel_des = [0, move_distance, 0] # [前进速度, 侧向速度, 角速度] # 发送命令 msg.life_count += 1 @@ -149,27 +136,30 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal warning("未检测到双轨道线", "警告") # 如果已经有了一些有效的检测，使用最后一次有效的偏差继续移动 - if len(deviation_queue) > 0: - # 使用衰减的最后偏差值继续移动 - lateral_velocity = kp * last_valid_deviation * 0.7 # 衰减系数，降低未检测到时的移动速度 - max_lateral_velocity = 0.2 # 降低未检测情况下的移动速度 - lateral_velocity = max(-max_lateral_velocity, min(max_lateral_velocity, lateral_velocity)) + if detection_success_count > 0: + # 使用最后一次有效偏差值计算移动距离 + deviation_rad = last_valid_deviation / pixels_per_radian + move_distance = math.tan(deviation_rad) * estimated_track_distance * 0.5 # 缩小系数，避免过度移动 - msg.vel_des = [0, lateral_velocity, 0] + # 限制最大移动距离 + max_move_distance = 0.15 + move_distance = max(-max_move_distance, min(max_move_distance, move_distance)) + + msg.vel_des = [0, move_distance, 0] msg.life_count += 1 ctrl.Send_cmd(msg) if observe: - warning(f"使用最后偏差值继续移动: {lateral_velocity:.3f}m/s", "暂停") + warning(f"使用最后偏差值继续移动: {move_distance:.3f}m", "暂停") else: # 如果一开始就没有检测到，可以尝试小范围搜索 if detection_total_count < 10: if detection_total_count % 2 == 0: - # 向右搜索，增加搜索速度 - msg.vel_des = [0, -0.15, 0] + # 向右搜索 + msg.vel_des = [0, -0.08, 0] else: - # 向左搜索，增加搜索速度 - msg.vel_des = [0, 0.15, 0] + # 向左搜索 + msg.vel_des = [0, 0.08, 0] msg.life_count += 1 ctrl.Send_cmd(msg) @@ -220,9 +210,8 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal success = True return success - def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_time=15, observe=False, - mode=11, gait_id=26, step_height=[0.06, 0.06]): + mode=11, gait_id=26, step_height=[0.06, 0.06], estimated_track_distance=1.0): """ 先居中到双轨道线中间，然后沿轨道线行走指定距离 @@ -236,6 +225,7 @@ def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_ mode: 控制模式，默认为11 gait_id: 步态ID，默认为26 step_height: 抬腿高度，默认为[0.06, 0.06] + estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米 返回: bool: 是否成功完成居中和轨道跟随 @@ -253,6 +243,7 @@ def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_ mode=mode, gait_id=gait_id, step_height=step_height, + estimated_track_distance=estimated_track_distance ) if not centering_success: diff --git a/utils/detect_dual_track_lines.py b/utils/detect_dual_track_lines.py index 31ee819..669fe92 100644 --- a/utils/detect_dual_track_lines.py +++ b/utils/detect_dual_track_lines.py @@ -685,7 +685,7 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True, center_point = (int(bottom_x), int(height)) # 计算中心线与图像中心线的偏差 - deviation = bottom_x - center_x + deviation = width / 2 - bottom_x result_img = None if observe or save_log: @@ -847,895 +847,3 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True, } return center_info, left_track_info, right_track_info - -def detect_center_based_dual_track_lines(image, observe=False, delay=1000, save_log=True, expected_track_width_ratio=0.45): - """ - 通过先检测中心线，然后向两侧扩展来检测双轨迹线 - - 参数: - image: 输入图像，可以是文件路径或者已加载的图像数组 - observe: 是否输出中间状态信息和可视化结果，默认为False - delay: 展示每个步骤的等待时间(毫秒) - save_log: 是否保存日志和图像 - expected_track_width_ratio: 预期轨道宽度占图像宽度的比例，默认为0.45 - - 返回: - tuple: (中心线信息, 左轨迹线信息, 右轨迹线信息) - 如果检测失败返回(None, None, None) - """ - # 如果输入是字符串（文件路径），则加载图像 - if isinstance(image, str): - img = cv2.imread(image) - else: - img = image.copy() - - if img is None: - error("无法加载图像", "失败") - return None, None, None - - # 获取图像尺寸 - height, width = img.shape[:2] - - # 计算图像中间区域的范围 - center_x = width // 2 - - # 转换到HSV颜色空间以便更容易提取黄色 - hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) - - # 标准黄色的HSV范围 - lower_yellow = np.array([15, 80, 80]) - upper_yellow = np.array([35, 255, 255]) - - # 创建黄色的掩码 - mask = cv2.inRange(hsv, lower_yellow, upper_yellow) - - # 对较暗的黄色使用更宽松的阈值 - lower_dark_yellow = np.array([10, 60, 60]) - upper_dark_yellow = np.array([40, 255, 255]) - dark_mask = cv2.inRange(hsv, lower_dark_yellow, upper_dark_yellow) - - # 合并掩码 - combined_mask = cv2.bitwise_or(mask, dark_mask) - - # 形态学操作以改善掩码 - kernel = np.ones((5, 5), np.uint8) - combined_mask = cv2.dilate(combined_mask, kernel, iterations=1) - combined_mask = cv2.erode(combined_mask, np.ones((3, 3), np.uint8), iterations=1) - - if observe: - debug("步骤1: 创建黄色掩码", "处理") - mask_display = cv2.cvtColor(combined_mask, cv2.COLOR_GRAY2BGR) - cv2.putText(mask_display, "Step 1: Yellow Mask (Center Based)", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) - cv2.putText(mask_display, f"Lower: {lower_yellow} + Darker Var.", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1) - cv2.putText(mask_display, f"Upper: {upper_yellow} + Darker Var.", (10, 55), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1) - cv2.imshow("黄色掩码", mask_display) - cv2.waitKey(delay) - - # 裁剪底部区域重点关注近处的黄线 - bottom_roi_height = int(height * 0.6) # 关注图像底部60% - bottom_roi = combined_mask[height-bottom_roi_height:, :] - - if observe: - debug("步骤1.5: 底部区域掩码", "处理") - bottom_roi_display = cv2.cvtColor(bottom_roi, cv2.COLOR_GRAY2BGR) - cv2.putText(bottom_roi_display, "Step 1.5: Bottom ROI (Center Based)", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) - cv2.putText(bottom_roi_display, f"ROI Height: {bottom_roi_height}px ({bottom_roi_height/height*100:.0f}%)", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1) - cv2.imshow("底部区域掩码", bottom_roi_display) - cv2.waitKey(delay) - - # 边缘检测 - edges_roi = cv2.Canny(bottom_roi, 50, 150, apertureSize=3) - - if observe: - debug("步骤2: 边缘检测 (底部ROI)", "处理") # Updated text - # Displaying edges from bottom_roi directly - edges_display = cv2.cvtColor(edges_roi, cv2.COLOR_GRAY2BGR) - cv2.putText(edges_display, "Step 2: Edge Detection (Canny on Bottom ROI)", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) - cv2.putText(edges_display, "Thresholds: (50, 150)", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1) - cv2.imshow("边缘检测 (底部ROI)", edges_display) # Updated window title - cv2.waitKey(delay) - - # INFO 霍夫变换检测直线 (on edges from bottom_roi) - lines = cv2.HoughLinesP(edges_roi, 1, np.pi/180, threshold=25, - minLineLength=width*min_line_length, maxLineGap=max_line_gap) - - # Adjust y-coordinates of lines detected in bottom_roi to be relative to the full image - if lines is not None: - offset_y = height - bottom_roi_height - for i in range(len(lines)): - # line[0] is [x1, y1, x2, y2] - lines[i][0][1] += offset_y # y1 - lines[i][0][3] += offset_y # y2 - - if lines is None or len(lines) == 0: - error("未检测到直线 (在底部ROI)", "失败") # Updated error message - return None, None, None - - if observe: - debug(f"步骤3: 检测到 {len(lines)} 条直线", "处理") - lines_img = img.copy() - cv2.putText(lines_img, "Step 3: Hough Lines", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) - cv2.putText(lines_img, f"Th:25, MinLen:{width*0.05:.1f}, MaxGap:{max_line_gap}", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 255), 1) - for line in lines: - x1, y1, x2, y2 = line[0] - cv2.line(lines_img, (x1, y1), (x2, y2), (0, 255, 0), 2) - cv2.imshow("检测到的直线", lines_img) - cv2.waitKey(delay) - - # 筛选近似垂直的线 - vertical_only_lines = [] - for line in lines: - x1, y1, x2, y2 = line[0] - - # 优先选择图像底部的线 - if y1 < height * 0.4 and y2 < height * 0.4: - continue # 忽略上半部分的线 - - # 计算斜率 (避免除零错误) - if abs(x2 - x1) < 5: # 几乎垂直的线 - slope = 100 # 设置一个较大的值表示接近垂直 - else: - slope = (y2 - y1) / (x2 - x1) - - # 筛选接近垂直的线 (斜率较大)，但允许更多倾斜度 - if abs(slope) > min_slope_threshold: - vertical_only_lines.append(line[0]) - - if observe: - debug(f"步骤3.2: 筛选出 {len(vertical_only_lines)} 条垂直候选线 (合并前)", "可视化") - pre_merge_lines_img = img.copy() - cv2.putText(pre_merge_lines_img, "Step 3.2: Vertical Candidates (Pre-Merge)", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) - colors = [(0, 0, 255), (0, 255, 0), (0, 255, 255), (255, 0, 0), (255, 0, 255), (255, 255, 0)] - for i, line in enumerate(vertical_only_lines): - x1, y1, x2, y2 = line - cv2.line(pre_merge_lines_img, (x1, y1), (x2, y2), colors[i % len(colors)], 2) # 使用红色显示合并前的线 - cv2.imshow("合并前的垂直候选线", pre_merge_lines_img) - cv2.waitKey(delay) - - if observe: - vertical_only_lines_tmp = vertical_only_lines.copy() - - vertical_only_lines = _merge_collinear_lines_iterative(vertical_only_lines, - min_initial_len=5.0, # 最小初始线段长度 - max_angle_diff_deg=4.0, # 最大允许角度差（度） - max_ep_gap_abs=max_line_gap, # 端点间最大绝对距离 - max_ep_gap_factor=1, # 端点间最大相对距离因子 - max_p_dist_abs=max_line_gap, # 点到线段最大绝对距离 - max_p_dist_factor=1) # 点到线段最大相对距离因子 - if observe: - print(f"合并前: {len(vertical_only_lines_tmp)} 条线, 合并后: {len(vertical_only_lines)} 条线") - debug(f"步骤3.5: 合并筛选出 {len(vertical_only_lines)} 条垂直候选线 (合并后)", "可视化") - - # 创建两个图像用于对比显示 - pre_merge_img = img.copy() - post_merge_img = img.copy() - - # 在两张图上添加标题 - cv2.putText(pre_merge_img, "合并前的垂直候选线", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) - cv2.putText(post_merge_img, "合并后的垂直候选线", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) - - # 为每条线分配不同的颜色 - colors = [ - (255, 0, 0), # 蓝色 - (0, 255, 0), # 绿色 - (0, 0, 255), # 红色 - (255, 255, 0), # 青色 - (255, 0, 255), # 品红 - (0, 255, 255), # 黄色 - ] - - # 绘制合并前的线 - for i, line in enumerate(vertical_only_lines_tmp): - x1, y1, x2, y2 = line - color = colors[i % len(colors)] - cv2.line(pre_merge_img, (x1, y1), (x2, y2), color, 2) - mid_x = (x1 + x2) // 2 - mid_y = (y1 + y2) // 2 - cv2.putText(pre_merge_img, str(i+1), (mid_x, mid_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - - # 绘制合并后的线 - for i, line in enumerate(vertical_only_lines): - x1, y1, x2, y2 = line[0] - color = colors[i % len(colors)] - cv2.line(post_merge_img, (x1, y1), (x2, y2), color, 2) - mid_x = (x1 + x2) // 2 - mid_y = (y1 + y2) // 2 - cv2.putText(post_merge_img, str(i+1), (mid_x, mid_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - - # 水平拼接两张图片 - combined_img = np.hstack((pre_merge_img, post_merge_img)) - cv2.imshow("合并前后的垂直候选线对比", combined_img) - cv2.waitKey(delay) - - vertical_lines = [] - for line in vertical_only_lines: - x1, y1, x2, y2 = line[0] - - # 优先选择图像底部的线 - if y1 < height * 0.4 and y2 < height * 0.4: - continue # 忽略上半部分的线 - - # 计算斜率 (避免除零错误) - if abs(x2 - x1) < 5: # 几乎垂直的线 - slope = 100 # 设置一个较大的值表示接近垂直 - else: - slope = (y2 - y1) / (x2 - x1) - - # 筛选接近垂直的线 (斜率较大)，但允许更多倾斜度 - if abs(slope) > min_slope_threshold: - line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2) - # 计算线的中点x坐标 - mid_x = (x1 + x2) / 2 - # 计算线的中点y坐标 - mid_y = (y1 + y2) / 2 - # 保存线段、其坐标、斜率和长度 - vertical_lines.append((line[0], mid_x, mid_y, slope, line_length)) - - if len(vertical_lines) < 2: - if len(vertical_lines) < 2: - error("未检测到足够的垂直线", "失败") - return None, None, None - - if observe: - debug(f"步骤4: 找到 {len(vertical_lines)} 条垂直线", "处理") - v_lines_img = img.copy() - cv2.putText(v_lines_img, "Step 4: Vertical Lines Filtered", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) - cv2.putText(v_lines_img, f"Min Slope Abs: {min_slope_threshold:.2f}", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 255), 1) - for line_info in vertical_lines: - line, _, _, slope, _ = line_info - x1, y1, x2, y2 = line - cv2.line(v_lines_img, (x1, y1), (x2, y2), (0, 255, 255), 2) - # 显示斜率 - cv2.putText(v_lines_img, f"{slope:.2f}", ((x1+x2)//2, (y1+y2)//2), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1) - cv2.imshow("垂直线", v_lines_img) - cv2.waitKey(delay) - - # 按x坐标将线分为左右两组 - left_lines = [line for line in vertical_lines if line[1] < center_x] - right_lines = [line for line in vertical_lines if line[1] > center_x] - - # 如果任一侧没有检测到线，则放宽左右两侧线的分组条件 - if not left_lines or not right_lines: - # 按x坐标排序所有垂直线 - vertical_lines.sort(key=lambda x: x[1]) - - # 如果有至少两条线，将最左侧的线作为左轨迹线，最右侧的线作为右轨迹线 - if len(vertical_lines) >= 2: - left_lines = [vertical_lines[0]] - right_lines = [vertical_lines[-1]] - else: - error("左侧或右侧未检测到轨迹线", "失败") - return None, None, None - - if observe: - debug(f"左侧候选线数量: {len(left_lines)}, 右侧候选线数量: {len(right_lines)}", "线候选") - - # 创建左右线可视化图像 - left_right_img = img.copy() - cv2.putText(left_right_img, "Left and Right Candidate Lines", (10, 20), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) - - # 绘制左侧候选线（蓝色） - for line_info in left_lines: - line = line_info[0] - x1, y1, x2, y2 = line - cv2.line(left_right_img, (x1, y1), (x2, y2), (255, 0, 0), 2) - # 显示斜率 - mid_x = (x1 + x2) // 2 - mid_y = (y1 + y2) // 2 - cv2.putText(left_right_img, f"L:{line_info[3]:.2f}", (mid_x, mid_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1) - - # 绘制右侧候选线（红色） - for line_info in right_lines: - line = line_info[0] - x1, y1, x2, y2 = line - cv2.line(left_right_img, (x1, y1), (x2, y2), (0, 0, 255), 2) - # 显示斜率 - mid_x = (x1 + x2) // 2 - mid_y = (y1 + y2) // 2 - cv2.putText(left_right_img, f"R:{line_info[3]:.2f}", (mid_x, mid_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1) - - cv2.imshow("左右候选线", left_right_img) - cv2.waitKey(delay) - - # 优化说明：在默认模式下，评分函数和线对选择都优先考虑更靠近图像中心的线段 - # 这有助于减少对图像边缘可能出现的干扰线的选择，提高轨道线检测的准确性 - - # INFO 改进的评分函数 - 同时考虑斜率、位置、长度和在图像中的位置 - def score_line(line_info, is_left): - _, mid_x, mid_y, slope, length = line_info - line_points = line_info[0] # 获取线段的端点坐标 - x1, y1, x2, y2 = line_points - - # 确保线段从上到下排序 - if y1 > y2: - x1, x2 = x2, x1 - y1, y2 = y2, y1 - - # 线段靠近底部评分 - y越大（越靠近底部）分数越高 - bottom_ratio = mid_y / height - y_score = min(1.0, bottom_ratio * 1.2) # 适当提高底部线段的权重 - - # 线段长度评分 - 线越长分数越高 - length_ratio = length / (height * 0.3) - length_score = min(1.0, length_ratio * 1.2) - - # 计算到图像中心的距离得分 - 更重视靠近垂直中线的线 - center_x = width / 2 - distance_to_center = abs(mid_x - center_x) - # 使用更陡峭的衰减函数，使得距离中心越近的线得分越高 - center_proximity_score = max(0, 1.0 - (distance_to_center / (width * 0.25)) ** 2) - - # 计算底部点与中心的偏差 - bottom_x = x1 - if abs(y2 - y1) > 1: # 非水平线 - t = (height - y1) / (y2 - y1) - if t >= 0: # 确保是向下延伸 - bottom_x = x1 + t * (x2 - x1) - - bottom_x_distance = abs(bottom_x - center_x) - bottom_x_score = max(0, 1.0 - (bottom_x_distance / (width * 0.25)) ** 2) - - # 斜率评分 - 轨道线应该有一定的倾斜度 - if abs(slope) > 5: - slope_score = 0.3 - else: - ideal_slope_magnitude = 0.8 - ideal_slope = -ideal_slope_magnitude if is_left else ideal_slope_magnitude - - if (is_left and slope > 0) or (not is_left and slope < 0): - slope_sign_score = 0.3 - else: - slope_sign_score = 1.0 - - slope_diff = abs(slope - ideal_slope) - slope_magnitude_score = max(0, 1.0 - slope_diff / 2.0) - slope_score = slope_sign_score * slope_magnitude_score - - # 检查线段是否从底部区域开始 - bottom_region_threshold = height * 0.7 - reaches_bottom = max(y1, y2) > bottom_region_threshold - bottom_reach_score = 1.0 if reaches_bottom else 0.5 - - # 综合评分 - 大幅提高中心接近性的权重 - final_score = ( - y_score * 0.1 + # 底部接近度 - length_score * 0.05 + # 线段长度 - center_proximity_score * 0.6 + # 与中心的接近度 (权重提高) - bottom_x_score * 0.15 + # 底部点位置 - slope_score * 0.05 + # 斜率合适性 - bottom_reach_score * 0.05 # 是否到达底部 - ) - - return final_score - # 如果有多条线，评估左右线对是否平行 - if len(left_lines) > 0 and len(right_lines) > 0: - # 计算最佳的左右线对 - best_pair_score = -1 - best_left_line = None - best_right_line = None - - # 先对左右线按照评分排序，只考虑评分较高的候选线（减少计算量） - left_lines = sorted(left_lines, key=lambda line: score_line(line, True), reverse=True) - right_lines = sorted(right_lines, key=lambda line: score_line(line, False), reverse=True) - left_line = left_lines[0] if left_lines else None - right_line = right_lines[0] if right_lines else None - if observe: - debug(f"选择最佳线对，评分: {best_pair_score:.2f}", "线对") - - # 创建评分可视化图像 - score_img = img.copy() - cv2.putText(score_img, "Line Scores Visualization", (10, 20), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) - - # 显示左侧线的评分 - for i, line_info in enumerate(left_lines): - line = line_info[0] - x1, y1, x2, y2 = line - score = score_line(line_info, True) - cv2.line(score_img, (x1, y1), (x2, y2), (255, 0, 0), 2) - mid_x = (x1 + x2) // 2 - mid_y = (y1 + y2) // 2 - cv2.putText(score_img, f"L{i+1}:{score:.2f}", (mid_x, mid_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1) - - # 显示右侧线的评分 - for i, line_info in enumerate(right_lines): - line = line_info[0] - x1, y1, x2, y2 = line - score = score_line(line_info, False) - cv2.line(score_img, (x1, y1), (x2, y2), (0, 0, 255), 2) - mid_x = (x1 + x2) // 2 - mid_y = (y1 + y2) // 2 - cv2.putText(score_img, f"R{i+1}:{score:.2f}", (mid_x, mid_y), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1) - - cv2.imshow("线评分可视化", score_img) - cv2.waitKey(delay) - else: - # 如果只有单侧有线，使用评分最高的线 - left_lines = sorted(left_lines, key=lambda line: score_line(line, True), reverse=True) - right_lines = sorted(right_lines, key=lambda line: score_line(line, False), reverse=True) - left_line = left_lines[0] if left_lines else None - right_line = right_lines[0] if right_lines else None - - # 确保两侧都有线 - if left_line is None or right_line is None: - error("无法确定合适的左右轨迹线对", "失败") - return None, None, None - - # 获取两条线的坐标 - left_x1, left_y1, left_x2, left_y2 = left_line[0] - right_x1, right_y1, right_x2, right_y2 = right_line[0] - - # 确保线段的顺序是从上到下 - if left_y1 > left_y2: - left_x1, left_x2 = left_x2, left_x1 - left_y1, left_y2 = left_y2, left_y1 - - if right_y1 > right_y2: - right_x1, right_x2 = right_x2, right_x1 - right_y1, right_y2 = right_y2, right_y1 - - - # 尝试延长线段到图像底部，处理被石板路部分遮挡的情况 - left_extended_y2 = height - if abs(left_x2 - left_x1) < 5: # 几乎垂直 - left_extended_x2 = left_x2 - else: - left_slope = (left_y2 - left_y1) / (left_x2 - left_x1) - left_extended_x2 = left_x1 + (left_extended_y2 - left_y1) / left_slope - - right_extended_y2 = height - if abs(right_x2 - right_x1) < 5: # 几乎垂直 - right_extended_x2 = right_x2 - else: - right_slope = (right_y2 - right_y1) / (right_x2 - right_x1) - right_extended_x2 = right_x1 + (right_extended_y2 - right_y1) / right_slope - - # 更新线段端点为延长后的坐标 - left_x2, left_y2 = int(left_extended_x2), left_extended_y2 - right_x2, right_y2 = int(right_extended_x2), right_extended_y2 - - # 尝试延长线段到图像底部，处理被石板路部分遮挡的情况 - left_extended_y2 = height - if abs(left_x2 - left_x1) < 5: # 几乎垂直 - left_extended_x2 = left_x2 - else: - left_slope = (left_y2 - left_y1) / (left_x2 - left_x1) - left_extended_x2 = left_x1 + (left_extended_y2 - left_y1) / left_slope - - right_extended_y2 = height - if abs(right_x2 - right_x1) < 5: # 几乎垂直 - right_extended_x2 = right_x2 - else: - right_slope = (right_y2 - right_y1) / (right_x2 - right_x1) - right_extended_x2 = right_x1 + (right_extended_y2 - right_y1) / right_slope - - # 更新线段端点为延长后的坐标 - left_x2, left_y2 = int(left_extended_x2), left_extended_y2 - right_x2, right_y2 = int(right_extended_x2), right_extended_y2 - - # 改进的中心线计算方法 - # 首先确定两条轨迹线的有效部分 - 以两条线段的y坐标重叠部分为准 - min_y = max(left_y1, right_y1) - max_y = min(left_y2, right_y2) - - # 如果两条线没有重叠的y部分，则使用整个图像范围 - if min_y >= max_y: - min_y = 0 - max_y = height - - # 计算中间路径点的方式：在多个高度上计算左右线的中点，然后拟合中心线 - num_points = 20 # 增加采样点数量以提高精度 - center_points = [] - - # 优化采样策略 - 在底部区域采样更密集 - sample_ys = [] - - # 前半部分采样点均匀分布 - first_half = np.linspace(min_y, min_y + (max_y - min_y) * 0.5, num_points // 4) - # 后半部分(更靠近底部)采样点更密集 - second_half = np.linspace(min_y + (max_y - min_y) * 0.5, max_y, num_points - num_points // 4) - - sample_ys = np.concatenate([first_half, second_half]) - - for y in sample_ys: - # 计算左侧线在当前y值处的x坐标 - if abs(left_y2 - left_y1) < 1: # 防止除零 - left_x = left_x1 - else: - t = (y - left_y1) / (left_y2 - left_y1) - left_x = left_x1 + t * (left_x2 - left_x1) - - # 计算右侧线在当前y值处的x坐标 - if abs(right_y2 - right_y1) < 1: # 防止除零 - right_x = right_x1 - else: - t = (y - right_y1) / (right_y2 - right_y1) - right_x = right_x1 + t * (right_x2 - right_x1) - - # 计算中心点 - center_x = (left_x + right_x) / 2 - center_points.append((center_x, y)) - - # 使用采样的中心点拟合中心线 - center_xs = np.array([p[0] for p in center_points]) - center_ys = np.array([p[1] for p in center_points]) - - # 使用多项式拟合改进中心线 - 选择合适的多项式阶数 - if len(center_points) >= 5: # 需要更多点来拟合更高阶多项式 - try: - # 尝试不同阶数的多项式拟合，选择最佳结果 - best_poly_coeffs = None - best_error = float('inf') - - for degree in [1, 2, 3]: # 尝试1-3阶多项式 - try: - # 使用多项式拟合 - poly_coeffs = np.polyfit(center_ys, center_xs, degree) - poly_func = np.poly1d(poly_coeffs) - - # 计算拟合误差 - predicted_xs = poly_func(center_ys) - error = np.mean(np.abs(predicted_xs - center_xs)) - - # 如果误差更小，更新最佳拟合 - if error < best_error: - best_error = error - best_poly_coeffs = poly_coeffs - except: - continue - - # 如果找到了有效的拟合，使用它 - if best_poly_coeffs is not None: - poly_coeffs = best_poly_coeffs - poly_func = np.poly1d(poly_coeffs) - - # 为了提高底部拟合精度，给予底部区域更高的权重重新拟合 - weights = np.ones_like(center_ys) - - # 计算距离底部的归一化距离 (0表示底部，1表示顶部) - normalized_distance = (max_y - center_ys) / max(1, (max_y - min_y)) - - # 设置权重，底部权重更高 - weights = 1.0 + 2.0 * (1.0 - normalized_distance) - - # 使用加权多项式拟合 - try: - weighted_poly_coeffs = np.polyfit(center_ys, center_xs, len(best_poly_coeffs) - 1, w=weights) - weighted_poly_func = np.poly1d(weighted_poly_coeffs) - - # 比较加权拟合与原始拟合 - weighted_predicted_xs = weighted_poly_func(center_ys) - weighted_error = np.mean(np.abs(weighted_predicted_xs - center_xs)) - - # 如果加权拟合更好，则使用它 - if weighted_error < best_error * 1.2: # 允许一定的误差增加，因为我们更关注底部精度 - poly_coeffs = weighted_poly_coeffs - poly_func = weighted_poly_func - except: - pass # 如果加权拟合失败，继续使用未加权的拟合 - - # 使用多项式生成中心线的起点和终点 - center_line_y1 = min_y - center_line_y2 = height # 延伸到图像底部 - - # 计算多项式在这些y值处的x坐标 - center_line_x1 = poly_func(center_line_y1) - center_line_x2 = poly_func(center_line_y2) - - # 计算中心线在图像底部的x坐标 - 用于计算偏离度 - bottom_x = poly_func(height) - - # 确保坐标在图像范围内 - bottom_x = max(0, min(width - 1, bottom_x)) - center_point = (int(bottom_x), int(height)) - - # 计算中心线的加权平均斜率 - 更关注底部区域的斜率 - if abs(center_line_y2 - center_line_y1) < 1: # 防止除零 - center_slope = 0 - else: - # 计算全局斜率 - global_slope = (center_line_x2 - center_line_x1) / (center_line_y2 - center_line_y1) - - # 计算底部区域的斜率，使用多个点来提高精度 - bottom_slopes = [] - bottom_region_start = height - height * 0.3 # 底部30%区域 - - if bottom_region_start > min_y: - # 在底部区域采样多个点计算局部斜率 - bottom_sample_count = 5 - bottom_ys = np.linspace(bottom_region_start, height, bottom_sample_count) - - for i in range(len(bottom_ys) - 1): - y1, y2 = bottom_ys[i], bottom_ys[i+1] - x1, x2 = poly_func(y1), poly_func(y2) - - # 确保坐标有效 - x1 = max(0, min(width - 1, x1)) - x2 = max(0, min(width - 1, x2)) - - local_slope = (x2 - x1) / max(0.1, (y2 - y1)) - bottom_slopes.append(local_slope) - - # 计算底部斜率的加权平均值，越靠近底部权重越高 - if bottom_slopes: - weights = np.linspace(1, 2, len(bottom_slopes)) - bottom_slope = np.average(bottom_slopes, weights=weights) - - # 加权平均，底部斜率权重更高 - center_slope = bottom_slope * 0.8 + global_slope * 0.2 - else: - center_slope = global_slope - else: - center_slope = global_slope - else: - # 如果所有多项式拟合都失败，退回到简单的线性拟合 - raise Exception("多项式拟合失败") - - except Exception as e: - warning(f"多项式拟合失败，使用简单中点计算: {e}", "拟合") - # 如果多项式拟合失败，退回到简单的中点计算方法 - center_line_x1 = (left_x1 + right_x1) / 2 - center_line_y1 = (left_y1 + right_y1) / 2 - center_line_x2 = (left_x2 + right_x2) / 2 - center_line_y2 = (left_y2 + right_y2) / 2 - - # 计算中心线的斜率 - if abs(center_line_x2 - center_line_x1) < 5: - center_slope = 100 # 几乎垂直 - else: - center_slope = (center_line_y2 - center_line_y1) / (center_line_x2 - center_line_x1) - - # 计算中心线延伸到图像底部的点 - if abs(center_slope) < 0.01: # 几乎水平 - bottom_x = center_line_x1 - else: - bottom_x = center_line_x1 + (height - center_line_y1) / center_slope - - bottom_x = max(0, min(width - 1, bottom_x)) - center_point = (int(bottom_x), int(height)) - else: - # 如果点数不足，退回到简单的中点计算方法 - center_line_x1 = (left_x1 + right_x1) / 2 - center_line_y1 = (left_y1 + right_y1) / 2 - center_line_x2 = (left_x2 + right_x2) / 2 - center_line_y2 = (left_y2 + right_y2) / 2 - - # 计算中心线的斜率 - if abs(center_line_x2 - center_line_x1) < 5: - center_slope = 100 # 几乎垂直 - else: - center_slope = (center_line_y2 - center_line_y1) / (center_line_x2 - center_line_x1) - - # 计算中心线延伸到图像底部的点 - if abs(center_slope) < 0.01: # 几乎水平 - bottom_x = center_line_x1 - else: - bottom_x = center_line_x1 + (height - center_line_y1) / center_slope - - bottom_x = max(0, min(width - 1, bottom_x)) - center_point = (int(bottom_x), int(height)) - - # 计算中心线与图像中心线的偏差 - deviation = bottom_x - center_x - - result_img = None - if observe or save_log: - result_img = img.copy() - # 绘制左右轨迹线 - try: - # 确保坐标在图像范围内并且是整数 - left_x1_safe = max(0, min(width - 1, left_x1)) - left_y1_safe = max(0, min(height - 1, left_y1)) - left_x2_safe = max(0, min(width - 1, left_x2)) - left_y2_safe = max(0, min(height - 1, left_y2)) - - cv2.line(result_img, (int(left_x1_safe), int(left_y1_safe)), - (int(left_x2_safe), int(left_y2_safe)), (255, 0, 0), 2) - except Exception as e: - warning(f"绘制左轨迹线错误: {e}", "绘图") - - try: - # 确保坐标在图像范围内并且是整数 - right_x1_safe = max(0, min(width - 1, right_x1)) - right_y1_safe = max(0, min(height - 1, right_y1)) - right_x2_safe = max(0, min(width - 1, right_x2)) - right_y2_safe = max(0, min(height - 1, right_y2)) - - cv2.line(result_img, (int(right_x1_safe), int(right_y1_safe)), - (int(right_x2_safe), int(right_y2_safe)), (0, 0, 255), 2) - except Exception as e: - warning(f"绘制右轨迹线错误: {e}", "绘图") - - # 绘制中心线 - 如果有多项式拟合，绘制拟合曲线 - if len(center_points) >= 3: - # 绘制拟合的中心曲线 - try: - curve_ys = np.linspace(min_y, height, 100) - curve_xs = poly_func(curve_ys) - - for i in range(len(curve_ys) - 1): - try: - # 确保坐标在图像范围内并且是整数 - x1 = max(0, min(width - 1, curve_xs[i])) - y1 = max(0, min(height - 1, curve_ys[i])) - x2 = max(0, min(width - 1, curve_xs[i+1])) - y2 = max(0, min(height - 1, curve_ys[i+1])) - - pt1 = (int(x1), int(y1)) - pt2 = (int(x2), int(y2)) - cv2.line(result_img, pt1, pt2, (0, 255, 0), 2) - except Exception as e: - warning(f"绘制曲线段错误: {e}", "绘图") - continue - - # 绘制采样点 - for pt in center_points: - try: - # 确保坐标在图像范围内并且是整数 - x = max(0, min(width - 1, pt[0])) - y = max(0, min(height - 1, pt[1])) - cv2.circle(result_img, (int(x), int(y)), 3, (0, 255, 255), -1) - except Exception as e: - warning(f"绘制采样点错误: {e}", "绘图") - continue - except Exception as e: - warning(f"绘制曲线错误: {e}", "绘图") - else: - # 绘制简单中心线 - try: - # 确保坐标在图像范围内并且是整数 - x1 = max(0, min(width - 1, center_line_x1)) - y1 = max(0, min(height - 1, center_line_y1)) - x2 = max(0, min(width - 1, center_line_x2)) - y2 = max(0, min(height - 1, center_line_y2)) - - cv2.line(result_img, (int(x1), int(y1)), (int(x2), int(y2)), (0, 255, 0), 2) - except Exception as e: - warning(f"绘制中心线错误: {e}", "绘图") - - # 绘制图像中心线 - try: - center_x_safe = max(0, min(width - 1, center_x)) - cv2.line(result_img, (int(center_x_safe), 0), (int(center_x_safe), height), (0, 0, 255), 1) - except Exception as e: - warning(f"绘制图像中心线错误: {e}", "绘图") - - # 标记中心点 - try: - # 确保中心点坐标在图像范围内 - center_x_safe = max(0, min(width - 1, center_point[0])) - center_y_safe = max(0, min(height - 1, center_point[1])) - cv2.circle(result_img, (int(center_x_safe), int(center_y_safe)), 10, (255, 0, 255), -1) - except Exception as e: - warning(f"绘制中心点错误: {e}", "绘图") - - # 显示偏差信息 - cv2.putText(result_img, f"Deviation: {deviation:.1f}px", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) - cv2.putText(result_img, "Final Result", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) - if 'best_pair_score' in locals() and best_pair_score != -1: - cv2.putText(result_img, f"Pair Score: {best_pair_score:.2f}", (10, 85), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) - current_y_offset = 105 - else: - current_y_offset = 85 - - cv2.putText(result_img, f"L-Slope: {left_line[3]:.2f}, R-Slope: {right_line[3]:.2f}", (10, current_y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) - current_y_offset += 20 - cv2.putText(result_img, f"Track Width: {right_line[1] - left_line[1]:.1f}px", (10, current_y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) - - if observe: - cv2.imshow("轨迹线检测结果", result_img) - cv2.waitKey(delay) - - # 保存日志图像 - if save_log and result_img is not None: - timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f") - log_dir = "logs/image" - os.makedirs(log_dir, exist_ok=True) - - # 保存结果图像 - result_img_path = os.path.join(log_dir, f"dual_track_{timestamp}.jpg") - cv2.imwrite(result_img_path, result_img) - - # 保存原图 - orig_img_path = os.path.join(log_dir, f"dual_track_orig_{timestamp}.jpg") - cv2.imwrite(orig_img_path, img) - - info(f"保存双轨迹线检测结果图像到: {result_img_path}", "日志") - info(f"保存原始图像到: {orig_img_path}", "日志") - - # 保存文本日志信息 - log_info = { - "timestamp": timestamp, - "center_point": (int(center_point[0]), int(center_point[1])), - "deviation": float(deviation), - "left_track_mid_x": float(left_line[1]), - "right_track_mid_x": float(right_line[1]), - "track_width": float(right_line[1] - left_line[1]), - "center_slope": float(center_slope), - } - info(f"双轨迹线检测结果: {log_info}", "日志") - - # 创建左右轨迹线和中心线信息 - left_track_info = { - "line": (left_x1, left_y1, left_x2, left_y2), - "slope": left_line[3], - "x_mid": left_line[1] - } - - right_track_info = { - "line": (right_x1, right_y1, right_x2, right_y2), - "slope": right_line[3], - "x_mid": right_line[1] - } - - center_info = { - "point": (int(center_point[0]), int(center_point[1])), - "deviation": float(deviation), - "slope": float(center_slope), - "is_vertical": abs(center_slope) > 5.0, # 判断是否接近垂直 - "track_width": float(right_line[1] - left_line[1]), # 两轨迹线之间的距离 - } - - return center_info, left_track_info, right_track_info - -def auto_detect_dual_track_lines(image, observe=False, delay=1000, save_log=True, max_retries=2, use_center_based=True): - """ - 自动检测双轨迹线，使用指定方法 - - 参数: - image: 输入图像，可以是文件路径或者已加载的图像数组 - observe: 是否输出中间状态信息和可视化结果，默认为False - delay: 展示每个步骤的等待时间(毫秒) - save_log: 是否保存日志和图像 - max_retries: 最大重试次数，用于处理复杂情况 - use_center_based: 是否使用基于中心线的检测方法，默认为True - - 返回: - tuple: (中心线信息, 左轨迹线信息, 右轨迹线信息) - """ - # 根据参数选择使用的检测方法 - if use_center_based: - info("使用中心线基础检测方法", "检测") - result = detect_center_based_dual_track_lines(image, observe, delay, save_log) - else: - info("使用传统检测方法", "检测") - result = detect_dual_track_lines(image, observe, delay, save_log) - - # 检查结果是否成功 - if result[0] is not None: - info("轨迹线检测成功", "检测") - return result - - # 如果失败且还有重试次数，尝试增强图像后重新检测 - if max_retries > 0: - warning(f"检测失败，尝试调整参数重新检测 (剩余重试次数: {max_retries})", "检测") - - # 对图像进行预处理以增强黄线检测 - if isinstance(image, str): - img = cv2.imread(image) - else: - img = image.copy() - - if img is not None: - # 增强图像对比度 - lab = cv2.cvtColor(img, cv2.COLOR_BGR2LAB) - l, a, b = cv2.split(lab) - clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8)) - l = clahe.apply(l) - lab = cv2.merge((l, a, b)) - enhanced_img = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR) - - # 使用增强后的图像重新尝试 - return auto_detect_dual_track_lines(enhanced_img, observe, delay, save_log, max_retries-1, use_center_based) - - error("轨迹线检测失败", "检测") - return None, None, None