till camera pose and 3D points: TODO: chirality

Code/EstimateFundamentalMatrix.py

@@ -44,9 +44,11 @@ def EstimateFundamentalMatrix(features):
         s = np.diag(s)
         s[2,2] = 0
         F = np.dot(u, np.dot(s, vt))
+
 
         if normalised:
             F = np.dot(T2.T, np.dot(F, T1))
+            F = F / F[2,2]
         return F
 
     else:

Code/ExtractCameraPose.py

@@ -1,20 +1,36 @@
 import numpy as np
 
 def ExtractCameraPose(E):
+    """
+    Args:
+        E (array): Essential Matrix
+        K (array): Intrinsic Matrix
+    Returns:
+        arrays: set of Rotation and Camera Centers
+    """
 
-    W = np.array([[0,-1,0],[1,0,0],[0,0,-1]])
-    Z = np.array([[0,1,0],[-1,0,0],[0,0,0]])
+    ##UPDATE
+    U, S, V_T = np.linalg.svd(E)
+    W = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
 
-    U,S,VT = np.linalg.svd(E)
-    S = np.dot(U, np.dot(Z, U.T))
-    R1 = np.dot(U, np.dot(W, VT))
-    R2 = np.dot(U, np.dot(W.T, VT))
+    # print("E svd U", U)
+    # print("E svd S", S)
+    # print("E svd U[:, 2]", U[:, 2])
+    R = []
+    C = []
+    R.append(np.dot(U, np.dot(W, V_T)))
+    R.append(np.dot(U, np.dot(W, V_T)))
+    R.append(np.dot(U, np.dot(W.T, V_T)))
+    R.append(np.dot(U, np.dot(W.T, V_T)))
+    C.append(U[:, 2])
+    C.append(-U[:, 2])
+    C.append(U[:, 2])
+    C.append(-U[:, 2])
 
-    t = np.array([-S[1,2],S[0,2],-S[0,1]]).reshape(3,1)
-
-    P1 = np.hstack((R1, t))
-    P2 = np.hstack((R1, -t))
-    P3 = np.hstack((R2, t))
-    P4 = np.hstack((R2, -t))
-    return [P1, P2, P3, P4]
+    for i in range(4):
+        if (np.linalg.det(R[i]) < 0):
+            R[i] = -R[i]
+            C[i] = -C[i]
+
+    return R, C
 

Code/GetInliersRANSAC.py

@@ -1,15 +1,16 @@
 import numpy as np
 from EstimateFundamentalMatrix import *
+import cv2
 
 def errorF(feature, F): 
     """
     check the epipolar constraint
     """
     x1,x2 = feature[3:5], feature[5:7]
-    x1tmp=np.array([x1[0], x1[1], 1]).T
-    x2tmp=np.array([x2[0], x2[1], 1])
+    x1tmp=np.array([x1[0], x1[1], 1])
+    x2tmp=np.array([x2[0], x2[1], 1]).T
 
-    error = np.dot(x1tmp, np.dot(F, x2tmp))
+    error = np.dot(x2tmp, np.dot(F, x1tmp))
 
     return np.abs(error)
 
@@ -25,24 +26,28 @@ def meanErrorF(features, F):
 
 
 def getInliers(features):
-    n_iterations = 1000
-    error_thresh = 0.02
+    n_iterations = 2000
+    error_thresh = 0.0002
     inliers_thresh = 0
     chosen_indices = []
-    chosen_f = 0
+    chosen_f = None
 
     for i in range(0, n_iterations):
-        indices = []
+
         #select 8 points randomly
         n_rows = features.shape[0]
         random_indices = np.random.choice(n_rows, size=8)
         features_8 = features[random_indices, :] 
         f_8 = EstimateFundamentalMatrix(features_8)
-        for j in range(n_rows):
-            feature = features[j]
-            error = errorF(feature, f_8)
-            if error < error_thresh:
-                indices.append(j)
+        # f_8, _ = cv2.findFundamentalMat(np.int32(features_8[:, 3:5]), np.int32(features_8[:, 5:7]),cv2.FM_LMEDS)
+        indices = []
+        if f_8 is not None:
+            for j in range(n_rows):
+                feature = features[j]
+                error = errorF(feature, f_8)
+
+                if error < error_thresh:
+                    indices.append(j)
 
         if len(indices) > inliers_thresh:
             inliers_thresh = len(indices)

Code/LinearTriangulation.py

@@ -0,0 +1,41 @@
+import numpy as np
+
+
+def LinearTriangulation(K, C1, R1, C2, R2, x1, x2):
+
+    I = np.identity(3)
+    C1 = np.reshape(C1, (3, 1))
+    C2 = np.reshape(C2, (3, 1))
+
+    P1 = np.dot(K, np.dot(R1, np.hstack((I, -C1))))
+    P2 = np.dot(K, np.dot(R2, np.hstack((I, -C2))))
+
+    p1T = P1[0,:].reshape(1,4)
+    p2T = P1[1,:].reshape(1,4)
+    p3T = P1[2,:].reshape(1,4)
+
+    p_dash_1T = P2[0,:].reshape(1,4)
+    p_dash_2T = P2[1,:].reshape(1,4)
+    p_dash_3T = P2[2,:].reshape(1,4)
+
+    all_X = []
+    for i in range(x1.shape[0]):
+        x = x1[i,0]
+        y = x1[i,1]
+        x_dash = x2[i,0]
+        y_dash = x2[i,1]
+
+
+        A = []
+        A.append((y * p3T) -  p2T)
+        A.append(p1T -  (x * p3T))
+        A.append((y_dash * p_dash_3T) -  p_dash_2T)
+        A.append(p_dash_1T -  (x_dash * p_dash_3T))
+
+        A = np.array(A).reshape(4,4)
+
+        _, _, vt = np.linalg.svd(A)
+        v = vt.T
+        x = v[:,-1]
+        all_X.append(x)
+    return np.array(all_X)