1411 lines
62 KiB
Python
1411 lines
62 KiB
Python
import cv2
|
||
import numpy as np
|
||
import os
|
||
import datetime
|
||
|
||
from utils.base_line_handler import _merge_collinear_lines_iterative
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
|
||
def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True,
|
||
min_slope_threshold=0.4,
|
||
min_line_length=0.05,
|
||
max_line_gap=40):
|
||
"""
|
||
检测左右两条平行的黄色轨道线，优化后能够更准确处理各种路况
|
||
|
||
参数:
|
||
image: 输入图像，可以是文件路径或者已加载的图像数组
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
delay: 展示每个步骤的等待时间(毫秒)
|
||
save_log: 是否保存日志和图像
|
||
|
||
min_slope_threshold: 最小斜率阈值
|
||
min_line_length: 最小线段长度
|
||
max_line_gap: 最大线段间距
|
||
|
||
返回:
|
||
tuple: (中心线信息, 左轨迹线信息, 右轨迹线信息)
|
||
如果检测失败返回(None, None, None)
|
||
|
||
改进:
|
||
- 使用多点采样+多项式拟合计算更准确的中心线
|
||
- 优化对左右轨道线的选择，考虑平行性和合理宽度
|
||
- 增强对倾斜轨道和石板路的处理能力
|
||
"""
|
||
# 如果输入是字符串（文件路径），则加载图像
|
||
if isinstance(image, str):
|
||
img = cv2.imread(image)
|
||
else:
|
||
img = image.copy()
|
||
|
||
if img is None:
|
||
error("无法加载图像", "失败")
|
||
return None, None, None
|
||
|
||
# 获取图像尺寸
|
||
height, width = img.shape[:2]
|
||
|
||
# 计算图像中间区域的范围
|
||
center_x = width // 2
|
||
|
||
# 转换到HSV颜色空间以便更容易提取黄色
|
||
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
||
|
||
# 标准黄色的HSV范围
|
||
lower_yellow = np.array([15, 80, 80])
|
||
upper_yellow = np.array([35, 255, 255])
|
||
|
||
# 创建黄色的掩码
|
||
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
|
||
|
||
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("步骤1: 创建黄色掩码", "处理")
|
||
mask_display = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
|
||
cv2.putText(mask_display, "Step 1: Yellow Mask", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
|
||
cv2.putText(mask_display, f"Lower: {lower_yellow}", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
|
||
cv2.putText(mask_display, f"Upper: {upper_yellow}", (10, 55), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
|
||
cv2.imshow("黄色掩码", mask_display)
|
||
cv2.waitKey(delay)
|
||
|
||
# 裁剪底部区域重点关注近处的黄线
|
||
bottom_roi_height = int(height * 0.4) # 增加关注区域到图像底部60%
|
||
bottom_roi = mask[height-bottom_roi_height:, :]
|
||
|
||
if observe:
|
||
debug("步骤1.5: 底部区域掩码", "处理")
|
||
bottom_roi_display = cv2.cvtColor(bottom_roi, cv2.COLOR_GRAY2BGR)
|
||
cv2.putText(bottom_roi_display, "Step 1.5: Bottom ROI", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
|
||
cv2.putText(bottom_roi_display, f"ROI Height: {bottom_roi_height}px ({bottom_roi_height/height*100:.0f}%)", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
|
||
cv2.imshow("底部区域掩码", bottom_roi_display)
|
||
cv2.waitKey(delay)
|
||
|
||
# INFO 边缘检测
|
||
edges = cv2.Canny(mask, 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)
|
||
cv2.putText(edges_display, "Thresholds: (50, 150)", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
|
||
cv2.imshow("边缘检测", edges_display)
|
||
cv2.waitKey(delay)
|
||
|
||
# INFO 霍夫变换检测直线
|
||
lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=25,
|
||
minLineLength=width*min_line_length, maxLineGap=max_line_gap)
|
||
|
||
if lines is None or len(lines) == 0:
|
||
error("未检测到直线", "失败")
|
||
return None, None, None
|
||
|
||
if observe:
|
||
debug(f"步骤3: 检测到 {len(lines)} 条直线", "处理")
|
||
lines_img = img.copy()
|
||
cv2.putText(lines_img, "Step 3: Hough Lines", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||
cv2.putText(lines_img, f"Th:25, MinLen:{width*0.05:.1f}, MaxGap:{max_line_gap}", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 255), 1)
|
||
for line in lines:
|
||
x1, y1, x2, y2 = line[0]
|
||
cv2.line(lines_img, (x1, y1), (x2, y2), (0, 255, 0), 2)
|
||
cv2.imshow("检测到的直线", lines_img)
|
||
cv2.waitKey(delay)
|
||
|
||
# 筛选近似垂直的线
|
||
vertical_only_lines = []
|
||
for line in lines:
|
||
x1, y1, x2, y2 = line[0]
|
||
|
||
# 优先选择图像底部的线
|
||
if y1 < height * 0.4 and y2 < height * 0.4:
|
||
continue # 忽略上半部分的线
|
||
|
||
# 计算斜率 (避免除零错误)
|
||
if abs(x2 - x1) < 5: # 几乎垂直的线
|
||
slope = 100 # 设置一个较大的值表示接近垂直
|
||
else:
|
||
slope = (y2 - y1) / (x2 - x1)
|
||
|
||
# 筛选接近垂直的线 (斜率较大)，但允许更多倾斜度
|
||
if abs(slope) > min_slope_threshold:
|
||
vertical_only_lines.append(line[0])
|
||
|
||
if observe:
|
||
debug(f"步骤3.2: 筛选出 {len(vertical_only_lines)} 条垂直候选线 (合并前)", "可视化")
|
||
pre_merge_lines_img = img.copy()
|
||
cv2.putText(pre_merge_lines_img, "Step 3.2: Vertical Candidates (Pre-Merge)", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
|
||
colors = [(0, 0, 255), (0, 255, 0), (0, 255, 255), (255, 0, 0), (255, 0, 255), (255, 255, 0)]
|
||
for i, line in enumerate(vertical_only_lines):
|
||
x1, y1, x2, y2 = line
|
||
cv2.line(pre_merge_lines_img, (x1, y1), (x2, y2), colors[i % len(colors)], 2) # 使用红色显示合并前的线
|
||
cv2.imshow("合并前的垂直候选线", pre_merge_lines_img)
|
||
cv2.waitKey(delay)
|
||
|
||
if observe:
|
||
vertical_only_lines_tmp = vertical_only_lines.copy()
|
||
|
||
vertical_only_lines = _merge_collinear_lines_iterative(vertical_only_lines,
|
||
min_initial_len=5.0, # 最小初始线段长度
|
||
max_angle_diff_deg=4.0, # 最大允许角度差（度）
|
||
max_ep_gap_abs=max_line_gap, # 端点间最大绝对距离
|
||
max_ep_gap_factor=1, # 端点间最大相对距离因子
|
||
max_p_dist_abs=max_line_gap, # 点到线段最大绝对距离
|
||
max_p_dist_factor=1) # 点到线段最大相对距离因子
|
||
if observe:
|
||
print(f"合并前: {len(vertical_only_lines_tmp)} 条线, 合并后: {len(vertical_only_lines)} 条线")
|
||
debug(f"步骤3.5: 合并筛选出 {len(vertical_only_lines)} 条垂直候选线 (合并后)", "可视化")
|
||
|
||
# 创建两个图像用于对比显示
|
||
pre_merge_img = img.copy()
|
||
post_merge_img = img.copy()
|
||
|
||
# 在两张图上添加标题
|
||
cv2.putText(pre_merge_img, "合并前的垂直候选线", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
|
||
cv2.putText(post_merge_img, "合并后的垂直候选线", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
|
||
|
||
# 为每条线分配不同的颜色
|
||
colors = [
|
||
(255, 0, 0), # 蓝色
|
||
(0, 255, 0), # 绿色
|
||
(0, 0, 255), # 红色
|
||
(255, 255, 0), # 青色
|
||
(255, 0, 255), # 品红
|
||
(0, 255, 255), # 黄色
|
||
]
|
||
|
||
# 绘制合并前的线
|
||
for i, line in enumerate(vertical_only_lines_tmp):
|
||
x1, y1, x2, y2 = line
|
||
color = colors[i % len(colors)]
|
||
cv2.line(pre_merge_img, (x1, y1), (x2, y2), color, 2)
|
||
mid_x = (x1 + x2) // 2
|
||
mid_y = (y1 + y2) // 2
|
||
cv2.putText(pre_merge_img, str(i+1), (mid_x, mid_y),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)
|
||
|
||
# 绘制合并后的线
|
||
for i, line in enumerate(vertical_only_lines):
|
||
x1, y1, x2, y2 = line[0]
|
||
color = colors[i % len(colors)]
|
||
cv2.line(post_merge_img, (x1, y1), (x2, y2), color, 2)
|
||
mid_x = (x1 + x2) // 2
|
||
mid_y = (y1 + y2) // 2
|
||
cv2.putText(post_merge_img, str(i+1), (mid_x, mid_y),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)
|
||
|
||
# 水平拼接两张图片
|
||
combined_img = np.hstack((pre_merge_img, post_merge_img))
|
||
cv2.imshow("合并前后的垂直候选线对比", combined_img)
|
||
cv2.waitKey(delay)
|
||
|
||
vertical_lines = []
|
||
for line in vertical_only_lines:
|
||
x1, y1, x2, y2 = line[0]
|
||
|
||
# 优先选择图像底部的线
|
||
if y1 < height * 0.4 and y2 < height * 0.4:
|
||
continue # 忽略上半部分的线
|
||
|
||
# 计算斜率 (避免除零错误)
|
||
if abs(x2 - x1) < 5: # 几乎垂直的线
|
||
slope = 100 # 设置一个较大的值表示接近垂直
|
||
else:
|
||
slope = (y2 - y1) / (x2 - x1)
|
||
|
||
# 筛选接近垂直的线 (斜率较大)，但允许更多倾斜度
|
||
if abs(slope) > min_slope_threshold:
|
||
line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2)
|
||
# 计算线的中点x坐标
|
||
mid_x = (x1 + x2) / 2
|
||
# 计算线的中点y坐标
|
||
mid_y = (y1 + y2) / 2
|
||
# 保存线段、其坐标、斜率和长度
|
||
vertical_lines.append((line[0], mid_x, mid_y, slope, line_length))
|
||
|
||
if len(vertical_lines) < 2:
|
||
if len(vertical_lines) < 2:
|
||
error("未检测到足够的垂直线", "失败")
|
||
return None, None, None
|
||
|
||
if observe:
|
||
debug(f"步骤4: 找到 {len(vertical_lines)} 条垂直线", "处理")
|
||
v_lines_img = img.copy()
|
||
cv2.putText(v_lines_img, "Step 4: Vertical Lines Filtered", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||
cv2.putText(v_lines_img, f"Min Slope Abs: {min_slope_threshold:.2f}", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 255), 1)
|
||
for line_info in vertical_lines:
|
||
line, _, _, slope, _ = line_info
|
||
x1, y1, x2, y2 = line
|
||
cv2.line(v_lines_img, (x1, y1), (x2, y2), (0, 255, 255), 2)
|
||
# 显示斜率
|
||
cv2.putText(v_lines_img, f"{slope:.2f}", ((x1+x2)//2, (y1+y2)//2),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1)
|
||
cv2.imshow("垂直线", v_lines_img)
|
||
cv2.waitKey(delay)
|
||
|
||
# 按x坐标将线分为左右两组
|
||
left_lines = [line for line in vertical_lines if line[1] < center_x]
|
||
right_lines = [line for line in vertical_lines if line[1] > center_x]
|
||
|
||
# 如果任一侧没有检测到线，则放宽左右两侧线的分组条件
|
||
if not left_lines or not right_lines:
|
||
# 按x坐标排序所有垂直线
|
||
vertical_lines.sort(key=lambda x: x[1])
|
||
|
||
# 如果有至少两条线，将最左侧的线作为左轨迹线，最右侧的线作为右轨迹线
|
||
if len(vertical_lines) >= 2:
|
||
left_lines = [vertical_lines[0]]
|
||
right_lines = [vertical_lines[-1]]
|
||
else:
|
||
error("左侧或右侧未检测到轨迹线", "失败")
|
||
return None, None, None
|
||
|
||
if observe:
|
||
debug(f"左侧候选线数量: {len(left_lines)}, 右侧候选线数量: {len(right_lines)}", "线候选")
|
||
|
||
# 创建左右线可视化图像
|
||
left_right_img = img.copy()
|
||
cv2.putText(left_right_img, "Left and Right Candidate Lines", (10, 20),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||
|
||
# 绘制左侧候选线（蓝色）
|
||
for line_info in left_lines:
|
||
line = line_info[0]
|
||
x1, y1, x2, y2 = line
|
||
cv2.line(left_right_img, (x1, y1), (x2, y2), (255, 0, 0), 2)
|
||
# 显示斜率
|
||
mid_x = (x1 + x2) // 2
|
||
mid_y = (y1 + y2) // 2
|
||
cv2.putText(left_right_img, f"L:{line_info[3]:.2f}", (mid_x, mid_y),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1)
|
||
|
||
# 绘制右侧候选线（红色）
|
||
for line_info in right_lines:
|
||
line = line_info[0]
|
||
x1, y1, x2, y2 = line
|
||
cv2.line(left_right_img, (x1, y1), (x2, y2), (0, 0, 255), 2)
|
||
# 显示斜率
|
||
mid_x = (x1 + x2) // 2
|
||
mid_y = (y1 + y2) // 2
|
||
cv2.putText(left_right_img, f"R:{line_info[3]:.2f}", (mid_x, mid_y),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)
|
||
|
||
cv2.imshow("左右候选线", left_right_img)
|
||
cv2.waitKey(delay)
|
||
|
||
# 优化说明：在默认模式下，评分函数和线对选择都优先考虑更靠近图像中心的线段
|
||
# 这有助于减少对图像边缘可能出现的干扰线的选择，提高轨道线检测的准确性
|
||
|
||
# INFO 改进的评分函数 - 同时考虑斜率、位置、长度和在图像中的位置
|
||
def score_line(line_info, is_left):
|
||
_, mid_x, mid_y, slope, length = line_info
|
||
line_points = line_info[0] # 获取线段的端点坐标
|
||
x1, y1, x2, y2 = line_points
|
||
|
||
# 确保线段从上到下排序
|
||
if y1 > y2:
|
||
x1, x2 = x2, x1
|
||
y1, y2 = y2, y1
|
||
|
||
# 线段靠近底部评分 - y越大（越靠近底部）分数越高
|
||
bottom_ratio = mid_y / height
|
||
y_score = min(1.0, bottom_ratio * 1.2) # 适当提高底部线段的权重
|
||
|
||
# 线段长度评分 - 线越长分数越高
|
||
length_ratio = length / (height * 0.3)
|
||
length_score = min(1.0, length_ratio * 1.2)
|
||
|
||
# 计算到图像中心的距离得分 - 更重视靠近垂直中线的线
|
||
center_x = width / 2
|
||
distance_to_center = abs(mid_x - center_x)
|
||
# 使用更陡峭的衰减函数，使得距离中心越近的线得分越高
|
||
center_proximity_score = max(0, 1.0 - (distance_to_center / (width * 0.25)) ** 2)
|
||
|
||
# 计算底部点与中心的偏差
|
||
bottom_x = x1
|
||
if abs(y2 - y1) > 1: # 非水平线
|
||
t = (height - y1) / (y2 - y1)
|
||
if t >= 0: # 确保是向下延伸
|
||
bottom_x = x1 + t * (x2 - x1)
|
||
|
||
bottom_x_distance = abs(bottom_x - center_x)
|
||
bottom_x_score = max(0, 1.0 - (bottom_x_distance / (width * 0.25)) ** 2)
|
||
|
||
# 斜率评分 - 轨道线应该有一定的倾斜度
|
||
if abs(slope) > 5:
|
||
slope_score = 0.3
|
||
else:
|
||
ideal_slope_magnitude = 0.8
|
||
ideal_slope = -ideal_slope_magnitude if is_left else ideal_slope_magnitude
|
||
|
||
if (is_left and slope > 0) or (not is_left and slope < 0):
|
||
slope_sign_score = 0.3
|
||
else:
|
||
slope_sign_score = 1.0
|
||
|
||
slope_diff = abs(slope - ideal_slope)
|
||
slope_magnitude_score = max(0, 1.0 - slope_diff / 2.0)
|
||
slope_score = slope_sign_score * slope_magnitude_score
|
||
|
||
# 检查线段是否从底部区域开始
|
||
bottom_region_threshold = height * 0.7
|
||
reaches_bottom = max(y1, y2) > bottom_region_threshold
|
||
bottom_reach_score = 1.0 if reaches_bottom else 0.5
|
||
|
||
# 综合评分 - 大幅提高中心接近性的权重
|
||
final_score = (
|
||
y_score * 0.1 + # 底部接近度
|
||
length_score * 0.05 + # 线段长度
|
||
center_proximity_score * 0.6 + # 与中心的接近度 (权重提高)
|
||
bottom_x_score * 0.15 + # 底部点位置
|
||
slope_score * 0.05 + # 斜率合适性
|
||
bottom_reach_score * 0.05 # 是否到达底部
|
||
)
|
||
|
||
return final_score
|
||
# 如果有多条线，评估左右线对是否平行
|
||
if len(left_lines) > 0 and len(right_lines) > 0:
|
||
# 计算最佳的左右线对
|
||
best_pair_score = -1
|
||
best_left_line = None
|
||
best_right_line = None
|
||
|
||
# 先对左右线按照评分排序，只考虑评分较高的候选线（减少计算量）
|
||
left_lines = sorted(left_lines, key=lambda line: score_line(line, True), reverse=True)
|
||
right_lines = sorted(right_lines, key=lambda line: score_line(line, False), reverse=True)
|
||
left_line = left_lines[0] if left_lines else None
|
||
right_line = right_lines[0] if right_lines else None
|
||
if observe:
|
||
debug(f"选择最佳线对，评分: {best_pair_score:.2f}", "线对")
|
||
|
||
# 创建评分可视化图像
|
||
score_img = img.copy()
|
||
cv2.putText(score_img, "Line Scores Visualization", (10, 20),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
|
||
|
||
# 显示左侧线的评分
|
||
for i, line_info in enumerate(left_lines):
|
||
line = line_info[0]
|
||
x1, y1, x2, y2 = line
|
||
score = score_line(line_info, True)
|
||
cv2.line(score_img, (x1, y1), (x2, y2), (255, 0, 0), 2)
|
||
mid_x = (x1 + x2) // 2
|
||
mid_y = (y1 + y2) // 2
|
||
cv2.putText(score_img, f"L{i+1}:{score:.2f}", (mid_x, mid_y),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1)
|
||
|
||
# 显示右侧线的评分
|
||
for i, line_info in enumerate(right_lines):
|
||
line = line_info[0]
|
||
x1, y1, x2, y2 = line
|
||
score = score_line(line_info, False)
|
||
cv2.line(score_img, (x1, y1), (x2, y2), (0, 0, 255), 2)
|
||
mid_x = (x1 + x2) // 2
|
||
mid_y = (y1 + y2) // 2
|
||
cv2.putText(score_img, f"R{i+1}:{score:.2f}", (mid_x, mid_y),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)
|
||
|
||
cv2.imshow("线评分可视化", score_img)
|
||
cv2.waitKey(delay)
|
||
else:
|
||
# 如果只有单侧有线，使用评分最高的线
|
||
left_lines = sorted(left_lines, key=lambda line: score_line(line, True), reverse=True)
|
||
right_lines = sorted(right_lines, key=lambda line: score_line(line, False), reverse=True)
|
||
left_line = left_lines[0] if left_lines else None
|
||
right_line = right_lines[0] if right_lines else None
|
||
|
||
# 确保两侧都有线
|
||
if left_line is None or right_line is None:
|
||
error("无法确定合适的左右轨迹线对", "失败")
|
||
return None, None, None
|
||
|
||
# 获取两条线的坐标
|
||
left_x1, left_y1, left_x2, left_y2 = left_line[0]
|
||
right_x1, right_y1, right_x2, right_y2 = right_line[0]
|
||
|
||
# 确保线段的顺序是从上到下
|
||
if left_y1 > left_y2:
|
||
left_x1, left_x2 = left_x2, left_x1
|
||
left_y1, left_y2 = left_y2, left_y1
|
||
|
||
if right_y1 > right_y2:
|
||
right_x1, right_x2 = right_x2, right_x1
|
||
right_y1, right_y2 = right_y2, right_y1
|
||
|
||
|
||
# 尝试延长线段到图像底部，处理被石板路部分遮挡的情况
|
||
left_extended_y2 = height
|
||
if abs(left_x2 - left_x1) < 5: # 几乎垂直
|
||
left_extended_x2 = left_x2
|
||
else:
|
||
left_slope = (left_y2 - left_y1) / (left_x2 - left_x1)
|
||
left_extended_x2 = left_x1 + (left_extended_y2 - left_y1) / left_slope
|
||
|
||
right_extended_y2 = height
|
||
if abs(right_x2 - right_x1) < 5: # 几乎垂直
|
||
right_extended_x2 = right_x2
|
||
else:
|
||
right_slope = (right_y2 - right_y1) / (right_x2 - right_x1)
|
||
right_extended_x2 = right_x1 + (right_extended_y2 - right_y1) / right_slope
|
||
|
||
# 更新线段端点为延长后的坐标
|
||
left_x2, left_y2 = int(left_extended_x2), left_extended_y2
|
||
right_x2, right_y2 = int(right_extended_x2), right_extended_y2
|
||
|
||
# 尝试延长线段到图像底部，处理被石板路部分遮挡的情况
|
||
left_extended_y2 = height
|
||
if abs(left_x2 - left_x1) < 5: # 几乎垂直
|
||
left_extended_x2 = left_x2
|
||
else:
|
||
left_slope = (left_y2 - left_y1) / (left_x2 - left_x1)
|
||
left_extended_x2 = left_x1 + (left_extended_y2 - left_y1) / left_slope
|
||
|
||
right_extended_y2 = height
|
||
if abs(right_x2 - right_x1) < 5: # 几乎垂直
|
||
right_extended_x2 = right_x2
|
||
else:
|
||
right_slope = (right_y2 - right_y1) / (right_x2 - right_x1)
|
||
right_extended_x2 = right_x1 + (right_extended_y2 - right_y1) / right_slope
|
||
|
||
# 更新线段端点为延长后的坐标
|
||
left_x2, left_y2 = int(left_extended_x2), left_extended_y2
|
||
right_x2, right_y2 = int(right_extended_x2), right_extended_y2
|
||
|
||
# 改进的中心线计算方法
|
||
# 首先确定两条轨迹线的有效部分 - 以两条线段的y坐标重叠部分为准
|
||
min_y = max(left_y1, right_y1)
|
||
max_y = min(left_y2, right_y2)
|
||
|
||
# 如果两条线没有重叠的y部分，则使用整个图像范围
|
||
if min_y >= max_y:
|
||
min_y = 0
|
||
max_y = height
|
||
|
||
# 计算中间路径点的方式：在多个高度上计算左右线的中点，然后拟合中心线
|
||
num_points = 20 # 增加采样点数量以提高精度
|
||
center_points = []
|
||
|
||
# 优化采样策略 - 在底部区域采样更密集
|
||
sample_ys = []
|
||
|
||
# 前半部分采样点均匀分布
|
||
first_half = np.linspace(min_y, min_y + (max_y - min_y) * 0.5, num_points // 4)
|
||
# 后半部分(更靠近底部)采样点更密集
|
||
second_half = np.linspace(min_y + (max_y - min_y) * 0.5, max_y, num_points - num_points // 4)
|
||
|
||
sample_ys = np.concatenate([first_half, second_half])
|
||
|
||
for y in sample_ys:
|
||
# 计算左侧线在当前y值处的x坐标
|
||
if abs(left_y2 - left_y1) < 1: # 防止除零
|
||
left_x = left_x1
|
||
else:
|
||
t = (y - left_y1) / (left_y2 - left_y1)
|
||
left_x = left_x1 + t * (left_x2 - left_x1)
|
||
|
||
# 计算右侧线在当前y值处的x坐标
|
||
if abs(right_y2 - right_y1) < 1: # 防止除零
|
||
right_x = right_x1
|
||
else:
|
||
t = (y - right_y1) / (right_y2 - right_y1)
|
||
right_x = right_x1 + t * (right_x2 - right_x1)
|
||
|
||
# 计算中心点
|
||
center_x = (left_x + right_x) / 2
|
||
center_points.append((center_x, y))
|
||
|
||
# 使用采样的中心点拟合中心线
|
||
center_xs = np.array([p[0] for p in center_points])
|
||
center_ys = np.array([p[1] for p in center_points])
|
||
|
||
# 使用多项式拟合改进中心线 - 选择合适的多项式阶数
|
||
if len(center_points) >= 5: # 需要更多点来拟合更高阶多项式
|
||
try:
|
||
# 尝试不同阶数的多项式拟合，选择最佳结果
|
||
best_poly_coeffs = None
|
||
best_error = float('inf')
|
||
|
||
for degree in [1, 2, 3]: # 尝试1-3阶多项式
|
||
try:
|
||
# 使用多项式拟合
|
||
poly_coeffs = np.polyfit(center_ys, center_xs, degree)
|
||
poly_func = np.poly1d(poly_coeffs)
|
||
|
||
# 计算拟合误差
|
||
predicted_xs = poly_func(center_ys)
|
||
error = np.mean(np.abs(predicted_xs - center_xs))
|
||
|
||
# 如果误差更小，更新最佳拟合
|
||
if error < best_error:
|
||
best_error = error
|
||
best_poly_coeffs = poly_coeffs
|
||
except:
|
||
continue
|
||
|
||
# 如果找到了有效的拟合，使用它
|
||
if best_poly_coeffs is not None:
|
||
poly_coeffs = best_poly_coeffs
|
||
poly_func = np.poly1d(poly_coeffs)
|
||
|
||
# 为了提高底部拟合精度，给予底部区域更高的权重重新拟合
|
||
weights = np.ones_like(center_ys)
|
||
|
||
# 计算距离底部的归一化距离 (0表示底部，1表示顶部)
|
||
normalized_distance = (max_y - center_ys) / max(1, (max_y - min_y))
|
||
|
||
# 设置权重，底部权重更高
|
||
weights = 1.0 + 2.0 * (1.0 - normalized_distance)
|
||
|
||
# 使用加权多项式拟合
|
||
try:
|
||
weighted_poly_coeffs = np.polyfit(center_ys, center_xs, len(best_poly_coeffs) - 1, w=weights)
|
||
weighted_poly_func = np.poly1d(weighted_poly_coeffs)
|
||
|
||
# 比较加权拟合与原始拟合
|
||
weighted_predicted_xs = weighted_poly_func(center_ys)
|
||
weighted_error = np.mean(np.abs(weighted_predicted_xs - center_xs))
|
||
|
||
# 如果加权拟合更好，则使用它
|
||
if weighted_error < best_error * 1.2: # 允许一定的误差增加，因为我们更关注底部精度
|
||
poly_coeffs = weighted_poly_coeffs
|
||
poly_func = weighted_poly_func
|
||
except:
|
||
pass # 如果加权拟合失败，继续使用未加权的拟合
|
||
|
||
# 使用多项式生成中心线的起点和终点
|
||
center_line_y1 = min_y
|
||
center_line_y2 = height # 延伸到图像底部
|
||
|
||
# 计算多项式在这些y值处的x坐标
|
||
center_line_x1 = poly_func(center_line_y1)
|
||
center_line_x2 = poly_func(center_line_y2)
|
||
|
||
# 计算中心线在图像底部的x坐标 - 用于计算偏离度
|
||
bottom_x = poly_func(height)
|
||
|
||
# 确保坐标在图像范围内
|
||
bottom_x = max(0, min(width - 1, bottom_x))
|
||
center_point = (int(bottom_x), int(height))
|
||
|
||
# 计算中心线的加权平均斜率 - 更关注底部区域的斜率
|
||
if abs(center_line_y2 - center_line_y1) < 1: # 防止除零
|
||
center_slope = 0
|
||
else:
|
||
# 计算全局斜率
|
||
global_slope = (center_line_x2 - center_line_x1) / (center_line_y2 - center_line_y1)
|
||
|
||
# 计算底部区域的斜率，使用多个点来提高精度
|
||
bottom_slopes = []
|
||
bottom_region_start = height - height * 0.3 # 底部30%区域
|
||
|
||
if bottom_region_start > min_y:
|
||
# 在底部区域采样多个点计算局部斜率
|
||
bottom_sample_count = 5
|
||
bottom_ys = np.linspace(bottom_region_start, height, bottom_sample_count)
|
||
|
||
for i in range(len(bottom_ys) - 1):
|
||
y1, y2 = bottom_ys[i], bottom_ys[i+1]
|
||
x1, x2 = poly_func(y1), poly_func(y2)
|
||
|
||
# 确保坐标有效
|
||
x1 = max(0, min(width - 1, x1))
|
||
x2 = max(0, min(width - 1, x2))
|
||
|
||
local_slope = (x2 - x1) / max(0.1, (y2 - y1))
|
||
bottom_slopes.append(local_slope)
|
||
|
||
# 计算底部斜率的加权平均值，越靠近底部权重越高
|
||
if bottom_slopes:
|
||
weights = np.linspace(1, 2, len(bottom_slopes))
|
||
bottom_slope = np.average(bottom_slopes, weights=weights)
|
||
|
||
# 加权平均，底部斜率权重更高
|
||
center_slope = bottom_slope * 0.8 + global_slope * 0.2
|
||
else:
|
||
center_slope = global_slope
|
||
else:
|
||
center_slope = global_slope
|
||
else:
|
||
# 如果所有多项式拟合都失败，退回到简单的线性拟合
|
||
raise Exception("多项式拟合失败")
|
||
|
||
except Exception as e:
|
||
warning(f"多项式拟合失败，使用简单中点计算: {e}", "拟合")
|
||
# 如果多项式拟合失败，退回到简单的中点计算方法
|
||
center_line_x1 = (left_x1 + right_x1) / 2
|
||
center_line_y1 = (left_y1 + right_y1) / 2
|
||
center_line_x2 = (left_x2 + right_x2) / 2
|
||
center_line_y2 = (left_y2 + right_y2) / 2
|
||
|
||
# 计算中心线的斜率
|
||
if abs(center_line_x2 - center_line_x1) < 5:
|
||
center_slope = 100 # 几乎垂直
|
||
else:
|
||
center_slope = (center_line_y2 - center_line_y1) / (center_line_x2 - center_line_x1)
|
||
|
||
# 计算中心线延伸到图像底部的点
|
||
if abs(center_slope) < 0.01: # 几乎水平
|
||
bottom_x = center_line_x1
|
||
else:
|
||
bottom_x = center_line_x1 + (height - center_line_y1) / center_slope
|
||
|
||
bottom_x = max(0, min(width - 1, bottom_x))
|
||
center_point = (int(bottom_x), int(height))
|
||
else:
|
||
# 如果点数不足，退回到简单的中点计算方法
|
||
center_line_x1 = (left_x1 + right_x1) / 2
|
||
center_line_y1 = (left_y1 + right_y1) / 2
|
||
center_line_x2 = (left_x2 + right_x2) / 2
|
||
center_line_y2 = (left_y2 + right_y2) / 2
|
||
|
||
# 计算中心线的斜率
|
||
if abs(center_line_x2 - center_line_x1) < 5:
|
||
center_slope = 100 # 几乎垂直
|
||
else:
|
||
center_slope = (center_line_y2 - center_line_y1) / (center_line_x2 - center_line_x1)
|
||
|
||
# 计算中心线延伸到图像底部的点
|
||
if abs(center_slope) < 0.01: # 几乎水平
|
||
bottom_x = center_line_x1
|
||
else:
|
||
bottom_x = center_line_x1 + (height - center_line_y1) / center_slope
|
||
|
||
bottom_x = max(0, min(width - 1, bottom_x))
|
||
center_point = (int(bottom_x), int(height))
|
||
|
||
# 计算中心线与图像中心线的偏差
|
||
deviation = bottom_x - center_x
|
||
|
||
result_img = None
|
||
if observe or save_log:
|
||
result_img = img.copy()
|
||
# 绘制左右轨迹线
|
||
try:
|
||
# 确保坐标在图像范围内并且是整数
|
||
left_x1_safe = max(0, min(width - 1, left_x1))
|
||
left_y1_safe = max(0, min(height - 1, left_y1))
|
||
left_x2_safe = max(0, min(width - 1, left_x2))
|
||
left_y2_safe = max(0, min(height - 1, left_y2))
|
||
|
||
cv2.line(result_img, (int(left_x1_safe), int(left_y1_safe)),
|
||
(int(left_x2_safe), int(left_y2_safe)), (255, 0, 0), 2)
|
||
except Exception as e:
|
||
warning(f"绘制左轨迹线错误: {e}", "绘图")
|
||
|
||
try:
|
||
# 确保坐标在图像范围内并且是整数
|
||
right_x1_safe = max(0, min(width - 1, right_x1))
|
||
right_y1_safe = max(0, min(height - 1, right_y1))
|
||
right_x2_safe = max(0, min(width - 1, right_x2))
|
||
right_y2_safe = max(0, min(height - 1, right_y2))
|
||
|
||
cv2.line(result_img, (int(right_x1_safe), int(right_y1_safe)),
|
||
(int(right_x2_safe), int(right_y2_safe)), (0, 0, 255), 2)
|
||
except Exception as e:
|
||
warning(f"绘制右轨迹线错误: {e}", "绘图")
|
||
|
||
# 绘制中心线 - 如果有多项式拟合，绘制拟合曲线
|
||
if len(center_points) >= 3:
|
||
# 绘制拟合的中心曲线
|
||
try:
|
||
curve_ys = np.linspace(min_y, height, 100)
|
||
curve_xs = poly_func(curve_ys)
|
||
|
||
for i in range(len(curve_ys) - 1):
|
||
try:
|
||
# 确保坐标在图像范围内并且是整数
|
||
x1 = max(0, min(width - 1, curve_xs[i]))
|
||
y1 = max(0, min(height - 1, curve_ys[i]))
|
||
x2 = max(0, min(width - 1, curve_xs[i+1]))
|
||
y2 = max(0, min(height - 1, curve_ys[i+1]))
|
||
|
||
pt1 = (int(x1), int(y1))
|
||
pt2 = (int(x2), int(y2))
|
||
cv2.line(result_img, pt1, pt2, (0, 255, 0), 2)
|
||
except Exception as e:
|
||
warning(f"绘制曲线段错误: {e}", "绘图")
|
||
continue
|
||
|
||
# 绘制采样点
|
||
for pt in center_points:
|
||
try:
|
||
# 确保坐标在图像范围内并且是整数
|
||
x = max(0, min(width - 1, pt[0]))
|
||
y = max(0, min(height - 1, pt[1]))
|
||
cv2.circle(result_img, (int(x), int(y)), 3, (0, 255, 255), -1)
|
||
except Exception as e:
|
||
warning(f"绘制采样点错误: {e}", "绘图")
|
||
continue
|
||
except Exception as e:
|
||
warning(f"绘制曲线错误: {e}", "绘图")
|
||
else:
|
||
# 绘制简单中心线
|
||
try:
|
||
# 确保坐标在图像范围内并且是整数
|
||
x1 = max(0, min(width - 1, center_line_x1))
|
||
y1 = max(0, min(height - 1, center_line_y1))
|
||
x2 = max(0, min(width - 1, center_line_x2))
|
||
y2 = max(0, min(height - 1, center_line_y2))
|
||
|
||
cv2.line(result_img, (int(x1), int(y1)), (int(x2), int(y2)), (0, 255, 0), 2)
|
||
except Exception as e:
|
||
warning(f"绘制中心线错误: {e}", "绘图")
|
||
|
||
# 绘制图像中心线
|
||
try:
|
||
center_x_safe = max(0, min(width - 1, center_x))
|
||
cv2.line(result_img, (int(center_x_safe), 0), (int(center_x_safe), height), (0, 0, 255), 1)
|
||
except Exception as e:
|
||
warning(f"绘制图像中心线错误: {e}", "绘图")
|
||
|
||
# 标记中心点
|
||
try:
|
||
# 确保中心点坐标在图像范围内
|
||
center_x_safe = max(0, min(width - 1, center_point[0]))
|
||
center_y_safe = max(0, min(height - 1, center_point[1]))
|
||
cv2.circle(result_img, (int(center_x_safe), int(center_y_safe)), 10, (255, 0, 255), -1)
|
||
except Exception as e:
|
||
warning(f"绘制中心点错误: {e}", "绘图")
|
||
|
||
# 显示偏差信息
|
||
cv2.putText(result_img, f"Deviation: {deviation:.1f}px", (10, 30),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
|
||
cv2.putText(result_img, "Final Result", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
|
||
if 'best_pair_score' in locals() and best_pair_score != -1:
|
||
cv2.putText(result_img, f"Pair Score: {best_pair_score:.2f}", (10, 85), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
|
||
current_y_offset = 105
|
||
else:
|
||
current_y_offset = 85
|
||
|
||
cv2.putText(result_img, f"L-Slope: {left_line[3]:.2f}, R-Slope: {right_line[3]:.2f}", (10, current_y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
|
||
current_y_offset += 20
|
||
cv2.putText(result_img, f"Track Width: {right_line[1] - left_line[1]:.1f}px", (10, current_y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
|
||
|
||
if observe:
|
||
cv2.imshow("轨迹线检测结果", result_img)
|
||
cv2.waitKey(delay)
|
||
|
||
# 保存日志图像
|
||
if save_log and result_img is not None:
|
||
timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f")
|
||
log_dir = "logs/image"
|
||
os.makedirs(log_dir, exist_ok=True)
|
||
|
||
# 保存结果图像
|
||
result_img_path = os.path.join(log_dir, f"dual_track_{timestamp}.jpg")
|
||
cv2.imwrite(result_img_path, result_img)
|
||
|
||
# 保存原图
|
||
orig_img_path = os.path.join(log_dir, f"dual_track_orig_{timestamp}.jpg")
|
||
cv2.imwrite(orig_img_path, img)
|
||
|
||
info(f"保存双轨迹线检测结果图像到: {result_img_path}", "日志")
|
||
info(f"保存原始图像到: {orig_img_path}", "日志")
|
||
|
||
# 保存文本日志信息
|
||
log_info = {
|
||
"timestamp": timestamp,
|
||
"center_point": (int(center_point[0]), int(center_point[1])),
|
||
"deviation": float(deviation),
|
||
"left_track_mid_x": float(left_line[1]),
|
||
"right_track_mid_x": float(right_line[1]),
|
||
"track_width": float(right_line[1] - left_line[1]),
|
||
"center_slope": float(center_slope),
|
||
}
|
||
info(f"双轨迹线检测结果: {log_info}", "日志")
|
||
|
||
# 创建左右轨迹线和中心线信息
|
||
left_track_info = {
|
||
"line": (left_x1, left_y1, left_x2, left_y2),
|
||
"slope": left_line[3],
|
||
"x_mid": left_line[1]
|
||
}
|
||
|
||
right_track_info = {
|
||
"line": (right_x1, right_y1, right_x2, right_y2),
|
||
"slope": right_line[3],
|
||
"x_mid": right_line[1]
|
||
}
|
||
|
||
center_info = {
|
||
"point": (int(center_point[0]), int(center_point[1])),
|
||
"deviation": float(deviation),
|
||
"slope": float(center_slope),
|
||
"is_vertical": abs(center_slope) > 5.0, # 判断是否接近垂直
|
||
"track_width": float(right_line[1] - left_line[1]), # 两轨迹线之间的距离
|
||
}
|
||
|
||
return center_info, left_track_info, right_track_info
|
||
|
||
def detect_center_based_dual_track_lines(image, observe=False, delay=1000, save_log=True, expected_track_width_ratio=0.45):
|
||
"""
|
||
通过先检测中心线，然后向两侧扩展来检测双轨迹线
|
||
|
||
参数:
|
||
image: 输入图像，可以是文件路径或者已加载的图像数组
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
delay: 展示每个步骤的等待时间(毫秒)
|
||
save_log: 是否保存日志和图像
|
||
expected_track_width_ratio: 预期轨道宽度占图像宽度的比例，默认为0.45
|
||
|
||
返回:
|
||
tuple: (中心线信息, 左轨迹线信息, 右轨迹线信息)
|
||
如果检测失败返回(None, None, None)
|
||
"""
|
||
# 如果输入是字符串（文件路径），则加载图像
|
||
if isinstance(image, str):
|
||
img = cv2.imread(image)
|
||
else:
|
||
img = image.copy()
|
||
|
||
if img is None:
|
||
error("无法加载图像", "失败")
|
||
return None, None, None
|
||
|
||
# 获取图像尺寸
|
||
height, width = img.shape[:2]
|
||
|
||
# 计算图像中间区域的范围
|
||
center_x = width // 2
|
||
|
||
# 转换到HSV颜色空间以便更容易提取黄色
|
||
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
||
|
||
# 标准黄色的HSV范围
|
||
lower_yellow = np.array([15, 80, 80])
|
||
upper_yellow = np.array([35, 255, 255])
|
||
|
||
# 创建黄色的掩码
|
||
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
|
||
|
||
# 对较暗的黄色使用更宽松的阈值
|
||
lower_dark_yellow = np.array([10, 60, 60])
|
||
upper_dark_yellow = np.array([40, 255, 255])
|
||
dark_mask = cv2.inRange(hsv, lower_dark_yellow, upper_dark_yellow)
|
||
|
||
# 合并掩码
|
||
combined_mask = cv2.bitwise_or(mask, dark_mask)
|
||
|
||
# 形态学操作以改善掩码
|
||
kernel = np.ones((5, 5), np.uint8)
|
||
combined_mask = cv2.dilate(combined_mask, kernel, iterations=1)
|
||
combined_mask = cv2.erode(combined_mask, np.ones((3, 3), np.uint8), iterations=1)
|
||
|
||
if observe:
|
||
debug("步骤1: 创建黄色掩码", "处理")
|
||
mask_display = cv2.cvtColor(combined_mask, cv2.COLOR_GRAY2BGR)
|
||
cv2.putText(mask_display, "Step 1: Yellow Mask (Center Based)", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
|
||
cv2.putText(mask_display, f"Lower: {lower_yellow} + Darker Var.", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
|
||
cv2.putText(mask_display, f"Upper: {upper_yellow} + Darker Var.", (10, 55), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
|
||
cv2.imshow("黄色掩码", mask_display)
|
||
cv2.waitKey(delay)
|
||
|
||
# 裁剪底部区域重点关注近处的黄线
|
||
bottom_roi_height = int(height * 0.6) # 关注图像底部60%
|
||
bottom_roi = combined_mask[height-bottom_roi_height:, :]
|
||
|
||
if observe:
|
||
debug("步骤1.5: 底部区域掩码", "处理")
|
||
bottom_roi_display = cv2.cvtColor(bottom_roi, cv2.COLOR_GRAY2BGR)
|
||
cv2.putText(bottom_roi_display, "Step 1.5: Bottom ROI (Center Based)", (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
|
||
cv2.putText(bottom_roi_display, f"ROI Height: {bottom_roi_height}px ({bottom_roi_height/height*100:.0f}%)", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
|
||
cv2.imshow("底部区域掩码", bottom_roi_display)
|
||
cv2.waitKey(delay)
|
||
|
||
# 边缘检测
|
||
edges = cv2.Canny(combined_mask, 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)
|
||
cv2.putText(edges_display, "Thresholds: (50, 150)", (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
|
||
cv2.imshow("边缘检测", edges_display)
|
||
cv2.waitKey(delay)
|
||
|
||
# 霍夫变换检测直线
|
||
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("未检测到直线 (或合并后无直线 - Center Based)", "失败")
|
||
return None, None, None
|
||
|
||
# 筛选近似垂直的线
|
||
vertical_lines = []
|
||
for line in lines:
|
||
x1, y1, x2, y2 = line[0]
|
||
|
||
# 优先选择图像底部的线
|
||
if y1 < height * 0.4 and y2 < height * 0.4:
|
||
continue # 忽略上半部分的线
|
||
|
||
# 计算斜率 (避免除零错误)
|
||
if abs(x2 - x1) < 5: # 几乎垂直的线
|
||
slope = 100 # 设置一个较大的值表示接近垂直
|
||
else:
|
||
slope = (y2 - y1) / (x2 - x1)
|
||
|
||
# 筛选接近垂直的线 (斜率较大)，但允许更多倾斜度
|
||
if abs(slope) > 0.5: # 降低斜率阈值以捕获更多候选线
|
||
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) < 1:
|
||
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
|
||
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.putText(v_lines_img_display, 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.waitKey(delay)
|
||
|
||
debug(f"步骤2.5 (Center Based): 初始检测到 {len(vertical_lines)} 条垂直线，尝试合并", "处理")
|
||
|
||
# 合并参数设置 (可以根据需要调整，这里使用与 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):
|
||
_, mid_x, mid_y, slope, length = line_info
|
||
|
||
# 中心接近度 - 线越接近图像中心得分越高
|
||
center_dist = abs(mid_x - center_x)
|
||
center_score = max(0, 1.0 - center_dist / (width * 0.25))
|
||
|
||
# 底部接近度 - y越大（越靠近底部）分数越高
|
||
bottom_ratio = mid_y / height
|
||
y_score = min(1.0, bottom_ratio * 1.2)
|
||
|
||
# 线段长度评分 - 线越长分数越高
|
||
length_ratio = length / (height * 0.3)
|
||
length_score = min(1.0, length_ratio * 1.2)
|
||
|
||
# 斜率评分 - 线应该接近垂直但不完全垂直
|
||
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)
|
||
|
||
# 综合评分，重点是中心接近度
|
||
final_score = (
|
||
center_score * 0.5 + # 中心接近度权重最高
|
||
y_score * 0.2 + # 底部接近度
|
||
length_score * 0.2 + # 线段长度
|
||
slope_score * 0.1 # 斜率合适性
|
||
)
|
||
|
||
return final_score
|
||
|
||
# 找到可能的中心线
|
||
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("未找到合适的左右轨迹线对", "失败")
|
||
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
|
||
|
||
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
|
||
|
||
# 计算中心线在图像底部的位置和偏离中心的距离
|
||
if abs(center_x2 - center_x1) < 5: # 几乎垂直
|
||
bottom_center_x = center_x2
|
||
else:
|
||
center_slope = (center_y2 - center_y1) / (center_x2 - center_x1)
|
||
bottom_center_x = center_x1 + (height - center_y1) / center_slope
|
||
|
||
# 确保坐标在图像范围内
|
||
bottom_center_x = max(0, min(width - 1, bottom_center_x))
|
||
deviation = bottom_center_x - center_x
|
||
|
||
# INFO 创建结果图像
|
||
result_img = None
|
||
if observe or save_log:
|
||
result_img = img.copy()
|
||
|
||
# 绘制中心线
|
||
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)
|
||
|
||
# 绘制图像中心线
|
||
cv2.line(result_img, (center_x, 0), (center_x, height), (0, 0, 255), 1)
|
||
|
||
# 标记中心点
|
||
cv2.circle(result_img, (int(bottom_center_x), height), 10, (255, 0, 255), -1)
|
||
|
||
# 显示偏差信息
|
||
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
|
||
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.waitKey(delay)
|
||
|
||
# 保存日志图像
|
||
if save_log and result_img is not None:
|
||
timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f")
|
||
log_dir = "logs/image"
|
||
os.makedirs(log_dir, exist_ok=True)
|
||
|
||
# 保存结果图像
|
||
result_img_path = os.path.join(log_dir, f"center_based_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")
|
||
cv2.imwrite(orig_img_path, img)
|
||
|
||
info(f"保存中心线基础双轨迹线检测结果图像到: {result_img_path}", "日志")
|
||
info(f"保存原始图像到: {orig_img_path}", "日志")
|
||
|
||
# 保存文本日志信息
|
||
log_info = {
|
||
"timestamp": timestamp,
|
||
"center_point": (int(bottom_center_x), height),
|
||
"deviation": float(deviation),
|
||
"left_track_mid_x": float(left_line[1]),
|
||
"right_track_mid_x": float(right_line[1]),
|
||
"track_width": float(right_line[1] - left_line[1]),
|
||
"center_slope": float(center_slope),
|
||
}
|
||
info(f"中心线基础双轨迹线检测结果: {log_info}", "日志")
|
||
|
||
# 创建左右轨迹线和中心线信息
|
||
left_track_info = {
|
||
"line": (left_x1, left_y1, left_x2, left_y2),
|
||
"slope": left_line[3],
|
||
"x_mid": left_line[1]
|
||
}
|
||
|
||
right_track_info = {
|
||
"line": (right_x1, right_y1, right_x2, right_y2),
|
||
"slope": right_line[3],
|
||
"x_mid": right_line[1]
|
||
}
|
||
|
||
center_info = {
|
||
"point": (int(bottom_center_x), height),
|
||
"deviation": float(deviation),
|
||
"slope": float(center_slope),
|
||
"is_vertical": abs(center_slope) > 5.0,
|
||
"track_width": float(right_line[1] - left_line[1])
|
||
}
|
||
|
||
return center_info, left_track_info, right_track_info
|
||
|
||
def auto_detect_dual_track_lines(image, observe=False, delay=1000, save_log=True, max_retries=2, use_center_based=True):
|
||
"""
|
||
自动检测双轨迹线，使用指定方法
|
||
|
||
参数:
|
||
image: 输入图像，可以是文件路径或者已加载的图像数组
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
delay: 展示每个步骤的等待时间(毫秒)
|
||
save_log: 是否保存日志和图像
|
||
max_retries: 最大重试次数，用于处理复杂情况
|
||
use_center_based: 是否使用基于中心线的检测方法，默认为True
|
||
|
||
返回:
|
||
tuple: (中心线信息, 左轨迹线信息, 右轨迹线信息)
|
||
"""
|
||
# 根据参数选择使用的检测方法
|
||
if use_center_based:
|
||
info("使用中心线基础检测方法", "检测")
|
||
result = detect_center_based_dual_track_lines(image, observe, delay, save_log)
|
||
else:
|
||
info("使用传统检测方法", "检测")
|
||
result = detect_dual_track_lines(image, observe, delay, save_log)
|
||
|
||
# 检查结果是否成功
|
||
if result[0] is not None:
|
||
info("轨迹线检测成功", "检测")
|
||
return result
|
||
|
||
# 如果失败且还有重试次数，尝试增强图像后重新检测
|
||
if max_retries > 0:
|
||
warning(f"检测失败，尝试调整参数重新检测 (剩余重试次数: {max_retries})", "检测")
|
||
|
||
# 对图像进行预处理以增强黄线检测
|
||
if isinstance(image, str):
|
||
img = cv2.imread(image)
|
||
else:
|
||
img = image.copy()
|
||
|
||
if img is not None:
|
||
# 增强图像对比度
|
||
lab = cv2.cvtColor(img, cv2.COLOR_BGR2LAB)
|
||
l, a, b = cv2.split(lab)
|
||
clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8))
|
||
l = clahe.apply(l)
|
||
lab = cv2.merge((l, a, b))
|
||
enhanced_img = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
|
||
|
||
# 使用增强后的图像重新尝试
|
||
return auto_detect_dual_track_lines(enhanced_img, observe, delay, save_log, max_retries-1, use_center_based)
|
||
|
||
error("轨迹线检测失败", "检测")
|
||
return None, None, None
|