mi-task/utils/detect_track.py

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值最大的边缘点
    优先检测下方横线，但在遇到下方线截断的情况时会考虑上边缘
    
    参数:
        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))
    
    # 寻找每列的最顶部点（上边缘点）
    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)} 条直线", "处理")
        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"步骤6: 找到 {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坐标排序）
    bottom_line = None
    top_line = None
    is_truncated = False  # 标记下方线是否被截断
    
    if len(horizontal_lines) > 1:
        # 按y坐标排序 (从大到小，底部的线排在前面)
        horizontal_lines.sort(key=lambda x: x[1], reverse=True)
        
        # 提取最底部和次底部的线段
        bottom_line = horizontal_lines[0]
        
        # 检查是否有明显的上下边缘
        y_coords = [line[1] for line in horizontal_lines]
        y_diffs = [y_coords[i] - y_coords[i+1] for i in range(len(y_coords)-1)]
        
        if len(y_diffs) > 0 and max(y_diffs) > height * 0.05:  # 如果有明显的高度差
            # 找到高度差最大的位置
            split_idx = y_diffs.index(max(y_diffs))
            
            # 分别获取下边缘线和上边缘线
            bottom_line = horizontal_lines[0]  # 最底部的线
            top_line = horizontal_lines[split_idx+1]  # 上边缘线
            
            # 检查上下两条线是否平行 - 计算斜率差异
            bottom_slope = bottom_line[2]
            top_slope = top_line[2]
            slope_diff = abs(bottom_slope - top_slope)
            
            # 计算两线的交点（如果存在）
            bottom_x1, bottom_y1, bottom_x2, bottom_y2 = bottom_line[0]
            top_x1, top_y1, top_x2, top_y2 = top_line[0]
            
            # 根据斜率差异判断是否平行
            # 如果斜率差异很小，认为基本平行
            if slope_diff > 0.05:  # 斜率差异超过阈值
                # 计算两条线延长后的交点
                # 线段方程: y = mx + b
                # 计算两条线的截距b
                bottom_b = bottom_y1 - bottom_slope * bottom_x1
                top_b = top_y1 - top_slope * top_x1
                
                # 检查交点是否在图像范围内或者附近
                # 求解 y = m1*x + b1 = m2*x + b2
                if abs(bottom_slope - top_slope) > 1e-6:  # 避免除以接近0的值
                    intersection_x = (top_b - bottom_b) / (bottom_slope - top_slope)
                    intersection_y = bottom_slope * intersection_x + bottom_b
                    
                    # 判断交点是否在图像宽度的2倍范围内
                    if -width <= intersection_x <= width * 2:
                        is_truncated = True
                        if observe:
                            debug(f"检测到上下边缘线不平行，交点: ({intersection_x:.1f}, {intersection_y:.1f})", "分析")
                            debug("判断下方线被截断", "分析")
                            
                            # 显示上下边缘线及其延长线和交点
                            intersect_img = img.copy()
                            # 画原始线段
                            cv2.line(intersect_img, (bottom_x1, bottom_y1), (bottom_x2, bottom_y2), (0, 255, 0), 2)
                            cv2.line(intersect_img, (top_x1, top_y1), (top_x2, top_y2), (255, 0, 255), 2)
                            
                            # 延长线段以显示交点
                            ext_left_x = max(0, int(intersection_x - width/2))
                            ext_right_x = min(width-1, int(intersection_x + width/2))
                            
                            # 计算延长线上的点
                            bottom_ext_left_y = int(bottom_slope * ext_left_x + bottom_b)
                            bottom_ext_right_y = int(bottom_slope * ext_right_x + bottom_b)
                            top_ext_left_y = int(top_slope * ext_left_x + top_b)
                            top_ext_right_y = int(top_slope * ext_right_x + top_b)
                            
                            # 绘制延长线
                            cv2.line(intersect_img, (ext_left_x, bottom_ext_left_y), 
                                    (ext_right_x, bottom_ext_right_y), (0, 255, 0), 1, cv2.LINE_DASHED)
                            cv2.line(intersect_img, (ext_left_x, top_ext_left_y), 
                                    (ext_right_x, top_ext_right_y), (255, 0, 255), 1, cv2.LINE_DASHED)
                            
                            # 标记交点
                            if 0 <= intersection_x < width and 0 <= intersection_y < height:
                                cv2.circle(intersect_img, (int(intersection_x), int(intersection_y)), 
                                        10, (0, 0, 255), -1)
                            
                            cv2.imshow("上下边缘线交点分析", intersect_img)
                            cv2.waitKey(delay)
            
            # 如果检测到下方线被截断，使用上边缘线来估计实际的下边缘线
            if is_truncated and top_line is not None:
                if observe:
                    debug("使用上边缘估计真实的下边缘", "处理")
                
                # 获取赛道平均宽度（可以是预先测量的固定值，或根据未截断部分测量）
                # 这里假设赛道宽度是固定的，可以根据实际情况调整
                track_width_pixels = height * 0.15  # 假设赛道宽度是图像高度的15%
                
                # 计算一个修正后的底部线段，方向与上边缘线平行，但位置下移
                corrected_bottom_slope = top_slope  # 使用上边缘的斜率
                
                # 计算上边缘线的方程: y = mx + b
                top_b = top_y1 - top_slope * top_x1
                
                # 计算修正后的下边缘线的截距，使其下移track_width_pixels距离
                # 由于是在图像坐标系，y轴向下，所以是加法
                corrected_bottom_b = top_b + track_width_pixels
                
                # 计算修正后的下边缘线的两个端点
                corrected_bottom_x1 = left_bound
                corrected_bottom_y1 = int(corrected_bottom_slope * corrected_bottom_x1 + corrected_bottom_b)
                corrected_bottom_x2 = right_bound
                corrected_bottom_y2 = int(corrected_bottom_slope * corrected_bottom_x2 + corrected_bottom_b)
                
                # 创建修正后的底部线段
                corrected_bottom_line = (
                    [corrected_bottom_x1, corrected_bottom_y1, corrected_bottom_x2, corrected_bottom_y2],
                    (corrected_bottom_y1 + corrected_bottom_y2) / 2,  # mid_y
                    corrected_bottom_slope,
                    np.sqrt((corrected_bottom_x2-corrected_bottom_x1)**2 + (corrected_bottom_y2-corrected_bottom_y1)**2)  # length
                )
                
                if observe:
                    # 显示修正后的线段
                    corrected_img = img.copy()
                    # 原始线段
                    cv2.line(corrected_img, (bottom_x1, bottom_y1), (bottom_x2, bottom_y2), (0, 255, 0), 2)
                    cv2.line(corrected_img, (top_x1, top_y1), (top_x2, top_y2), (255, 0, 255), 2)
                    # 修正后的线段
                    cv2.line(corrected_img, (corrected_bottom_x1, corrected_bottom_y1), 
                           (corrected_bottom_x2, corrected_bottom_y2), (0, 0, 255), 2)
                    
                    cv2.putText(corrected_img, "截断的底边", (bottom_x1, bottom_y1 - 10), 
                              cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
                    cv2.putText(corrected_img, "上边缘", (top_x1, top_y1 - 10), 
                              cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 255), 2)
                    cv2.putText(corrected_img, "修正后的底边", (corrected_bottom_x1, corrected_bottom_y1 - 10), 
                              cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
                    
                    cv2.imshow("修正后的边缘线", corrected_img)
                    cv2.waitKey(delay)
                
                # 使用修正后的底部线作为选定的线
                bottom_line = corrected_bottom_line
    else:
        # 只有一条水平线
        bottom_line = horizontal_lines[0]
    
    # 使用底部线段作为最终选择
    selected_line = bottom_line[0]
    selected_slope = bottom_line[2]
    
    # 提取线段端点
    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)
    
    # 获取线上的更多点
    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"步骤7: 找到边缘点 {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 is_truncated:
            cv2.putText(slope_img, "下方线被截断(已修正)", (10, 190), 
                        cv2.FONT_HERSHEY_SIMPLEX, 1, (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,
            "is_truncated": is_truncated
        }
        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,  # 交点到图像底部的距离
        "is_truncated": is_truncated,  # 下方线是否被截断并修正
        # "points": selected_points  # 添加选定的点组
    }
    
    return bottom_edge_point, edge_info

# 用法示例
if __name__ == "__main__":
    pass

def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True):
    """
    检测左右两条平行的黄色轨道线
    
    参数:
    image: 输入图像，可以是文件路径或者已加载的图像数组
    observe: 是否输出中间状态信息和可视化结果，默认为False
    delay: 展示每个步骤的等待时间(毫秒)
    save_log: 是否保存日志和图像
    
    返回:
    tuple: (中心线信息, 左轨迹线信息, 右轨迹线信息)
    """
    # 如果输入是字符串（文件路径），则加载图像
    if isinstance(image, str):
        img = cv2.imread(image)
    else:
        img = image.copy()
    
    if img is None:
        error("无法加载图像", "失败")
        return None, None, None
    
    # 获取图像尺寸
    height, width = img.shape[:2]
    
    # 计算图像中间区域的范围
    center_x = width // 2
    
    # 转换到HSV颜色空间以便更容易提取黄色
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    
    # 黄色的HSV范围 - 扩大范围以更好地捕捉不同光照条件下的黄色
    lower_yellow = np.array([15, 80, 80])  # 更宽松的黄色下限
    upper_yellow = np.array([35, 255, 255])  # 更宽松的黄色上限
    
    # 创建黄色的掩码
    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
    
    # 形态学操作以改善掩码
    kernel = np.ones((5, 5), np.uint8)  # 增大kernel尺寸
    mask = cv2.dilate(mask, kernel, iterations=1)
    mask = cv2.erode(mask, np.ones((3, 3), np.uint8), iterations=1)  # 添加腐蚀操作去除噪点
    
    if observe:
        debug("步骤1: 创建黄色掩码", "处理")
        cv2.imshow("黄色掩码", mask)
        cv2.waitKey(delay)
    
    # 裁剪底部区域重点关注近处的黄线
    bottom_roi_height = int(height * 0.4)  # 关注图像底部40%区域
    bottom_roi = mask[height-bottom_roi_height:, :]
    
    if observe:
        debug("步骤1.5: 底部区域掩码", "处理")
        cv2.imshow("底部区域掩码", bottom_roi)
        cv2.waitKey(delay)
    
    # 边缘检测
    edges = cv2.Canny(mask, 50, 150, apertureSize=3)
    
    if observe:
        debug("步骤2: 边缘检测", "处理")
        cv2.imshow("边缘检测", edges)
        cv2.waitKey(delay)
    
    # 霍夫变换检测直线 - 降低minLineLength以检测到较短的线段
    lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=25, 
                           minLineLength=width*0.05, maxLineGap=40)  # 更宽松的参数
    
    if lines is None or len(lines) == 0:
        error("未检测到直线", "失败")
        return None, None, None
    
    if observe:
        debug(f"步骤3: 检测到 {len(lines)} 条直线", "处理")
        lines_img = img.copy()
        for line in lines:
            x1, y1, x2, y2 = line[0]
            cv2.line(lines_img, (x1, y1), (x2, y2), (0, 255, 0), 2)
        cv2.imshow("检测到的直线", lines_img)
        cv2.waitKey(delay)
    
    # 筛选近似垂直的线
    vertical_lines = []
    for line in lines:
        x1, y1, x2, y2 = line[0]
        
        # 优先选择图像底部的线
        if y1 < height * 0.5 and y2 < height * 0.5:
            continue  # 忽略上半部分的线
        
        # 计算斜率 (避免除零错误)
        if abs(x2 - x1) < 5:  # 几乎垂直的线
            slope = 100  # 设置一个较大的值表示接近垂直
        else:
            slope = (y2 - y1) / (x2 - x1)
        
        # 筛选接近垂直的线 (斜率较大)，但允许更多倾斜度
        if abs(slope) > 0.75:  # 降低垂直线的斜率阈值，允许更多倾斜的线
            line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2)
            # 计算线的中点x坐标
            mid_x = (x1 + x2) / 2
            # 计算线的中点y坐标
            mid_y = (y1 + y2) / 2
            # 保存线段、其坐标、斜率和长度
            vertical_lines.append((line[0], mid_x, mid_y, slope, line_length))
    
    if len(vertical_lines) < 2:
        error("未检测到足够的垂直线", "失败")
        return None, None, None
    
    if observe:
        debug(f"步骤4: 找到 {len(vertical_lines)} 条垂直线", "处理")
        v_lines_img = img.copy()
        for line_info in vertical_lines:
            line, _, _, slope, _ = line_info
            x1, y1, x2, y2 = line
            cv2.line(v_lines_img, (x1, y1), (x2, y2), (0, 255, 255), 2)
            # 显示斜率
            cv2.putText(v_lines_img, f"{slope:.2f}", ((x1+x2)//2, (y1+y2)//2), 
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1)
        cv2.imshow("垂直线", v_lines_img)
        cv2.waitKey(delay)
    
    # 优先选择更接近图像底部的线 - 根据y坐标均值排序
    vertical_lines.sort(key=lambda x: x[2], reverse=True)  # 按mid_y从大到小排序
    
    # 按x坐标将线分为左右两组
    left_lines = [line for line in vertical_lines if line[1] < center_x]
    right_lines = [line for line in vertical_lines if line[1] > center_x]
    
    # 如果任一侧没有检测到线，则放宽左右两侧线的分组条件
    if not left_lines or not right_lines:
        # 按x坐标排序所有垂直线
        vertical_lines.sort(key=lambda x: x[1])
        
        # 如果有至少两条线，将最左侧的线作为左轨迹线，最右侧的线作为右轨迹线
        if len(vertical_lines) >= 2:
            left_lines = [vertical_lines[0]]
            right_lines = [vertical_lines[-1]]
        else:
            error("左侧或右侧未检测到轨迹线", "失败")
            return None, None, None
    
    # 从左右两组中各选择一条最佳的线
    # 优先选择同时满足：1. 更靠近底部 2. 足够长 3. 更接近中心的线
    def score_line(line_info, is_left):
        _, mid_x, mid_y, _, length = line_info
        # y越大（越靠近底部）分数越高
        y_score = mid_y / height
        # 线越长分数越高
        length_score = min(1.0, length / (height * 0.3))
        # 与预期位置的接近程度
        expected_x = center_x * 0.3 if is_left else center_x * 1.7
        x_score = 1.0 - min(1.0, abs(mid_x - expected_x) / (center_x * 0.5))
        # 综合评分
        return y_score * 0.5 + length_score * 0.3 + x_score * 0.2
    
    # 对左右线组进行评分并排序
    left_lines = sorted(left_lines, key=lambda line: score_line(line, True), reverse=True)
    right_lines = sorted(right_lines, key=lambda line: score_line(line, False), reverse=True)
    
    left_line = left_lines[0]
    right_line = right_lines[0]
    
    # 获取两条线的坐标
    left_x1, left_y1, left_x2, left_y2 = left_line[0]
    right_x1, right_y1, right_x2, right_y2 = right_line[0]
    
    # 确保线段的顺序是从上到下
    if left_y1 > left_y2:
        left_x1, left_x2 = left_x2, left_x1
        left_y1, left_y2 = left_y2, left_y1
    
    if right_y1 > right_y2:
        right_x1, right_x2 = right_x2, right_x1
        right_y1, right_y2 = right_y2, right_y1
    
    # 计算中心线
    center_line_x1 = (left_x1 + right_x1) // 2
    center_line_y1 = (left_y1 + right_y1) // 2
    center_line_x2 = (left_x2 + right_x2) // 2
    center_line_y2 = (left_y2 + right_y2) // 2
    
    # 计算中心线的斜率
    if abs(center_line_x2 - center_line_x1) < 5:
        center_slope = 100  # 几乎垂直
    else:
        center_slope = (center_line_y2 - center_line_y1) / (center_line_x2 - center_line_x1)
    
    # 计算中心线延伸到图像底部的点
    if abs(center_slope) < 0.01:  # 几乎垂直
        bottom_x = center_line_x1
    else:
        bottom_x = int(center_line_x1 + (height - center_line_y1) / center_slope)
    center_point = (bottom_x, height)
    
    # 计算中心线与图像中心线的偏差
    deviation = bottom_x - center_x
    
    result_img = None
    if observe or save_log:
        result_img = img.copy()
        # 绘制左右轨迹线
        cv2.line(result_img, (left_x1, left_y1), (left_x2, left_y2), (255, 0, 0), 2)
        cv2.line(result_img, (right_x1, right_y1), (right_x2, right_y2), (0, 0, 255), 2)
        # 绘制中心线
        cv2.line(result_img, (center_line_x1, center_line_y1), (center_line_x2, center_line_y2), (0, 255, 0), 2)
        cv2.line(result_img, (center_line_x2, center_line_y2), center_point, (0, 255, 0), 2)
        # 绘制图像中心线
        cv2.line(result_img, (center_x, 0), (center_x, height), (0, 0, 255), 1)
        # 标记中心点
        cv2.circle(result_img, center_point, 10, (255, 0, 255), -1)
        
        # 显示偏差信息
        cv2.putText(result_img, f"Deviation: {deviation}px", (10, 30), 
                    cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
        
        if observe:
            cv2.imshow("轨迹线检测结果", result_img)
            cv2.waitKey(delay)
    
    # 保存日志图像
    if save_log and result_img is not None:
        timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f")
        log_dir = "logs/image"
        os.makedirs(log_dir, exist_ok=True)
        img_path = os.path.join(log_dir, f"dual_track_{timestamp}.jpg")
        cv2.imwrite(img_path, result_img)
        info(f"保存双轨迹线检测结果图像到: {img_path}", "日志")
        
        # 保存文本日志信息
        log_info = {
            "timestamp": timestamp,
            "center_point": center_point,
            "deviation": deviation,
            "left_track_mid_x": left_line[1],
            "right_track_mid_x": right_line[1],
            "track_width": right_line[1] - left_line[1],
            "center_slope": center_slope
        }
        info(f"双轨迹线检测结果: {log_info}", "日志")
    
    # 创建左右轨迹线和中心线信息
    left_track_info = {
        "line": left_line[0],
        "slope": left_line[3],  # 注意：索引更改为3，因为保存结构更改了
        "x_mid": left_line[1]
    }
    
    right_track_info = {
        "line": right_line[0],
        "slope": right_line[3],  # 注意：索引更改为3
        "x_mid": right_line[1]
    }
    
    center_info = {
        "point": center_point,
        "deviation": deviation,
        "slope": center_slope,
        "is_vertical": abs(center_slope) > 5.0,  # 判断是否接近垂直
        "track_width": right_line[1] - left_line[1]  # 两轨迹线之间的距离
    }
    
    return center_info, left_track_info, right_track_info

def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
    """
    检测视野左侧黄色轨道线，用于机器狗左侧靠线移动
    
    参数:
    image: 输入图像，可以是文件路径或者已加载的图像数组
    observe: 是否输出中间状态信息和可视化结果，默认为False
    delay: 展示每个步骤的等待时间(毫秒)
    save_log: 是否保存日志和图像
    
    返回:
    tuple: (线信息字典, 最佳跟踪点)
    """
    # 如果输入是字符串（文件路径），则加载图像
    if isinstance(image, str):
        img = cv2.imread(image)
    else:
        img = image.copy()
    
    if img is None:
        error("无法加载图像", "失败")
        return None, None
    
    # 获取图像尺寸
    height, width = img.shape[:2]
    
    # 计算图像中间和左侧区域的范围
    center_x = width // 2
    # 主要关注视野的左半部分
    left_region_width = center_x
    left_region_height = height
    left_bound = 0
    right_bound = center_x
    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([15, 80, 80])  # 更宽松的黄色下限
    upper_yellow = np.array([35, 255, 255])  # 更宽松的黄色上限
    
    # 创建黄色的掩码
    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
    
    # 形态学操作以改善掩码
    kernel = np.ones((5, 5), np.uint8)  # 增大kernel尺寸
    mask = cv2.dilate(mask, kernel, iterations=1)
    mask = cv2.erode(mask, np.ones((3, 3), np.uint8), iterations=1)  # 添加腐蚀操作去除噪点
    
    if observe:
        debug("步骤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)
    
    # 霍夫变换检测直线
    lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=25, 
                           minLineLength=height*0.15, maxLineGap=50)  # 调整参数以检测更长的线段
    
    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.7:  # 设置较宽松的垂直线斜率阈值
            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, _, _, length = line_info
        # 线段越长分数越高
        length_score = min(1.0, length / (height * 0.3))
        # 越靠近左边分数越高
        position_score = 1.0 - (mid_x / center_x)
        # 综合评分
        return length_score * 0.7 + position_score * 0.3
    
    # 对线段进行评分并排序
    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)
    
    # 计算线与地面的交点
    # 使用线段的方程: (y - y1) = slope * (x - x1)
    # 地面对应图像底部: y = height
    # 解这个方程得到交点的x坐标
    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