From af364ebccbdc823faee7c520256dc54a64b47499 Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Sat, 31 May 2025 14:12:52 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=BC=BA=E5=8F=8C=E8=BD=A8=E8=BF=B9?= =?UTF-8?q?=E7=BA=BF=E6=A3=80=E6=B5=8B=E5=8A=9F=E8=83=BD=EF=BC=8C=E4=BC=98?= =?UTF-8?q?=E5=8C=96=E4=BA=86=E8=BE=B9=E7=BC=98=E6=A3=80=E6=B5=8B=E5=92=8C?= =?UTF-8?q?=E7=BA=BF=E6=9D=A1=E7=AD=9B=E9=80=89=E9=80=BB=E8=BE=91=EF=BC=8C?= =?UTF-8?q?=E6=94=B9=E8=BF=9B=E4=BA=86=E8=AF=84=E5=88=86=E6=9C=BA=E5=88=B6?= =?UTF-8?q?=E4=BB=A5=E6=8F=90=E9=AB=98=E6=A3=80=E6=B5=8B=E5=87=86=E7=A1=AE?= =?UTF-8?q?=E6=80=A7=E3=80=82=E6=9B=B4=E6=96=B0=E4=BA=86=E8=B0=83=E8=AF=95?= =?UTF-8?q?=E4=BF=A1=E6=81=AF=E5=92=8C=E6=97=A5=E5=BF=97=E8=AE=B0=E5=BD=95?= =?UTF-8?q?=EF=BC=8C=E4=BE=BF=E4=BA=8E=E5=8F=AF=E8=A7=86=E5=8C=96=E5=92=8C?= =?UTF-8?q?=E7=BB=93=E6=9E=9C=E8=BF=BD=E8=B8=AA=E3=80=82=E5=90=8C=E6=97=B6?= =?UTF-8?q?=EF=BC=8C=E8=B0=83=E6=95=B4=E4=BA=86=E5=9B=BE=E5=83=8F=E8=B7=AF?= =?UTF-8?q?=E5=BE=84=E4=BB=A5=E9=80=82=E5=BA=94=E6=96=B0=E7=9A=84=E6=B5=8B?= =?UTF-8?q?=E8=AF=95=E7=94=A8=E4=BE=8B=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- logs/robot_2025-05-31.log | 48 ++ test/test_image.py | 2 +- utils/detect_dual_track_lines.py | 963 +++++++++++++++++++++---------- 3 files changed, 696 insertions(+), 317 deletions(-) diff --git a/logs/robot_2025-05-31.log b/logs/robot_2025-05-31.log index f68690b..f6ea804 100644 --- a/logs/robot_2025-05-31.log +++ b/logs/robot_2025-05-31.log @@ -845,3 +845,51 @@ 2025-05-31 13:25:34 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250531_132534_837444.jpg 2025-05-31 13:25:34 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/dual_track_orig_20250531_132534_837444.jpg 2025-05-31 13:25:34 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250531_132534_837444', 'center_point': (812, 1080), 'deviation': -5.684341886080801e-13, 'left_track_mid_x': 677.5, 'right_track_mid_x': 1050.0, 'track_width': 372.5, 'center_slope': -0.32784677374841315} +2025-05-31 14:08:45 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-31 14:08:46 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-31 14:08:47 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-31 14:08:48 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 65 条直线 +2025-05-31 14:08:49 | DEBUG | utils.log_helper - 🐞 步骤3.2: 筛选出 30 条垂直候选线 (合并前) +2025-05-31 14:08:50 | DEBUG | utils.log_helper - 🐞 步骤3.5: 合并筛选出 9 条垂直候选线 (合并后) +2025-05-31 14:08:51 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 9 条垂直线 +2025-05-31 14:08:52 | DEBUG | utils.log_helper - 🐞 左侧候选线数量: 6, 右侧候选线数量: 3 +2025-05-31 14:08:53 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: -1.00 +2025-05-31 14:08:55 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250531_140855_669647.jpg +2025-05-31 14:08:55 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/dual_track_orig_20250531_140855_669647.jpg +2025-05-31 14:08:55 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250531_140855_669647', 'center_point': (1028, 1080), 'deviation': 0.0, 'left_track_mid_x': 948.0, 'right_track_mid_x': 1032.5, 'track_width': 84.5, 'center_slope': 0.231117824773414} +2025-05-31 14:08:58 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-31 14:08:59 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-31 14:09:00 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-31 14:09:01 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 65 条直线 +2025-05-31 14:09:02 | DEBUG | utils.log_helper - 🐞 步骤3.2: 筛选出 30 条垂直候选线 (合并前) +2025-05-31 14:09:03 | DEBUG | utils.log_helper - 🐞 步骤3.5: 合并筛选出 9 条垂直候选线 (合并后) +2025-05-31 14:09:04 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 9 条垂直线 +2025-05-31 14:09:05 | DEBUG | utils.log_helper - 🐞 左侧候选线数量: 6, 右侧候选线数量: 3 +2025-05-31 14:09:06 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: -1.00 +2025-05-31 14:09:09 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250531_140908_978483.jpg +2025-05-31 14:09:09 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/dual_track_orig_20250531_140908_978483.jpg +2025-05-31 14:09:09 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250531_140908_978483', 'center_point': (1028, 1080), 'deviation': 0.0, 'left_track_mid_x': 948.0, 'right_track_mid_x': 1032.5, 'track_width': 84.5, 'center_slope': 0.231117824773414} +2025-05-31 14:12:05 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-31 14:12:06 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-31 14:12:07 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 (底部ROI) +2025-05-31 14:12:08 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 36 条直线 +2025-05-31 14:12:09 | DEBUG | utils.log_helper - 🐞 步骤3.2: 筛选出 18 条垂直候选线 (合并前) +2025-05-31 14:12:10 | DEBUG | utils.log_helper - 🐞 步骤3.5: 合并筛选出 6 条垂直候选线 (合并后) +2025-05-31 14:12:11 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 6 条垂直线 +2025-05-31 14:12:12 | DEBUG | utils.log_helper - 🐞 左侧候选线数量: 3, 右侧候选线数量: 3 +2025-05-31 14:12:13 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: -1.00 +2025-05-31 14:12:15 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250531_141215_609842.jpg +2025-05-31 14:12:15 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/dual_track_orig_20250531_141215_609842.jpg +2025-05-31 14:12:15 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250531_141215_609842', 'center_point': (772, 1080), 'deviation': 0.0, 'left_track_mid_x': 633.5, 'right_track_mid_x': 1032.5, 'track_width': 399.0, 'center_slope': -0.37845007646115425} +2025-05-31 14:12:20 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-31 14:12:21 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-31 14:12:22 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 (底部ROI) +2025-05-31 14:12:23 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 36 条直线 +2025-05-31 14:12:24 | DEBUG | utils.log_helper - 🐞 步骤3.2: 筛选出 18 条垂直候选线 (合并前) +2025-05-31 14:12:25 | DEBUG | utils.log_helper - 🐞 步骤3.5: 合并筛选出 6 条垂直候选线 (合并后) +2025-05-31 14:12:26 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 6 条垂直线 +2025-05-31 14:12:27 | DEBUG | utils.log_helper - 🐞 左侧候选线数量: 3, 右侧候选线数量: 3 +2025-05-31 14:12:28 | DEBUG | utils.log_helper - 🐞 选择最佳线对，评分: -1.00 +2025-05-31 14:12:30 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250531_141230_795713.jpg +2025-05-31 14:12:30 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/dual_track_orig_20250531_141230_795713.jpg +2025-05-31 14:12:30 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250531_141230_795713', 'center_point': (772, 1080), 'deviation': 0.0, 'left_track_mid_x': 633.5, 'right_track_mid_x': 1032.5, 'track_width': 399.0, 'center_slope': -0.37845007646115425} diff --git a/test/test_image.py b/test/test_image.py index 9672dc3..ae03804 100644 --- a/test/test_image.py +++ b/test/test_image.py @@ -10,7 +10,7 @@ from utils.detect_dual_track_lines import detect_dual_track_lines, auto_detect_d # 图片路径 # image_path = "res/path/image_20250514_024347.png" -image_path = "logs/res/1/dual_track_orig_20250531_012429_250427.jpg" +image_path = "logs/res/3/dual_track_orig_20250531_060458_547428.jpg" # 确保图片存在 if not os.path.exists(image_path): diff --git a/utils/detect_dual_track_lines.py b/utils/detect_dual_track_lines.py index e54dd0f..31ee819 100644 --- a/utils/detect_dual_track_lines.py +++ b/utils/detect_dual_track_lines.py @@ -84,22 +84,32 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True, cv2.waitKey(delay) # INFO 边缘检测 - edges = cv2.Canny(mask, 50, 150, apertureSize=3) + # Apply Canny to bottom_roi instead of the full mask + edges_roi = cv2.Canny(bottom_roi, 50, 150, apertureSize=3) if observe: - debug("步骤2: 边缘检测", "处理") - edges_display = cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR) - cv2.putText(edges_display, "Step 2: Edge Detection (Canny)", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) + 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("边缘检测", edges_display) + cv2.imshow("边缘检测 (底部ROI)", edges_display) # Updated window title cv2.waitKey(delay) - # INFO 霍夫变换检测直线 - lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=25, + # 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("未检测到直线", "失败") + error("未检测到直线 (在底部ROI)", "失败") # Updated error message return None, None, None if observe: @@ -914,26 +924,46 @@ def detect_center_based_dual_track_lines(image, observe=False, delay=1000, save_ cv2.waitKey(delay) # 边缘检测 - edges = cv2.Canny(combined_mask, 50, 150, apertureSize=3) + edges_roi = cv2.Canny(bottom_roi, 50, 150, apertureSize=3) if observe: - debug("步骤2: 边缘检测", "处理") - edges_display = cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR) - cv2.putText(edges_display, "Step 2: Edge Detection (Canny - Center Based)", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) + 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("边缘检测", edges_display) + cv2.imshow("边缘检测 (底部ROI)", edges_display) # Updated window title cv2.waitKey(delay) - # 霍夫变换检测直线 - lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=25, - minLineLength=width*0.05, maxLineGap=40) + # 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("未检测到直线 (或合并后无直线 - Center Based)", "失败") + 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_lines = [] + vertical_only_lines = [] for line in lines: x1, y1, x2, y2 = line[0] @@ -948,7 +978,93 @@ def detect_center_based_dual_track_lines(image, observe=False, delay=1000, save_ slope = (y2 - y1) / (x2 - x1) # 筛选接近垂直的线 (斜率较大)，但允许更多倾斜度 - if abs(slope) > 0.5: # 降低斜率阈值以捕获更多候选线 + 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 @@ -956,295 +1072,208 @@ def detect_center_based_dual_track_lines(image, observe=False, delay=1000, save_ mid_y = (y1 + y2) / 2 # 保存线段、其坐标、斜率和长度 vertical_lines.append((line[0], mid_x, mid_y, slope, line_length)) - - if len(vertical_lines) < 1: - error("未检测到足够的垂直线", "失败") - return None, None, None + + 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_display = img.copy() - cv2.putText(v_lines_img_display, "Step 4: Vertical Lines Filtered (Center Based)", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) - cv2.putText(v_lines_img_display, "Min Slope Abs: 0.50", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 255), 1) # Slope threshold is 0.5 here + 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_display, (x1, y1), (x2, y2), (0, 255, 255), 2) + cv2.line(v_lines_img, (x1, y1), (x2, y2), (0, 255, 255), 2) # 显示斜率 - cv2.putText(v_lines_img_display, f"{slope:.2f}", ((x1+x2)//2, (y1+y2)//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_display) + cv2.imshow("垂直线", v_lines_img) cv2.waitKey(delay) - debug(f"步骤2.5 (Center Based): 初始检测到 {len(vertical_lines)} 条垂直线，尝试合并", "处理") + # 按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]) - # 合并参数设置 (可以根据需要调整，这里使用与 detect_dual_track_lines 类似的设置) - # 注意：这里的 max_line_gap 是 HoughLinesP 本身的参数，不是函数参数 - # 如果需要独立于 detect_dual_track_lines 中的 max_line_gap，需要单独定义或传递 - current_max_line_gap = 40 # 来自 HoughLinesP - min_len_for_merge_cb = 5.0 - angle_thresh_deg_cb = 10.0 - ep_gap_abs_cb = current_max_line_gap / 2.0 - ep_gap_factor_cb = 0.25 - p_dist_abs_cb = current_max_line_gap / 4.0 - p_dist_factor_cb = 0.1 - - # vertical_lines = _merge_collinear_lines_iterative(vertical_lines, - # min_initial_len=min_len_for_merge_cb, - # max_angle_diff_deg=angle_thresh_deg_cb, - # max_ep_gap_abs=ep_gap_abs_cb, - # max_ep_gap_factor=ep_gap_factor_cb, - # max_p_dist_abs=p_dist_abs_cb, - # max_p_dist_factor=p_dist_factor_cb) - - # INFO 评分函数 - 用于找到最可能的中心线 - def score_center_line(line_info): + # 如果有至少两条线，将最左侧的线作为左轨迹线，最右侧的线作为右轨迹线 + 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 - # 中心接近度 - 线越接近图像中心得分越高 - center_dist = abs(mid_x - center_x) - center_score = max(0, 1.0 - center_dist / (width * 0.25)) + # 确保线段从上到下排序 + if y1 > y2: + x1, x2 = x2, x1 + y1, y2 = y2, y1 - # 底部接近度 - y越大（越靠近底部）分数越高 + # 线段靠近底部评分 - y越大（越靠近底部）分数越高 bottom_ratio = mid_y / height - y_score = min(1.0, bottom_ratio * 1.2) + y_score = min(1.0, bottom_ratio * 1.2) # 适当提高底部线段的权重 # 线段长度评分 - 线越长分数越高 length_ratio = length / (height * 0.3) length_score = min(1.0, length_ratio * 1.2) - # 斜率评分 - 线应该接近垂直但不完全垂直 - if abs(slope) > 10: # 几乎垂直 - slope_score = 0.7 - else: - # 理想斜率在1-5之间 - ideal_slope = 2.0 - slope_diff = abs(abs(slope) - ideal_slope) - slope_score = max(0, 1.0 - slope_diff / 3.0) + # 计算到图像中心的距离得分 - 更重视靠近垂直中线的线 + 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 = ( - center_score * 0.5 + # 中心接近度权重最高 - y_score * 0.2 + # 底部接近度 - length_score * 0.2 + # 线段长度 - slope_score * 0.1 # 斜率合适性 + 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 - # 找到可能的中心线 - center_candidates = sorted(vertical_lines, key=score_center_line, reverse=True) - - if observe: - debug(f"步骤3.5: 找到 {len(center_candidates)} 条可能的中心线", "处理") - center_candidates_img_display = img.copy() - cv2.putText(center_candidates_img_display, "Step 3: Center Candidates (Center Based)", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) - for line_info in center_candidates: - line, _, _, slope, _ = line_info - x1, y1, x2, y2 = line - cv2.line(center_candidates_img_display, (x1, y1), (x2, y2), (0, 255, 255), 2) - # 显示斜率 - cv2.putText(center_candidates_img_display, f"{slope:.2f}", ((x1+x2)//2, (y1+y2)//2), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1) - cv2.imshow("可能的中心线", center_candidates_img_display) - cv2.waitKey(delay) - - # 如果有足够的候选线，取前3个进行评估 - if len(center_candidates) >= 3: - center_candidates = center_candidates[:3] - - # 获取左侧和右侧的线 - left_lines = [line for line in vertical_lines if line[1] < center_x - width * 0.1] # 确保在中心左侧一定距离 - right_lines = [line for line in vertical_lines if line[1] > center_x + width * 0.1] # 确保在中心右侧一定距离 - - # 按照到图像中心的距离排序 - left_lines.sort(key=lambda x: center_x - x[1]) # 从最靠近中心到最远 - right_lines.sort(key=lambda x: x[1] - center_x) # 从最靠近中心到最远 - - # 如果左侧或右侧没有足够的线，可能无法进行检测 - if not left_lines or not right_lines: - warning("左侧或右侧未检测到足够的线段", "检测") - # 尝试放宽条件 - 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: - error("即使放宽条件，仍无法找到左右两侧的线段", "失败") - return None, None, None - - # 对于每个可能的中心线，尝试找到最佳的左右轨迹线对 - best_result = None - best_score = -1 - - for center_candidate in center_candidates: - center_line_coords = center_candidate[0] - center_x1, center_y1, center_x2, center_y2 = center_line_coords - center_mid_x = center_candidate[1] - - # 确保中心线从上到下排序 - if center_y1 > center_y2: - center_x1, center_x2 = center_x2, center_x1 - center_y1, center_y2 = center_y2, center_y1 - - # 扩展中心线到图像底部 - if abs(center_x2 - center_x1) < 5: # 几乎垂直 - center_extended_x2 = center_x2 - center_slope = 100 - else: - center_slope = (center_y2 - center_y1) / (center_x2 - center_x1) - center_extended_x2 = center_x1 + (height - center_y1) / center_slope - - center_x2, center_y2 = int(center_extended_x2), height - - # 计算预期的轨道宽度 - # 根据图像尺寸调整预期轨道宽度 - if width <= 640: # 小尺寸图像 - expected_track_width = width * expected_track_width_ratio - else: # 大尺寸图像 - expected_track_width = width * expected_track_width_ratio * 1.2 # 大图像可能需要更宽的轨道 - - half_track_width = expected_track_width / 2 - - # 基于中心线位置，预测左右轨道线的位置 - if abs(center_slope) > 5: # 几乎垂直中心线 - expected_left_x = center_mid_x - half_track_width - expected_right_x = center_mid_x + half_track_width - else: - # 考虑中心线的斜率计算预期位置 - perpendicular_slope = -1 / center_slope - len_perpendicular = np.sqrt(1 + perpendicular_slope**2) - unit_perp_x = 1 / len_perpendicular - - expected_left_x = center_mid_x - half_track_width * unit_perp_x - expected_right_x = center_mid_x + half_track_width * unit_perp_x - - # 找到最接近预期位置的左右轨道线 - # 给定一个预期位置和候选线，计算分数 - def score_track_line(line_info, expected_x, side): - _, mid_x, mid_y, slope, length = line_info - - # 位置接近度 - pos_dist = abs(mid_x - expected_x) - pos_score = max(0, 1.0 - pos_dist / (width * 0.2)) - - # 底部接近度 - 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) - - # 斜率评分 - 检查是否与中心线斜率一致 - # 左侧线应该与中心线斜率相似或稍微向内倾斜 - # 右侧线应该与中心线斜率相似或稍微向内倾斜 - if abs(center_slope) > 5: # 中心线几乎垂直 - ideal_slope = 100 if abs(slope) > 5 else 5 * (1 if side == 'right' else -1) - else: - # 中心线不垂直，左右线应与中心线有相似但略有内倾的斜率 - sign_mult = 1 if side == 'right' else -1 - ideal_slope = center_slope + sign_mult * 0.2 - - slope_diff = abs(slope - ideal_slope) - slope_score = max(0, 1.0 - slope_diff / max(1, abs(ideal_slope))) - - # 针对左右侧的特殊评分条件 - if side == 'left': - # 左侧线的x坐标应该小于中心线 - if mid_x >= center_mid_x: - return 0.0 - # 并且应该在一个合理范围内（不要太远） - if mid_x < center_mid_x - width * 0.4: - return 0.0 - else: # 右侧 - # 右侧线的x坐标应该大于中心线 - if mid_x <= center_mid_x: - return 0.0 - # 并且应该在一个合理范围内（不要太远） - if mid_x > center_mid_x + width * 0.4: - return 0.0 - - # 综合评分，位置接近度最重要 - final_score = ( - pos_score * 0.5 + # 位置接近度 - y_score * 0.2 + # 底部接近度 - length_score * 0.2 + # 线长 - slope_score * 0.1 # 斜率合适性 - ) - - return final_score - - # 为左右轨道线评分 - left_scores = [(line, score_track_line(line, expected_left_x, 'left')) for line in left_lines] - right_scores = [(line, score_track_line(line, expected_right_x, 'right')) for line in right_lines] - - # 过滤掉评分为0的线 - left_scores = [item for item in left_scores if item[1] > 0] - right_scores = [item for item in right_scores if item[1] > 0] - - # 如果没有足够的候选线，跳过这个中心线 - if not left_scores or not right_scores: - continue - - # 按评分排序 - left_scores.sort(key=lambda x: x[1], reverse=True) - right_scores.sort(key=lambda x: x[1], reverse=True) - - # 取最高分的左右线 - best_left_line = left_scores[0][0] - best_right_line = right_scores[0][0] - - # 检查左右线之间的距离是否合理 - left_mid_x = best_left_line[1] - right_mid_x = best_right_line[1] - track_width = right_mid_x - left_mid_x - - # 计算轨道宽度评分 - width_ratio = track_width / expected_track_width - if 0.7 <= width_ratio <= 1.3: # 允许一定范围的偏差 - width_score = 1.0 - else: - width_score = max(0, 1.0 - abs(1.0 - width_ratio) * 2) - - # 计算左右线的对称性 - symmetry_score = 1.0 - abs((center_mid_x - left_mid_x) - (right_mid_x - center_mid_x)) / (track_width / 2) - symmetry_score = max(0, symmetry_score) - - # 总体评分 - pair_score = ( - left_scores[0][1] * 0.3 + # 左线评分 - right_scores[0][1] * 0.3 + # 右线评分 - width_score * 0.3 + # 轨道宽度合理性 - symmetry_score * 0.1 # 对称性 - ) - - if pair_score > best_score: - best_score = pair_score - best_result = (center_candidate, best_left_line, best_right_line, center_slope) - - # 如果没有找到合适的结果，返回失败 - if best_result is None: - error("未找到合适的左右轨迹线对", "失败") + # 确保两侧都有线 + if left_line is None or right_line is None: + error("无法确定合适的左右轨迹线对", "失败") return None, None, None - # 提取最佳结果 - center_line, left_line, right_line, center_slope = best_result - - # 获取线段坐标 - center_coords = center_line[0] - left_coords = left_line[0] - right_coords = right_line[0] - - center_x1, center_y1, center_x2, center_y2 = center_coords - left_x1, left_y1, left_x2, left_y2 = left_coords - right_x1, right_y1, right_x2, right_y2 = right_coords - - # 确保线段从上到下排序 - if center_y1 > center_y2: - center_x1, center_x2 = center_x2, center_x1 - center_y1, center_y2 = center_y2, center_y1 + # 获取两条线的坐标 + 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 @@ -1253,55 +1282,357 @@ def detect_center_based_dual_track_lines(image, observe=False, delay=1000, save_ right_x1, right_x2 = right_x2, right_x1 right_y1, right_y2 = right_y2, right_y1 - # 计算中心线在图像底部的位置和偏离中心的距离 - if abs(center_x2 - center_x1) < 5: # 几乎垂直 - bottom_center_x = center_x2 + + # 尝试延长线段到图像底部，处理被石板路部分遮挡的情况 + left_extended_y2 = height + if abs(left_x2 - left_x1) < 5: # 几乎垂直 + left_extended_x2 = left_x2 else: - center_slope = (center_y2 - center_y1) / (center_x2 - center_x1) - bottom_center_x = center_x1 + (height - center_y1) / center_slope + left_slope = (left_y2 - left_y1) / (left_x2 - left_x1) + left_extended_x2 = left_x1 + (left_extended_y2 - left_y1) / left_slope - # 确保坐标在图像范围内 - bottom_center_x = max(0, min(width - 1, bottom_center_x)) - deviation = bottom_center_x - center_x + 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 - # INFO 创建结果图像 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}", "绘图") - # 绘制中心线 - cv2.line(result_img, (center_x1, center_y1), (center_x2, center_y2), (0, 255, 0), 2) - - # 绘制左轨迹线 - cv2.line(result_img, (int(left_x1), int(left_y1)), (int(left_x2), int(left_y2)), (255, 0, 0), 2) - - # 绘制右轨迹线 - cv2.line(result_img, (int(right_x1), int(right_y1)), (int(right_x2), int(right_y2)), (0, 0, 255), 2) + # 绘制中心线 - 如果有多项式拟合，绘制拟合曲线 + 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}", "绘图") # 绘制图像中心线 - cv2.line(result_img, (center_x, 0), (center_x, height), (0, 0, 255), 1) - + 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}", "绘图") + # 标记中心点 - cv2.circle(result_img, (int(bottom_center_x), height), 10, (255, 0, 255), -1) + 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 (Center Based)", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) - if 'best_score' in locals() and best_score != -1: - cv2.putText(result_img, f"Pair Score: {best_score:.2f}", (10, 85), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) - current_y_offset = 105 + 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"Center Slope: {center_slope:.2f}", (10, current_y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) - current_y_offset += 20 + 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.imshow("轨迹线检测结果", result_img) cv2.waitKey(delay) # 保存日志图像 @@ -1311,27 +1642,27 @@ def detect_center_based_dual_track_lines(image, observe=False, delay=1000, save_ os.makedirs(log_dir, exist_ok=True) # 保存结果图像 - result_img_path = os.path.join(log_dir, f"center_based_dual_track_{timestamp}.jpg") + 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"center_based_dual_track_orig_{timestamp}.jpg") + 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"保存双轨迹线检测结果图像到: {result_img_path}", "日志") info(f"保存原始图像到: {orig_img_path}", "日志") # 保存文本日志信息 log_info = { "timestamp": timestamp, - "center_point": (int(bottom_center_x), height), + "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}", "日志") + info(f"双轨迹线检测结果: {log_info}", "日志") # 创建左右轨迹线和中心线信息 left_track_info = { @@ -1347,11 +1678,11 @@ def detect_center_based_dual_track_lines(image, observe=False, delay=1000, save_ } center_info = { - "point": (int(bottom_center_x), height), + "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]) + "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