import cv2 import numpy as np import os import datetime 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 # TEST # 如果输入是字符串（文件路径），则加载图像 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) # origin image origin_image_path = os.path.join(log_dir, f"origin_horizontal_edge_{timestamp}.jpg") cv2.imwrite(origin_image_path, img) info(f"保存原始图像到: {origin_image_path}", "日志") 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: 输入图像，可以是文件路径或者已加载的图像数组 observe: 是否输出中间状态信息和可视化结果，默认为False delay: 展示每个步骤的等待时间(毫秒) save_log: 是否保存日志和图像 返回: edge_point: 赛道前方边缘点的坐标 (x, y) edge_info: 边缘信息字典 """ observe = False # TEST # 如果输入是字符串（文件路径），则加载图像 if isinstance(image, str): img = cv2.imread(image) else: img = image.copy() if img is None: error("无法加载图像", "失败") return None, None if save_log: timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f") origin_image_path = os.path.join("logs/image", f"origin_horizontal_edge_{timestamp}.jpg") cv2.imwrite(origin_image_path, img) info(f"保存原始图像到: {origin_image_path}", "日志") # 获取图像尺寸 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 # 定义合理的值范围 - 修改为更关注中上部区域 valid_y_range = (height * 0.1, height * 0.6) # 有效的y坐标范围（中上部分图像），扩大范围到90% max_slope = 0.2 # 最大允许斜率（接近水平） min_line_length = width * 0.2 # 最小线长度 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)) # 寻找每列的最顶部点（上边缘点） top_points = [] for col in non_zero_cols: col_points = np.where(search_mask[:, col] > 0)[0] if len(col_points) > 0: top_row = np.min(col_points) top_points.append((left_bound + col, top_bound + top_row)) if observe: debug("检测底部和顶部边缘点", "处理") edge_points_img = img.copy() for point in bottom_points: cv2.circle(edge_points_img, point, 3, (0, 255, 0), -1) for point in top_points: cv2.circle(edge_points_img, point, 3, (255, 0, 255), -1) cv2.imshow("边缘点", edge_points_img) cv2.waitKey(delay) # 边缘检测 edges = cv2.Canny(mask, 50, 150, apertureSize=3) if observe: debug("步骤4: 边缘检测", "处理") 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"步骤5: 检测到 {len(lines)} 条直线", "处理") print(f"len(lines): {len(lines)}") lines_img = img.copy() for i, line in enumerate(lines): x1, y1, x2, y2 = line[0] # 根据线段长度使用不同颜色 line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2) # 使用HSV颜色空间生成不同的颜色 hue = (i * 30) % 180 # 每30度一个颜色 color = cv2.cvtColor(np.uint8([[[hue, 255, 255]]]), cv2.COLOR_HSV2BGR)[0][0] color = (int(color[0]), int(color[1]), int(color[2])) cv2.line(lines_img, (x1, y1), (x2, y2), color, 2) cv2.putText(lines_img, f"{i}", ((x1+x2)//2, (y1+y2)//2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) cv2.imshow("检测到的直线", lines_img) cv2.waitKey(delay) # 过滤和合并相似的线段 filtered_lines = [] for line in lines: x1, y1, x2, y2 = line[0] # 确保x1 < x2 if x1 > x2: x1, x2 = x2, x1 y1, y2 = y2, y1 filtered_lines.append([x1, y1, x2, y2]) # 合并相似线段 merged_lines = [] used_indices = set() for i, line1 in enumerate(filtered_lines): if i in used_indices: continue x1, y1, x2, y2 = line1 similar_lines = [line1] used_indices.add(i) # 查找与当前线段相似的其他线段 for j, line2 in enumerate(filtered_lines): if j in used_indices or i == j: continue x3, y3, x4, y4 = line2 # 计算两条线段的斜率 slope1 = (y2 - y1) / (x2 - x1) if abs(x2 - x1) > 5 else 100 slope2 = (y4 - y3) / (x4 - x3) if abs(x4 - x3) > 5 else 100 # 计算两条线段的中点 mid1_x, mid1_y = (x1 + x2) / 2, (y1 + y2) / 2 mid2_x, mid2_y = (x3 + x4) / 2, (y3 + y4) / 2 # 计算中点之间的距离 mid_dist = np.sqrt((mid2_x - mid1_x)**2 + (mid2_y - mid1_y)**2) # 计算线段端点之间的最小距离 end_dists = [ np.sqrt((x1-x3)**2 + (y1-y3)**2), np.sqrt((x1-x4)**2 + (y1-y4)**2), np.sqrt((x2-x3)**2 + (y2-y3)**2), np.sqrt((x2-x4)**2 + (y2-y4)**2) ] min_end_dist = min(end_dists) # 判断两条线段是否相似：满足以下条件之一 # 1. 斜率接近且中点距离不太远 # 2. 斜率接近且端点之间距离很近（可能是连接的线段） # 3. 端点非常接近（几乎连接），且斜率差异不太大 if (abs(slope1 - slope2) < 0.15 and mid_dist < height * 0.15) or \ (abs(slope1 - slope2) < 0.1 and min_end_dist < height * 0.05) or \ (min_end_dist < height * 0.03 and abs(slope1 - slope2) < 0.25): similar_lines.append(line2) used_indices.add(j) # 如果找到相似线段，合并它们 if len(similar_lines) > 1: # 合并所有相似线段的端点 all_points = [] for line in similar_lines: all_points.append((line[0], line[1])) # 起点 all_points.append((line[2], line[3])) # 终点 # 找出x坐标的最小值和最大值 min_x = min(p[0] for p in all_points) max_x = max(p[0] for p in all_points) # 使用所有点拟合一条直线 x_points = np.array([p[0] for p in all_points]).reshape(-1, 1) y_points = np.array([p[1] for p in all_points]) # 使用RANSAC拟合更稳定的直线 ransac = linear_model.RANSACRegressor(residual_threshold=5.0) ransac.fit(x_points, y_points) # 获取拟合的斜率和截距 merged_slope = ransac.estimator_.coef_[0] merged_intercept = ransac.estimator_.intercept_ # 计算新的端点 y_min = int(merged_slope * min_x + merged_intercept) y_max = int(merged_slope * max_x + merged_intercept) # 添加合并后的线段 merged_lines.append([min_x, y_min, max_x, y_max]) else: # 如果没有相似线段，直接添加原线段 merged_lines.append(line1) # 将合并后的线段转换为霍夫变换的格式 merged_hough_lines = [] for line in merged_lines: merged_hough_lines.append(np.array([[line[0], line[1], line[2], line[3]]])) if observe: debug(f"步骤5.1: 合并后剩余 {len(merged_hough_lines)} 条线", "处理") merged_img = img.copy() for i, line in enumerate(merged_hough_lines): x1, y1, x2, y2 = line[0] # 使用HSV颜色空间生成不同的颜色 hue = (i * 50) % 180 # 每50度一个颜色 color = cv2.cvtColor(np.uint8([[[hue, 255, 255]]]), cv2.COLOR_HSV2BGR)[0][0] color = (int(color[0]), int(color[1]), int(color[2])) cv2.line(merged_img, (x1, y1), (x2, y2), color, 3) # 显示线段编号 cv2.putText(merged_img, f"{i}", ((x1+x2)//2, (y1+y2)//2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) cv2.imshow("合并后的线段", merged_img) cv2.waitKey(delay) # 使用合并后的线段继续处理 lines = merged_hough_lines # 筛选水平线，但放宽斜率条件 horizontal_lines = [] # 分别存储上方和下方的水平线 lower_horizontal_lines = [] upper_horizontal_lines = [] # 定义上下分界线位置 - 修改为图像的60%处，使上方区域更大 lower_upper_boundary = height * 0.6 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) < max_slope: # 确保线在搜索区域内 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) # 过滤掉短线段 if line_length >= min_line_length: # 计算线段在图像中的位置得分 # 修改：更偏好中部和上部的线段，使用高斯函数来优化位置评分 optimal_y = height * 0.45 # 最佳高度在图像45%处 position_score = np.exp(-0.5 * ((mid_y - optimal_y) / (height * 0.2))**2) # 计算长度得分（越长越好） length_score = min(1.0, line_length / (width * 0.5)) # 计算斜率得分（越水平越好） slope_score = max(0.0, 1.0 - abs(slope) / max_slope) # 计算线段位于图像中央的程度 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.5 + length_score * 0.15 + slope_score * 0.25 + center_score * 0.1 # 保存线段、其y坐标、斜率、长度和质量得分 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 observe: print(f"horizontal_lines: {horizontal_lines}") 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)} 条水平线 (下方: {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 # 根据得分调整线的颜色，得分越高越绿 color = (int(255 * (1-score)), int(255 * score), 0) cv2.line(h_lines_img, (x1, y1), (x2, y2), color, 2) # 显示斜率和得分 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) # 修改：优先选择上方的线，如果没有上方的线才考虑下方的线 if 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("选择上方水平线", "处理") elif 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("没有合适的上方线，选择下方水平线", "处理") else: # 理论上不会进入这个分支，因为前面已经检查过horizontal_lines非空 if observe: error("未检测到合格的水平线", "失败") return None, None # 提取线段端点 x1, y1, x2, y2 = selected_line # 确保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) # 如果得分过低，可能是错误识别，尝试使用边缘点拟合 if selected_score < 0.4 and len(top_points) >= 5: # 修改：优先使用顶部点进行拟合 if observe: debug(f"线段质量得分过低: {selected_score:.2f}，尝试使用边缘点拟合", "处理") # 筛选中上部分的点 valid_points = [p for p in top_points if valid_y_range[0] <= p[1] <= valid_y_range[1]] if len(valid_points) >= 5: # 使用RANSAC拟合直线以去除异常值 x_points = np.array([p[0] for p in valid_points]).reshape(-1, 1) y_points = np.array([p[1] for p in valid_points]) ransac = linear_model.RANSACRegressor(residual_threshold=5.0) ransac.fit(x_points, y_points) # 获取拟合参数 fitted_slope = ransac.estimator_.coef_[0] intercept = ransac.estimator_.intercept_ # 检查斜率是否在合理范围内 if abs(fitted_slope) < max_slope: # 计算拟合线的inliers比例 inlier_mask = ransac.inlier_mask_ inlier_ratio = sum(inlier_mask) / len(inlier_mask) # 如果有足够的内点，使用拟合的直线 if inlier_ratio > 0.5: # 使用拟合的直线参数计算线段端点 x1 = left_bound y1 = int(fitted_slope * x1 + intercept) x2 = right_bound y2 = int(fitted_slope * x2 + intercept) selected_slope = fitted_slope selected_line = [x1, y1, x2, y2] # 重新计算边缘点 if y1 > y2: bottom_edge_point = (x1, y1) else: bottom_edge_point = (x2, y2) if observe: debug(f"使用拟合直线，斜率: {fitted_slope:.4f}, 内点比例: {inlier_ratio:.2f}", "处理") fitted_line_img = img.copy() cv2.line(fitted_line_img, (x1, y1), (x2, y2), (0, 255, 255), 2) for i, point in enumerate(valid_points): color = (0, 255, 0) if inlier_mask[i] else (0, 0, 255) cv2.circle(fitted_line_img, point, 3, color, -1) cv2.imshow("拟合线和内点", fitted_line_img) cv2.waitKey(delay) # 获取线上的更多点 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)) # 对结果进行合理性检查 valid_result = True reason = "" # 修改：调整有效范围检查，适应中上部分的线 if not (valid_y_range[0] <= bottom_edge_point[1] <= valid_y_range[1]): # 如果线在图像更上方，只要不太高也可以接受 if bottom_edge_point[1] < valid_y_range[0] and bottom_edge_point[1] > height * 0.2: pass # 接受较高的线 else: valid_result = False reason += "边缘点y坐标超出有效范围; " # 检查线段长度 line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2) if line_length < min_line_length: valid_result = False reason += "线段长度不足; " # 检查是否有足够的点 if len(selected_points) < 5: valid_result = False reason += "选定点数量不足; " # 计算这个点到中线的距离 distance_to_center = bottom_edge_point[0] - center_x # 检查到中心的距离是否合理 if abs(distance_to_center) > width * 0.8: valid_result = False reason += "到中心距离过大; " # 计算中线与检测到的横向线的交点 # 横向线方程: 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)) # 修改：放宽交点y坐标的有效范围检查 if intersection_y < height * 0.2 or intersection_y > height * 0.95: valid_result = False reason += "交点y坐标超出有效范围; " # 计算交点到图像底部的距离（以像素为单位） distance_to_bottom = height - intersection_y # 如果结果无效，可能需要返回失败 if not valid_result and observe: warning(f"检测结果不合理: {reason}", "警告") result_img = None if observe or save_log: slope_img = img.copy() # 画出检测到的线 line_color = (0, 255, 0) if valid_result else (0, 0, 255) cv2.line(slope_img, (x1, y1), (x2, y2), line_color, 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.line(slope_img, (0, int(lower_upper_boundary)), (width, int(lower_upper_boundary)), (255, 0, 255), 1) # 画出有效高度范围 cv2.line(slope_img, (0, int(valid_y_range[0])), (width, int(valid_y_range[0])), (255, 255, 0), 1) cv2.line(slope_img, (0, int(valid_y_range[1])), (width, int(valid_y_range[1])), (255, 255, 0), 1) cv2.putText(slope_img, f"Slope: {selected_slope:.4f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, line_color, 2) cv2.putText(slope_img, f"Distance to center: {distance_to_center}px", (10, 70), cv2.FONT_HERSHEY_SIMPLEX, 1, line_color, 2) cv2.putText(slope_img, f"Distance to bottom: {distance_to_bottom:.1f}px", (10, 110), cv2.FONT_HERSHEY_SIMPLEX, 1, line_color, 2) cv2.putText(slope_img, f"中线交点: ({intersection_point[0]}, {intersection_point[1]})", (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 1, line_color, 2) cv2.putText(slope_img, f"质量得分: {selected_score:.2f}", (10, 190), cv2.FONT_HERSHEY_SIMPLEX, 1, line_color, 2) if not valid_result: cv2.putText(slope_img, f"警告: {reason}", (10, 230), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2) if observe: debug("显示边缘斜率和中线交点", "显示") cv2.imshow("边缘斜率和中线交点", slope_img) cv2.waitKey(delay) result_img = slope_img # 保存日志图像 if save_log: timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f") log_dir = "logs/image" os.makedirs(log_dir, exist_ok=True) # 保存原图 origin_image_path = os.path.join(log_dir, f"origin_horizontal_edge_{timestamp}.jpg") cv2.imwrite(origin_image_path, img) info(f"保存原始图像到: {origin_image_path}", "日志") if result_img is not None: 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, "score": selected_score, "valid": valid_result, "reason": reason if not valid_result else "", "is_upper_line": len(upper_horizontal_lines) > 0 and selected_line == upper_horizontal_lines[0][0] } info(f"横向边缘检测结果: {log_info}", "日志") # 如果结果无效，但检测到了一些线，仍然返回结果，不拒绝太靠近底部的线 if not valid_result and "边缘点y坐标超出有效范围" in reason and bottom_edge_point[1] > height * 0.8: # 仍然接受靠近底部的线 valid_result = True reason = "" # 如果结果无效，可能需要返回失败 if not valid_result: return None, None # 创建边缘信息字典 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, # 交点到图像底部的距离 "score": selected_score, # 线段质量得分 "is_upper_line": len(upper_horizontal_lines) > 0 and selected_line == upper_horizontal_lines[0][0] # 是否为上方线 # "points": selected_points # 添加选定的点组 } return bottom_edge_point, edge_info def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True): """ 检测左右两条平行的黄色轨道线 参数: image: 输入图像，可以是文件路径或者已加载的图像数组 observe: 是否输出中间状态信息和可视化结果，默认为False delay: 展示每个步骤的等待时间(毫秒) save_log: 是否保存日志和图像 返回: tuple: (中心线信息, 左轨迹线信息, 右轨迹线信息) """ # 如果输入是字符串（文件路径），则加载图像 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) # 形态学操作以改善掩码 kernel = np.ones((5, 5), np.uint8) # 增大kernel尺寸 mask = cv2.dilate(mask, kernel, iterations=1) mask = cv2.erode(mask, np.ones((3, 3), np.uint8), iterations=1) # 添加腐蚀操作去除噪点 if observe: debug("步骤1: 创建黄色掩码", "处理") cv2.imshow("黄色掩码", mask) cv2.waitKey(delay) # 裁剪底部区域重点关注近处的黄线 bottom_roi_height = int(height * 0.4) # 关注图像底部40%区域 bottom_roi = mask[height-bottom_roi_height:, :] if observe: debug("步骤1.5: 底部区域掩码", "处理") cv2.imshow("底部区域掩码", bottom_roi) cv2.waitKey(delay) # 边缘检测 edges = cv2.Canny(mask, 50, 150, apertureSize=3) if observe: debug("步骤2: 边缘检测", "处理") cv2.imshow("边缘检测", edges) cv2.waitKey(delay) # 霍夫变换检测直线 - 降低minLineLength以检测到较短的线段 lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=25, minLineLength=width*0.05, maxLineGap=40) # 更宽松的参数 if lines is None or len(lines) == 0: error("未检测到直线", "失败") return None, None, None if observe: debug(f"步骤3: 检测到 {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) # 筛选近似垂直的线 vertical_lines = [] for line in lines: x1, y1, x2, y2 = line[0] # 优先选择图像底部的线 if y1 < height * 0.5 and y2 < height * 0.5: continue # 忽略上半部分的线 # 计算斜率 (避免除零错误) if abs(x2 - x1) < 5: # 几乎垂直的线 slope = 100 # 设置一个较大的值表示接近垂直 else: slope = (y2 - y1) / (x2 - x1) # 筛选接近垂直的线 (斜率较大)，但允许更多倾斜度 if abs(slope) > 0.75: # 降低垂直线的斜率阈值，允许更多倾斜的线 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: error("未检测到足够的垂直线", "失败") return None, None, None if observe: debug(f"步骤4: 找到 {len(vertical_lines)} 条垂直线", "处理") v_lines_img = img.copy() 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) # 优先选择更接近图像底部的线 - 根据y坐标均值排序 vertical_lines.sort(key=lambda x: x[2], reverse=True) # 按mid_y从大到小排序 # 按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 # 从左右两组中各选择一条最佳的线 # 优先选择同时满足：1. 更靠近底部 2. 足够长 3. 更接近中心的线 def score_line(line_info, is_left): _, mid_x, mid_y, _, length = line_info # y越大（越靠近底部）分数越高 y_score = mid_y / height # 线越长分数越高 length_score = min(1.0, length / (height * 0.3)) # 与预期位置的接近程度 expected_x = center_x * 0.3 if is_left else center_x * 1.7 x_score = 1.0 - min(1.0, abs(mid_x - expected_x) / (center_x * 0.5)) # 综合评分 return y_score * 0.5 + length_score * 0.3 + x_score * 0.2 # 对左右线组进行评分并排序 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] right_line = right_lines[0] # 获取两条线的坐标 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 # 计算中心线 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 = int(center_line_x1 + (height - center_line_y1) / center_slope) center_point = (bottom_x, height) # 计算中心线与图像中心线的偏差 deviation = bottom_x - center_x result_img = None if observe or save_log: result_img = img.copy() # 绘制左右轨迹线 cv2.line(result_img, (left_x1, left_y1), (left_x2, left_y2), (255, 0, 0), 2) cv2.line(result_img, (right_x1, right_y1), (right_x2, right_y2), (0, 0, 255), 2) # 绘制中心线 cv2.line(result_img, (center_line_x1, center_line_y1), (center_line_x2, center_line_y2), (0, 255, 0), 2) cv2.line(result_img, (center_line_x2, center_line_y2), center_point, (0, 255, 0), 2) # 绘制图像中心线 cv2.line(result_img, (center_x, 0), (center_x, height), (0, 0, 255), 1) # 标记中心点 cv2.circle(result_img, center_point, 10, (255, 0, 255), -1) # 显示偏差信息 cv2.putText(result_img, f"Deviation: {deviation}px", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) 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) img_path = os.path.join(log_dir, f"dual_track_{timestamp}.jpg") cv2.imwrite(img_path, result_img) info(f"保存双轨迹线检测结果图像到: {img_path}", "日志") # 保存文本日志信息 log_info = { "timestamp": timestamp, "center_point": center_point, "deviation": deviation, "left_track_mid_x": left_line[1], "right_track_mid_x": right_line[1], "track_width": right_line[1] - left_line[1], "center_slope": center_slope } info(f"双轨迹线检测结果: {log_info}", "日志") # 创建左右轨迹线和中心线信息 left_track_info = { "line": left_line[0], "slope": left_line[3], # 注意：索引更改为3，因为保存结构更改了 "x_mid": left_line[1] } right_track_info = { "line": right_line[0], "slope": right_line[3], # 注意：索引更改为3 "x_mid": right_line[1] } center_info = { "point": center_point, "deviation": deviation, "slope": center_slope, "is_vertical": abs(center_slope) > 5.0, # 判断是否接近垂直 "track_width": right_line[1] - left_line[1] # 两轨迹线之间的距离 } return center_info, left_track_info, right_track_info def detect_left_side_track(image, observe=False, delay=1000, save_log=True): """ 检测视野左侧黄色轨道线，用于机器狗左侧靠线移动 参数: image: 输入图像，可以是文件路径或者已加载的图像数组 observe: 是否输出中间状态信息和可视化结果，默认为False delay: 展示每个步骤的等待时间(毫秒) save_log: 是否保存日志和图像 返回: tuple: (线信息字典, 最佳跟踪点) """ # 如果输入是字符串（文件路径），则加载图像 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 # 主要关注视野的左半部分，但稍微扩大一点以确保捕捉到左侧线 left_region_width = int(center_x * 1.2) # 扩大左侧搜索区域 left_bound = 0 right_bound = min(width, left_region_width) bottom_bound = height top_bound = 0 if observe: debug("步骤1: 原始图像已加载", "加载") region_img = img.copy() # 绘制左侧搜索区域 cv2.rectangle(region_img, (left_bound, top_bound), (right_bound, bottom_bound), (255, 0, 0), 2) cv2.line(region_img, (center_x, 0), (center_x, height), (0, 0, 255), 2) # 中线 cv2.imshow("左侧搜索区域", region_img) cv2.waitKey(delay) # 转换到HSV颜色空间以便更容易提取黄色 hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) # 黄色的HSV范围 - 进一步扩大范围以适应不同光照条件 lower_yellow = np.array([12, 70, 70]) # 更宽松的黄色下限 upper_yellow = np.array([38, 255, 255]) # 更宽松的黄色上限 # 创建黄色的掩码 mask = cv2.inRange(hsv, lower_yellow, upper_yellow) # 形态学操作以改善掩码 kernel = np.ones((5, 5), np.uint8) mask = cv2.dilate(mask, kernel, iterations=1) mask = cv2.erode(mask, np.ones((3, 3), np.uint8), iterations=1) if observe: debug("步骤2: 创建黄色掩码", "处理") cv2.imshow("黄色掩码", mask) cv2.waitKey(delay) # 裁剪左侧区域 left_region_mask = mask[:, left_bound:right_bound] if observe: debug("步骤3: 左侧区域掩码", "处理") cv2.imshow("左侧区域掩码", left_region_mask) cv2.waitKey(delay) # 边缘检测 edges = cv2.Canny(mask, 50, 150, apertureSize=3) if observe: debug("步骤4: 边缘检测", "处理") cv2.imshow("边缘检测", edges) cv2.waitKey(delay) # 霍夫变换检测直线 - 降低minLineLength以更好地检测近距离的线段 lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=20, minLineLength=height*0.1, maxLineGap=60) # 调整参数以检测更短的线段 if lines is None or len(lines) == 0: error("未检测到直线", "失败") return None, None if observe: debug(f"步骤5: 检测到 {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) # 筛选左侧区域内的近似垂直线，放宽条件以捕获更多可能的线 left_vertical_lines = [] for line in lines: x1, y1, x2, y2 = line[0] # 确保线在左侧区域内 if not (max(x1, x2) <= right_bound): continue # 计算斜率 (避免除零错误) if abs(x2 - x1) < 5: # 几乎垂直的线 slope = 100 # 设置一个较大的值表示接近垂直 else: slope = (y2 - y1) / (x2 - x1) # 放宽垂直线的斜率范围，以适应近距离时线的倾斜 if abs(slope) > 0.5: # 降低垂直线斜率阈值 line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2) # 计算线的中点坐标 mid_x = (x1 + x2) / 2 mid_y = (y1 + y2) / 2 # 保存线段、其坐标、斜率和长度 left_vertical_lines.append((line[0], mid_x, mid_y, slope, line_length)) if len(left_vertical_lines) == 0: error("左侧区域未检测到垂直线", "失败") return None, None if observe: debug(f"步骤6: 左侧区域找到 {len(left_vertical_lines)} 条垂直线", "处理") left_lines_img = img.copy() for line_info in left_vertical_lines: line, _, _, slope, _ = line_info x1, y1, x2, y2 = line cv2.line(left_lines_img, (x1, y1), (x2, y2), (0, 255, 255), 2) # 显示斜率 cv2.putText(left_lines_img, f"{slope:.2f}", ((x1+x2)//2, (y1+y2)//2), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1) cv2.imshow("左侧垂直线", left_lines_img) cv2.waitKey(delay) # 修改评分函数，优先选择最左侧的线 def score_left_line(line_info): _, mid_x, mid_y, slope, length = line_info # 线段越长分数越高，但降低权重 length_score = min(1.0, length / (height * 0.3)) # 越靠近左边分数越高，增加权重 position_score = 1.0 - (mid_x / center_x) # 优先选择在图像下半部分的线段 height_score = min(1.0, mid_y / (height * 0.6)) # 斜率得分，垂直度越高越好 slope_score = min(1.0, abs(slope) / 20) if abs(slope) < 100 else 1.0 # 综合评分，加大位置权重 return length_score * 0.2 + position_score * 0.5 + height_score * 0.2 + slope_score * 0.1 # 对线段进行评分并排序 left_vertical_lines = sorted(left_vertical_lines, key=score_left_line, reverse=True) # 选择最佳的左侧线段 best_left_line = left_vertical_lines[0] line, mid_x, mid_y, slope, length = best_left_line x1, y1, x2, y2 = line # 确保线段的顺序是从上到下 if y1 > y2: x1, x2 = x2, x1 y1, y2 = y2, y1 # 计算最佳跟踪点 - 选择线段底部较靠近机器人的点 tracking_point = (x2, y2) if y2 > y1 else (x1, y1) # 计算线与地面的交点 if abs(slope) < 0.01: # 几乎垂直 ground_intersection_x = x1 else: ground_intersection_x = x1 + (height - y1) / slope ground_intersection = (int(ground_intersection_x), height) # 计算线与图像左边界的距离（以像素为单位） distance_to_left = mid_x result_img = None if observe or save_log: result_img = img.copy() # 绘制检测到的最佳左侧线 cv2.line(result_img, (x1, y1), (x2, y2), (255, 0, 0), 2) # 绘制图像中线 cv2.line(result_img, (center_x, 0), (center_x, height), (0, 0, 255), 1) # 标记最佳跟踪点和地面交点 cv2.circle(result_img, tracking_point, 10, (0, 255, 0), -1) cv2.circle(result_img, ground_intersection, 10, (0, 0, 255), -1) # 显示信息 cv2.putText(result_img, f"斜率: {slope:.2f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) cv2.putText(result_img, f"距左边界: {distance_to_left:.1f}px", (10, 70), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) cv2.putText(result_img, f"地面交点: ({ground_intersection[0]}, {ground_intersection[1]})", (10, 110), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) if observe: debug("步骤7: 左侧最佳跟踪线和点", "显示") 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) # 保存原图 original_img_path = os.path.join(log_dir, f"original_{timestamp}.jpg") cv2.imwrite(original_img_path, img) info(f"保存原始图像到: {original_img_path}", "日志") img_path = os.path.join(log_dir, f"left_track_{timestamp}.jpg") cv2.imwrite(img_path, result_img) info(f"保存左侧轨迹线检测结果图像到: {img_path}", "日志") # 保存文本日志信息 log_info = { "timestamp": timestamp, "tracking_point": tracking_point, "ground_intersection": ground_intersection, "distance_to_left": distance_to_left, "slope": slope, "line_mid_x": mid_x } info(f"左侧轨迹线检测结果: {log_info}", "日志") # 创建线段信息字典 track_info = { "line": line, "slope": slope, "tracking_point": tracking_point, "ground_intersection": ground_intersection, "distance_to_left": distance_to_left, "mid_x": mid_x, "mid_y": mid_y, "is_vertical": abs(slope) > 5.0 # 判断是否接近垂直 } return track_info, tracking_point