laneDetector.py

#################################################
# OpenCV based python program for lane detection
# Image input size : 320x240
################################################
import cv2
import numpy as np
import scipy as sp
import sys 
from scipy.interpolate import UnivariateSpline

MAX_KERNEL_LENGTH=5 
# camera translation matrix
trans = np.matrix([[1, 0, 0, -160], 
    [0, 1, 0, 25],
    [0, 0, 1, 120]])
# camera intrinsic matrix
ins = np.matrix( [[ 319.06941775,    0.,          158.30433212],
     [   0.,          343.74672367,   90.86584741],
     [   0.,           0.,            1.]])

beta = 0.0
alpha = -0.070
gamma = 0.0
# camera rotation matrix
Ry = np.matrix([[np.cos(beta), 0, np.sin(beta)],
    [0, 1, 0],
    [-np.sin(beta), 0, np.cos(beta)]])

Rx = np.matrix([[1, 0, 0],
    [0, np.cos(alpha), -np.sin(alpha)],
    [0, np.sin(alpha), np.cos(alpha)]])

Rz = np.matrix([[np.cos(gamma), -np.sin(gamma), 0],
    [np.sin(gamma), np.cos(gamma), 0],
    [0, 0, 1]])

R = reduce(np.dot, [Rx, Ry, Rz])
distcoeff = np.mat([  4.21897644e-01,  -4.62375619e+00,  -1.43237677e-02,  
    -3.93417016e-03, 1.95337373e+01])
mat = np.dot(ins, R)
mat = np.dot(mat, trans)
mat = np.delete(mat, [1], axis=1)
img = cv2.imread(sys.argv[1])
cv2.imshow("ori", img)
if not img.data:
    raise Exception("read image failed")
x, y = img.shape[:2]
plane  = cv2.warpPerspective(img, mat, (y, 2*x)
        ,flags=cv2.INTER_LINEAR | cv2.WARP_INVERSE_MAP
        )

view = cv2.flip(plane, 0)

gass = cv2.GaussianBlur(view, (0,0), 3)
sharp = cv2.addWeighted(view, 1.5, gass, -0.5, 0)

# extract lanes using hsl feature
hsl2 = cv2.cvtColor(sharp, cv2.COLOR_BGR2HLS)
lower_white = np.array([80,0,180], dtype=np.uint8)
upper_white = np.array([105,255,255], dtype=np.uint8)

mask2 = cv2.inRange(hsl2, lower_white, upper_white)
res2 = cv2.bitwise_and(view, view, mask = mask2)

for i in xrange(1, MAX_KERNEL_LENGTH, 2):
    blurred = cv2.medianBlur(res2, i)

channels = cv2.split(blurred)
chan = channels[2]

kernel = np.ones((5,5), np.uint8)
closing = cv2.morphologyEx(chan, cv2.MORPH_CLOSE, kernel)

lines = cv2.HoughLinesP(closing, 1, np.pi/180, 80, 30, 10)
rows, _, _ = lines.shape
lines = lines.reshape(rows, 4)

pois = lines[:, :1]
pois =  pois.reshape(-1)
mini = min(pois)
maxm = max(pois)
mid = (mini + maxm)/2

left_lines, right_lines = lines[lines[:,0] < mid], lines[lines[:,0] >= mid]

def spline_fitting(lines):
    lines = lines.reshape(lines.shape[0]*2, 2)
    pts = np.array(dict(lines).items())
    y, x = pts[:,0], pts[:,1]
    new_x = np.linspace(0, chan.shape[0], 10)
    f = sp.interpolate.UnivariateSpline(x, y)
    new_y = f(new_x)
    line_pts = np.column_stack((new_y, new_x)).astype(int)
    return line_pts

print "left fitting"
lline_pts = spline_fitting(left_lines)
print "right fitting"
rline_pts = spline_fitting(right_lines)

back = np.zeros((480, 320, 3), np.uint8)
cv2.polylines(back, [rline_pts], True, (0, 255, 200), 3)
cv2.polylines(back, [lline_pts], True, (200, 255, 0 ), 3)
cv2.imshow("res", back)
back = cv2.flip(back, 0)
cv2.imshow("flip_view", back)
world  = cv2.warpPerspective(back, mat, (320, 240) ,flags=cv2.INTER_LINEAR)
res = cv2.addWeighted(img, .6, world, .4, 0)
cv2.imshow("world", res)

cv2.waitKey(0)
cv2.destroyAllWindows()