├── README.md ├── data └── final_project_point_cloud.fuse ├── images ├── final_result.png ├── final_result_orig.png ├── pc1.png ├── pc10.png ├── pc11.png ├── pc12.png ├── pc13.png ├── pc14.png ├── pc15.png ├── pc16.png ├── pc17.png ├── pc18.png ├── pc19.png ├── pc2.png ├── pc20.png ├── pc21.png ├── pc3.png ├── pc4.png ├── pc5.png ├── pc6.png ├── pc7.png ├── pc8.png ├── pc9.png ├── road_plane_1.png ├── road_plane_2.png ├── road_plane_seg.png ├── road_seg_1.png ├── road_xyz_1.png ├── road_xyz_2.png └── sklearn_clustering.png └── lane-detection.py /README.md: -------------------------------------------------------------------------------- 1 | # lane-detection 2 | 3 | Authors: [William Wang](https://github.com/willshw), [Felix Wang](https://yanweiw.github.io) 4 | 5 | ### Problem Statement 6 | 7 | Find equations for lane markings in point cloud data. 8 | 9 | ![](images/road_xyz_1.png) 10 | 11 | ### Data Specification 12 | 13 | final_project_point_cloud.fuse : 14 | 15 | Point cloud data. 16 | Data format: 17 | [latitude] [longitude] [altitude] [intensity] 18 | 19 | Notice lane markings are reflexive and thus corresponding points will have higher intensity. 20 | 21 | ### Usage 22 | 23 | 1. To get pcl_viewer: sudo apt-get install pcl-tool 24 | 2. To open and display .pcd file: pcl_viewer filename.pcd 25 | - To turn on intensity, press 5 26 | 27 | Package Used: 28 | 1. PCL 29 | 2. pyproj 30 | 3. numpy 31 | 4. scipy 32 | 5. matplotlib 33 | 6. sklearn 34 | 35 | How-To run program: 36 | 37 | 1. Put program in the same data folder as **final_project_point_cloud.fuse** 38 | 2. python lane-detection.py 39 | 40 | ### Method 41 | 42 | Please refer to the [slides](https://docs.google.com/presentation/d/13mMAI7IvOBTep5CeOmJUhD1hfpUZbfGIj1Kam1IZmLU/edit?usp=sharing) for details. Feel free to contact authors if you have further questions. 43 | -------------------------------------------------------------------------------- /images/final_result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/final_result.png -------------------------------------------------------------------------------- /images/final_result_orig.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/final_result_orig.png -------------------------------------------------------------------------------- /images/pc1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc1.png -------------------------------------------------------------------------------- /images/pc10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc10.png -------------------------------------------------------------------------------- /images/pc11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc11.png -------------------------------------------------------------------------------- /images/pc12.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc12.png -------------------------------------------------------------------------------- /images/pc13.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc13.png -------------------------------------------------------------------------------- /images/pc14.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc14.png -------------------------------------------------------------------------------- /images/pc15.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc15.png -------------------------------------------------------------------------------- /images/pc16.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc16.png -------------------------------------------------------------------------------- /images/pc17.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc17.png -------------------------------------------------------------------------------- /images/pc18.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc18.png -------------------------------------------------------------------------------- /images/pc19.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc19.png -------------------------------------------------------------------------------- /images/pc2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc2.png -------------------------------------------------------------------------------- /images/pc20.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc20.png -------------------------------------------------------------------------------- /images/pc21.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc21.png -------------------------------------------------------------------------------- /images/pc3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc3.png -------------------------------------------------------------------------------- /images/pc4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc4.png -------------------------------------------------------------------------------- /images/pc5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc5.png -------------------------------------------------------------------------------- /images/pc6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc6.png -------------------------------------------------------------------------------- /images/pc7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc7.png -------------------------------------------------------------------------------- /images/pc8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc8.png -------------------------------------------------------------------------------- /images/pc9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/pc9.png -------------------------------------------------------------------------------- /images/road_plane_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/road_plane_1.png -------------------------------------------------------------------------------- /images/road_plane_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/road_plane_2.png -------------------------------------------------------------------------------- /images/road_plane_seg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/road_plane_seg.png -------------------------------------------------------------------------------- /images/road_seg_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/road_seg_1.png -------------------------------------------------------------------------------- /images/road_xyz_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/road_xyz_1.png -------------------------------------------------------------------------------- /images/road_xyz_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/road_xyz_2.png -------------------------------------------------------------------------------- /images/sklearn_clustering.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/willshw/lane-detection/afe6c5d7a308dd6b2576811c460d6105b81ddabc/images/sklearn_clustering.png -------------------------------------------------------------------------------- /lane-detection.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import copy 4 | import numpy as np 5 | import pyproj as pj 6 | from numpy.polynomial import polynomial as P 7 | 8 | from scipy import stats 9 | from sklearn.cluster import KMeans, DBSCAN 10 | 11 | import pcl 12 | 13 | import matplotlib.pyplot as plt 14 | import matplotlib.colors as clr 15 | from mpl_toolkits.mplot3d import Axes3D 16 | 17 | def get_xy(lat, lon): 18 | ''' 19 | convert lat, lon to x, y 20 | ''' 21 | # setup your projections 22 | crs_wgs = pj.Proj(init='epsg:4326') # assuming you're using WGS84 geographic 23 | crs_bng = pj.Proj(init='epsg:3857') # use a locally appropriate projected CRS 24 | 25 | # then cast your geographic coordinate pair to the projected system 26 | x, y = pj.transform(crs_wgs, crs_bng, lon, lat) 27 | 28 | return x, y 29 | 30 | def get_latlon(x, y): 31 | ''' 32 | convert x, y to lat, lon 33 | ''' 34 | # setup your projections 35 | crs_wgs = pj.Proj(init='epsg:4326') # assuming you're using WGS84 geographic 36 | crs_bng = pj.Proj(init='epsg:3857') # use a locally appropriate projected CRS 37 | 38 | # then cast your geographic coordinate pair to the projected system 39 | lat, lon = pj.transform(crs_bng, crs_wgs, x, y) 40 | 41 | return lat, lon 42 | 43 | def normalize(i): 44 | return i/np.linalg.norm(i) 45 | 46 | def plot(data, num, step): 47 | ''' 48 | use matplotlib to visualize points 49 | ''' 50 | 51 | fig = plt.figure() 52 | 53 | ax = Axes3D(fig) 54 | ax.scatter(data[0:num:step,0], data[0:num:step,1], data[0:num:step,2], c=data[0:num:step,3]) 55 | ax.set_xlabel('X Label') 56 | ax.set_ylabel('Y Label') 57 | ax.set_zlabel('Z Label') 58 | 59 | plt.show() 60 | 61 | def find_road_plane(points): 62 | 63 | # cloud = pcl.PointCloud() 64 | # cloud.from_array(xyz_data[:,0:3].astype('float32')) 65 | 66 | cloud = pcl.PointCloud_PointXYZI() 67 | cloud.from_array(xyz_data.astype('float32')) 68 | 69 | # fitler statistical outlier 70 | # fil_stat = cloud.make_statistical_outlier_filter() 71 | # fil_stat = cloud.make_statistical_outlier_filter() 72 | # fil_stat.set_mean_k(50) 73 | # fil_stat.set_std_dev_mul_thresh(1.0) 74 | # cloud_filtered = fil_stat.filter() 75 | 76 | # print "Statistical Inlier Number:", cloud_filtered.size 77 | 78 | # find normal plane 79 | seg = cloud.make_segmenter_normals(ksearch=50) 80 | seg.set_optimize_coefficients(True) 81 | seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE) 82 | seg.set_normal_distance_weight(0.001) 83 | seg.set_method_type(pcl.SAC_RANSAC) 84 | seg.set_max_iterations(100) 85 | seg.set_distance_threshold(0.3) 86 | indices, model = seg.segment() 87 | 88 | print "Road Plane Model:", model 89 | 90 | cloud_plane = cloud.extract(indices, negative=False) 91 | 92 | # NG : const char* not str 93 | # cloud_plane.to_file('table_scene_mug_stereo_textured_plane.pcd') 94 | pcl.save(cloud_plane, 'road_plane.pcd') 95 | print "Road plane point cloud file road_plane.pcd saved." 96 | 97 | return cloud_plane.to_array(), np.array(indices) 98 | 99 | def read_points(filename): 100 | file = open(filename, "r") 101 | txt_data = file.readlines() 102 | 103 | raw_data = np.zeros((len(txt_data), len(txt_data[0].split()))) 104 | xyz_data = np.zeros((len(txt_data), len(txt_data[0].split()))) 105 | # rgb = np.zeros((len(txt_data), 3)) 106 | 107 | for idx, line in enumerate(txt_data): 108 | raw_data[idx,:] = line.split() 109 | xyz_data[idx,:] = copy.deepcopy(raw_data[idx,:]) 110 | 111 | # (r, g, b) = colorsys.hsv_to_rgb(float(xyz_data[idx,-1]) / 255, 1.0, 1.0) 112 | # R, G, B = int(255 * r), int(255 * g), int(255 * b) 113 | # rgb[idx,:] = np.array([R, G, B]) 114 | 115 | x, y = get_xy(xyz_data[:, 0], xyz_data[:, 1]) 116 | xyz_data[:, 0] = np.array(x) 117 | xyz_data[:, 1] = np.array(y) 118 | 119 | 120 | xyz_data[:, 2] = xyz_data[:, 2] - np.min(xyz_data[:, 2]) 121 | 122 | 123 | print "Finished reading raw data." 124 | 125 | return xyz_data 126 | 127 | def plot_points(points, skip=100, point_size=1): 128 | points = points - np.min(points, axis=0) 129 | # plt.ion() 130 | norm = clr.Normalize(vmin=0,vmax=255) 131 | 132 | fig = plt.figure() 133 | ax = Axes3D(fig) 134 | 135 | max_range = np.array([points[:,0].max()-points[:,0].min(), points[:,1].max()-points[:,1].min(), points[:,2].max()-points[:,2].min()]).max() / 2.0 136 | mid_x = (points[:,0].max() + points[:,0].min()) * 0.5 137 | mid_y = (points[:,1].max() + points[:,1].min()) * 0.5 138 | mid_z = (points[:,2].max() + points[:,2].min()) * 0.5 139 | 140 | ax.set_xlim(mid_x - max_range, mid_x + max_range) 141 | ax.set_ylim(mid_y - max_range, mid_y + max_range) 142 | ax.set_zlim(mid_z - max_range, mid_z + max_range) 143 | 144 | ax.set_xlabel('X') 145 | ax.set_ylabel('Y') 146 | ax.set_zlabel('Z') 147 | 148 | num = points.shape[0] 149 | sc = ax.scatter(points[0:num:skip,0], points[0:num:skip,1], \ 150 | points[0:num:skip,2], c=points[0:num:skip,3], norm=norm, s=point_size) 151 | 152 | plt.colorbar(sc) 153 | plt.show() 154 | 155 | def thresholding(points, threshold=30): 156 | return points[np.logical_and(points[:,3]>threshold, points[:,3]<256)] 157 | 158 | def line_clustering(points, orig_points): 159 | # 2d clustering based on line structure in x-y plane first 160 | fig2=plt.figure() 161 | ax2 = fig2.add_subplot(111) 162 | xy = points[:, 0:2] 163 | xy = xy - np.min(xy, axis=0) 164 | ax2.scatter(xy[:,0], xy[:,1]) 165 | 166 | # initialization of m and c 167 | A = np.vstack([xy[:,0], np.ones(len(xy))]).T 168 | m, c = np.linalg.lstsq(A, xy[:,1])[0] 169 | c1 = c-20# + np.random.normal(0, 10) 170 | c2 = c-10 171 | c3 = c# + np.random.normal(0, 10) 172 | c4 = c+10 173 | c5 = c+20 174 | plt.plot(xy[:,0], m*xy[:,0] + c1, 'r') 175 | plt.plot(xy[:,0], m*xy[:,0] + c2, 'g') 176 | plt.plot(xy[:,0], m*xy[:,0] + c3, 'b') 177 | plt.plot(xy[:,0], m*xy[:,0] + c4, 'y') 178 | plt.plot(xy[:,0], m*xy[:,0] + c5, 'k') 179 | 180 | 181 | for i in range(10): 182 | # classify 183 | y1 = np.absolute(A.dot([[m],[c1]]) - xy[:,[1]]) 184 | y2 = np.absolute(A.dot([[m],[c2]]) - xy[:,[1]]) 185 | y3 = np.absolute(A.dot([[m],[c3]]) - xy[:,[1]]) 186 | y4 = np.absolute(A.dot([[m],[c4]]) - xy[:,[1]]) 187 | y5 = np.absolute(A.dot([[m],[c5]]) - xy[:,[1]]) 188 | 189 | k1 = np.squeeze(np.all([y1 30, 3] = 255 323 | points = copy.deepcopy(orig_points) 324 | 325 | skip = 10 326 | points = points - np.min(orig_points, axis=0) 327 | line_xyz = line - np.min(orig_points, axis=0) 328 | min_line_xyz = np.min(line_xyz, axis=0) 329 | 330 | # plt.ion() 331 | 332 | # print "Min X:{:15.2f}, Max X:{:15.2f}".format(np.min(points[:,0]), np.max(points[:,0])) 333 | # print "Min Y:{:15.2f}, Max Y:{:15.2f}".format(np.min(points[:,1]), np.max(points[:,1])) 334 | # print "Min Z:{:15.2f}, Max Z:{:15.2f}".format(np.min(points[:,2]), np.max(points[:,2])) 335 | 336 | # print "Min X:{:15.2f}, Max X:{:15.2f}".format(np.min(line_xyz[:,0]), np.max(line_xyz[:,0])) 337 | # print "Min Y:{:15.2f}, Max Y:{:15.2f}".format(np.min(line_xyz[:,1]), np.max(line_xyz[:,1])) 338 | # print "Min Z:{:15.2f}, Max Z:{:15.2f}".format(np.min(line_xyz[:,2]), np.max(line_xyz[:,2])) 339 | 340 | norm = clr.Normalize(vmin=0,vmax=255) 341 | 342 | fig = plt.figure() 343 | ax_f = Axes3D(fig) 344 | 345 | max_range = np.array([points[:,0].max()-points[:,0].min(), points[:,1].max()-points[:,1].min(), points[:,2].max()-points[:,2].min()]).max() / 2.0 346 | mid_x = (points[:,0].max() + points[:,0].min()) * 0.5 347 | mid_y = (points[:,1].max() + points[:,1].min()) * 0.5 348 | mid_z = (points[:,2].max() + points[:,2].min()) * 0.5 349 | 350 | ax_f.set_xlim(mid_x - max_range, mid_x + max_range) 351 | ax_f.set_ylim(mid_y - max_range, mid_y + max_range) 352 | ax_f.set_zlim(mid_z - max_range, mid_z + max_range) 353 | 354 | ax_f.set_xlabel('X') 355 | ax_f.set_ylabel('Y') 356 | ax_f.set_zlabel('Z') 357 | 358 | num = points.shape[0] 359 | sc = ax_f.scatter(points[0:num:skip,0], points[0:num:skip,1], \ 360 | points[0:num:skip,2], c=points[0:num:skip,3], norm=norm, s=4) 361 | 362 | for i in range(len(K)): 363 | X = K[i][:,0] 364 | Y = K[i][:,1:3] 365 | p=P.polyfit(X,Y,2) 366 | 367 | ax_f.plot(xs + min_line_xyz[0], p[0,0] + p[1,0]*(xs) + p[2,0]*(xs)**2 + min_line_xyz[1], \ 368 | p[0,1] + p[1,1]*xs + p[2,1]*xs**2 + min_line_xyz[2]) 369 | 370 | plt.colorbar(sc) 371 | plt.show() 372 | 373 | if __name__ == '__main__': 374 | 375 | xyz_data = read_points("final_project_point_cloud.fuse") 376 | 377 | # save xyz data to pcd file 378 | road_xyz = pcl.PointCloud_PointXYZI() 379 | road_xyz.from_array(xyz_data.astype('float32')) 380 | pcl.save(road_xyz, 'road_xyz.pcd') 381 | print "Road plane segment point cloud file road_xyz.pcd saved." 382 | 383 | print "Min X:{:15.2f}, Max X:{:15.2f}".format(np.min(xyz_data[:,0]), np.max(xyz_data[:,0])) 384 | print "Min Y:{:15.2f}, Max Y:{:15.2f}".format(np.min(xyz_data[:,1]), np.max(xyz_data[:,1])) 385 | print "Min Z:{:15.2f}, Max Z:{:15.2f}".format(np.min(xyz_data[:,2]), np.max(xyz_data[:,2])) 386 | print "Min I:{:15.2f}, Max I:{:15.2f}".format(np.min(xyz_data[:,3]), np.max(xyz_data[:,3])) 387 | 388 | # find road plane using pcl call 389 | road_plane, road_plane_idx = find_road_plane(xyz_data) 390 | road_plane_flatten = road_plane[:,0:2] 391 | 392 | # cluster road plane, and find the road segment 393 | db = DBSCAN(eps=0.5, min_samples=5).fit_predict(road_plane_flatten) 394 | 395 | largest_cluster_label = stats.mode(db).mode[0] 396 | largest_cluster_points_idx = np.array(np.where(db == largest_cluster_label)).ravel() 397 | 398 | road_plane_seg_idx = road_plane_idx[largest_cluster_points_idx] 399 | road_plane_seg = copy.deepcopy(xyz_data[road_plane_seg_idx, :]) 400 | 401 | # save road segment point cloud to numpy and pcd file 402 | 403 | np.save('road_plane_seg.npy', road_plane_seg) 404 | print "Road plane segment point count:{}".format(road_plane_seg.shape) 405 | print "Road plane segment point cloud file road_plane_seg.npy saved." 406 | 407 | road_plane_seg_cloud = pcl.PointCloud_PointXYZI() 408 | road_plane_seg_cloud.from_array(road_plane_seg.astype('float32')) 409 | 410 | pcl.save(road_plane_seg_cloud, 'road_plane_seg.pcd') 411 | print "Road plane segment point cloud file road_plane_seg.pcd saved." 412 | 413 | # plot road plane seg 414 | # plot_points(road_plane_seg, skip=100) 415 | 416 | line = thresholding(road_plane_seg) 417 | # plot_points(line, skip=1) 418 | 419 | line_clustering(line, xyz_data) 420 | 421 | # intensity = copy.deepcopy(xyz_data[:, 3]) 422 | # xyz_data[intensity > 30, 3] = 255 423 | # points = copy.deepcopy(xyz_data) 424 | # plot_points(points, skip=10, point_size=4) --------------------------------------------------------------------------------