# Polynomial fit ret = line_fit(img) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] save_file = 'example_images/polyfit_' + out_image_file viz2(img, ret, save_file=save_file) # Do full annotation on original image # Code is the same as in 'line_fit_video.py' orig = mpimg.imread('test_images/' + image_file) undist = cv2.undistort(orig, mtx, dist, None, mtx) left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) bottom_y = undist.shape[0] - 1 bottom_x_left = left_fit[0] * (bottom_y** 2) + left_fit[1] * bottom_y + left_fit[2] bottom_x_right = right_fit[0] * ( bottom_y**2) + right_fit[1] * bottom_y + right_fit[2] vehicle_offset = undist.shape[1] / 2 - (bottom_x_left + bottom_x_right) / 2 xm_per_pix = 3.7 / 700 # meters per pixel in x dimension vehicle_offset *= xm_per_pix img = final_viz(undist, left_fit, right_fit, m_inv, left_curve, right_curve, vehicle_offset) plt.imshow(img) plt.savefig('example_images/annotated_' + out_image_file)
def annotate_image(img_in): """ 使用车道线标记标注输入图像 返回带注释的图像 """ detectedfast = False # 根据上一帧结果拟合检测线,省略每次求直方图 T = 0.8 global mtx, dist, left_line, right_line, detected, arrfitL, arrfitR global left_curve, right_curve, left_lane_inds, right_lane_inds global top_x_left_history, top_x_right_history, bottom_x_left_history, bottom_x_right_history global left_LaneFit_history, right_LaneFit_history # Undistort未失真, threshold阈值, perspective transform视角转换 # undist = cv2.undistort(img_in, mtx, dist, None, mtx) undist = img_in # 边缘检测阈值 img, abs_bin, mag_bin, dir_bin, WY_BIN = combined_thresh(undist) # 透视变换 binary_warped, binary_unwarped, m, m_inv = perspective_transform(img) # cv2.imshow('img', undist) # cv2.imshow('binary_warped', binary_warped) # Perform polynomial fit执行多项式拟合 if not detected: # Slow line fit ret = line_fit(binary_warped, T) if len(ret) > 0: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] out_img = ret['out_img'] histo = ret['histo'] # Get moving average of line fit coefficients # 获得线性拟合系数的移动平均值 left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) # Calculate curvature计算曲率 left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) if detectedfast: detected = True # slow line fit always detects the line else: return img_in else: # implies detected == True # Fast line fit left_fit = left_line.get_fit() right_fit = right_line.get_fit() ret = tune_fit(binary_warped, left_fit, right_fit) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Only make updates if we detected lines in current frame if ret is not None: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) else: detected = False # 偏移量 vehicle_offset, bottom_x_left, bottom_x_right, top_x_left, top_x_right = calc_vehicle_offset( undist, left_fit, right_fit) """ PMH:采用历史帧更新数据 """ top_x_left_history, bottom_x_left_history, left_LaneFit_history, is_left_filtered = filter_history( top_x_left_history, bottom_x_left_history, left_LaneFit_history, top_x_left, bottom_x_left, left_fit) top_x_right_history, bottom_x_right_history, right_LaneFit_history, is_right_filtered = filter_history( top_x_right_history, bottom_x_right_history, right_LaneFit_history, top_x_right, bottom_x_right, right_fit) if is_left_filtered is True: print('左车道线被过滤!') left_fit = left_LaneFit_history.get_latest() else: left_fit = left_LaneFit_history.get_smoothed() if is_right_filtered is True: print('右车道线被过滤!') right_fit = right_LaneFit_history.get_latest() else: right_fit = right_LaneFit_history.get_smoothed() # # 存储左车道线(曲线的参数a,b,c、曲率半径、车道偏移量、图片序号) # tupcurveL = (left_curve, vehicle_offset, bottom_x_left, num_img) # tupcurveL = (left_curve, bottom_x_left) # newleft = left_fit + tupcurveL # arrfitL.append(newleft) # # # 存储右车道线(曲线的参数a,b,c、曲率半径、车道偏移量、图片序号) # # tupcurveR = (right_curve, vehicle_offset, bottom_x_right, num_img) # tupcurveR = (right_curve, bottom_x_right) # newright = right_fit + tupcurveR # arrfitR.append(newright) # Perform final visualization on top of original undistorted image result, color_warp, new_warp, newwarpNO = final_viz( undist, left_fit, right_fit, m_inv, left_curve, right_curve, vehicle_offset, is_left_filtered, is_right_filtered) # cv2.imshow('result', result) # viz1(binary_warped, ret, save_file=None) # return img, WY_BIN, binary_unwarped, binary_warped, color_warp, result, arrfitL, arrfitR, new_warp, newwarpNO, out_img, histo # return arrfitL, arrfitR, new_warp, newwarpNO, color_warp, result, out_img, histo return result
def annotate_image(img_in): """ Annotate the input image with lane line markings Returns annotated image """ global mtx, dist, left_line, right_line, detected global left_curve, right_curve, left_lane_inds, right_lane_inds # Undistort, threshold, perspective transform undist = cv2.undistort(img_in, mtx, dist, None, mtx) img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(undist) binary_warped, binary_unwarped, m, m_inv = perspective_transform(img) # Perform polynomial fit if not detected: # Slow line fit ret = line_fit(binary_warped) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Get moving average of line fit coefficients left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) # Calculate curvature left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) detected = True # slow line fit always detects the line else: # implies detected == True # Fast line fit left_fit = left_line.get_fit() right_fit = right_line.get_fit() ret = tune_fit(binary_warped, left_fit, right_fit) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Only make updates if we detected lines in current frame if ret is not None: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) else: detected = False vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit) # Perform final visualization on top of original undistorted image result1 = final_viz(undist, left_fit, right_fit, m_inv, left_curve, right_curve, vehicle_offset , binary_warped) result2 = omer_viz2(binary_warped, ret) final_result = np.vstack((result1, result2)) return final_result
def annotate_frame(self, frame): #global mtx, dist, left_line, right_line, detected #global left_curve, right_curve, left_lane_inds, right_lane_inds # Undistort, threshold, perspective transform undist = cv2.undistort(frame, self.mtx, self.dist, None, self.mtx) img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(undist) binary_warped, binary_unwarped, m, m_inv = perspective_transform(img) # Perform polynomial fit if not self.detected: # Slow line fit ret = line_fit(binary_warped) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Get moving average of line fit coefficients left_fit = self.left_line.add_fit(left_fit) right_fit = self.right_line.add_fit(right_fit) # Calculate curvature self.left_curve, self.right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) self.detected = True # slow line fit always detects the line else: # implies detected == True # Fast line fit left_fit = self.left_line.get_fit() right_fit = self.right_line.get_fit() ret = tune_fit(binary_warped, left_fit, right_fit) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Only make updates if we detected lines in current frame if ret is not None: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] left_fit = self.left_line.add_fit(left_fit) right_fit = self.right_line.add_fit(right_fit) self.left_curve, self.right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) else: self.detected = False vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit) # Perform final visualization on top of original undistorted image result = final_viz(undist, left_fit, right_fit, m_inv, self.left_curve, self.right_curve, vehicle_offset) return result
def annotate_image(img_in): """ Annotate the input image with lane line markings Returns annotated image """ global mtx, dist, left_line, right_line, detected, frameCount, retLast global left_curve, right_curve, left_lane_inds, right_lane_inds frameCount += 1 src = np.float32( [[200, 720], [1100, 720], [520, 500], [760, 500]]) x = [src[0, 0], src[1, 0], src[3, 0], src[2, 0], src[0, 0]] y = [src[0, 1], src[1, 1], src[3, 1], src[2, 1], src[0, 1]] # Undistort, threshold, perspective transform undist = cv2.undistort(img_in, mtx, dist, None, mtx) img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(undist) binary_warped, binary_unwarped, m, m_inv = perspective_transform(img) # Perform polynomial fit if not detected: # Slow line fit ret = line_fit(binary_warped) # if detect no lanes, use last result instead. if len(ret) == 0: ret = retLast left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] out_img = ret['out_img'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] histogram = ret['histo'] # Get moving average of line fit coefficients left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) # Calculate curvature left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) detected = True # slow line fit always detects the line else: # implies detected == True # Fast line fit left_fit = left_line.get_fit() right_fit = right_line.get_fit() ret = tune_fit(binary_warped, left_fit, right_fit) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Only make updates if we detected lines in current frame if ret is not None: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) else: detected = False vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit) # Perform final visualization on top of original undistorted image result = final_viz(undist, left_fit, right_fit, m_inv, left_curve, right_curve, vehicle_offset) retLast = ret save_viz2 = './output_images/polyfit_test%d.jpg' % (frameCount) viz2(binary_warped, ret, save_viz2) save_warped = './output_images/warped_test%d.jpg' % (frameCount) plt.imshow(binary_warped, cmap='gray', vmin=0, vmax=1) if save_warped is None: plt.show() else: plt.savefig(save_warped) plt.gcf().clear() save_binary = './output_images/binary_test%d.jpg' % (frameCount) plt.imshow(img, cmap='gray', vmin=0, vmax=1) if save_binary is None: plt.show() else: plt.savefig(save_binary) plt.gcf().clear() if frameCount > 0: fig = plt.gcf() fig.set_size_inches(16.5, 8.5) plt.subplot(2, 3, 1) plt.imshow(undist) # plt.plot(undist) plt.plot(x, y) plt.title('undist') plt.subplot(2, 3, 2) plt.imshow(hls_bin, cmap='gray', vmin=0, vmax=1) plt.title('hls_bin') plt.subplot(2, 3, 3) plt.imshow(abs_bin, cmap='gray', vmin=0, vmax=1) plt.title('abs_bin') plt.subplot(2, 3, 4) plt.imshow(img, cmap='gray', vmin=0, vmax=1) plt.title('img') plt.subplot(2, 3, 5) plt.imshow(out_img) plt.title('out_img') plt.subplot(2, 3, 6) plt.imshow(result, cmap='gray', vmin=0, vmax=1) plt.title('result') save_result = 'D:/code/github_code/CarND-Advanced-Lane-Lines-P4/output_images/result-test%d.jpg' % (frameCount) if save_result is None: plt.show() else: plt.savefig(save_result) plt.gcf().clear() return result
def annotate_image(img_in): """ Annotate the input image with lane line markings Returns annotated image """ global mtx, dist, left_line, right_line, detected global left_curve, right_curve, left_lane_inds, right_lane_inds # Undistort, threshold, perspective transform undist = cv2.undistort(img_in, mtx, dist, None, mtx) img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(undist) binary_warped, binary_unwarped, m, m_inv = perspective_transform(img) # Perform polynomial fit if not detected: # Slow line fit ret = line_fit(binary_warped) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Get moving average of line fit coefficients left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) # Calculate curvature left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) detected = True # slow line fit always detects the line else: # implies detected == True # Fast line fit left_fit = left_line.get_fit() right_fit = right_line.get_fit() ret = tune_fit(binary_warped, left_fit, right_fit) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Only make updates if we detected lines in current frame if ret is not None: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) else: detected = False vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit) # Perform final visualization on top of original undistorted image result = final_viz(undist, left_fit, right_fit, m_inv, left_curve, right_curve, vehicle_offset) return result
def get_offset(img_in): """ Annotate the input image with lane line markings Returns annotated image """ global mtx, dist, left_line, right_line, detected global left_curve, right_curve, left_lane_inds, right_lane_inds # Undistort, threshold, perspective transform undist = cv2.undistort(img_in, mtx, dist, None, mtx) img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(undist) binary_warped, binary_unwarped, m, m_inv = perspective_transform(img) # Perform polynomial fit if not detected: # Slow line fit ret = line_fit(binary_warped) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Get moving average of line fit coefficients left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) # Calculate curvature left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) detected = True # slow line fit always detects the line else: # implies detected == True # Fast line fit left_fit = left_line.get_fit() right_fit = right_line.get_fit() ret = tune_fit(binary_warped, left_fit, right_fit) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Only make updates if we detected lines in current frame if ret is not None: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) else: detected = False vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit) float_offset_value = vehicle_offset.item() #Return the offset in type 'float'. #vehicle_offset is a numpy.Float64 but we just want the float value to be able to publish return float_offset_value
def annotate_image(img_in, src, dst, detectedfast, T, num_img): """ 使用车道线标记标注输入图像 返回带注释的图像 """ global mtx, dist, left_line, right_line, detected, arrfitL, arrfitR global left_curve, right_curve, left_lane_inds, right_lane_inds # Undistort未失真, threshold阈值, perspective transform视角转换 # undist = cv2.undistort(img_in, mtx, dist, None, mtx) undist = img_in # 边缘检测阈值 img, abs_bin, mag_bin, dir_bin, WY_BIN = combined_thresh(undist) # 透视变换 binary_warped, binary_unwarped, m, m_inv = perspective_transform( img, src, dst) # Perform polynomial fit执行多项式拟合 if not detected: # Slow line fit ret = line_fit(binary_warped, T) if len(ret) > 0: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Get moving average of line fit coefficients # 获得线性拟合系数的移动平均值 left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) # Calculate curvature计算曲率 left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) if detectedfast: detected = True # slow line fit always detects the line else: return img_in else: # implies detected == True # Fast line fit left_fit = left_line.get_fit() right_fit = right_line.get_fit() ret = tune_fit(binary_warped, left_fit, right_fit) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Only make updates if we detected lines in current frame if ret is not None: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) else: detected = False # 偏移量 vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit) # 存储左车道线(曲线的参数a,b,c、曲率半径、车道偏移量、图片序号) tupcurveL = (left_curve, vehicle_offset, num_img) newleft = left_fit + tupcurveL arrfitL.append(newleft) # 存储右车道线(曲线的参数a,b,c、曲率半径、车道偏移量、图片序号) tupcurveR = (right_curve, vehicle_offset, num_img) newright = right_fit + tupcurveR arrfitR.append(newright) # Perform final visualization on top of original undistorted image result, color_warp, new_warp, newwarpNO = final_viz( undist, left_fit, right_fit, m_inv, left_curve, right_curve, vehicle_offset) # viz1(binary_warped, ret, save_file=None) return img, binary_unwarped, binary_warped, color_warp, result, arrfitL, arrfitR, new_warp, newwarpNO
def annotate_image(img_in): """ Annotate the input image with lane line markings Returns annotated image """ # Get Frames and Image properties g.frames = g.frames + 1 g.vi_width = img_in.shape[1] g.vi_height = img_in.shape[0] # Undistort, threshold, perspective transform # Transforms an image to compensate for lens distortion. # Python: cv2.undistort(src, cameraMatrix, distCoeffs[, dst[, newCameraMatrix]]) -> dst # src - Input (distorted) image. # dst - Output (corrected) image that has the same size and type as src . # cameraMatrix - Input camera matrix A = Matrix(3,3) [[fx,0,cx], [0,fy,cy] [0,0,1] # distCoeffs - Input vector of distortion coefficients (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]]) of 4, 5, or 8 elements. # --> If the vector is NULL/empty, the zero distortion coefficients are assumed. # newCameraMatrix - Camera matrix of the distorted image. By default, # --> it is the same as cameraMatrix but you may additionally scale and shift the result by using a different matrix. if (RLD_ZERO_DISTORSION != True): undist = cv2.undistort(img_in, g.mtx, g.dist, None, g.mtx) # print ('DISTORSION CORRECTED undist: ', undist) else: undist = cv2.undistort(img_in, g.mtx, None, None, g.mtx) # print ('DISTORSION NOT CORRECTED undist: ', undist) # Display undistort image if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2: if g.cold_boot: cv2.namedWindow('undist_image', cv2.WINDOW_NORMAL) cv2.resizeWindow('undist_image', g.s_win_width, g.s_win_height) cv2.moveWindow('undist_image', 0, g.s_height // 2 + g.s_win_height) undist_bgr = cv2.cvtColor(undist, cv2.COLOR_RGB2BGR) cv2.imshow('undist_image', undist_bgr) # Combine all threshold mask on the undistort image #histogram_calc(undist) img, abs_bin, mag_bin, dir_bin, hls_bin, hsv_bin = combined_thresh(undist) #img = combined_canny(undist) if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2: if g.cold_boot: cv2.namedWindow('undist_comb_thresh', cv2.WINDOW_NORMAL) cv2.resizeWindow('undist_comb_thresh', g.s_win_width, g.s_win_height) cv2.moveWindow( 'undist_comb_thresh', 3 * g.s_win_width, 2 * g.s_win_height - g.s_win_height // 2 + 2 * g.s_win_height_offset) cv2.imshow('undist_comb_thresh', img) # Apply perpective transformation/warp binary image binary_warped, binary_unwarped, m, m_inv, src, dst = perspective_transform( img) unwarped_trapez = (np.dstack( (binary_unwarped, binary_unwarped, binary_unwarped)) * 255).astype('uint8') if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2: if g.cold_boot: cv2.namedWindow('warped_image', cv2.WINDOW_NORMAL) cv2.resizeWindow('warped_image', g.s_win_width, g.s_win_height) cv2.moveWindow('warped_image', 4 * g.s_win_width, 3 * g.s_win_height) cv2.imshow('warped_image', binary_warped) if g.cold_boot: cv2.namedWindow('unwarped_image', cv2.WINDOW_NORMAL) cv2.resizeWindow('unwarped_image', g.s_win_width, g.s_win_height) cv2.moveWindow('unwarped_image', 3 * g.s_win_width, 3 * g.s_win_height) if g.trackbar_enabled: cv2.createTrackbar('top', 'unwarped_image', int(g.trap_top_width * 100), 100, nothing) cv2.createTrackbar('bottom', 'unwarped_image', int(g.trap_bottom_width * 100), 100, nothing) cv2.createTrackbar('height', 'unwarped_image', int(g.trap_height * 100), 100, nothing) cv2.setTrackbarPos('top', 'unwarped_image', int(g.trap_top_width * 100)) cv2.setTrackbarPos('bottom', 'unwarped_image', int(g.trap_bottom_width * 100)) cv2.setTrackbarPos('height', 'unwarped_image', int(g.trap_height * 100)) if g.trackbar_enabled: l_top = cv2.getTrackbarPos('top', 'unwarped_image') g.trap_top_width = float(l_top) / 100 l_bottom = cv2.getTrackbarPos('bottom', 'unwarped_image') g.trap_bottom_width = float(l_bottom) / 100 l_height = cv2.getTrackbarPos('height', 'unwarped_image') g.trap_height = float(l_height) / 100 cv2.polylines(unwarped_trapez, np.int32([src]), True, (255, 255, 0), 1, 0) cv2.imshow('unwarped_image', unwarped_trapez) # Perform polynomial fit left_fit = None right_fit = None if not g.detected: # Slow line fit ret = line_fit(binary_warped) #print ('ret:', ret) if ret is not None: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] g.left_lane_inds = ret['left_lane_inds'] g.right_lane_inds = ret['right_lane_inds'] # Get moving average of line fit coefficients left_fit = g.left_line.add_fit(left_fit) right_fit = g.right_line.add_fit(right_fit) # Calculate curvature g.left_curve, g.right_curve = calc_curve(g.left_lane_inds, g.right_lane_inds, nonzerox, nonzeroy) if g.detect_fast_mode_allowed: g.detected = True # slow line fit always detects the line else: g.detected = False # Force the slow mode for ever if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2: viz1(binary_warped, ret, save_file=None) viz2(binary_warped, ret, save_file=None) else: if not g.degraded_viz_mode: g.detected = False else: # implies g.detected == True # Fast line fit left_fit = g.left_line.get_fit() right_fit = g.right_line.get_fit() ret = tune_fit(binary_warped, left_fit, right_fit) # Only make updates if we detected lines in current frame if ret is not None: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] g.left_lane_inds = ret['left_lane_inds'] g.right_lane_inds = ret['right_lane_inds'] left_fit = g.left_line.add_fit(left_fit) right_fit = g.right_line.add_fit(right_fit) g.left_curve, g.right_curve = calc_curve(g.left_lane_inds, g.right_lane_inds, nonzerox, nonzeroy) if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2: #viz1(binary_warped, ret, save_file=None) viz2(binary_warped, ret, save_file=None) else: if not g.degraded_viz_mode: g.detected = False if ret is not None: vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit) # Perform final visualization on top of original undistorted image # print ('g.detected:',g.detected ,'g.left_curve:',g.left_curve ,'g.right_curve:',g.right_curve) # print ('left_fit:',left_fit ,'right_fit:',right_fit) result = final_viz(undist, left_fit, right_fit, m_inv, g.left_curve, g.right_curve, vehicle_offset) else: if g.degraded_viz_mode and g.left_line.len_fit( ) != 0 and g.right_line.len_fit() != 0: if left_fit is None or right_fit is None: left_fit = g.left_line.get_fit() right_fit = g.right_line.get_fit() vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit) result = final_viz(undist, left_fit, right_fit, m_inv, g.left_curve, g.right_curve, vehicle_offset) else: result = undist if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2: if g.cold_boot: cv2.namedWindow('final_visu', cv2.WINDOW_NORMAL) cv2.resizeWindow('final_visu', 640, 480) cv2.putText(result, 'Filter: ' + g.combined_filter_type, (1, int(3 * g.vi_height * VI_TEXT_OFFSET / VI_HEIGHT)), cv2.FONT_HERSHEY_SIMPLEX, g.vi_height * VI_FONT_SIZE / VI_HEIGHT, (0, 255, 0), 1, cv2.LINE_AA) cv2.putText( result, 'Res: ' + str(g.vi_width) + 'x' + str(g.vi_height) + ' - Frame:' + str(g.frames), (1, int(4 * g.vi_height * VI_TEXT_OFFSET / VI_HEIGHT)), cv2.FONT_HERSHEY_SIMPLEX, g.vi_height * VI_FONT_SIZE / VI_HEIGHT, (255, 0, 0), 1, cv2.LINE_AA) cv2.putText( result, 'pX: ' + str(round(g.scale_px_width, 2)) + ' mm/px' + ' l_size:' + str(round(g.lane_size_px * g.scale_px_width / 1000, 2)) + ' m', (1, int(5 * g.vi_height * VI_TEXT_OFFSET / VI_HEIGHT)), cv2.FONT_HERSHEY_SIMPLEX, g.vi_height * VI_FONT_SIZE / VI_HEIGHT, (255, 0, 0), 1, cv2.LINE_AA) cv2.putText(result, g.detect_mode + ' Recovery: ' + str(g.degraded_viz_count), (1, int(6 * g.vi_height * VI_TEXT_OFFSET / VI_HEIGHT)), cv2.FONT_HERSHEY_SIMPLEX, g.vi_height * VI_FONT_SIZE / VI_HEIGHT, (0, 255, 255), 1, cv2.LINE_AA) result_bgr = cv2.cvtColor(result, cv2.COLOR_RGB2BGR) cv2.imshow('final_visu', result_bgr) if cv2.waitKey(1) & 0xFF == ord('q'): sys.exit(0) g.cold_boot = False return result
# Polynomial fit ret = line_fit(img) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] save_file = 'example_images/polyfit_' + out_image_file viz2(img, ret, save_file=save_file) # Do full annotation on original image # Code is the same as in 'line_fit_video.py' orig = mpimg.imread('test_images/' + image_file) undist = cv2.undistort(orig, mtx, dist, None, mtx) left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) bottom_y = undist.shape[0] - 1 bottom_x_left = left_fit[0]*(bottom_y**2) + left_fit[1]*bottom_y + left_fit[2] bottom_x_right = right_fit[0]*(bottom_y**2) + right_fit[1]*bottom_y + right_fit[2] vehicle_offset = undist.shape[1]/2 - (bottom_x_left + bottom_x_right)/2 xm_per_pix = 3.7/700 # meters per pixel in x dimension vehicle_offset *= xm_per_pix img = final_viz(undist, left_fit, right_fit, m_inv, left_curve, right_curve, vehicle_offset) plt.imshow(img) plt.savefig('example_images/annotated_' + out_image_file)