F7 = sub.sevenpoint(data['pts1'][:7, :], data['pts2'][:7, :], 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' # 3.1 C1 = np.concatenate([np.random.rand(3, 3), np.ones([3, 1])], axis=1) C2 = np.concatenate([np.random.rand(3, 3), np.ones([3, 1])], axis=1) P, err = sub.triangulate(C1, data['pts1'], C2, data['pts2']) assert P.shape == (N, 3), 'triangulate returns Nx3 matrix P' assert np.isscalar(err), 'triangulate returns scalar err' # 4.1 x2, y2 = sub.epipolarCorrespondence(im1, im2, F8, data['pts1'][0, 0], data['pts1'][0, 1]) assert np.isscalar(x2) & np.isscalar( y2), 'epipolarCoorespondence returns x & y coordinates' # 5.1 # F, inliers = sub.ransacF(data['pts1'], data['pts2'], M) # assert F.shape == (3, 3), 'ransacF returns 3x3 matrix' # 5.2 r = np.ones([3, 1]) R = sub.rodrigues(r) assert R.shape == (3, 3), 'rodrigues returns 3x3 matrix' R = np.eye(3) r = sub.invRodrigues(R) assert (r.shape == (3, )) | (r.shape
# Finding points in first image N = 288 x1_selected = selected_points['x1'] y1_selected = selected_points['y1'] selected_points_1 = np.zeros([N, 2]) for i in range(N): selected_points_1[i][0] = x1_selected[i] selected_points_1[i][1] = y1_selected[i] selected_points_1 = np.asarray(selected_points_1) # Finding corresponding points N = len(x1_selected) x2_selected = [] y2_selected = [] for i in range(N): x_t, y_t = sub.epipolarCorrespondence(im1, im2, F8, int(x1_selected[i]), int(y1_selected[i])) x2_selected.append(x_t) y2_selected.append(y_t) x2_selected = np.asarray(x2_selected) y2_selected = np.asarray(y2_selected) selected_points_2 = np.zeros([N, 2]) for i in range(N): selected_points_2[i][0] = x2_selected[i] selected_points_2[i][1] = y2_selected[i] selected_points_2 = np.asarray(selected_points_2) # Finding 3D points in space M2, C2, P = findM2.find(M2s, C1, selected_points_1, selected_points_2, K2) # np.savez('../results/q4_2.npz', M1 = M1, M2 = M2, C1 = C1, C2 = C2)
I1 = plt.imread('../data/im1.png') I2 = plt.imread('../data/im2.png') pts1 = corresp["pts1"] pts2 = corresp["pts2"] x1 = templeCoords["x1"].reshape(-1) y1 = templeCoords["y1"].reshape(-1) x2 = x1.copy() y2 = y1.copy() M = np.max(I1.shape) F = submission.eightpoint(pts1, pts2, M) for i in range(len(x1)): x2[i], y2[i] = submission.epipolarCorrespondence(I1, I2, F, x1[i], y1[i]) pointset1 = np.concatenate((x1.reshape((-1, 1)), y1.reshape((-1, 1))), axis=1) pointset2 = np.concatenate((x2.reshape((-1, 1)), y2.reshape((-1, 1))), axis=1) K1 = K['K1'] K2 = K['K2'] E = submission.essentialMatrix(F, K1, K2) M2s = helper.camera2(E) M1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]]) C1 = K1 @ M1 index = 0 error_now = 10000000 C2_save = 0
intrinsics = np.load('../data/intrinsics.npz') K1 = intrinsics['K1'] K2 = intrinsics['K2'] M1 = np.array([ [ 1,0,0,0 ], [ 0,1,0,0 ], [ 0,0,1,0 ] ]) C1 = K1.dot(M1) # FIND EPIPOLAR PTS2 CORRESPONDANCES pts1_new = [] pts2_new = [] for i in range(x1.shape[0]): x2, y2 = sub.epipolarCorrespondence(im1, im2, F, x1[i], y1[i]) if x2 is not None: pts1_new.append([ x1[i], y1[i] ]) pts2_new.append([ x2, y2 ]) pts1_new = np.asarray(pts1_new) pts2_new = np.asarray(pts2_new) # print('pts1_new shape: ', pts1_new.shape) # print('pts2_new shape: ', pts2_new.shape) # FIND 3D POINTS USING TRIANGULATION P_best, C2_best, M2_best, err_best = findM2.bestM2(pts1_new, pts2_new, F, K1, K2) np.savez('../data/q4_2.npz', F=F, M1=M1, M2=M2_best, C1=C1 , C2=C2_best ) print(np.load('../data/q4_2.npz').files)
K1 = intrinsics['K1'] K2 = intrinsics['K2'] im1 = plt.imread('../data/im1.png') im2 = plt.imread('../data/im2.png') M = max(im1.shape) F = submission.eightpoint(pts1, pts2, M) E = submission.essentialMatrix(F, K1, K2) X = np.zeros((len(x1), 1)) Y = np.zeros((len(x1), 1)) for i in range(len(x1)): X[i], Y[i] = submission.epipolarCorrespondence(im1, im2, F, x1[i], y1[i]) p1 = np.hstack((x1, y1)) p2 = np.hstack((X, Y)) M1 = np.array([[1.0, 0, 0, 0], [0, 1.0, 0, 0], [0, 0, 1.0, 0]]) M2s = helper.camera2(E) C1 = K1.dot(M1) min_error = np.inf M_final = np.array([]) P_final = 0 C_final = 0 for i in range(M2s.shape[2]): C2 = K2.dot(M2s[:, :, i])
use_temple = False if use_temple: templeCoords = np.load('../data/templeCoords.npz') x1s = templeCoords['x1'] y1s = templeCoords['y1'] x2s, y2s = list(), list() # get x2 y2 from x1 y1 for i in range(x1s.shape[0]): x1, y1 = x1s[i, 0], y1s[i, 0] x2, y2 = epipolarCorrespondence(im1, im2, F, x1, y1) x2s.append(x2) y2s.append(y2) x2s, y2s = np.array(x2s).reshape(-1, 1), np.array(y2s).reshape(-1, 1) pts1 = np.concatenate((x1s, y1s), axis=1) pts2 = np.concatenate((x2s, y2s), axis=1) N = pts2.shape[0] ax = plt.subplot() ax.imshow(im2) for i in range(N): x, y = pts2[i, :] ax.text(x, y, ('%d' % i), fontsize=10, color=(0, 1, 0)) plt.show()
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]) correspPoint = submission.epipolarCorrespondence(im1, im2, FEight, x, y) #print (correspPoint) #correspPoint = np.reshape(correspPoint, (1,2)) if i == 0: im2Array = correspPoint else: im2Array = np.vstack((im2Array, correspPoint)) Points3D, error = submission.triangulate(C1, templePoints, C2, im2Array) #print (error) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') xmin, xmax = np.min(Points3D[:, 0]), np.max(Points3D[:, 0]) ymin, ymax = np.min(Points3D[:, 1]), np.max(Points3D[:, 1]) zmin, zmax = np.min(Points3D[:, 2]), np.max(Points3D[:, 2])
def epipolarMatchGUI(I1, I2, F): e1, e2 = _epipoles(F) sy, sx, _ = I2.shape f, [ax1, ax2] = plt.subplots(1, 2, figsize=(12, 9)) ax1.imshow(I1) ax1.set_title('Select a point in this image') ax1.set_axis_off() ax2.imshow(I2) ax2.set_title('Verify that the corresponding point \n is on the epipolar line in this image') ax2.set_axis_off() pts1 = [] pts2 = [] while True and len(pts1)<9: plt.sca(ax1) inpt = plt.ginput(1, mouse_stop=2) if not inpt: break x, y = inpt[0] xc = int(x) yc = int(y) pts1.append([xc,yc]) v = np.array([xc, yc, 1]) l = F.dot(v) s = np.sqrt(l[0]**2+l[1]**2) if s == 0: print('ERROR: Zero line vector in displayEpipolar') break l = l/s if l[0] != 0: ye = sy-1 ys = 0 xe = -(l[1] * ye + l[2])/l[0] xs = -(l[1] * ys + l[2])/l[0] else: xe = sx-1 xs = 0 ye = -(l[0] * xe + l[2])/l[1] ys = -(l[0] * xs + l[2])/l[1] # plt.plot(x,y, '*', 'MarkerSize', 6, 'LineWidth', 2) ax1.plot(x, y, '*', MarkerSize=6, linewidth=2) ax2.plot([xs, xe], [ys, ye], linewidth=2) # draw points x2, y2 = sub.epipolarCorrespondence(I1, I2, F, xc, yc) ax2.plot(x2, y2, 'ro', MarkerSize=8, linewidth=2) plt.draw() pts2.append([x2,y2]) pts1 = np.vstack(pts1) pts2 = np.vstack(pts2) np.savez('../results/q4_1.npz',F=F,pts1=pts1,pts2=pts2)
# print(intrinsics_data.files) # [print(i) for i in intrinsics_data] E = sub.essentialMatrix(F8, intrinsics_data["K1"], intrinsics_data["K2"]) print("q3-1:\n", E) M2s = hlpr.camera2(E) # print(M2s.shape, "\n", M2s[:,:,0]) # 3.2 M1 = np.hstack((np.eye(3), np.zeros((3, 1)))) M2 = findM2(M2s, intrinsics_data["K1"], intrinsics_data["K2"], data['pts1'], data['pts2']) C1 = intrinsics_data["K1"] @ M1 C2 = intrinsics_data["K2"] @ M2 P, err = sub.triangulate(C1, data['pts1'], C2, data['pts2']) assert P.shape == (N, 3), 'triangulate returns Nx3 matrix P' assert np.isscalar(err), 'triangulate returns scalar err' # np.savez('q3_3.npz', M2=M2, C2=C2, P=P) # 4.1 print(data_select['x1'][0], data_select['y1'][0]) x2, y2 = sub.epipolarCorrespondence(im1, im2, F8, data_select['x1'][0], data_select['y1'][0]) assert np.isscalar(x2) & np.isscalar( y2), 'epipolarCorrespondence returns x & y coordinates' # pts1, pts2 = hlpr.epipolarMatchGUI(im1, im2, F8) # np.savez('q4_1.npz', F=F8, pts1=pts1, pts2=pts2) # np.savez('q4_1.npz', F=F8, pts1=data['pts1'], pts2=data['pts2']) print('Format check passed.')
im2 = plt.imread('../data/im2.png') intrinsics_data = np.load('../data/intrinsics.npz') N = data['pts1'].shape[0] M = 640 F8 = sub.eightpoint(data['pts1'], data['pts2'], M) E = sub.essentialMatrix(F8, intrinsics_data["K1"], intrinsics_data["K2"]) M2s = hlpr.camera2(E) M1 = np.hstack((np.eye(3), np.zeros((3, 1)))) M2 = findM2(M2s, intrinsics_data["K1"], intrinsics_data["K2"], data['pts1'], data['pts2']) C1 = intrinsics_data["K1"] @ M1 C2 = intrinsics_data["K2"] @ M2 pts1 = np.hstack([data_select['x1'], data_select['y1']]) pts2 = np.zeros_like(pts1) for i, pt in enumerate(pts1): x2, y2 = sub.epipolarCorrespondence(im1, im2, F8, pt[0], pt[1]) pts2[i] = np.array([x2, y2]) P, err = sub.triangulate(C1, pts1, C2, pts2) ax = plt.axes(projection='3d') ax.scatter(P[:, 0], P[:, 1], P[:, 2], c=P[:, 2]) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') plt.show() # np.savez('q4_2.npz', F=F8, M1=M1, M2=M2, C1=C1, C2=C2)
def epipolarMatchGUI(I1, I2, F): e1, e2 = _epipoles(F) sy, sx, _ = I2.shape f, [ax1, ax2] = plt.subplots(1, 2, figsize=(12, 9)) ax1.imshow(I1) ax1.set_title('Select a point in this image') ax1.set_axis_off() ax2.imshow(I2) ax2.set_title( 'Verify that the corresponding point \n is on the epipolar line in this image' ) ax2.set_axis_off() pts1 = [] pts2 = [] while True: plt.sca(ax1) x, y = plt.ginput(1, mouse_stop=2)[0] xc = int(x) yc = int(y) v = np.array([xc, yc, 1]) l = F.dot(v) s = np.sqrt(l[0]**2 + l[1]**2) if s == 0: error('Zero line vector in displayEpipolar') l = l / s if l[0] != 0: ye = sy - 1 ys = 0 xe = -(l[1] * ye + l[2]) / l[0] xs = -(l[1] * ys + l[2]) / l[0] else: xe = sx - 1 xs = 0 ye = -(l[0] * xe + l[2]) / l[1] ys = -(l[0] * xs + l[2]) / l[1] # plt.plot(x,y, '*', 'MarkerSize', 6, 'LineWidth', 2); ax1.plot(x, y, '*', MarkerSize=6, linewidth=2) ax2.plot([xs, xe], [ys, ye], linewidth=2) # draw points x2, y2 = sub.epipolarCorrespondence(I1, I2, F, xc, yc) ax2.plot(x2, y2, 'ro', MarkerSize=8, linewidth=2) pts1.append([xc, yc]) pts2.append([x2, y2]) ''' #debug x2, y2, sbegin, send = sub.epipolarCorrespondence(I1, I2, F, xc, yc) ax2.plot(x2, y2, 'ro', MarkerSize=8, linewidth=2) bx, by = sbegin[0, 0], sbegin[1, 0] ex, ey = send[0, 0], send[1, 0] ax2.plot(bx, by, 'go', MarkerSize=4, linewidth=2) ax2.plot(ex, ey, 'bo', MarkerSize=4, linewidth=2) ''' pts1_o = np.array(pts1) pts2_o = np.array(pts2) np.savez("../results/q4_1.npz", F=F, pts1=pts1_o, pts2=pts2_o) print("yes") plt.draw()
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)
def epipolarMatchGUI(I1, I2, F): e1, e2 = _epipoles(F) sy, sx, _ = I2.shape f, [ax1, ax2] = plt.subplots(1, 2, figsize=(12, 9)) ax1.imshow(I1) ax1.set_title('Select a point in this image') ax1.set_axis_off() ax2.imshow(I2) ax2.set_title('Verify that the corresponding point \n is on the epipolar line in this image') ax2.set_axis_off() # Exit the plot after choosing num_points points num_points = 10 i = 0 pts1, pts2 = [], [] while i < num_points: plt.sca(ax1) x, y = plt.ginput(1, mouse_stop=2)[0] xc = int(x) yc = int(y) v = np.array([xc, yc, 1]) l = F.dot(v) s = np.sqrt(l[0]**2+l[1]**2) if s == 0: print('Zero line vector in displayEpipolar') l = l / s; if l[0] != 0: ye = sy-1 ys = 0 xe = -(l[1] * ye + l[2])/l[0] xs = -(l[1] * ys + l[2])/l[0] else: xe = sx-1 xs = 0 ye = -(l[0] * xe + l[2])/l[1] ys = -(l[0] * xs + l[2])/l[1] # plt.plot(x,y, '*', 'MarkerSize', 6, 'LineWidth', 2); ax1.plot(x, y, '*', MarkerSize=6, linewidth=2) ax2.plot([xs, xe], [ys, ye], linewidth=2) # draw points x2, y2 = sub.epipolarCorrespondence(I1, I2, F, xc, yc) ax2.plot(x2, y2, 'ro', MarkerSize=8, linewidth=2) plt.draw() pts1.append([xc, yc]) pts2.append([x2, y2]) i += 1 pts1 = np.array(pts1) pts2 = np.array(pts2) plt.show() np.savez('q4_1.npz', F=F, pts1=pts1, pts2=pts2)