1654 lines
67 KiB
Python
1654 lines
67 KiB
Python
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
|
||
|
||
# 获取图像尺寸
|
||
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.5, height) # 有效的y坐标范围（下半部分图像）
|
||
max_slope = 0.15 # 最大允许斜率（接近水平）
|
||
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=25,
|
||
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)} 条直线", "处理")
|
||
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 = []
|
||
# 分别存储上方和下方的水平线
|
||
lower_horizontal_lines = []
|
||
upper_horizontal_lines = []
|
||
|
||
# 定义上下分界线位置 (以图像中部再下移一点作为分界)
|
||
lower_upper_boundary = height * 0.65
|
||
|
||
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 and mid_y >= valid_y_range[0]:
|
||
# 计算线段在图像中的位置得分（越靠近底部得分越高）
|
||
position_score = min(1.0, (mid_y - valid_y_range[0]) / (valid_y_range[1] - valid_y_range[0]))
|
||
|
||
# 计算长度得分（越长越好）
|
||
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.6 + length_score * 0.2 + slope_score * 0.15 + center_score * 0.05
|
||
|
||
# 保存线段、其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 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 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
|
||
|
||
# 确保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(bottom_points) >= 5:
|
||
if observe:
|
||
debug(f"线段质量得分过低: {selected_score:.2f}，尝试使用边缘点拟合", "处理")
|
||
|
||
# 筛选下半部分的点
|
||
valid_bottom_points = [p for p in bottom_points if p[1] >= valid_y_range[0]]
|
||
|
||
if len(valid_bottom_points) >= 5:
|
||
# 使用RANSAC拟合直线以去除异常值
|
||
x_points = np.array([p[0] for p in valid_bottom_points]).reshape(-1, 1)
|
||
y_points = np.array([p[1] for p in valid_bottom_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_bottom_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]):
|
||
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 not (valid_y_range[0] <= intersection_y <= valid_y_range[1]):
|
||
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.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 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_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,
|
||
"score": selected_score,
|
||
"valid": valid_result,
|
||
"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}", "日志")
|
||
|
||
# 如果结果无效，可能需要返回失败
|
||
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_lower_line": len(lower_horizontal_lines) > 0 and selected_line == lower_horizontal_lines[0][0] # 是否为下方线
|
||
# "points": selected_points # 添加选定的点组
|
||
}
|
||
|
||
return bottom_edge_point, edge_info
|
||
|
||
def detect_horizontal_track_edge_v3(image, observe=False, delay=1000, save_log=True):
|
||
"""
|
||
检测正前方横向黄色赛道的边缘，并返回y值最大的边缘点
|
||
优先检测中部和上部的横线，特别是对于远处的横线
|
||
|
||
参数:
|
||
image: 输入图像，可以是文件路径或者已加载的图像数组
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
delay: 展示每个步骤的等待时间(毫秒)
|
||
save_log: 是否保存日志和图像
|
||
返回:
|
||
edge_point: 赛道前方边缘点的坐标 (x, y)
|
||
edge_info: 边缘信息字典
|
||
"""
|
||
observe = False
|
||
# 如果输入是字符串（文件路径），则加载图像
|
||
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.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 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_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_left_side_track(image, observe=False, delay=1000, save_log=True):
|
||
"""
|
||
检测视野左侧黄色轨道线，用于机器狗左侧靠线移动
|
||
|
||
参数:
|
||
image: 输入图像，可以是文件路径或者已加载的图像数组
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
delay: 展示每个步骤的等待时间(毫秒)
|
||
save_log: 是否保存日志和图像
|
||
|
||
返回:
|
||
tuple: (线信息字典, 最佳跟踪点)
|
||
"""
|
||
observe = False
|
||
# 如果输入是字符串（文件路径），则加载图像
|
||
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))
|
||
|
||
# 综合评分，加大位置权重
|
||
return length_score * 0.2 + position_score * 0.5 + height_score * 0.2
|
||
|
||
# 对线段进行评分并排序
|
||
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
|