Reprojection equation function commits

.gitignore

@@ -149,4 +149,4 @@ models/
 .vscode/
 runs/
 wandb/
-*.DS_Store/
+**/.DS_Store

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

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])