cp1.py

import cv2
import numpy as np
from collections import deque

left_angle_history = deque(maxlen=5)
right_angle_history = deque(maxlen=5)

def detect_and_draw_lines(img):
    angle_threshold = 15
    white_line_segments = []
    current_white_segment = 0
    segment_counter = 0
    white_lines_edges = detect_white_lines(img)
    yellow_lines_edges = detect_yellow_lines(img)
    line_img = np.copy(img)

    left_angle = None
    right_angle = None

    yellow_lines = cv2.HoughLinesP(
        yellow_lines_edges, 1, np.pi/180, threshold=20, minLineLength=30, maxLineGap=500)
    if yellow_lines is not None:
        for line in yellow_lines:
            x1, y1, x2, y2 = line[0]
            angle = np.degrees(np.arctan2(y2 - y1, x2 - x1))
            if abs(angle) < angle_threshold or abs(abs(angle) - 180) < angle_threshold:
                continue
            cv2.line(line_img, (x1, y1), (x2, y2), (200, 0, 0), 5)
            mid_x = (x1 + x2) // 2
            if mid_x < img.shape[1] // 2:
                left_angle = angle
            else:
                right_angle = angle

    white_lines = cv2.HoughLinesP(
        white_lines_edges, 1, np.pi/180, threshold=80, minLineLength=50, maxLineGap=100)
    if white_lines is not None:
        valid_white_lines = [
            line for line in white_lines 
            if 20 < abs(np.arctan2(line[0][3]-line[0][1], line[0][2]-line[0][0]) * 180/np.pi) < 70
        ]
        if valid_white_lines:
            white_line_segments = valid_white_lines
            segment_counter += 1
            if segment_counter >= current_white_segment < len(white_line_segments):
                current_white_segment += 1
                segment_counter = 0
            for i in range(min(current_white_segment, len(white_line_segments))):
                x1, y1, x2, y2 = white_line_segments[i][0]
                angle = np.degrees(np.arctan2(y2 - y1, x2 - x1))
                cv2.line(line_img, (x1, y1), (x2, y2), (0, 0, 255), 30)
                mid_x = (x1 + x2) // 2
                if mid_x < img.shape[1] // 2:
                    left_angle = angle
                else:
                    right_angle = angle

    return line_img, left_angle, right_angle

def detect_curve_direction(left_angle, right_angle):
    if left_angle is None and right_angle is None:
        return "Curva: Indefinida"

    if left_angle is not None and right_angle is not None:
        if left_angle < -39 and right_angle < 27:
            return "Curva para a esquerda"
        elif left_angle > -36 and right_angle > 30:
            return "Curva para a direita"
        elif -39 <= left_angle <= -36 and 27 <= right_angle <= 30:
            return "Reta"


    if left_angle is not None:
        if left_angle < -39:
            return "Curva para a esquerda"
        elif left_angle > -36:
            return "Curva para a direita"
        elif -39 <= left_angle <= -36:
            return "Reta"

    if right_angle is not None:
        if right_angle < 27:
            return "Curva para a esquerda"
        elif right_angle > 30:
            return "Curva para a direita"
        elif 27 <= right_angle <= 30:
            return "Reta"

    return "Curva: Indefinida"

def draw_angle_overlay(img, left_angle, right_angle):
    if left_angle is not None:
        left_angle_history.append(left_angle)
        left_angle = sum(left_angle_history) / len(left_angle_history)
    if right_angle is not None:
        right_angle_history.append(right_angle)
        right_angle = sum(right_angle_history) / len(right_angle_history)

    font = cv2.FONT_HERSHEY_SIMPLEX
    font_scale = 0.7
    color = (0, 255, 0)
    thickness = 2
    y0 = 30
    dy = 30
    curve_text = detect_curve_direction(left_angle, right_angle)
    cv2.putText(img, curve_text, (img.shape[1] - 300, y0 + 2 * dy), font, font_scale, color, thickness, cv2.LINE_AA)
    left_text = f"Linha esquerda: {left_angle:.1f}°" if left_angle is not None else "Linha esquerda: N/A"
    right_text = f"Linha direita: {right_angle:.1f}°" if right_angle is not None else "Linha direita: N/A"
    cv2.putText(img, left_text, (img.shape[1] - 300, y0), font, font_scale, color, thickness, cv2.LINE_AA)
    cv2.putText(img, right_text, (img.shape[1] - 300, y0 + dy), font, font_scale, color, thickness, cv2.LINE_AA)
    return img


def draw_lines(img):
    line_img, left_angle, right_angle = detect_and_draw_lines(img)
    line_img = draw_angle_overlay(line_img, left_angle, right_angle)
    return line_img


def cars_mask(img):
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    lower_black = np.array([0, 0, 0])
    upper_black = np.array([180, 255, 50])
    lower_white = np.array([0, 0, 200])
    upper_white = np.array([180, 30, 255])
    mask_black = cv2.inRange(hsv, lower_black, upper_black)
    mask_white = cv2.inRange(hsv, lower_white, upper_white)
    combined_mask = cv2.bitwise_or(mask_black, mask_white)
    kernel = np.ones((5, 5), np.uint8)
    cleaned_mask = cv2.morphologyEx(combined_mask, cv2.MORPH_OPEN, kernel)
    filled_mask = cv2.morphologyEx(cleaned_mask, cv2.MORPH_CLOSE, kernel)
    height, width = img.shape[:2]
    roi_vertices = np.array([[
        (int(width * 0.4), int(height * 0.75)),
        (int(width * 0.6), int(height * 0.6)),
        (int(width * 0.7), int(height * 0.6)),
        (int(width * 2), int(height * 0.7))
    ]], dtype=np.int32)
    roi_mask = np.zeros_like(filled_mask)
    cv2.fillPoly(roi_mask, [roi_vertices], 255)
    masked_edges = cv2.bitwise_and(filled_mask, roi_mask)
    contours, _ = cv2.findContours(masked_edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    for contour in contours:
        if cv2.contourArea(contour) > 1200:
            x, y, w, h = cv2.boundingRect(contour)
            cv2.rectangle(img, (x, y), (x+w, y+h), (0, 0, 255), 2)
    return img

def apply_roi(img):
    height, width = img.shape[:2]
    roi_vertices = np.array([[
        (int(width * 0.1), height),
        (int(width * 0.45), int(height * 0.6)),
        (int(width * 0.55), int(height * 0.6)),
        (int(width * 1), height)
    ]], dtype=np.int32)
    return roi_vertices

def apply_mask(img, vertices):
    mask = np.zeros_like(img)
    cv2.fillPoly(mask, vertices, 255)
    masked_img = cv2.bitwise_and(img, mask)
    return masked_img

def detect_white_lines(img):
    roi_vertices = apply_roi(img)
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    lower_white = np.array([0, 0, 200])
    upper_white = np.array([180, 30, 255])
    mask = cv2.inRange(hsv, lower_white, upper_white)
    mask = apply_mask(mask, roi_vertices)
    edges = cv2.Canny(mask, 50, 150)
    kernel = np.ones((3, 3), np.uint8)
    edges = cv2.dilate(edges, kernel, iterations=1)
    return edges

def detect_yellow_lines(img):
    roi_vertices = apply_roi(img)
    frame = cv2.GaussianBlur(img, (9, 9), 0)
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    lower_yellow = np.array([15, 75, 140])
    upper_yellow = np.array([40, 255, 255])
    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
    mask = apply_mask(mask, roi_vertices)
    edges = cv2.Canny(mask, 50, 150)
    return edges

def image_da_webcam(img):
    result = cars_mask(img) & draw_lines(img)
    return result

cv2.namedWindow("preview")
vc = cv2.VideoCapture("project_video.mp4")
vc.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
vc.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)

if vc.isOpened():
    rval, frame = vc.read()
else:
    rval = False

while rval:
    img = image_da_webcam(frame)
    cv2.imshow("preview", img)
    rval, frame = vc.read()
    key = cv2.waitKey(4)
    if key == 27:
        break

cv2.destroyWindow("preview")
vc.release()