From 6a0810b562dbded4bf21ee408c14804dd06cdbf4 Mon Sep 17 00:00:00 2001 From: isaiahrivera21 Date: Tue, 12 Mar 2024 15:53:31 -0400 Subject: [PATCH] Following much better --- ros2/lanefollowingtest/LaneDetection.py | 338 ++++++++++++++++++++++++ 1 file changed, 338 insertions(+) create mode 100644 ros2/lanefollowingtest/LaneDetection.py diff --git a/ros2/lanefollowingtest/LaneDetection.py b/ros2/lanefollowingtest/LaneDetection.py new file mode 100644 index 000000000..ace8909eb --- /dev/null +++ b/ros2/lanefollowingtest/LaneDetection.py @@ -0,0 +1,338 @@ +import cv2 +import numpy as np + +vidcap = cv2.VideoCapture("LaneVideo.mp4") +success, image = vidcap.read() + + +def nothing(x): + pass + + +def Plot_line(binary_warped, smoothen=False,prevFrameCount=6): #used Udacity's code to plot the lines and windows over lanes + histogram = np.sum(binary_warped[binary_warped.shape[0]//2:, :], axis=0) + # histogram = np.sum(binary_warped[binary_warped.shape[0]//2:,:], axis=0) + # Create an output image to draw on and visualize the result + out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255 + # Find the peak of the left and right halves of the histogram + # These will be the starting point for the left and right lines + midpoint = np.int32(histogram.shape[0]/2) + leftx_base = np.argmax(histogram[:midpoint]) + rightx_base = np.argmax(histogram[midpoint:]) + midpoint + lane_width= abs(rightx_base-leftx_base) + # Choose the number of sliding windows + nwindows = 9 + # Set height of windows + window_height = np.int32(binary_warped.shape[0]/nwindows) + # Identify the x and y positions of all nonzero pixels in the image + nonzero = binary_warped.nonzero() + nonzeroy = np.array(nonzero[0]) + nonzerox = np.array(nonzero[1]) + # Current positions to be updated for each window + leftx_current = leftx_base + rightx_current = rightx_base + # Set the width of the windows +/- margin + margin = 200 + # Set minimum number of pixels found to recenter window + minpix = 20 + # Create empty lists to receive left and right lane pixel indices + left_lane_inds = [] + right_lane_inds = [] + + + + + + # Step through the windows one by one + for window in range(nwindows): + # Identify window boundaries in x and y (and right and left) + win_y_low = binary_warped.shape[0] - (window+1)*window_height + win_y_high = binary_warped.shape[0] - window*window_height + win_xleft_low = leftx_current - margin + win_xleft_high = leftx_current + margin + win_xright_low = rightx_current - margin + win_xright_high = rightx_current + margin + # Draw the windows on the visualization image + cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high), + (0,255,0), 2) + cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high), + (0,255,0), 2) + # Identify the nonzero pixels in x and y within the window + good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & + (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0] + good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & + (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0] + # Append these indices to the lists + left_lane_inds.append(good_left_inds) + right_lane_inds.append(good_right_inds) + # If you found > minpix pixels, recenter next window on their mean position + if len(good_left_inds) > minpix: + leftx_current = np.int32(np.mean(nonzerox[good_left_inds])) + if len(good_right_inds) > minpix: + rightx_current = np.int32(np.mean(nonzerox[good_right_inds])) + + # Concatenate the arrays of indices + try: + left_lane_inds = np.concatenate(left_lane_inds) + right_lane_inds = np.concatenate(right_lane_inds) + except ValueError: + pass + + + + + # Extract left and right line pixel positions + leftx = nonzerox[left_lane_inds] + lefty = nonzeroy[left_lane_inds] + rightx = nonzerox[right_lane_inds] + righty = nonzeroy[right_lane_inds] + + + + + # Fit a second order polynomial to each + left_fit = np.polyfit(lefty, leftx, 2) + right_fit = np.polyfit(righty, rightx, 2) + + # Ideas. Either use a from pervious point thing + # simple idea: just filter out the empty spots + + + + if(smoothen): + global fit_prev_left + global fit_prev_right + global fit_sum_left + global fit_sum_right + if(len(fit_prev_left)>prevFrameCount): + fit_sum_left-= fit_prev_left.pop(0) + fit_sum_right-= fit_prev_right.pop(0) + + fit_prev_left.append(left_fit) + fit_prev_right.append(right_fit) + fit_sum_left+=left_fit + fit_sum_right+= right_fit + + no_of_fit_values=len(fit_prev_left) + left_fit= fit_sum_left/no_of_fit_values + right_fit= fit_sum_right/no_of_fit_values + + + ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] ) + left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2] + right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2] + + out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0] + out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255] + + nonzero = binary_warped.nonzero() + nonzeroy = np.array(nonzero[0]) + nonzerox = np.array(nonzero[1]) + + window_img = np.zeros_like(out_img) + # Generate a polygon to illustrate the search window area + # And recast the x and y points into usable format for cv2.fillPoly() + left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, ploty]))]) + left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin, + ploty])))]) + left_line_pts = np.hstack((left_line_window1, left_line_window2)) + right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, ploty]))]) + right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin, + ploty])))]) + right_line_pts = np.hstack((right_line_window1, right_line_window2)) + + # Draw the lane onto the warped blank image + cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0)) + cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0)) + + left_line_pts = np.array([np.transpose(np.vstack([left_fitx, ploty]))], dtype=np.int32) + right_line_pts = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))], dtype=np.int32) + + cv2.polylines(out_img, left_line_pts, isClosed=False, color=(0, 255, 255), thickness=3) + cv2.polylines(out_img, right_line_pts, isClosed=False, color=(0, 255, 255), thickness=3) + + result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0) + + return out_img, result, left_fitx,right_fitx,ploty,left_fit, right_fit,left_lane_inds,right_lane_inds,lane_width + +def measure_position_meters(binary_warped, left_fit, right_fit): + # Define conversion in x from pixels space to meters + xm_per_pix = .37/700 # meters per pixel in x dimension + # Choose the y value corresponding to the bottom of the image + y_max = binary_warped.shape[0] + # Calculate left and right line positions at the bottom of the image + left_x_pos = left_fit[0]*y_max**2 + left_fit[1]*y_max + left_fit[2] + right_x_pos = right_fit[0]*y_max**2 + right_fit[1]*y_max + right_fit[2] + # Calculate the x position of the center of the lane + center_lanes_x_pos = (left_x_pos + right_x_pos)//2 + # Calculate the deviation between the center of the lane and the center of the picture + # The car is assumed to be placed in the center of the picture + # If the deviation is negative, the car is on the felt hand side of the center of the lane + veh_pos = ((binary_warped.shape[1]//2) - center_lanes_x_pos) * xm_per_pix + veh_pos = abs(veh_pos) / 40000 + return veh_pos + + + + + +cv2.namedWindow("Trackbars") + +cv2.createTrackbar("L - H", "Trackbars", 0, 255, nothing) +cv2.createTrackbar("L - S", "Trackbars", 0, 255, nothing) +cv2.createTrackbar("L - V", "Trackbars", 200, 255, nothing) +cv2.createTrackbar("U - H", "Trackbars", 255, 255, nothing) +cv2.createTrackbar("U - S", "Trackbars", 50, 255, nothing) +cv2.createTrackbar("U - V", "Trackbars", 255, 255, nothing) + +while success: + success, image = vidcap.read() + frame = cv2.resize(image, (640,480)) + + ## Choosing points for perspective transformation + tl = (222,387) + bl = (70 ,472) + tr = (400,380) + br = (538,472) + + cv2.circle(frame, tl, 5, (0,0,255), -1) + cv2.circle(frame, bl, 5, (0,0,255), -1) + cv2.circle(frame, tr, 5, (0,0,255), -1) + cv2.circle(frame, br, 5, (0,0,255), -1) + + ## Aplying perspective transformation + pts1 = np.float32([tl, bl, tr, br]) + pts2 = np.float32([[0, 0], [0, 480], [640, 0], [640, 480]]) + + # Matrix to warp the image for birdseye window + matrix = cv2.getPerspectiveTransform(pts1, pts2) + transformed_frame = cv2.warpPerspective(frame, matrix, (640,480)) + + ### Object Detection + # Image Thresholding + hsv_transformed_frame = cv2.cvtColor(transformed_frame, cv2.COLOR_BGR2HSV) + + l_h = cv2.getTrackbarPos("L - H", "Trackbars") + l_s = cv2.getTrackbarPos("L - S", "Trackbars") + l_v = cv2.getTrackbarPos("L - V", "Trackbars") + u_h = cv2.getTrackbarPos("U - H", "Trackbars") + u_s = cv2.getTrackbarPos("U - S", "Trackbars") + u_v = cv2.getTrackbarPos("U - V", "Trackbars") + + lower = np.array([l_h,l_s,l_v]) + upper = np.array([u_h,u_s,u_v]) + mask = cv2.inRange(hsv_transformed_frame, lower, upper) + + #Histogram + histogram = np.sum(mask[mask.shape[0]//2:, :], axis=0) + midpoint = np.int32(histogram.shape[0]/2) + left_base = np.argmax(histogram[:midpoint]) + right_base = np.argmax(histogram[midpoint:]) + midpoint + + #Sliding Window + y = 472 + lx = [] + rx = [] + + # Set the width of the windows +/- margin + margin = 100 + # Set minimum number of pixels found to recenter window + minpix = 50 + + msk = mask.copy() + out_img, result, left_fitx,right_fitx,ploty,left_fit, right_fit,left_lane_inds,right_lane_inds,lane_width = Plot_line(msk) + + pos = measure_position_meters(msk,left_fitx,right_fitx) + print(pos) + + + + + while y>0: + nonzero = msk.nonzero() + nonzeroy = np.array(nonzero[0]) + nonzerox = np.array(nonzero[1]) + # print(nonzero) + + + ## Left threshold + img = mask[y-40:y, left_base-50:left_base+50] + contours, _ = cv2.findContours(img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + for contour in contours: + M = cv2.moments(contour) + if M["m00"] != 0: + cx = int(M["m10"]/M["m00"]) + cy = int(M["m01"]/M["m00"]) + lx.append(left_base-50 + cx) + left_base = left_base-50 + cx + + ## Right threshold + img = mask[y-40:y, right_base-50:right_base+50] + contours, _ = cv2.findContours(img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + for contour in contours: + M = cv2.moments(contour) + if M["m00"] != 0: + cx = int(M["m10"]/M["m00"]) + cy = int(M["m01"]/M["m00"]) + rx.append(right_base-50 + cx) + right_base = right_base-50 + cx + + + + cv2.rectangle(msk, (left_base-50,y), (left_base+50,y-40), (255,255,255), 2) + cv2.rectangle(msk, (right_base-50,y), (right_base+50,y-40), (255,255,255), 2) + y -= 40 + + + + + leftx = nonzerox[lx] + lefty = nonzeroy[lx] + rightx = nonzerox[rx] + righty = nonzeroy[rx] + + # if bool(lx) and bool(rx): + # left_fit = np.polyfit(lefty, leftx, 2) + # right_fit = np.polyfit(righty, rightx, 2) + + # # Generate y values for the entire height of the image + # ploty = np.linspace(0, transformed_frame.shape[0] - 1, transformed_frame.shape[0]) + + # # Generate x values using the polynomial fits + # left_fitx = np.polyval(left_fit, ploty) + # right_fitx = np.polyval(right_fit, ploty) + + # # Create an image to draw the lane lines + # line_image = np.zeros_like(msk) + + # # Draw the left lane line + # for i in range(len(left_fitx)): + # cv2.circle(line_image, (int(left_fitx[i]), int(ploty[i])), 1, 255, -1) + + # # Draw the right lane line + # for i in range(len(right_fitx)): + # cv2.circle(line_image, (int(right_fitx[i]), int(ploty[i])), 1, 255, -1) + + # # Combine the original image with the drawn lane lines + # result = cv2.addWeighted(mask, 1, cv2.cvtColor(line_image, cv2.COLOR_GRAY2BGR), 0.3, 0) + + + + + + + + + + + + cv2.imshow("Original", frame) + cv2.imshow("Bird's Eye View", transformed_frame) + cv2.imshow("Lane Detection - Image Thresholding", mask) + cv2.imshow("Lane Detection - Sliding Windows", msk) + # cv2.imshow("Outimg" , out_img) + cv2.imshow("R",result) + # Display the result + + if cv2.waitKey(10) == 27: + break \ No newline at end of file