def measure_curvature_real_dat(left_fit_cr, right_fit_cr):
'''
Calculates the curvature of polynomial functions in meters.
'''
# Define conversions in x and y from pixels space to meters
ym_per_pix = 30 / 720 # meters per pixel in y dimension
xm_per_pix = 3.7 / 700 # meters per pixel in x dimension
# Start by generating our fake example data
# Make sure to feed in your real data instead in your project!
#ploty, left_fit_cr, right_fit_cr = generate_data(ym_per_pix, xm_per_pix)
ploty = np.linspace(0,
GlobalVar.get_orig_image().shape[0] - 1,
GlobalVar.get_orig_image().shape[0])
# Define y-value where we want radius of curvature
# We'll choose the maximum y-value, corresponding to the bottom of the image
y_eval = np.max(ploty)
##### TO-DO: Implement the calculation of R_curve (radius of curvature) #####
left_curverad = ((1 + ((2 * (left_fit_cr[0] ) *(y_eval * ym_per_pix ) + (left_fit_cr[1] )) ** 2)) ** (3 / 2)) \
/ (np.absolute(2 * (left_fit_cr[0] ) )) ## Implement the calculation of the left line here
right_curverad = ((1 + ((2 * (right_fit_cr[0] ) * (y_eval * ym_per_pix ) + (right_fit_cr[1] )) ** 2)) ** (3 / 2)) \
/ (np.absolute(2 * (right_fit_cr[0] ))) ## Implement the calculation of the left line here
return left_curverad, right_curverad
def pipeline(img):
# Get undistorted image
out_img = img
GlobalVar().set_orig_image(img)
undistorted_img = get_undistorted_image(img)
thresh_bin_img = get_threshold_binary_image(undistorted_img)
warped_img = apply_perspective(thresh_bin_img)
left_fit, right_fit, left_fitx, right_fitx, leftx, lefty, rightx, righty, out_img = find_lane_boundary(
warped_img)
from util.radius_curve import measure_curvature_real
left_curverad, right_curverad = measure_curvature_real(
warped_img, left_fitx, right_fitx)
average_left_fitx = left_fitx
average_right_fitx = right_fitx
offset = 0.0
average_left_fit = left_fit
average_right_fit = right_fit
if is_santiycheck_ok(img.shape, left_fit, right_fit, left_fitx, right_fitx,
leftx, lefty, rightx, righty, left_curverad,
right_curverad):
left_line, right_line = initialize_lines(left_fit, left_fitx, leftx,
lefty, right_fit, right_fitx,
rightx, righty)
offset, offsetLeftLine, offsetRightLine = calculate_offset(
img, left_fitx, right_fitx)
left_line.line_base_pos = offsetLeftLine
right_line.line_base_pos = offsetRightLine
GlobalVar().set_offset(offset)
left_line.radius_of_curvature = left_curverad
right_line.radius_of_curvature = right_curverad
average_left_fit, average_right_fit, average_left_fitx, average_right_fitx = process_lines(
left_fit, left_fitx, left_line, right_fit, right_fitx, right_line)
GlobalVar().line_detected.append(True)
GlobalVar().left_lines.append(left_line)
GlobalVar().right_lines.append(right_line)
elif (len(GlobalVar().left_lines) > 0) & (len(GlobalVar().right_lines) >
0):
average_left_fit, average_right_fit, average_left_fitx, average_right_fitx, left_curverad, right_curverad, offset = get_last_conf_vals(
)
ploty = np.linspace(0, img.shape[0] - 1, img.shape[0])
GlobalVar().set_left_fit(average_left_fit)
GlobalVar().set_right_fit(average_right_fit)
if bool(np.array(average_left_fitx).any()) & bool(
np.array(average_right_fitx).any()):
offset = GlobalVar().offset
out_img = plot_back_to_orig(average_left_fitx, average_right_fitx,
ploty)
annotate_vals(left_curverad, offset, out_img, right_curverad)
# print(left_curverad, 'm', right_curverad, 'm', '\n')
return out_img
def is_lane_detected():
ret_bool = True
if ((GlobalVar().get_idx()) % (GlobalVar().line_detected.maxlen)) == 0:
line_detected = GlobalVar().line_detected
ret_bool = False
for line_bool in line_detected:
ret_bool |= line_bool
return ret_bool
def get_undistorted_image(img):
# Use cv2.calibrateCamera() and cv2.undistort()
# Convert to grayscale
grayscaleimage = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
obj_points, img_points, = GlobalVar().ret_calib_points()
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
obj_points, img_points, grayscaleimage.shape[::-1], None, None)
undist = cv2.undistort(img, mtx, dist, None, mtx)
return undist
def process_lines(left_fit, left_fitx, left_line, right_fit, right_fitx,
right_line):
left_lines = GlobalVar().left_lines
recent_xfitted = []
recent_polycoeff = []
average_left_fit = left_fit
average_left_fitx = left_fitx
average_right_fitx = right_fitx
if (len(left_lines) > 0):
for temp_line in left_lines:
recent_xfitted.append(temp_line.current_fitx)
recent_polycoeff.append(temp_line.current_fit)
average_left_fitx = np.mean(recent_xfitted, axis=0, keepdims=True)
if len(recent_polycoeff) > 1:
average_left_fit = np.mean(recent_polycoeff,
axis=0,
keepdims=False)
fit_diifs = np.subtract(left_fit,
recent_polycoeff[len(recent_polycoeff) - 1])
left_line.best_fit = average_left_fit
left_line.bestx = average_left_fitx
left_line.diffs = fit_diifs
# Right Lane Lines
right_lines = GlobalVar().right_lines
recent_xfitted = []
recent_polycoeff = []
average_right_fit = right_fit
if (len(right_lines) > 0):
for temp_line in right_lines:
recent_xfitted.append(temp_line.current_fitx)
recent_polycoeff.append(temp_line.current_fit)
average_right_fitx = np.mean(recent_xfitted, axis=0, keepdims=True)
if len(recent_polycoeff) > 1:
average_right_fit = np.mean(recent_polycoeff,
axis=0,
keepdims=False)
fit_diifs = np.subtract(right_fit,
recent_polycoeff[len(recent_polycoeff) - 1])
right_line.best_fit = average_right_fit
right_line.bestx = average_right_fitx
right_line.diffs = fit_diifs
return average_left_fit, average_right_fit, average_left_fitx, average_right_fitx
def plot_back_to_orig(left_fitx, right_fitx, ploty):
# Create an image to draw the lines on
warp_zero = np.zeros_like(GlobalVar().get_orig_image()).astype(np.uint8)
#color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
#ploty = np.linspace(0, orig_image.shape[0] - 1, orig_image.shape[0])
# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
pts_right = np.array(
[np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(warp_zero, np.int_([pts]), (0, 255, 0))
Minv = cv2.getPerspectiveTransform(dst, src)
# Warp the blank back to original image space using inverse perspective matrix (Minv)
newwarp = cv2.warpPerspective(warp_zero, Minv,
(GlobalVar().get_orig_image().shape[1],
GlobalVar().get_orig_image().shape[0]))
# Combine the result with the original image
result = cv2.addWeighted(GlobalVar().get_orig_image(), 1, newwarp, 0.3, 0)
return result
def get_last_conf_vals():
average_left_fitx = []
average_right_fitx = []
left_curverad = []
right_curverad = []
if len(GlobalVar().left_lines) > 0:
left_lane = GlobalVar().left_lines[len(GlobalVar().left_lines) - 1]
left_curverad = left_lane.radius_of_curvature
average_left_fitx = left_lane.bestx
average_left_fit = left_lane.best_fit
if len(GlobalVar().right_lines) > 0:
right_lane = GlobalVar().right_lines[len(GlobalVar().right_lines) - 1]
right_curverad = right_lane.radius_of_curvature
average_right_fitx = right_lane.bestx
average_right_fit = right_lane.best_fit
offset = GlobalVar().get_offset()
return average_left_fit, average_right_fit, average_left_fitx, average_right_fitx, left_curverad, right_curverad, offset
def find_lane_boundary(img):
from util.sliding_window import fit_polynomial
#left_fitx = right_fitx = leftx = lefty = rightx = righty = []
if (GlobalVar().get_idx() <= 0) | (not is_lane_detected()) | bool(
np.array(GlobalVar().get_left_fit()).any()) & bool(
np.array(GlobalVar().get_right_fit()).any()):
left_fit, right_fit, left_fitx, right_fitx, leftx, lefty, rightx, righty, out_img = fit_polynomial(
img)
GlobalVar().set_left_fit(left_fit)
GlobalVar().set_right_fit(right_fit)
elif GlobalVar().get_left_fit().shape[0] == img.shape[0]:
from util.prev_poly import search_around_poly
left_fit, right_fit, left_fitx, right_fitx, leftx, lefty, rightx, righty = search_around_poly(
img,
GlobalVar().get_left_fit(),
GlobalVar().get_right_fit())
GlobalVar().set_idx(GlobalVar().get_idx() + 1)
# win_center_img = find_window_centroids(img)
return GlobalVar().get_left_fit(), GlobalVar().get_right_fit(
), left_fitx, right_fitx, leftx, lefty, rightx, righty, out_img
import glob
import cv2
from util.global_variables import GlobalVar
def test_get_undistort_image(objpoints, imgpoints):
# Make a list of calibration images
#images = glob.glob('../test_images/*.jpg')
images = glob.glob('../camera_cal/*.jpg')
# Step through the list and search for chessboard corners
idx = 1
for fname in images:
img = cv2.imread(fname)
from util.Utils import get_undistorted_image
undist = get_undistorted_image(img)
cv2.imwrite("../output_images/calib" + str(idx) + "_output.jpg",
undist)
idx += 1
obj_points, img_points = GlobalVar().ret_calib_points()
test_get_undistort_image(obj_points, img_points)