diff --git a/logs/robot_2025-05-22.log b/logs/robot_2025-05-22.log new file mode 100644 index 0000000..87f4a92 --- /dev/null +++ b/logs/robot_2025-05-22.log @@ -0,0 +1,90 @@ +2025-05-22 12:20:16 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-22 12:20:18 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-22 12:20:19 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分 +2025-05-22 12:20:20 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点 +2025-05-22 12:20:21 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-22 12:20:22 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 8 条直线 +2025-05-22 12:20:23 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 5 条水平线 (下方: 5, 上方: 0) +2025-05-22 12:20:24 | DEBUG | utils.log_helper - 🐞 选择下方水平线 +2025-05-22 12:20:24 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点 +2025-05-22 12:20:25 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122025_111891.jpg +2025-05-22 12:20:25 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122025_111891', 'edge_point': (1919, 862), 'distance_to_center': 959, 'slope': 0.03486394557823129, 'distance_to_bottom': 251.43452380952385, 'intersection_point': (960, 828), 'score': 0.6679311933106575, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])} +2025-05-22 12:20:38 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-22 12:20:39 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-22 12:20:40 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分 +2025-05-22 12:20:41 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点 +2025-05-22 12:20:42 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-22 12:20:43 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 8 条直线 +2025-05-22 12:20:44 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 5 条水平线 (下方: 5, 上方: 0) +2025-05-22 12:20:45 | DEBUG | utils.log_helper - 🐞 选择下方水平线 +2025-05-22 12:20:45 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点 +2025-05-22 12:20:47 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122046_987809.jpg +2025-05-22 12:20:47 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122046_987809', 'edge_point': (1919, 862), 'distance_to_center': 959, 'slope': 0.03486394557823129, 'distance_to_bottom': 251.43452380952385, 'intersection_point': (960, 828), 'score': 0.6679311933106575, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])} +2025-05-22 12:21:15 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-22 12:21:16 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-22 12:21:17 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分 +2025-05-22 12:21:18 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点 +2025-05-22 12:21:19 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-22 12:21:20 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 8 条直线 +2025-05-22 12:21:21 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 5 条水平线 (下方: 5, 上方: 0) +2025-05-22 12:21:22 | DEBUG | utils.log_helper - 🐞 选择下方水平线 +2025-05-22 12:21:22 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点 +2025-05-22 12:21:23 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122123_802326.jpg +2025-05-22 12:21:23 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122123_802326', 'edge_point': (1919, 862), 'distance_to_center': 959, 'slope': 0.03486394557823129, 'distance_to_bottom': 251.43452380952385, 'intersection_point': (960, 828), 'score': 0.6679311933106575, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])} +2025-05-22 12:22:26 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-22 12:22:28 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-22 12:22:29 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分 +2025-05-22 12:22:30 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点 +2025-05-22 12:22:31 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-22 12:22:32 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 8 条直线 +2025-05-22 12:22:33 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 5 条水平线 (下方: 5, 上方: 0) +2025-05-22 12:22:34 | DEBUG | utils.log_helper - 🐞 选择下方水平线 +2025-05-22 12:22:34 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点 +2025-05-22 12:22:35 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122235_241410.jpg +2025-05-22 12:22:35 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122235_241410', 'edge_point': (1919, 862), 'distance_to_center': 959, 'slope': 0.03486394557823129, 'distance_to_bottom': 251.43452380952385, 'intersection_point': (960, 828), 'score': 0.6679311933106575, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])} +2025-05-22 12:24:10 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-22 12:24:11 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-22 12:24:12 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分 +2025-05-22 12:24:13 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点 +2025-05-22 12:24:14 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-22 12:24:15 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 28 条直线 +2025-05-22 12:24:16 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 6 条水平线 (下方: 6, 上方: 0) +2025-05-22 12:24:17 | DEBUG | utils.log_helper - 🐞 选择下方水平线 +2025-05-22 12:24:17 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点 +2025-05-22 12:24:18 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122418_738782.jpg +2025-05-22 12:24:18 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122418_738782', 'edge_point': (91, 1078), 'distance_to_center': -869, 'slope': -0.07092198581560284, 'distance_to_bottom': 63.631205673758814, 'intersection_point': (960, 1016), 'score': 0.8014868667518458, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])} +2025-05-22 12:24:54 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-22 12:24:55 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-22 12:24:56 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分 +2025-05-22 12:24:57 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点 +2025-05-22 12:24:58 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-22 12:24:59 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 11 条直线 +2025-05-22 12:25:00 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 1 条水平线 (下方: 1, 上方: 0) +2025-05-22 12:25:01 | DEBUG | utils.log_helper - 🐞 选择下方水平线 +2025-05-22 12:25:01 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点 +2025-05-22 12:25:02 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_122502_339369.jpg +2025-05-22 12:25:02 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_122502_339369', 'edge_point': (1439, 899), 'distance_to_center': 479, 'slope': -0.07083333333333333, 'distance_to_bottom': 147.07083333333333, 'intersection_point': (960, 932), 'score': 0.5594172208352652, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])} +2025-05-22 12:33:02 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_123302_872381.jpg +2025-05-22 12:33:02 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_123302_872381', 'edge_point': (320, 1009), 'distance_to_center': -640, 'slope': -0.07331047777324741, 'distance_to_bottom': 117.91870577487839, 'intersection_point': (960, 962)} +2025-05-22 12:33:26 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_123326_235741.jpg +2025-05-22 12:33:26 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_123326_235741', 'edge_point': (320, 1009), 'distance_to_center': -640, 'slope': -0.07331047777324741, 'distance_to_bottom': 117.91870577487839, 'intersection_point': (960, 962)} +2025-05-22 12:34:09 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-22 12:34:10 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-22 12:34:10 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分 +2025-05-22 12:34:11 | DEBUG | utils.log_helper - 🐞 正在处理底部边缘点 +2025-05-22 12:34:12 | DEBUG | utils.log_helper - 🐞 显示拟合线段 +2025-05-22 12:34:13 | DEBUG | utils.log_helper - 👁️ 步骤5: 找到边缘点 (320, 1009) +2025-05-22 12:34:14 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点 +2025-05-22 12:34:14 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_123414_979258.jpg +2025-05-22 12:34:14 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_123414_979258', 'edge_point': (320, 1009), 'distance_to_center': -640, 'slope': -0.07331047777324741, 'distance_to_bottom': 117.91870577487839, 'intersection_point': (960, 962)} +2025-05-22 12:34:34 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-22 12:34:35 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-22 12:34:36 | DEBUG | utils.log_helper - 🐞 步骤3: 提取黄色部分 +2025-05-22 12:34:36 | DEBUG | utils.log_helper - 🐞 检测底部和顶部边缘点 +2025-05-22 12:34:37 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-22 12:34:38 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 11 条直线 +2025-05-22 12:34:39 | DEBUG | utils.log_helper - 🐞 步骤6: 找到 1 条水平线 (下方: 1, 上方: 0) +2025-05-22 12:34:40 | DEBUG | utils.log_helper - 🐞 选择下方水平线 +2025-05-22 12:34:40 | DEBUG | utils.log_helper - 🐞 显示边缘斜率和中线交点 +2025-05-22 12:34:41 | INFO | utils.log_helper - ℹ️ 保存横向边缘检测结果图像到: logs/image/horizontal_edge_20250522_123441_043307.jpg +2025-05-22 12:34:41 | INFO | utils.log_helper - ℹ️ 横向边缘检测结果: {'timestamp': '20250522_123441_043307', 'edge_point': (1439, 899), 'distance_to_center': 479, 'slope': -0.07083333333333333, 'distance_to_bottom': 147.07083333333333, 'intersection_point': (960, 932), 'score': 0.5594172208352652, 'valid': True, 'reason': '', 'is_lower_line': array([ True, True, True, True])} diff --git a/res/path/test-3.jpg b/res/path/test-3.jpg new file mode 100644 index 0000000..2f55eb2 Binary files /dev/null and b/res/path/test-3.jpg differ diff --git a/res/path/test/result_image_20250514_024347.png b/res/path/test/result_image_20250514_024347.png deleted file mode 100644 index 04fd57f..0000000 Binary files a/res/path/test/result_image_20250514_024347.png and /dev/null differ diff --git a/test/task-path-track/yellow_track_demo.py b/test/task-path-track/yellow_track_demo.py index dcc8b2e..88cf740 100644 --- a/test/task-path-track/yellow_track_demo.py +++ b/test/task-path-track/yellow_track_demo.py @@ -9,7 +9,7 @@ current_dir = os.path.dirname(os.path.abspath(__file__)) project_root = os.path.dirname(os.path.dirname(current_dir)) sys.path.append(project_root) -from utils.detect_track import detect_horizontal_track_edge +from utils.detect_track import detect_horizontal_track_edge, detect_horizontal_track_edge_v2 def process_image(image_path, save_dir=None, show_steps=False): """处理单张图像""" @@ -17,7 +17,7 @@ def process_image(image_path, save_dir=None, show_steps=False): # 检测赛道并估算距离 start_time = time.time() - edge_point, edge_info = detect_horizontal_track_edge(image_path, observe=show_steps, save_log=True) + edge_point, edge_info = detect_horizontal_track_edge(image_path, observe=show_steps, save_log=True, delay=800) processing_time = time.time() - start_time # 输出结果 @@ -44,8 +44,8 @@ def process_image(image_path, save_dir=None, show_steps=False): def main(): parser = argparse.ArgumentParser(description='黄色赛道检测演示程序') - parser.add_argument('--input', type=str, default='res/path/test-2.jpg', help='输入图像或视频的路径') - parser.add_argument('--output', type=str, default='res/path/test/result_image_20250514_024313.png', help='输出结果的保存路径') + parser.add_argument('--input', type=str, default='res/path/image_20250513_162556.png', help='输入图像或视频的路径') + parser.add_argument('--output', type=str, default='res/path/test/image_20250513_162556.png', help='输出结果的保存路径') parser.add_argument('--type', type=str, choices=['image', 'video'], help='输入类型，不指定会自动检测') parser.add_argument('--show', default=True, action='store_true', help='显示处理步骤') diff --git a/utils/detect_track.py b/utils/detect_track.py index 8ad3bf3..6716732 100644 --- a/utils/detect_track.py +++ b/utils/detect_track.py @@ -6,9 +6,346 @@ from sklearn import linear_model from utils.log_helper import get_logger, debug, info, warning, error, success def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True): + """ + 检测正前方横向黄色赛道的边缘，并返回y值最大的边缘点 + # INFO 原来的版本 + + 参数: + image: 输入图像，可以是文件路径或者已加载的图像数组 + observe: 是否输出中间状态信息和可视化结果，默认为False + delay: 展示每个步骤的等待时间(毫秒) + save_log: 是否保存日志和图像 + 返回: + edge_point: 赛道前方边缘点的坐标 (x, y) + edge_info: 边缘信息字典 + """ + # observe = False # TSET + # 如果输入是字符串（文件路径），则加载图像 + if isinstance(image, str): + img = cv2.imread(image) + else: + img = image.copy() + + if img is None: + error("无法加载图像", "失败") + return None, None + + # 获取图像尺寸 + height, width = img.shape[:2] + + # 计算图像中间区域的范围（用于专注于正前方的赛道） + center_x = width // 2 + search_width = int(width * 2/3) # 搜索区域宽度为图像宽度的2/3 + search_height = height # 搜索区域高度为图像高度的1/1 + left_bound = center_x - search_width // 2 + right_bound = center_x + search_width // 2 + bottom_bound = height + top_bound = height - search_height + + if observe: + debug("步骤1: 原始图像已加载", "加载") + search_region_img = img.copy() + # 绘制搜索区域 + cv2.rectangle(search_region_img, (left_bound, top_bound), (right_bound, bottom_bound), (255, 0, 0), 2) + cv2.line(search_region_img, (center_x, 0), (center_x, height), (0, 0, 255), 2) # 中线 + cv2.imshow("搜索区域", search_region_img) + cv2.waitKey(delay) + + # 转换到HSV颜色空间以便更容易提取黄色 + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + + # 黄色的HSV范围 + lower_yellow = np.array([20, 100, 100]) + upper_yellow = np.array([30, 255, 255]) + + # 创建黄色的掩码 + mask = cv2.inRange(hsv, lower_yellow, upper_yellow) + + # 添加形态学操作以改善掩码 + kernel = np.ones((3, 3), np.uint8) + mask = cv2.dilate(mask, kernel, iterations=1) + + if observe: + debug("步骤2: 创建黄色掩码", "处理") + cv2.imshow("黄色掩码", mask) + cv2.waitKey(delay) + + # 应用掩码，只保留黄色部分 + yellow_only = cv2.bitwise_and(img, img, mask=mask) + + if observe: + debug("步骤3: 提取黄色部分", "处理") + cv2.imshow("只保留黄色", yellow_only) + cv2.waitKey(delay) + + # 裁剪掩码到搜索区域 + search_mask = mask[top_bound:bottom_bound, left_bound:right_bound] + + # 找到掩码在搜索区域中最底部的非零点位置 + bottom_points = [] + non_zero_cols = np.where(np.any(search_mask, axis=0))[0] + + # 寻找每列的最底部点 + for col in non_zero_cols: + col_points = np.where(search_mask[:, col] > 0)[0] + if len(col_points) > 0: + bottom_row = np.max(col_points) + bottom_points.append((left_bound + col, top_bound + bottom_row)) + + if len(bottom_points) < 3: + # 如果找不到足够的底部点，使用canny+霍夫变换 + edges = cv2.Canny(mask, 50, 150, apertureSize=3) + + if observe: + debug("步骤3.1: 边缘检测", "处理") + cv2.imshow("边缘检测", edges) + cv2.waitKey(delay) + + # 使用霍夫变换检测直线 - 调低阈值以检测短线段 + lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=30, + minLineLength=width*0.1, maxLineGap=30) + + if lines is None or len(lines) == 0: + if observe: + error("未检测到直线", "失败") + return None, None + + if observe: + debug(f"步骤4: 检测到 {len(lines)} 条直线", "处理") + lines_img = img.copy() + 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) + + # 筛选水平线，但放宽斜率条件 + horizontal_lines = [] + for line in lines: + x1, y1, x2, y2 = line[0] + + # 计算斜率 (避免除零错误) + if abs(x2 - x1) < 5: # 几乎垂直的线 + continue + + slope = (y2 - y1) / (x2 - x1) + + # 筛选接近水平的线 (斜率接近0)，但容许更大的倾斜度 + if abs(slope) < 0.3: + # 确保线在搜索区域内 + if ((left_bound <= x1 <= right_bound and top_bound <= y1 <= bottom_bound) or + (left_bound <= x2 <= right_bound and top_bound <= y2 <= bottom_bound)): + # 计算线的中点y坐标 + mid_y = (y1 + y2) / 2 + line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2) + # 保存线段、其y坐标和长度 + horizontal_lines.append((line[0], mid_y, slope, line_length)) + + if not horizontal_lines: + if observe: + error("未检测到水平线", "失败") + return None, None + + if observe: + debug(f"步骤4.1: 找到 {len(horizontal_lines)} 条水平线", "处理") + h_lines_img = img.copy() + for line_info in horizontal_lines: + line, _, slope, _ = line_info + x1, y1, x2, y2 = line + cv2.line(h_lines_img, (x1, y1), (x2, y2), (0, 255, 255), 2) + # 显示斜率 + cv2.putText(h_lines_img, f"{slope:.2f}", ((x1+x2)//2, (y1+y2)//2), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1) + cv2.imshow("水平线", h_lines_img) + cv2.waitKey(delay) + + # 按y坐标排序 (从大到小，底部的线排在前面) + horizontal_lines.sort(key=lambda x: x[1], reverse=True) + + # 取最靠近底部且足够长的线作为横向赛道线 + selected_line = None + selected_slope = 0 + for line_info in horizontal_lines: + line, _, slope, length = line_info + if length > width * 0.1: # 确保线足够长 + selected_line = line + selected_slope = slope + break + + if selected_line is None and horizontal_lines: + # 如果没有足够长的线，就取最靠近底部的线 + selected_line = horizontal_lines[0][0] + selected_slope = horizontal_lines[0][2] + + if selected_line is None: + if observe: + error("无法选择合适的线段", "失败") + return None, None + + x1, y1, x2, y2 = selected_line + else: + # 使用底部点拟合直线 + if observe: + debug("正在处理底部边缘点", "处理") + bottom_points_img = img.copy() + for point in bottom_points: + cv2.circle(bottom_points_img, point, 3, (0, 255, 0), -1) + cv2.imshow("底部边缘点", bottom_points_img) + cv2.waitKey(delay) + + # 使用RANSAC拟合直线以去除异常值 + x_points = np.array([p[0] for p in bottom_points]).reshape(-1, 1) + y_points = np.array([p[1] for p in bottom_points]) + + # 如果点过少或分布不够宽，返回None + if len(bottom_points) < 3 or np.max(x_points) - np.min(x_points) < width * 0.1: + if observe: + warning("底部点太少或分布不够宽", "警告") + return None, None + + ransac = linear_model.RANSACRegressor(residual_threshold=5.0) + ransac.fit(x_points, y_points) + + # 获取拟合参数 + selected_slope = ransac.estimator_.coef_[0] + intercept = ransac.estimator_.intercept_ + + # 检查斜率是否在合理范围内 + if abs(selected_slope) > 0.3: + if observe: + warning(f"拟合斜率过大: {selected_slope:.4f}", "警告") + return None, None + + # 使用拟合的直线参数计算线段端点 + x1 = left_bound + y1 = int(selected_slope * x1 + intercept) + x2 = right_bound + y2 = int(selected_slope * x2 + intercept) + + if observe: + debug("显示拟合线段", "处理") + fitted_line_img = img.copy() + cv2.line(fitted_line_img, (x1, y1), (x2, y2), (0, 255, 255), 2) + cv2.imshow("拟合线段", fitted_line_img) + cv2.waitKey(delay) + + # 确保x1 < x2 + if x1 > x2: + x1, x2 = x2, x1 + y1, y2 = y2, y1 + + # 找到线上y值最大的点作为边缘点(最靠近相机的点) + if y1 > y2: + bottom_edge_point = (x1, y1) + else: + bottom_edge_point = (x2, y2) + + # 获取线上的更多点 + selected_points = [] + step = 5 # 每5个像素取一个点 + for x in range(max(left_bound, int(min(x1, x2))), min(right_bound, int(max(x1, x2)) + 1), step): + y = int(selected_slope * (x - x1) + y1) + if top_bound <= y <= bottom_bound: + selected_points.append((x, y)) + + if observe: + debug(f"步骤5: 找到边缘点 {bottom_edge_point}", "检测") + edge_img = img.copy() + # 画线 + cv2.line(edge_img, (x1, y1), (x2, y2), (0, 255, 0), 2) + # 绘制所有点 + for point in selected_points: + cv2.circle(edge_img, point, 3, (255, 0, 0), -1) + # 标记边缘点 + cv2.circle(edge_img, bottom_edge_point, 10, (0, 0, 255), -1) + cv2.imshow("选定的横向线和边缘点", edge_img) + cv2.waitKey(delay) + + # 计算这个点到中线的距离 + distance_to_center = bottom_edge_point[0] - center_x + + # 计算中线与检测到的横向线的交点 + # 横向线方程: y = slope * (x - x1) + y1 + # 中线方程: x = center_x + # 解这个方程组得到交点坐标 + intersection_x = center_x + intersection_y = selected_slope * (center_x - x1) + y1 + intersection_point = (int(intersection_x), int(intersection_y)) + + # 计算交点到图像底部的距离（以像素为单位） + distance_to_bottom = height - intersection_y + + result_img = None + if observe or save_log: + slope_img = img.copy() + # 画出检测到的线 + cv2.line(slope_img, (x1, y1), (x2, y2), (0, 255, 0), 2) + # 标记边缘点 + cv2.circle(slope_img, bottom_edge_point, 10, (0, 0, 255), -1) + # 画出中线 + cv2.line(slope_img, (center_x, 0), (center_x, height), (0, 0, 255), 2) + # 标记中线与横向线的交点 + cv2.circle(slope_img, intersection_point, 12, (255, 0, 255), -1) + cv2.circle(slope_img, intersection_point, 5, (255, 255, 255), -1) + # 画出交点到底部的距离线 + cv2.line(slope_img, intersection_point, (intersection_x, height), (255, 255, 0), 2) + + cv2.putText(slope_img, f"Slope: {selected_slope:.4f}", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + cv2.putText(slope_img, f"Distance to center: {distance_to_center}px", (10, 70), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + cv2.putText(slope_img, f"Distance to bottom: {distance_to_bottom:.1f}px", (10, 110), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + cv2.putText(slope_img, f"中线交点: ({intersection_point[0]}, {intersection_point[1]})", (10, 150), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + + if observe: + debug("显示边缘斜率和中线交点", "显示") + cv2.imshow("边缘斜率和中线交点", slope_img) + cv2.waitKey(delay) + + result_img = slope_img + + # 保存日志图像 + 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) + img_path = os.path.join(log_dir, f"horizontal_edge_{timestamp}.jpg") + cv2.imwrite(img_path, result_img) + info(f"保存横向边缘检测结果图像到: {img_path}", "日志") + + # 保存文本日志信息 + log_info = { + "timestamp": timestamp, + "edge_point": bottom_edge_point, + "distance_to_center": distance_to_center, + "slope": selected_slope, + "distance_to_bottom": distance_to_bottom, + "intersection_point": intersection_point + } + info(f"横向边缘检测结果: {log_info}", "日志") + + # 创建边缘信息字典 + edge_info = { + "x": bottom_edge_point[0], + "y": bottom_edge_point[1], + "distance_to_center": distance_to_center, + "slope": selected_slope, + "is_horizontal": abs(selected_slope) < 0.05, # 判断边缘是否接近水平 + "points_count": len(selected_points), # 该组中点的数量 + "intersection_point": intersection_point, # 中线与横向线的交点 + "distance_to_bottom": distance_to_bottom, # 交点到图像底部的距离 + # "points": selected_points # 添加选定的点组 + } + + return bottom_edge_point, edge_info + +def detect_horizontal_track_edge_v2(image, observe=False, delay=1000, save_log=True): """ 检测正前方横向黄色赛道的边缘，并返回y值最大的边缘点 优先检测下方横线，但在遇到下方线截断的情况时会考虑上边缘 + 但容易识别到上面的线。 参数: image: 输入图像，可以是文件路径或者已加载的图像数组 @@ -143,6 +480,13 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True # 筛选水平线，但放宽斜率条件 horizontal_lines = [] + # 分别存储上方和下方的水平线 + lower_horizontal_lines = [] + upper_horizontal_lines = [] + + # 定义上下分界线位置 (以图像中部再下移一点作为分界) + lower_upper_boundary = height * 0.65 + for line in lines: x1, y1, x2, y2 = line[0] @@ -176,20 +520,35 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True mid_x = (x1 + x2) / 2 center_score = max(0.0, 1.0 - abs(mid_x - center_x) / (width * 0.3)) - # 计算综合得分 - quality_score = position_score * 0.4 + length_score * 0.3 + slope_score * 0.2 + center_score * 0.1 + # 计算综合得分，增加位置得分的权重，更强调下方线 + quality_score = position_score * 0.6 + length_score * 0.2 + slope_score * 0.15 + center_score * 0.05 # 保存线段、其y坐标、斜率、长度和质量得分 - horizontal_lines.append((line[0], mid_y, slope, line_length, quality_score)) + line_info = (line[0], mid_y, slope, line_length, quality_score) + + # 区分上方和下方的线 + if mid_y >= lower_upper_boundary: + lower_horizontal_lines.append(line_info) + else: + upper_horizontal_lines.append(line_info) + + # 同时保存到总列表中 + horizontal_lines.append(line_info) if not horizontal_lines: if observe: error("未检测到合格的水平线", "失败") return None, None + # 根据质量得分排序上方和下方的水平线 + lower_horizontal_lines.sort(key=lambda x: x[4], reverse=True) + upper_horizontal_lines.sort(key=lambda x: x[4], reverse=True) + if observe: - debug(f"步骤6: 找到 {len(horizontal_lines)} 条水平线", "处理") + debug(f"步骤6: 找到 {len(horizontal_lines)} 条水平线 (下方: {len(lower_horizontal_lines)}, 上方: {len(upper_horizontal_lines)})", "处理") h_lines_img = img.copy() + + # 绘制所有水平线 for line_info in horizontal_lines: line, _, slope, _, score = line_info x1, y1, x2, y2 = line @@ -199,16 +558,30 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True # 显示斜率和得分 cv2.putText(h_lines_img, f"{slope:.2f}|{score:.2f}", ((x1+x2)//2, (y1+y2)//2), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) + + # 绘制上下分界线 + cv2.line(h_lines_img, (0, int(lower_upper_boundary)), (width, int(lower_upper_boundary)), (255, 0, 255), 1) cv2.imshow("水平线", h_lines_img) cv2.waitKey(delay) - # 根据质量得分排序水平线 - horizontal_lines.sort(key=lambda x: x[4], reverse=True) - - # 取质量最高的线段作为最终选择 - selected_line = horizontal_lines[0][0] - selected_slope = horizontal_lines[0][2] - selected_score = horizontal_lines[0][4] + # 优先选择下方的线，如果没有下方的线才考虑上方的线 + if lower_horizontal_lines: + selected_line = lower_horizontal_lines[0][0] + selected_slope = lower_horizontal_lines[0][2] + selected_score = lower_horizontal_lines[0][4] + if observe: + debug("选择下方水平线", "处理") + elif upper_horizontal_lines: + selected_line = upper_horizontal_lines[0][0] + selected_slope = upper_horizontal_lines[0][2] + selected_score = upper_horizontal_lines[0][4] + if observe: + debug("没有合适的下方线，选择上方水平线", "处理") + else: + # 理论上不会进入这个分支，因为前面已经检查过horizontal_lines非空 + if observe: + error("未检测到合格的水平线", "失败") + return None, None # 提取线段端点 x1, y1, x2, y2 = selected_line @@ -348,6 +721,8 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True cv2.circle(slope_img, intersection_point, 5, (255, 255, 255), -1) # 画出交点到底部的距离线 cv2.line(slope_img, intersection_point, (intersection_x, height), (255, 255, 0), 2) + # 画出上下分界线 + cv2.line(slope_img, (0, int(lower_upper_boundary)), (width, int(lower_upper_boundary)), (255, 0, 255), 1) cv2.putText(slope_img, f"Slope: {selected_slope:.4f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, line_color, 2) @@ -390,7 +765,8 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True "intersection_point": intersection_point, "score": selected_score, "valid": valid_result, - "reason": reason if not valid_result else "" + "reason": reason if not valid_result else "", + "is_lower_line": len(lower_horizontal_lines) > 0 and selected_line == lower_horizontal_lines[0][0] } info(f"横向边缘检测结果: {log_info}", "日志") @@ -409,15 +785,12 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True "intersection_point": intersection_point, # 中线与横向线的交点 "distance_to_bottom": distance_to_bottom, # 交点到图像底部的距离 "score": selected_score, # 线段质量得分 + "is_lower_line": len(lower_horizontal_lines) > 0 and selected_line == lower_horizontal_lines[0][0] # 是否为下方线 # "points": selected_points # 添加选定的点组 } return bottom_edge_point, edge_info -# 用法示例 -if __name__ == "__main__": - pass - def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True): """ 检测左右两条平行的黄色轨道线