无人驾驶工程师学习笔记（六）——Calculating Lane Curvature

计算道路曲率的steps

知道汽车的速度，行驶状态以及车道的弯曲程度就可以计算出这个角度。
所以我们需要确定道路的弯曲程度，即曲率。

1. 利用掩膜和阈值化技术来检测车道线
2. 进行透视变换获得车道的鸟瞰图
3. 对车道线进行多项式拟合，从多项式中可以获得车道线的曲率。
   For a lane line that is close to vertical, you can fit a line using this formula: f(y) = Ay^2 + By + C, where A, B, and C are coefficients.
   A gives you the curvature of the lane line, B gives you the heading or direction that the line is pointing, and C gives you the position of the line based on how far away it is from the very left of an image (y = 0).

透视

物体距离观察点越远看起来越小的现象称为透视。
在将图像变为鸟瞰图时，可以使用透视变换来放大距离较远的物体。
在透视变换中，我们需要选中四个点，映射到扭转图像中的四个点上。

steps如下：
In the last quiz you calibrated the camera, so here I’m giving you the camera matrix, mtx, and the distortion coefficients dist to start with.

1. Undistort the image using cv2.undistort() with mtx and dist

undist = cv2.undistort(img, mtx, dist, None, mtx)

2. Convert to grayscale

gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

3. Find the chessboard corners

ret,corners = cv2.findChessboardCorners(gray,(8,6),None)

4. Draw corners

    img =cv2.drawChessboardCorners(img,(8,6),corners,ret)

5. Define 4 source points (the outer 4 corners detected in the chessboard pattern) 在原图中手动选择四个点，得到所选中四个点的x和y的坐标值，这些点可定义透视变换。

6. Define 4 destination points (must be listed in the same order as src points!)
   定义扭曲图像所需的矩形平面，选择四个不同的点作为源图像的映射，目测位于目标图像中的矩形，并选择这四个点，这四个点定义了我们想要的图像。即目标点。
   There are many other ways to select source points. For example, many perspective transform algorithms will programmatically detect four source points in an image based on edge or corner detection and analyzing attributes like color and surrounding pixels.

7. Use cv2.getPerspectiveTransform() to get M, the transform matrix
   计算透视变换矩阵，由getPerspectiveTransform函数返回。该函数接受源图像和目标图像坐标点，返回透视变换的映射矩阵M。还可以用反向透视变换恢复这个图像，只需要在这个函数中对换目标点和源图像点。

M = cv2.getPerspectiveTransform(src, dst)
Minv = cv2.getPerspectiveTransform(dst, src)

8. use cv2.warpPerspective() to apply M and warp your image to a top-down view。
   接下来，只需要对原始图像使用变换矩阵M就能得到变换后的扭转图像。

warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)

练习

import pickle
import cv2
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.image as mpimg

# Read in the saved camera matrix and distortion coefficients
# These are the arrays you calculated using cv2.calibrateCamera()
dist_pickle = pickle.load( open( "wide_dist_pickle.p", "rb" ) )
mtx = dist_pickle["mtx"]
dist = dist_pickle["dist"]

# Read in an image
img = cv2.imread('test_image2.png')
nx = 8 # the number of inside corners in x
ny = 6 # the number of inside corners in y
h,w = img.shape[0:2]
# MODIFY THIS FUNCTION TO GENERATE OUTPUT 
# THAT LOOKS LIKE THE IMAGE ABOVE
def corners_unwarp(img, nx, ny, mtx, dist):
    # Pass in your image into this function
    # Write code to do the following steps
    # 1) Undistort using mtx and dist
    undist = cv2.undistort(img, mtx, dist, None, None)
    # 2) Convert to grayscale
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    # 3) Find the chessboard corners
    ret,corners = cv2.findChessboardCorners(gray,(nx,ny),None)
    #print (corners)
    # 4) If corners found: 
    if ret :
            # a) draw corners
            img =cv2.drawChessboardCorners(img,(nx,ny),corners,ret)
            # b) define 4 source points src = np.float32([[,],[,],[,],[,]])
            #src =np.float32([[578.6074,277.9262],[989.3963,333.20044],[584.69696,634.26733],[983.95966,575.9613]])
            #src =np.float32([[462.55078,161.32762],[1030.1245,269.8507],[478.81427,748.9179],[1021.08344,637.51074]])
            #src =np.float32([[578.6074,277.9262],[1030.1245,269.8507],[478.81427,748.9179],[1021.08344,637.51074]])
           
            offset = 100 # offset for dst points
            # Grab the image shape
            img_size = (gray.shape[1], gray.shape[0])
    
            # For source points I'm grabbing the outer four detected corners
            src = np.float32([corners[0], corners[nx-1], corners[-1], corners[-nx]])
            # For destination points, I'm arbitrarily choosing some points to be
            # a nice fit for displaying our warped result 
            # again, not exact, but close enough for our purposes
            dst = np.float32([[offset, offset], [img_size[0]-offset, offset], 
                                         [img_size[0]-offset, img_size[1]-offset], 
                                         [offset, img_size[1]-offset]])
                                         
                                         
                 #Note: you could pick any four of the detected corners 
                 # as long as those four corners define a rectangle
                 #One especially smart way to do this would be to use four well-chosen
                 # corners that were automatically detected during the undistortion steps
                 #We recommend using the automatic detection of corners in your code
            # c) define 4 destination points dst = np.float32([[,],[,],[,],[,]])
            # dst = np.float32([[230,230],[1050,230],[230,770],[1050,770]])
            # dst = np.float32([[80,80],[1200,80],[80,920],[1200,920]])
            # dst = np.float32([[90,90],[1100,90],[90,900],[1100,900]])
            # dst = np.float32([[250,250],[1200,90],[90,900],[1200,900]])
            # d) use cv2.getPerspectiveTransform() to get M, the transform matrix
            M = cv2.getPerspectiveTransform(src, dst)
            # e) use cv2.warpPerspective() to warp your image to a top-down view
            warped = cv2.warpPerspective(img, M, (w,h), flags=cv2.INTER_LINEAR)
    #delete the next two lines
   # M = None
   # warped = np.copy(img) 
    return warped, M

top_down, perspective_M = corners_unwarp(img, nx, ny, mtx, dist)
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(img)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(top_down)
ax2.set_title('Undistorted and Warped Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

运行结果：