files added

Author: naitri

DisparityMap.py

@@ -0,0 +1,138 @@
+import numpy as np
+import cv2
+import glob
+import matplotlib.pyplot as plt
+from tqdm import tqdm
+
+
+
+def get_blocks(img,window_size):
+    height,width = img.shape
+    window_set=[]
+    window_loc = []
+    for y in range(window_size, height - window_size):
+        for x in range(window_size, width - window_size):
+            img_window = img[y:y + window_size, x:x + window_size]
+            window_set.append(img_window)
+            window_loc.append((y,x))
+    return window_set,window_loc
+
+def get_boundary_condition(x,y,search_range,width,height):
+    
+
+    range_min = max(0, x-search_range)
+    range_max = min(width,x+search_range )
+
+    return range_max,range_min
+
+def get_sdd(imgl_window,imgr_window):
+    max = 1e10
+    if imgl_window.shape == imgr_window.shape:
+        diff = np.abs(imgl_window - imgr_window)
+
+        SSD = np.sum(np.square(imgl_window - imgr_window))
+        return SSD
+    else:
+        return max
+
+def get_disparity(img1,img2,search_range,type):
+
+    if type==1:
+        print("method 1")
+        imgl = img1.copy()
+        imgr = img2.copy()
+        disparityImg = np.zeros(img2.shape)
+        window_size = 21
+        
+        
+        #get a patch in left image
+        height,width = img1.shape
+        for y in tqdm(range(window_size,height-window_size)):
+            for x in range(window_size,width-window_size):
+
+                
+                #get window boundary conditions
+                # range_max,range_min = get_boundary_condition(x,y,50,width,height)
+                range_min = max(0, x-100)
+                range_max = min(width,x+100)
+                imgl_window = imgl[y:y+window_size, x:x+window_size]
+               
+                first = True
+                min_val = None
+                index = None
+                #search for corresponding patch in right image
+                for i in range(range_min,range_max):
+
+                    imgr_window = imgr[y:y+window_size, i:i+window_size]
+              
+
+                    SSD = get_sdd(imgl_window,imgr_window)
+                    if first:
+                        min_val = SSD
+                        index = i
+                        first = False
+                    else:
+                        if SSD < min_val:
+                            min_val = SSD
+                            index = i
+                
+
+
+                d = np.abs(index - x)
+
+                disparityImg[y,x] = d
+        
+        return disparityImg
+
+    if type == 2:
+        print("method 2")
+        # img1 = cv2.resize(img1, (int(img1.shape[1]*0.5), int(img1.shape[0] *0.5)))
+        # img2 = cv2.resize(img2, (int(img2.shape[1]*0.5), int(img2.shape[0] *0.5)))
+        imgl = img1.copy()
+        imgr = img2.copy()
+        disparityImg = np.zeros(img2.shape)
+        window_size = 15
+        #get a patch in left image
+        height,width = img1.shape
+        
+        imgl = imgl.astype(np.int32)
+        imgr = imgr.astype(np.int32)
+
+        for y in tqdm(range(0,height-window_size)):
+
+            imgl_window_set = []
+            imgr_window_set = []
+
+            for x in range(0,width-window_size):
+                left_window = imgl[y:y+window_size, x:x+window_size]
+                right_window = imgr[y:y+window_size, x:x+window_size]
+                imgl_window_set.append(left_window.flatten())
+                imgr_window_set.append(right_window.flatten())
+
+            imgl_window_set = np.array(imgl_window_set)
+            imgr_window_set = np.array(imgr_window_set)
+            print(len(imgl_window_set))
+            for i in range(len(imgl_window_set)):
+
+                SSD = np.sum(np.square(imgr_window_set- imgl_window_set[i]),axis=1)
+                index = np.argmin(SSD)
+                disparity = np.abs(i - index)
+                # disparity = ((disparity+1) / 2) * 255
+                disparityImg[y,i] = np.uint8(disparity)
+
+
+
+        return disparityImg
+
+    else:   
+        return np.zeros(img2.shape)
+
+
+
+
+def get_depth(disparityImg,b,f):
+    depth = (b * f) / (disparityImg + 1e-10)
+    depth[depth > 10000] = 10000
+
+    depth_map = np.uint8(depth * 255 / np.max(depth))
+    return depth_map

EstimateFEmatrix.py

@@ -0,0 +1,112 @@
+import numpy as np
+import cv2
+import glob
+
+def normalize_points(pts):
+    '''
+    https://www.cc.gatech.edu/classes/AY2016/cs4476_fall/results/proj3/html/arao83/index.html
+    '''
+    mean_ =np.mean(pts,axis=0)
+
+    #finding centre
+    u = pts[:,0] - mean_[0]
+    v = pts[:,1] - mean_[1]
+
+    sd_u = 1/np.std(pts[:,0])
+    sd_v = 1/np.std(pts[:,1])
+    Tscale = np.array([[sd_u,0,0],[0,sd_v,0],[0,0,1]])
+    Ta = np.array([[1,0,-mean_[0]],[0,1,-mean_[1]],[0,0,1]])
+    T = np.dot(Tscale,Ta)
+
+    pt = np.column_stack((pts,np.ones(len(pts))))
+    norm_pts = (np.dot(T,pt.T)).T
+
+    return norm_pts,T
+
+def estimate_F(img1_pts,img2_pts):
+
+    #normalize points
+    img1_pts,T1 = normalize_points(img1_pts)
+    img2_pts,T2 = normalize_points(img2_pts)
+
+    x1 = img1_pts[:,0]
+    y1 = img1_pts[:,1]
+    x1dash = img2_pts[:,0]
+    y1dash = img2_pts[:,1]
+    A = np.zeros((len(x1),9))
+
+    for i in range(len(x1)):
+        A[i] = np.array([x1dash[i]*x1[i],x1dash[i]*y1[i],x1dash[i], y1dash[i]*x1[i],y1dash[i]*y1[i],y1dash[i],x1[i],y1[i],1])
+
+    #taking SVD of A for estimation of F
+    U, S, V = np.linalg.svd(A,full_matrices=True)
+    F_est = V[-1, :]
+    F_est = F_est.reshape(3,3)
+
+    # Enforcing rank 2 for F
+    ua,sa,va = np.linalg.svd(F_est,full_matrices=True)
+    sa = np.diag(sa)
+
+    sa[2,2] = 0
+   
+
+    F = np.dot(ua,np.dot(sa,va))
+    # F, mask = cv2.findFundamentalMat(img1_pts,img2_pts,cv2.FM_LMEDS)
+    F = np.dot(T2.T, np.dot(F, T1))
+
+    return F
+
+def ransac(pt1,pt2):
+    n_rows = np.array(pt1).shape[0]
+    no_iter = 1000
+    threshold = 0.05
+    inliers = 0
+    
+    final_indices = []
+    for i in range(no_iter):
+        indices = []
+
+        #randomly select 8 points
+        random = np.random.choice(n_rows,size = 8)
+        img1_8pt = pt1[random]
+        img2_8pt = pt2[random]
+    
+        F_est = estimate_F(img1_8pt,img2_8pt)
+
+        for j in range(n_rows):
+            x1 = pt1[j]
+            x2 = pt2[j]
+
+            #error computation
+            pt1_ = np.array([x1[0],x1[1],1])
+            pt2_ = np.array([x2[0],x2[1],1])
+            error = np.dot(pt1_.T,np.dot(F_est,pt2_))
+            
+            if np.abs(error) < threshold:
+                indices.append(j)
+                
+        if len(indices) > inliers:
+            inliers = len(indices)
+            final_indices = indices
+            F = F_est
+
+ 
+    img1_points = pt1[final_indices]
+    img2_points = pt2[final_indices]
+
+
+    return img1_points,img2_points, F
+
+
+
+
+
+def estimate_E(k1,k2,F):
+    E_est = np.dot(k2.T,np.dot(F,k1))
+    #reconstructing E by correcting singular values
+    U, S, V = np.linalg.svd(E_est,full_matrices=True)
+    S = np.diag(S)
+    S[0,0],S[1,1],S[2,2] = 1,1,0
+    E = np.dot(U,np.dot(S,V))
+
+    return E

PoseEstimation.py

@@ -0,0 +1,119 @@
+
+import numpy as np
+import cv2
+import glob
+import matplotlib.pyplot as plt
+
+
+
+
+def point_triangulation(k1,k2,pt1,pt2,R1,C1,R2,C2):
+    points_3d = []
+
+    I = np.identity(3)
+    C1 = C1.reshape(3,1)
+    C2 = C2.reshape(3,1)
+
+    #calculating projection matrix P = K[R|T]
+    P1 = np.dot(k1,np.dot(R1,np.hstack((I,-C1))))
+    P2 = np.dot(k2,np.dot(R2,np.hstack((I,-C2))))
+  
+    #homogeneous coordinates for images
+    xy = np.hstack((pt1,np.ones((len(pt1),1))))
+    xy_cap = np.hstack((pt2,np.ones((len(pt1),1))))
+
+    
+    p1,p2,p3 = P1
+    p1_cap, p2_cap,p3_cap = P2
+
+    #constructing contraints matrix
+    for i in range(len(xy)):
+        A = []
+        x = xy[i][0]
+        y = xy[i][1]
+        x_cap = xy_cap[i][0]
+        y_cap = xy_cap[i][1] 
+        
+        A.append((y*p3) - p2)
+        A.append((x*p3) - p1)
+        
+        A.append((y_cap*p3_cap)- p2_cap)
+        A.append((x_cap*p3_cap) - p1_cap)
+
+        A = np.array(A).reshape(4,4)
+
+        _, _, v = np.linalg.svd(A)
+        x_ = v[-1,:]
+        x_ = x_/x_[-1]
+        x_ = x_[:3]
+        points_3d.append(x_)
+
+
+    return points_3d
+
+def linear_triangulation(R_Set,T_Set,pt1,pt2,k1,k2):
+    R1_ = np.identity(3)
+    T1_ = np.zeros((3,1))
+    points_3d_set = []
+    for i in range(len(R_Set)):
+        points3d = point_triangulation(k1,k2,pt1,pt2,R1_,T1_,R_Set[i],T_Set[i])
+        points_3d_set.append(points3d)
+
+    return points_3d_set
+
+def get_RTset(E):
+
+    U, S, V = np.linalg.svd(E,full_matrices=True)
+    W = np.array([[0,-1,0],[1,0,0],[0,0,1]])
+
+    R1 = np.dot(U,np.dot(W,V))
+    R2 = np.dot(U,np.dot(W,V))
+    R3 = np.dot(U,np.dot(W.T,V))
+    R4 = np.dot(U,np.dot(W.T,V))
+
+    T1 = U[:,2]
+    T2 = -U[:,2]
+    T3 = U[:,2]
+    T4 = -U[:,2]
+
+    R = [R1,R2,R3,R4]
+    T = [T1,T2,T3,T4]
+
+    for i in range(len(R)):
+        if (np.linalg.det(R[i]) < 0):
+            R[i] = -R[i]
+            T[i] = -T[i]
+
+    return R, T
+
+def compute_cheriality(pt,r3,t):
+    count_depth = 0
+    for xy in pt:
+        if np.dot(r3,(xy-t)) > 0 and t[2] > 0:
+            count_depth +=1
+    return count_depth
+
+def extract_pose(E,pt1,pt2,k1,k2):
+    #get four rotation and translation matrices
+    R_set, T_set = get_RTset(E)
+
+    #get 3D points using triangulation
+    pts_3d = linear_triangulation(R_set,T_set,pt1,pt2,k1,k2)
+    threshold = 0
+    #Four sets are available for each possibility
+    for i in range(len(R_set)):
+        R = R_set[i]
+        T = T_set[i]
+        r3 = R[2]
+        pt3d = pts_3d[i]
+
+        #calculating which R satisfies the condition
+        num_depth_positive = compute_cheriality(pt3d,r3,T)
+        if num_depth_positive > threshold:
+            index = i 
+            threshold = num_depth_positive
+
+    R_best = R_set[index]
+    T_best = T_set[index]
+
+    return R_best,T_best