diff --git a/.gitignore b/.gitignore index 4bfba59e..a203c008 100644 --- a/.gitignore +++ b/.gitignore @@ -149,4 +149,4 @@ models/ .vscode/ runs/ wandb/ -*.DS_Store/ +**/.DS_Store diff --git a/src/lane_detection/UNet-LaneDetection/Lane_Projection.py b/src/lane_detection/UNet-LaneDetection/Lane_Projection.py new file mode 100644 index 00000000..5e3c1748 --- /dev/null +++ b/src/lane_detection/UNet-LaneDetection/Lane_Projection.py @@ -0,0 +1,61 @@ +import cv2 +import numpy as np + +def project_2D_to_3D_camera(x_array, y_array, depth_to_y_map, f_x, f_y, o_x, o_y): + ''' Given the pixel image coordinates and a constant mapping of depth to y-values, + calculate the resulting projection of points in the camera coordinate frame. + Requires the focal lengths in x and y and the optical centers + ''' + x_cam_coords = [] + y_cam_coords = [] + z_cam_coords = [] + + for x,y in zip(x_array,y_array): + x_project = (x - o_x)/f_x*depth_to_y_map[y] + y_project = (y - o_y)/f_y*depth_to_y_map[y] + z_project = depth_to_y_map[y] + + x_cam_coords.append(x_project) + y_cam_coords.append(y_project) + z_cam_coords.append(z_project) + + return x_cam_coords, y_cam_coords, z_cam_coords + +def transform_cam_to_world(M_ext, x_array, y_array, z_array): + ''' Transform your points from camera coordinates back to world coordinates + This is done via reversing the rotation and then translating + M_ext = |R T| ====> Upper 3 x 3 matrix is the rotation + |0 1| ====> Last col, first 3 rows is the translation + Assuming that your R matrix is orthogonal, R_inv = R_T = transpose(R) + The reverse of the translation is to simply add the -T vector to the points + You should be fine in the sense of using 3D coordinates and not homogeneous + (We aren't taking the inverse of M_ext) + So, to reverse, do R_inv(transpose([X Y Z]_cam) - T) = transpose([X Y Z]_world) + ''' + + R_inv = np.array(M_ext[0:3,0:3]).T # Assuming orthgonality + T = np.array([M_ext[0:3,3]]).T + + x_world = [] + y_world = [] + z_world = [] + + for x_c,y_c,z_c in zip(x_array,y_array,z_array): + cam_coords = np.array([[x_c,y_c,z_c]]).T + world_coords = np.matmul(R_inv,cam_coords - T) + x_world.append(world_coords[0,0]) + y_world.append(world_coords[1,0]) + z_world.append(world_coords[2,0]) + + return x_world, y_world, z_world + +def gen_depth_to_y_map(min_z, max_z, num_y_pixels): + ''' Create the constant depth to y map ''' + + depth_to_y_map = {} + delta_z = (max_z-min_z)/num_y_pixels + + for n,y in enumerate(range(num_y_pixels-1,-1,-1),1): + depth_to_y_map[y] = delta_z*n + + return depth_to_y_map diff --git a/src/lane_detection/UNet-LaneDetection/lane_fit.py b/src/lane_detection/UNet-LaneDetection/lane_fit.py index 56a0709a..58af1d29 100644 --- a/src/lane_detection/UNet-LaneDetection/lane_fit.py +++ b/src/lane_detection/UNet-LaneDetection/lane_fit.py @@ -5,7 +5,7 @@ import matplotlib.pyplot as plt import time -img_pth = '/Users/jasonyuan/Desktop/Test9.png' +img_pth = '/Users/jasonyuan/Desktop/Test_mask.png' # Add later to catch RankWarning # import numpy as np @@ -40,13 +40,13 @@ def lane_fitting(points): x_width = abs(sorted_points_x[-1][1] - sorted_points_x[0][1]) y_width = abs(sorted_points_y[-1][0] - sorted_points_y[0][0]) - if (x_width < 15) or (y_width < 15): # Hard-coded parameter, update maybe + if x_width < 20 or y_width < 25 or len(points) < 200: # Hard-coded parameter, update maybe return [[],[]] # print(sorted_points) pts_added = 0 total_pts = len(points) - num_windows = 20 + num_windows = 30 slice = int(total_pts//num_windows) @@ -60,15 +60,15 @@ def lane_fitting(points): x_avg = np.mean(group,axis=0)[1] y_avg = np.mean(group,axis=0)[0] - sigma_x = np.sqrt(np.sum(np.power(group[:,1]-x_avg,2))/group.shape[0]) - sigma_y = np.sqrt(np.sum(np.power(group[:,0]-y_avg,2))/group.shape[0]) + # sigma_x = np.sqrt(np.sum(np.power(group[:,1]-x_avg,2))/group.shape[0]) + # sigma_y = np.sqrt(np.sum(np.power(group[:,0]-y_avg,2))/group.shape[0]) # print(sigma_x, sigma_y) - if (sigma_x < 5) and (sigma_y < 5): - fit_points.append([y_avg,x_avg]) + # if (sigma_x < 5) and (sigma_y < 5): + fit_points.append([y_avg,x_avg]) - if len(fit_points) == 0: + if len(fit_points) <= 3: return [[],[]] fit_points = np.array(fit_points) @@ -77,10 +77,64 @@ def lane_fitting(points): x = fit_points[:,1] y = fit_points[:,0] tck,u = interpolate.splprep([x,y],k=3,s=32) + # print(u) + # u = np.linspace(u[0]-0.05,u[-1]+0.05,500) # print(tck) out = interpolate.splev(u,tck) return out +def gen_fit_points(image): + + if len(image.shape) == 3: + input = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + else: + input = image + + params = cv2.SimpleBlobDetector_Params() + params.blobColor = 255 + params.minDistBetweenBlobs = 20 + params.minThreshold = 0 + params.maxThreshold = 255 + params.minArea = 10 + params.filterByCircularity = True + params.minCircularity = 0.05 + params.maxCircularity = 2 + params.filterByConvexity = True + params.minConvexity = 0.05 + params.maxConvexity = 2 + params.filterByInertia = True + params.minInertiaRatio = 0.05 + params.maxInertiaRatio = 2 + detector = cv2.SimpleBlobDetector_create(params) + keypoints = detector.detect(input) + + for kp in keypoints: + cv2.circle(input,(int(kp.pt[0]),int(kp.pt[1])),int(kp.size/2),0,-1) + + input_norm = input/255 + all_fit_points = [] + + rows = np.where(input_norm==1)[0].reshape(-1,1) + cols = np.where(input_norm==1)[1].reshape(-1,1) + coords = np.concatenate((rows,cols),axis=1) # (y,x) points + + if coords.shape[0] < 5: + return all_fit_points + + clustering = DBSCAN(eps=5, min_samples=35).fit(coords) + labels = clustering.labels_ + + clusters = sort_by_cluster(labels,coords) + + for label,pts in clusters.items(): + if label == -1: + continue + else: + out = lane_fitting(pts) + all_fit_points.append(out) + + return all_fit_points + if __name__ == "__main__": # start = time.perf_counter() @@ -91,7 +145,7 @@ def lane_fitting(points): cols = np.where(input_norm==1)[1].reshape(-1,1) coords = np.concatenate((rows,cols),axis=1) # (y,x) points - clustering = DBSCAN(eps=15, min_samples=30).fit(coords) + clustering = DBSCAN(eps=9, min_samples=35).fit(coords) labels = clustering.labels_ for i,pt in enumerate(coords): @@ -118,13 +172,6 @@ def lane_fitting(points): if label == -1: continue else: - # coefficients = lane_fitting(pts,15) - # poly = np.poly1d(coefficients) - # min_x = pts[0][1] - # max_x = pts[len(pts)-1][1] - # - # xrange = np.linspace(min_x,max_x,endpoint=True) - # plt.plot(xrange,poly(xrange),'-',c='k') out = lane_fitting(pts) # print(out[0]) # print(out[1])