def test_M2_solution(pts1, pts2, intrinsics, M): ''' Estimate all possible M2 and return the correct M2 and 3D points P :param pred_pts1: :param pred_pts2: :param intrinsics: :param M: a scalar parameter computed as max (imwidth, imheight) :return: M2, the extrinsics of camera 2 C2, the 3x4 camera matrix P, 3D points after triangulation (Nx3) ''' F = submission.eightpoint(pts1, pts2, M) F = np.asarray(F) intrinsics = np.load("../data/intrinsics.npz") K1 = intrinsics['K1'] K2 = intrinsics['K2'] #print(K1.shape,K2.shape) e = submission.essentialMatrix(F, K1, K2) M1 = M1 = np.zeros((3, 4)) M1[0, 0] = 1 M1[1, 1] = 1 M1[2, 2] = 1 M2_ = helper.camera2(e) print(M2_.shape) C1 = K1 @ M1 tee = 0 err_final = 10000 for i in range(0, 4): print(i) M2 = M2_[:, :, i] C2 = K2 @ M2 #print(C2.shape) W, err = submission.triangulate(C1, pts1, C2, pts2) z = W[:, 2] #print(W[:,2]) tee = z[z < 0] #print(tee) #print(err) #if err<err_final: if len(tee) == 0: err_final = err #print(err_final) C2_final = C2 M2_final = M2 P_final = W print(P_final) print(err_final) return M2_final, C2_final, P_final
def bestM2(pts1, pts2, M, K1, K2): F = sub.eightpoint(pts1, pts2, M) E = sub.essentialMatrix(F, K1, K2) M1 = np.zeros((3,4)) M1[0,0] = 1 M1[1,1] = 1 M1[2,2] = 1 C1 = np.matmul(K1, M1) P = [] error = [] num_pos = [] Marray = hp.camera2(E) h,w,d = Marray.shape for i in range(0, d): C2 = np.matmul(K2, Marray[:,:,i]) Pi, erri = sub.triangulate(C1, pts1, C2, pts2) P.append(Pi) error.append(erri) ind = np.where(Pi[:,2] > 0) num_pos.append(len(ind[0])) P = np.stack(P, axis=-1) correct = np.equal(num_pos, P.shape[0]) ind = np.where(correct)[0][0] M2 = Marray[:,:,ind] M2 = np.reshape(M2, (M2.shape[0],M2.shape[1])) P = P[:,:,ind] P = np.reshape(P, (P.shape[0],P.shape[1])) C2 = np.matmul(K2,M2) return M1,C1,M2,C2,P
def test_M2_solution(pts1, pts2, intrinsics): ''' Estimate all possible M2 and return the correct M2 and 3D points P :param pred_pts1: :param pred_pts2: :param intrinsics: :return: M2, the extrinsics of camera 2 C2, the 3x4 camera matrix P, 3D points after triangulation (Nx3) ''' K1 = intrinsics['K1'] K2 = intrinsics['K2'] M1 = np.hstack(( np.identity(3), np.zeros((3,1)) )) M = 640 F = sub.eightpoint(pts1, pts2, M) E = sub.essentialMatrix(F, K1, K2) M2 = helper.camera2(E) C1 = K1 @ M1 err_min = float('inf') for i in range(M2.shape[-1]): temp_C2 = K2 @ M2[:,:,i] temp_P, err = sub.triangulate(C1, pts1, temp_C2, pts2) if err < err_min: # if np.min(temp_P[:,-1]) > 0: C2 = temp_C2 P = temp_P M2_single = M2[:,:,i] # print('errors:', err) return M2_single, C2, P
def q4_1(I1, I2): if os.path.exists(q4_1_file): with np.load(q4_1_file) as data: F = data['F'] assert F.shape == (3, 3), f"F shape is {F.shape} instead of (3, 3)" pts1 = data['pts1'] pts2 = data['pts2'] assert pts1.shape == pts2.shape, \ f"pt1s shape {pts1.shape} != pts2 size {pts2.shape}" print(f"F:\n{F} matched {len(pts1)} points") else: if os.path.exists(q2_1_file): with np.load(q2_1_file, allow_pickle=True) as data: F = data['F'] else: with np.load("../data/some_corresp.npz") as data: pts1 = data['pts1'] pts2 = data['pts2'] M = max(I1.shape[0], I1.shape[1]) F = eightpoint(pts1=pts1, pts2=pts2, M=M) np.savez(q2_1_file, F=F, M=M) # Epipolar matching I1 = I1[::, ::, ::-1] I2 = I2[::, ::, ::-1] epipolarMatchGUI(I1=I1, I2=I2, F=F)
def main(): data = np.load('../data/some_corresp.npz') im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') N = data['pts1'].shape[0] M = 640 intrinsics = np.load('../data/intrinsics.npz') K1 = intrinsics['K1'] K2 = intrinsics['K2'] F = sub.eightpoint(data['pts1'], data['pts2'], M) E = sub.essentialMatrix(F, K1, K2) M1 = np.zeros((3, 4)) M1[0, 0] = 1 M1[1, 1] = 1 M1[2, 2] = 1 M2s = camera2(E) C1 = K1.dot(M1) for i in range(4): M2 = M2s[:, :, i] C2 = K2.dot(M2) P, err = sub.triangulate(C1, data['pts1'], C2, data['pts2']) if (P[:, -1] >= 0.0).all(): break np.savez('q3_3.npz', M2=M2, C2=C2, P=P)
def bundle_adjustment(): data = np.load('../data/some_corresp_noisy.npz') im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') N = data['pts1'].shape[0] M = max(im1.shape[0], im1.shape[1]) # Estimate fundamental matrix F F = sub.eightpoint(data['pts1'], data['pts2'], M) # Get 2D points and essential matrix E intrinsics = np.load('../data/intrinsics.npz') K1 = intrinsics['K1'] K2 = intrinsics['K2'] pts1 = data['pts1'] pts2 = data['pts2'] F, inliers = sub.ransacF(pts1, pts2, M) E = sub.essentialMatrix(F, K1, K2) # Get four possible decomposition M2 from E M2s = helper.camera2(E) # Testing four M2 through triangulation to get a correct M2 M1 = np.hstack((np.eye(3), np.zeros((3, 1)))) C1 = np.dot(K1, M1) for i in range(4): M2 = M2s[:, :, i] C2 = np.dot(K2, M2) w, err = sub.triangulate(C1, pts1[inliers], C2, pts2[inliers]) if np.all(w[:, -1] > 0): break print("Reprojection error with bundle adjustment:", err) # Get 3D points M2, P = sub.bundleAdjustment(K1, M1, pts1[inliers], K2, M2, pts2[inliers], w) print("Optimized matrix:", M2) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.scatter(P[:, 0], P[:, 1], P[:, 2], marker='o', s=2) ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') plt.show(block=True)
def findM2(): data = np.load('../data/some_corresp.npz') # data = np.load('../data/some_corresp_noisy.npz') Ks = np.load('../data/intrinsics.npz') K1 = Ks['K1'] K2 = Ks['K2'] pts1 = data['pts1'] pts2 = data['pts2'] im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') M = np.max(np.shape(im1)) # print(np.shape(im1)) # print(M) F = sub.eightpoint(data['pts1'], data['pts2'], M) # using RANSAC to find F # F,inliers = sub.ransacF(data['pts1'], data['pts2'], M) E = sub.essentialMatrix(F, K1, K2) print(E) M1 = np.hstack(((np.eye(3)), np.zeros((3, 1)))) M2s = helper.camera2(E) row, col, num = np.shape(M2s) print(M1) C1 = np.matmul(K1, M1) # minerr = np.inf # res = 0 for i in range(num): M2 = M2s[:, :, i] C2 = np.matmul(K2, M2) P, err = sub.triangulate(C1, pts1, C2, pts2) if (np.all(P[:, 2] > 0)): break # if (err<minerr): # minerr = err # res = i # M2 = M2s[:,:,res] # C2 = np.matmul(K2, M2) if (os.path.isfile('q3_3.npz') == False): np.savez('q3_3.npz', M2=M2, C2=C2, P=P) return M1, C1, M2, C2, F
def main(): data = np.load('../data/some_corresp.npz') im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') N = data['pts1'].shape[0] M = 640 intrinsics = np.load('../data/intrinsics.npz') temple_pts = np.load('../data/templeCoords.npz') x1, y1 = temple_pts['x1'], temple_pts['y1'] x1 = np.squeeze(x1) y1 = np.squeeze(y1) K1 = intrinsics['K1'] K2 = intrinsics['K2'] F = sub.eightpoint(data['pts1'], data['pts2'], M) E = sub.essentialMatrix(F, K1, K2) M1 = np.zeros((3, 4)) M1[0, 0] = 1 M1[1, 1] = 1 M1[2, 2] = 1 M2s = camera2(E) C1 = K1.dot(M1) x2 = [] y2 = [] for i, j in zip(x1, y1): k, l = sub.epipolarCorrespondence(im1, im2, F, i, j) x2.append(k) y2.append(l) x2 = np.asarray(x2) y2 = np.asarray(y2) for i in range(4): M2 = M2s[:, :, i] C2 = K2.dot(M2) P, err = sub.triangulate(C1, np.array([x1, y1]).T, C2, np.array([x2, y2]).T) if (P[:, -1] >= 0.0).all(): break visualize_3d(P) np.savez('q4_2.npz', F=F, M1=M1, M2=M2, C1=C1, C2=C2)
def q2_1(I1, I2, M): # Find the fundamental matrix with np.load("../data/some_corresp.npz") as data: pts1 = data['pts1'] pts2 = data['pts2'] F = eightpoint(pts1=pts1, pts2=pts2, M=M) # np.savez(q2_1_file, F=F, M=M) # sanity check with np.load(q2_1_file) as data: F = data['F'] assert F.shape == (3, 3), f"F shape is {F.shape} instead of (3, 3)" M = data['M'] print(f"F:\n{F}\nM:{M}") # Display epipolar lines I1 = I1[::, ::, ::-1] I2 = I2[::, ::, ::-1] displayEpipolarF(I1, I2, F)
def ransacF(pts1, pts2, M): num_iter=5000 tol=10e-3 #create a set of random index to match points randIdx=np.random.randint(0,pts1.shape[0]-1,size=num_iter*8,dtype=int) randIdx=randIdx.reshape(num_iter,8) max_inliers=0 for i in range(num_iter): index=randIdx[i,:] P1=pts1[index,:] P2=pts2[index,:] F=sub.eightpoint(P1,P2,M) num_inliers=0 inliers_points=np.repeat(False,len(pts1)) for m in range(len(pts1)): p2=np.vstack([pts2[m,0],pts2[m,1],1]) epipolar_line = np.dot(F,p2) # a = epipolar_line[0] # b = epipolar_line[1] # c = epipolar_line[2] p1=np.hstack([pts1[m,0],pts1[m,1],1]) diff= abs(np.dot(p1,epipolar_line)) if diff<tol: num_inliers=num_inliers+1 inliers_points[m]=True if num_inliers>max_inliers: max_inliers=num_inliers inliers=inliers_points F_best=F print('times:',i) print('max num of inlier points',max_inliers) num_iter=5000 tol=3*10e-4 #create a set of random index to match points randIdx=np.random.randint(0,pts1.shape[0]-1,size=num_iter*7,dtype=int) randIdx=randIdx.reshape(num_iter,7) max_inliers=0 for i in range(num_iter): print('times:',i) index=randIdx[i,:] P1=pts1[index,:] P2=pts2[index,:] Farray=sub.sevenpoint(P1,P2,M) inliers_points=np.repeat(False,len(pts1)) for j in range(Farray.shape[2]): num_inliers=0 F=Farray[:,:,j] for m in range(len(pts1)): p2=np.hstack([pts2[m,0],pts2[m,1],1]) # a = epipolar_line[0] # b = epipolar_line[1] # c = epipolar_line[2] p1=np.vstack([pts1[m,0],pts1[m,1],1]) epipolar_line = np.dot(F,p1) diff= abs(np.dot(p2,epipolar_line)) if diff<tol: num_inliers=num_inliers+1 inliers_points[m]=True if num_inliers>max_inliers: max_inliers=num_inliers inliers=inliers_points F_best=F print('max num of inlier points',max_inliers) return F_best,inliers
if __name__ == '__main__': data=np.load('../data/some_corresp_noisy.npz') im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') pt1=data['pts1'] pt2=data['pts2'] intrinsics = np.load('../data/intrinsics.npz') K1 = intrinsics['K1'] K2 = intrinsics['K2'] M=650 [F, inliers]=sub.ransacF(pt1,pt2,M) pts1_inliers=pt1[inliers,:] pts2_inliers=pt2[inliers,:] F = sub.eightpoint(pts1_inliers, pts2_inliers, M) # F=sub.eightpoint(pt1,pt2,M) # pts1_inliers=pt1 # pts2_inliers=pt2 E = sub.essentialMatrix(F, K1, K2) E = E / E[2,2] M1 = np.eye(3,4) C1 = np.dot(K1, M1) M2s = helper.camera2(E) M2_init=np.zeros([3,4]) C2 = np.zeros([3,4]) for i in range(M2s.shape[2]): C2 = np.dot(K1,M2s[:, :, i]) [P, err] = sub.triangulate(C1,pts1_inliers,C2,pts2_inliers)
K1 = intrinsics['K1'] K2 = intrinsics['K2'] im1 = cv2.imread('../data/im1.png') im2 = cv2.imread('../data/im2.png') temple_coords = np.load('../data/templeCoords.npz') x1 = temple_coords['x1'] y1 = temple_coords['y1'] M = max(im1.shape[0],im1.shape[1]) F_array = submission.eightpoint(pts1, pts2, M) E = submission.essentialMatrix(F_array, K1, K2) I= np.eye(3) iter_range=4 zero_array=np.zeros((3, 1)) M1 = np.hstack((I, zero_array)) C1 = np.matmul(K1 ,M1) M2_prev = helper.camera2(E) for i in range(iter_range): C2 = np.matmul(K2 ,M2_prev[:, :, i]) W, err = submission.triangulate(C1, pts1, C2, pts2) print (err)
pts = np.load('../data/templeCoords.npz') xPoints = pts['x1'] yPoints = pts['y1'] templePoints = np.append(xPoints, yPoints, axis=1) im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') M = max(im1.shape) pts = np.load('../data/some_corresp.npz') pts1 = pts['pts1'] pts2 = pts['pts2'] intrinsics = np.load('../data/intrinsics.npz') FEight = submission.eightpoint(pts1, pts2, M) M2, C2, _ = findM2.test_M2_solution(pts1, pts2, intrinsics, M) #print (M2, C2) M1 = np.zeros([3, 4]) M1[0, 0] = 1 M1[1, 1] = 1 M1[2, 2] = 1 K1 = intrinsics['K1'] C1 = np.matmul(K1, M1) im2Array = np.array([]) #helper.epipolarMatchNoGUI(im1, im2, FEight, xPoints, yPoints) for i in range(xPoints.shape[0]): x = int(xPoints[i]) y = int(yPoints[i])
(255, 0, 255), (255, 0, 255), (255, 0, 255), (255, 0, 255)] colors = [ 'blue', 'blue', 'blue', 'blue', 'red', 'magenta', 'green', 'green', 'green', 'green', 'red', 'red', 'red', 'red', 'magenta', 'magenta', 'magenta', 'magenta' ] # 2.1 pts = np.load('../data/some_corresp.npz') pts1 = pts['pts1'] pts2 = pts['pts2'] im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') M = np.max(im1.shape) F = submission.eightpoint(pts1, pts2, M) # EightPoint algrithm to find F # F7 = submission.sevenpoint(pts1, pts2, M) # EightPoint algrithm to find F # np.savez('q2_1.npz', F=F, M=M) # # helper.displayEpipolarF(im1, im2, F) # Visualize result # # # 3.1 # # import camera instrinsics K = np.load('../data/intrinsics.npz') K1 = K['K1'] K2 = K['K2'] E = submission.essentialMatrix(F, K1, K2) # # # 4.1 # np.savez('q4_1.npz', F = F, pts1 = pts1, pts2 = pts2) # sel_pts1 , sel_pts2 = helper.epipolarMatchGUI(im1, im2, F)
def no_bundle_adjustment(): data = np.load('../data/some_corresp.npz') im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') N = data['pts1'].shape[0] M = max(im1.shape[0], im1.shape[1]) # Estimate fundamental matrix F F = sub.eightpoint(data['pts1'], data['pts2'], M) # Get essential matrix E intrinsics = np.load('../data/intrinsics.npz') K1 = intrinsics['K1'] K2 = intrinsics['K2'] E = sub.essentialMatrix(F, K1, K2) # Read temple coordinates file temple_coords = np.load('../data/templeCoords.npz') x1_temple = temple_coords['x1'] y1_temple = temple_coords['y1'] # Get 2D points in image2 using F n = x1_temple.shape[0] x2_temple, y2_temple = [], [] for i in range(n): x2, y2 = sub.epipolarCorrespondence(im1, im2, F, x1_temple[i, 0], y1_temple[i, 0]) x2_temple.append(x2) y2_temple.append(y2) x2_temple = np.array(x2_temple).reshape((n, 1)) y2_temple = np.array(y2_temple).reshape((n, 1)) pts1_temple = np.hstack((x1_temple, y1_temple)) pts2_temple = np.hstack((x2_temple, y2_temple)) # Get four possible decomposition M2 from E M2s = helper.camera2(E) # Testing four M2 through triangulation to get a correct M2 M1 = np.hstack((np.eye(3), np.zeros((3, 1)))) C1 = np.dot(K1, M1) errs = np.zeros(4) for i in range(4): M2 = M2s[:, :, i] C2 = np.dot(K2, M2) w, err = sub.triangulate(C1, pts1_temple, C2, pts2_temple) if np.all(w[:, -1] > 0): break print("Reprojection error with no bundle adjustment:", err) np.savez('q4_2.npz', F=F, M1=M1, M2=M2, C1=C1, C2=C2) # Get 3D points P_temple = w fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.scatter(P_temple[:, 0], P_temple[:, 1], P_temple[:, 2], marker='o', s=2) ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') plt.show(block=True)
# Plot the 3D points plot3D(points3D) ''' Q4.2: 1. Integrating everything together. 2. Loads necessary files from ../data/ and visualizes 3D reconstruction using scatter3 ''' if __name__ == '__main__': # Load data points points = np.load('../data/templeCoords.npz') some_corresp = np.load('../data/some_corresp.npz') intrinsics = np.load('../data/intrinsics.npz') # Load images im1 = scipy.ndimage.imread('../data/im1.png') im2 = scipy.ndimage.imread('../data/im2.png') # Compute fundamental matrix F = sub.eightpoint(some_corresp['pts1'], some_corresp['pts2'], 640) # Get epipolar data C1, C2, M1, M2, P = findM2(some_corresp['pts1'], some_corresp['pts2'], F, intrinsics['K1'], intrinsics['K2']) pts1 = np.hstack((points['x1'], points['y1'])).astype('int') # Run correspondence and visualize point cloud visualize(pts1, F, C1, C2, im1, im2)
im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') K = np.load('../data/intrinsics.npz') K1 = K['K1'] K2 = K['K2'] temple = np.load('../data/templeCoords.npz') x1 = temple['x1'] y1 = temple['y1'] temple_pts = np.hstack((x1, y1)) M = 640 F = submission.eightpoint(pts1, pts2, M) # EightPoint algrithm to find F E = submission.essentialMatrix(F, K1, K2) x2 = np.empty((x1.shape[0], 1)) y2 = np.empty((x1.shape[0], 1)) for i in range(x1.shape[0]): corresp = submission.epipolarCorrespondence(im1, im2, F, x1[i], y1[i]) x2[i] = corresp[0] y2[i] = corresp[1] temple_pts2 = np.hstack((x2, y2)) M1 = np.eye(3) M1 = np.hstack((M1, np.zeros([3, 1]))) M2_all = helper.camera2(E)
3. Save the correct M2, C2, and P to q3_3.npz ''' import numpy as np import submission import helper import matplotlib.pyplot as plt data = np.load('../data/some_corresp.npz') im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') dataK = np.load('../data/intrinsics.npz') N = data['pts1'].shape[0] M = 640 F = submission.eightpoint(data['pts1'], data['pts2'], M) E = submission.essentialMatrix(F, dataK['K1'], dataK['K2']) Ms = helper.camera2(E) M_mat = np.eye(3) M_mat = np.hstack((M_mat, np.zeros((3, 1)))) C1 = np.dot(dataK['K1'], M_mat) err = np.inf for idx in range(Ms.shape[2]): C2 = np.dot(dataK['K2'], Ms[:, :, idx])
import submission as sub import helper data = np.load('../data/some_corresp.npz') im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') intr = np.load('../data/intrinsics.npz') K1 = intr['K1'] K2 = intr['K2'] N = data['pts1'].shape[0] M = 640 # 2.1 F8 = sub.eightpoint(data['pts1'], data['pts2'], M) assert F8.shape == (3, 3), 'eightpoint returns 3x3 matrix' print('F8') print(F8) #helper.displayEpipolarF(im1, im2, F8) # 2.2 #F7 = sub.sevenpoint(data['pts1'][:7, :], data['pts2'][:7, :], M) F7 = sub.sevenpoint(data['pts1'][70:77, :], data['pts2'][70:77, :], M) assert (len(F7) == 1) | (len(F7) == 3), 'sevenpoint returns length-1/3 list' for f7 in F7: assert f7.shape == (3, 3), 'seven returns list of 3x3 matrix' #print(F7.shape) print('F7')
# Find the fundamental matrix with np.load("../data/some_corresp.npz") as data: pts1 = data['pts1'] pts2 = data['pts2'] N = len(pts1) F_file = "q2-1.npz" if os.path.exists(F_file): with np.load(F_file, allow_pickle=True) as data: F = data['F'] else: I1 = cv2.imread("../data/im1.png") I2 = cv2.imread("../data/im2.png") M = max(I1.shape[0], I1.shape[1]) F = eightpoint(pts1=pts1, pts2=pts2, M=M) # np.savez(F_file, F=F, M=M) print(f"F:\n{F}") # Find the essential matrix with np.load("../data/intrinsics.npz") as data: K1 = data['K1'] K2 = data['K2'] E_file = "q3-1.npz" if os.path.exists(E_file): with np.load(E_file, allow_pickle=True) as data: E = data['E'] else: E = essentialMatrix(F=F, K1=K1, K2=K2) np.savez(E_file, E=E)
import numpy as np import cv2 from submission import eightpoint, essentialMatrix im1 = cv2.imread('../data/im1.png')[:, :, ::-1] im2 = cv2.imread('../data/im2.png')[:, :, ::-1] corresp = np.load('../data/some_corresp.npz') pts1 = corresp['pts1'] pts2 = corresp['pts2'] M = np.max(im1.shape) F = eightpoint(pts1, pts2, M) intrinsics = np.load('../data/intrinsics.npz') K1, K2 = intrinsics['K1'], intrinsics['K2'] E = essentialMatrix(F, K1, K2) print(E)
def test_M2_solution(pts1, pts2, intrinsics, M): ''' Estimate all possible M2 and return the correct M2 and 3D points P :param pred_pts1: :param pred_pts2: :param intrinsics: :param M: a scalar parameter computed as max (imwidth, imheight) :return: M2, the extrinsics of camera 2 C2, the 3x4 camera matrix P, 3D points after triangulation (Nx3) ''' FEight = submission.eightpoint(pts1, pts2, M) KIntrinsic1 = intrinsics['K1'] KIntrinsic2 = intrinsics['K2'] E = submission.essentialMatrix(FEight, KIntrinsic1, KIntrinsic2) ExtrinsicMList = helper.camera2(E) ProjMatrix1 = np.zeros([3, 4]) ProjMatrix1[0, 0] = 1 ProjMatrix1[1, 1] = 1 ProjMatrix1[2, 2] = 1 maxCount = -1 minError = 999999999 bestC2 = np.zeros([3, 4]) P = np.zeros((pts1.shape[0], 3)) for i in range(ExtrinsicMList.shape[2]): M1 = ProjMatrix1 M2 = ExtrinsicMList[:, :, i] [W1, err1] = submission.triangulate(np.matmul(KIntrinsic1, M1), pts1, np.matmul(KIntrinsic2, M2), pts2) zIndicesCam1 = W1[:, 2] validZValCam1 = np.where(zIndicesCam1 > 0) Rinv = np.linalg.inv(M2[:, 0:3]) tInv = -np.matmul(np.linalg.inv(M2[:, 0:3]), M2[:, 3]) M2_new = ProjMatrix1 M1_new = np.zeros((3, 4)) M1_new[:, 0:3] = Rinv M1_new[:, 3] = tInv [W2, err2] = submission.triangulate(np.matmul(KIntrinsic1, M1_new), pts1, np.matmul(KIntrinsic2, M2_new), pts2) zIndicesCam2 = W2[:, 2] validZValCam2 = np.where(zIndicesCam2 > 0) #print (validZValCam2, validZValCam1) validZBothCam = np.intersect1d(validZValCam1, validZValCam2) #print (validZBothCam) validCount = validZBothCam.size if validCount > maxCount: maxCount = validCount maxError = err1 bestM2 = M2 P = W1 #print (maxCount) M2 = bestM2 C2 = np.matmul(KIntrinsic2, M2) #print (P) return M2, C2, P
''' import numpy as np import cv2 import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D import submission as sub from helper import camera2 im1 = cv2.imread('../data/im1.png') im2 = cv2.imread('../data/im2.png') pts = np.load('../data/some_corresp.npz') pts1 = pts["pts1"] pts2 = pts["pts2"] M = max((im1.shape[0], im1.shape[1])) F = sub.eightpoint(pts1, pts2, M) intrinsics = np.load('../data/intrinsics.npz') K1 = intrinsics['K1'] K2 = intrinsics['K2'] E = sub.essentialMatrix(F, K1, K2) M1 = np.concatenate((np.eye(3), np.ones((3, 1))), axis=1) C1 = np.dot(K1, M1) M2s = camera2(E) coords = np.load('../data/templeCoords.npz') x1 = coords['x1'] y1 = coords['y1'] pts1 = np.hstack([x1, y1])
points = np.load("../data/some_corresp.npz") intrinsics = np.load("../data/intrinsics.npz") K1 = intrinsics['K1'] K2 = intrinsics['K2'] p1 = points['pts1'] p2 = points['pts2'] h, w, c = img1.shape pointsTemple = np.load("../data/templeCoords.npz") xT1 = pointsTemple['x1'] yT1 = pointsTemple['y1'] xT2 = [] yT2 = [] M = max(h, w) F = submission.eightpoint(p1, p2, M) E = submission.essentialMatrix(F, K1, K2) for i in range(len(xT1)): #print(xT1[0]) x2, y2 = submission.epipolarCorrespondence(img1, img2, F, xT1[i, 0], yT1[i, 0]) xT2.append(x2) yT2.append(y2) points = [xT2, yT2] #print(xT2,yT2) ptsT1 = np.stack((xT1[:, 0], yT1[:, 0])) ptsT1 = np.moveaxis(ptsT1, 0, -1)