def q5_3(I1, I2, M): with np.load("../data/some_corresp_noisy.npz") as data: pts1 = data['pts1'] pts2 = data['pts2'] F, inliers = ransacF(pts1=pts1, pts2=pts2, M=M, nIters=200, tol=4.5) print(f"F:\n{F} matched {np.sum(inliers)}/{len(pts1)} points") # Keep inliers only pts1 = pts1[inliers] pts2 = pts2[inliers] # Get Cameras with np.load("../data/intrinsics.npz") as data: K1 = data['K1'] K2 = data['K2'] print(f"K1:\n{K1}\nK2:\n{K2}") M1 = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0]]) C1 = K1 @ M1 E = essentialMatrix(F, K1, K2) print(f"E:\n{E}") M2s = camera2(E) for i in range(4): M2_init = M2s[:, :, i] C2 = K2 @ M2_init P_init, err_init = triangulate(C1=C1, pts1=pts1, C2=C2, pts2=pts2) # Valid solution would be the one where z is positive. Thus, the points # are in front of both cameras. if np.all(P_init[:, -1] > 0): print(f"M2_init found for i={i+1}:\n{M2_init}\nError:{err_init}") break M2, P_opt = bundleAdjustment(K1=K1, M1=M1, p1=pts1, K2=K2, M2_init=M2_init, p2=pts2, P_init=P_init) C2 = K2 @ M2 P_opt_, err_opt = triangulate(C1=C1, pts1=pts1, C2=C2, pts2=pts2) print(f"M2_opt:\n{M2}\nError:{err_opt}\nP_opt: {P_opt.shape}\n") # Plot fig, ax = plt.subplots(1, 2, subplot_kw=dict(projection='3d')) ax[0].set_title(f'Initial - Rep. err: {round(err_init, 5)} ') ax[1].set_title(f'Optimized - Rep. err: {round(err_opt, 5)} ') ax[0].scatter(P_init[:, 0].tolist(), P_init[:, 1].tolist(), P_init[:, 2].tolist(), 'b') ax[1].scatter(P_opt[:, 0].tolist(), P_opt[:, 1].tolist(), P_opt[:, 2].tolist(), 'r') plt.show() plt.close()
def q5_1(I1, I2, M): with np.load("../data/some_corresp_noisy.npz") as data: pts1 = data['pts1'] pts2 = data['pts2'] inliers = len(pts1) # F = eightpoint(pts1, pts2, M) F, inliers = ransacF(pts1=pts1, pts2=pts2, M=M, nIters=200, tol=4.5) print(f"F:\n{F} matched {np.sum(inliers)}/{len(pts1)} points") I1 = I1[::, ::, ::-1] I2 = I2[::, ::, ::-1] displayEpipolarF(I1, I2, F)
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 q53(): 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) # using RANSAC to find F F, inliers = sub.ransacF(data['pts1'], data['pts2'], M) print(F) 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[inliers], C2, pts2[inliers]) if (np.all(P[:, 2] > 0)): break # P, err = sub.triangulate(C1, pts1[inliers], C2, pts2[inliers]) M2, P = sub.bundleAdjustment(K1, M1, pts1[inliers], K2, M2, pts2[inliers], P) fig = plt.figure() ax = Axes3D(fig) ax.scatter(P[:, 0], P[:, 1], P[:, 2]) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') # ax.set_zlim3d(3., 4.5) plt.show()
def getBaParams(data, M): intrinsics = np.load('../data/intrinsics.npz') K1 = intrinsics['K1'] K2 = intrinsics['K2'] F, inliers = sub.ransacF(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'][inliers], C2, data['pts2'][inliers]) if (P[:, -1] >= 0.0).all(): break return K1, K2, M1, M2, P, inliers
import helper import submission as sub from mpl_toolkits.mplot3d import Axes3D 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])
# 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 = 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 == (3, 1)), 'invRodrigues returns 3x1 vector' # 5.3 K1 = np.random.rand(3, 3) K2 = np.random.rand(3, 3)
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' epipolarMatchGUI(im1, im2, F8) # 5.1 """ You can opt to uncomment this if extra credit q5 is implemented. Note this only checks formatting. """ F, inliers = sub.ransacF(data['pts1'], data['pts2'], M, tol=12) assert F.shape == (3, 3), 'ransacF returns 3x3 matrix' displayEpipolarF(im1, im2, F) # 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 == (3, 1)), 'invRodrigues returns 3x1 vector' # 5.3 K1 = np.random.rand(3, 3)
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') pts1=data['pts1'] pts2=data['pts2'] M=650 [F, inliers]=sub.ransacF(pts1,pts2,M) print('best F:\n',F) # print('inliers=',inliers) pts1_inliers=pts1[inliers,:] pts2_inliers=pts2[inliers,:] F = sub.eightpoint(pts1_inliers, pts2_inliers, M) helper.displayEpipolarF(im1, im2, F) # # directly run eightpoint on noisy correspondance # F = sub.eightpoint(pts1, pts2, M) # helper.displayEpipolarF(im1, im2, F)
import numpy as np import cv2 from helper import displayEpipolarF from submission import eightpoint, ransacF im1 = cv2.imread('../data/im1.png')[:, :, ::-1] im2 = cv2.imread('../data/im2.png')[:, :, ::-1] corresp = np.load('../data/some_corresp_noisy.npz') pts1 = corresp['pts1'] pts2 = corresp['pts2'] M = np.max(im1.shape) print('======no RANSAC=======') F1 = eightpoint(pts1, pts2, M) print(F1) displayEpipolarF(im1, im2, F1) print('======RANSAC=======') F2, _ = ransacF(pts1, pts2, M) print(F2) displayEpipolarF(im1, im2, F2)
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' print('3.1 done') # 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' print('4.1 done') # 5.1 F, inliers = sub.ransacF(data['pts1'], data['pts2'], M, 5, 0.9) assert F.shape == (3, 3), 'ransacF returns 3x3 matrix' print('5.1 done') # 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 == (3, 1)), 'invRodrigues returns 3x1 vector' print('5.2 done') # 5.3
from submission import eightpoint, ransacF, epipolarCorrespondence, triangulate, essentialMatrix, bundleAdjustment from helper import camera2 from mpl_toolkits.mplot3d import Axes3D import matplotlib.pyplot as plt im1 = cv2.imread('../data/im1.png')[:, :, ::-1] im2 = cv2.imread('../data/im2.png')[:, :, ::-1] corresp = np.load('../data/some_corresp_noisy.npz') pts1 = corresp['pts1'] pts2 = corresp['pts2'] M = np.max(im1.shape) print('%d corresps' % pts1.shape[0]) F, inliers = ransacF(pts1, pts2, M) pts1, pts2 = pts1[inliers, :], pts2[inliers, :] 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
helper.displayEpipolarF(im1, im2, Farray[1]) np.savez('q2_2.npz', F=Farray[1], M=M, pts1=pts1, pts2=pts2) # 3.1 intrinsic = np.load('../data/intrinsics.npz') K1, K2 = intrinsic['K1'], intrinsic['K2'] E = sub.essentialMatrix(F8, K1, K2) print(E) # 4.1 selected_pts1, selected_pts2 = helper.epipolarMatchGUI(im1, im2, F8) #np.savez('q4_1.npz', F=F8, pts1=selected_pts1, pts2=selected_pts2) # 5.1 noise_data = np.load('../data/some_corresp_noisy.npz') F, inliers = sub.ransacF(noise_data['pts1'], noise_data['pts2'], M) np.savez('tmpnew.npz', F=F, inliers=inliers) # helper.displayEpipolarF(im1, im2, F) # F_compare = sub.eightpoint(noise_data['pts1'], noise_data['pts2'], M) # helper.displayEpipolarF(im1, im2, F_compare) # 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 == (3, 1)), 'invRodrigues returns 3x1 vector' # 5.3
# np.savez('q4_1.npz', F = F, pts1 = pts1, pts2 = pts2) # sel_pts1 , sel_pts2 = helper.epipolarMatchGUI(im1, im2, F) # # 5.1 pts = np.load('../data/some_corresp_noisy.npz') pts1 = pts['pts1'] pts2 = pts['pts2'] # # Without RANSAC F = submission.eightpoint(pts1, pts2, M) # # helper.displayEpipolarF(im1,im2,F) # # RANSAC nIters = 110 tol = 0.85 F, inliers = submission.ransacF(pts1, pts2, M, nIters, tol) # print("Acccuracy of Ransac: ", (np.count_nonzero(inliers)/len(inliers))) F = submission.eightpoint(pts1[inliers, :], pts2[inliers, :], M) # np.savez('F_ransac.npz',F=F, inliers = inliers) E = submission.essentialMatrix(F, K1, K2) # # helper.displayEpipolarF(im1, im2, F) # # 5.3 M1 = np.eye(3) M1 = np.hstack((M1, np.zeros([3, 1]))) M2_all = helper.camera2(E) C1 = np.dot(K1, M1)
# Q4.1 # np.savez('../results/q4_1.npz', F = F8, pts1 = data['pts1'], pts2 = data['pts2']) # helper.epipolarMatchGUI(im1, im2, F8) # Code edited in helper function for saving pts1 and pts2 # Q4.2 # See viualize.py # Q5.1 data2 = np.load('../data/some_corresp_noisy.npz') # for n,v in data2.items(): # print (n) F, inlier_1, inlier_2 = sub.ransacF(data2['pts1'], data2['pts2'], M) # helper.displayEpipolarF(im1, im2, F) # Computing F8 for noisy points # F8_noisy = sub.eightpoint(data2['pts1'], data2['pts2'], M) # helper.displayEpipolarF(im1, im2, F8_noisy) # Q5.2 # a) # r = np.ones([3, 1]) # R = sub.rodrigues(r)
# ''' # helper.displayEpipolarF(I1, I2, F_ransac) ''' Q5_3 ''' print("Running Question 5_3") corresp = np.load("../data/some_corresp_noisy.npz") pts1 = corresp["pts1"] pts2 = corresp["pts2"] I1 = plt.imread('../data/im1.png') I2 = plt.imread('../data/im2.png') M = np.maximum(I1.shape[0],I1.shape[1]) F, pts1_in, pts2_in = submission.ransacF(pts1, pts2, M) # np.savez("../data/F_ransac.npz", F=F, pts1_in=pts1_in, pts2_in=pts2_in) # F = np.load("../data/F_ransac.npz")["F"] # pts1_in = np.load("../data/F_ransac.npz")["pts1_in"] # pts2_in = np.load("../data/F_ransac.npz")["pts2_in"] K = np.load("../data/intrinsics.npz") 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