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 findM2(pts1, pts2, F, K1, K2):
# Compute essential matrix
E = sub.essentialMatrix(F, K1, K2)
# Copmute camera 1 matrix
M1 = np.concatenate([np.eye(3), np.zeros([3, 1])], axis=1)
C1 = K1 @ M1
# Find the correct M2
best_data = [None, None, None]
best_err = np.finfo('float').max
M2s = helper.camera2(E)
for i in range(4):
# Compute camera 2 matrix
C2 = K2 @ M2s[:, :, i]
P, err = sub.triangulate(C1, pts1, C2, pts2)
# Ensure all Z values are positive
if len(P[P[:, 2] > 0]) != P.shape[0]: continue
# Save the correct data
if err < best_err:
best_err = err
best_data = [M2s[:, :, i], C2, P]
# Save the data
# np.savez('q3_3', M2=best_data[0], C2=best_data[1], P=best_data[2])
# np.savez('q4_2', F=F, M1=M1, C1=C1, M2=best_data[0], C2=best_data[1])
return C1, best_data[1], M1, best_data[0], best_data[2]
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 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 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 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 findM2(pts1, pts2, F):
K = np.load('../data/intrinsics.npz')
K1 = K['K1']
K2 = K['K2']
E = sub.essentialMatrix(F, K1, K2)
M2s = helper.camera2(E) # four possible
R = np.zeros((4, 3, 3))
C2 = np.zeros((4, 3, 4))
C1 = np.zeros((3, 4))
C1[0, 0] = 1
C1[1, 1] = 1
C1[2, 2] = 1
C1 = K1 @ C1
for i in range(4):
print(C2[i].shape, K2.shape, M2s[:, :, i].shape)
C2[i] = K2 @ M2s[:, :, i]
P, err = sub.triangulate(C1, pts1, C2[i], pts2)
if (P[:, -1] >= 0).all():
M2 = M2s[:, :, i]
print('find', i, err)
C2 = C2[i]
# np.savez('q3_3.npz', M2=M2, C2=C2, P=P)
break
# print(np.load('q3_3.npz')['C2'].shape)
return M2
def q42():
M1, C1, M2, C2, F = findM2.findM2()
data = np.load('../data/templeCoords.npz')
im1 = plt.imread('../data/im1.png')
im2 = plt.imread('../data/im2.png')
x1 = data['x1']
y1 = data['y1']
print(x1.shape)
num, temp = x1.shape
pts1 = np.hstack((x1, y1))
pts2 = np.zeros((num, 2))
for i in range(num):
x2, y2 = sub.epipolarCorrespondence(im1, im2, F, x1[i, 0], y1[i, 0])
pts2[i, 0] = x2
pts2[i, 1] = y2
print(i)
P, err = sub.triangulate(C1, pts1, C2, pts2)
# print(err)
# bundle adjustment
# M2, P = sub.bundleAdjustment(K1, M1, pts1, K2, M2, pts2, 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, 4.2)
ax.set_xlim3d(-0.8, 0.6)
plt.show()
if (os.path.isfile('q4_2.npz') == False):
np.savez('q4_2.npz', F=F, M1=M1, M2=M2, C1=C1, C2=C2)
def find(M2, C1, selected_points_1, selected_points_2, K2):
for i in range(4):
C2 = np.matmul(K2, M2[:,:,i])
P, err = sub.triangulate(C1, selected_points_1, C2, selected_points_2)
if np.all(P[:,2] > 0):
sol = M2[:,:,i]
C2 = np.matmul(K2, M2[:,:,i])
break
return sol, C2, P
def find(M2s, C1, pts1, pts2, K2):
N = M2s.shape[2]
C2s = []
for i in range(N):
M2 = M2s[:, :, i]
C2s.append(np.matmul(K2, M2s[:, :, i]))
P, err = sub.triangulate(C1, pts1, C2s[i], pts2)
if (P[:, -1] >= 0.0).all():
break
return M2, C2s[i], P
def findM2(E, k1, k2, pts1, pts2):
M2s = camera2(E)
M1 = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
C1 = np.matmul(k1, M1)
C2 = np.matmul(k2, M2s[:, :, 0])
[w, err] = triangulate(C1, pts1, C2, pts2)
min_err = err
final_C2 = C2
final_w = w
final_M2 = M2s[:, :, 0]
for i in range(1, 4):
C2 = np.matmul(k2, M2s[:, :, i])
[w, err] = triangulate(C1, pts1, C2, pts2)
if err < min_err:
final_C2 = C2
final_w = w
final_M2 = M2s[:, :, i]
return M1, final_M2, C1, final_C2, final_w
def visualize(pts1, F, C1, C2, im1, im2):
# Estimate stereo correspondences
pts2 = np.asarray([
sub.epipolarCorrespondence(im1, im2, F, pts1[i, 0], pts1[i, 1])
for i in range(pts1.shape[0])
])
# Triangulate points
points3D, _ = sub.triangulate(C1, pts1, C2, pts2)
# Plot the 3D points
plot3D(points3D)
def findM2(M2s, K1, K2, pts1, pts2):
M1 = np.hstack((np.eye(3), np.zeros((3,1))))
C1 = K1 @ M1
n = M2s.shape[-1]
for i in range(n):
M2 = M2s[:,:,i]
C2 = K2 @ M2
P, err = sub.triangulate(C1, pts1, C2, pts2)
pw = np.hstack((P[0],[1])).reshape((4,1))
if((M2 @ pw)[-1]>=0 and P[0,-1]>=0):
return M2
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_function(K1, K2, Ms, pts1, pts2):
import submission as sub
for i in range(4):
M1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
M2 = Ms[:, :, i]
C1 = K1 @ M1
C2 = K2 @ M2
[w, err] = sub.triangulate(C1, pts1, C2, pts2)
index = np.where(w[:, 2] > 0)
index = np.array(index)
if (index.shape[1] == w.shape[0]):
correct_M = Ms[:, :, 2]
correct_C2 = C2
Points = w
return correct_M, M1, correct_C2, C1, Points
def main():
im1 = plt.imread('../data/im1.png')
im2 = plt.imread('../data/im2.png')
K_matrices = np.load('../data/intrinsics.npz')
data = np.load('../data/templeCoords.npz')
pts1 = data['x1']
pts1 = np.concatenate([pts1,data['y1']],axis=1)
M = 640
F = np.array([[ 9.78833285e-10, -1.32135929e-07, 1.12585666e-03],
[-5.73843315e-08, 2.96800276e-09, -1.17611996e-05],
[-1.08269003e-03, 3.04846703e-05, -4.47032655e-03]])
K1 = K_matrices['K1']
K2 = K_matrices['K2']
pts2 = []
for pt in pts1:
x,y = sub.epipolarCorrespondence(im1, im2, F, pt[0], pt[1])
pts2.append([x,y])
pts2 = np.vstack(pts2)
M1 = np.concatenate([np.eye(3), np.zeros([3, 1])], axis=1)
C1 = np.dot(K1,M1)
M2 = np.array([[ 0.99942701, 0.03331428, 0.0059843 , -0.02601138],
[-0.03372743, 0.96531375, 0.25890503, -1. ],
[ 0.00284851, -0.25895852, 0.96588424, 0.07981688]])
C2 = np.dot(K2,M2)
'''
C1 = np.array([[1.5204e+03, 0.0000e+00, 3.0232e+02, 0.0000e+00],
[0.0000e+00, 1.5259e+03, 2.4687e+02, 0.0000e+00],
[0.0000e+00, 0.0000e+00, 1.0000e+00, 0.0000e+00]])
C2 = np.array([[ 1.52038999e+03, -2.76373041e+01, 3.01104652e+02, -1.54174686e+01],
[-5.07614677e+01, 1.40904317e+03, 6.33511035e+02, -1.50619561e+03],
[ 2.84850950e-03, -2.58958519e-01, 9.65884243e-01, 7.98168772e-02]])
'''
pts_3D, error = sub.triangulate(C1, pts1, C2, pts2)
plt_pt = pts_3D[pts_3D[:,2]>3.2]
np.savez('../results/q4_2.npz',F=F,M1=M1,M2=M2,C1=C1,C2=C2)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(plt_pt[:,0], plt_pt[:,1], plt_pt[:,2], c='r', marker='o')
plt.show()
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 Visualize(I1, I2, x1, y1, C1, C2, F):
x2s = []
y2s = []
for i in range(len(x1)):
[x2, y2] = sub.epipolarCorrespondence(I1, I2, F, x1[i, 0], y1[i, 0])
x2s.append(x2)
y2s.append(y2)
pts1 = np.stack((x1[:, 0], y1[:, 0]), axis=1)
pts2 = np.stack((np.array(x2s), np.array(y2s)), axis=1)
[w, err] = sub.triangulate(C1, pts1, C2, pts2)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(w[:, 0], w[:, 1], w[:, 2])
plt.show()
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 bestM2(pts1, pts2, F, K1, K2):
# CALCULATE E
E = sub.essentialMatrix(F, K1, K2)
# CALCULATE M1 and M2
M1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
M2_list = helper.camera2(E)
# TRIANGULATION
C1 = K1.dot(M1)
P_best = np.zeros((pts1.shape[0], 3))
M2_best = np.zeros((3, 4))
C2_best = np.zeros((3, 4))
err_best = np.inf
error_list = []
index = 0
for i in range(M2_list.shape[2]):
M2 = M2_list[:, :, i]
C2 = K2.dot(M2)
P_i, err = sub.triangulate(C1, pts1, C2, pts2)
error_list.append(err)
z_list = P_i[:, 2]
if all(z > 0 for z in z_list):
index = i
err_best = err
P_best = P_i
M2_best = M2
C2_best = C2
# print('error_list: ', error_list)
# print('err_best: ', err_best)
# print('M2_best: ', M2_best )
# print('C2_best: ', C2_best )
# print('P_best: ', P_best )
# print('index: ', index)
return P_best, C2_best, M2_best, err_best
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
x1 = coords['x1']
y1 = coords['y1']
pts1 = np.hstack([x1, y1])
pts2 = np.zeros(pts1.shape)
for i in range(x1.shape[0]):
x = pts1[i, 0]
y = pts1[i, 1]
x2, y2 = sub.epipolarCorrespondence(im1, im2, F, x, y)
pts2[i, :] = [x2, y2]
for i in range(M2s.shape[2]):
M2 = M2s[:, :, i]
C2 = np.dot(K2, M2)
P, err = sub.triangulate(C1, pts1, C2, pts2)
if np.min(P[:, 2]) > 0:
break
np.savez('q4_2.npz', F=F, M1=M1, M2=M2, C1=C1, C2=C2)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim3d(-2.5, 1)
ax.set_ylim3d(-2.5, -0.25)
ax.set_zlim3d(8, 11.5)
ax.scatter(P[:, 0], P[:, 1], P[:, 2], c='b', marker='o')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
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)
if(min(P[:,2]) > 0):
M2_init=M2s[:,:,i]
print('initial M2',M2_init)
P_init=P
else:
print('pass')
[M2,P_optimized]=sub.bundleAdjustment(K1, M1, pts1_inliers, K2, M2_init, pts2_inliers, P_init)
print('optimized M2',M2)
P_optimized=P_optimized.reshape(-1,3)
print('optimized P shape:',P_optimized.shape)
# plot 3d scatter
fig = plt.figure()
for f7 in F7:
assert f7.shape == (3, 3), 'seven returns list of 3x3 matrix'
#print(F7.shape)
print('F7')
print(F7)
#helper.displayEpipolarF(im1, im2, F7[0])
# 3.1
print('E')
print(sub.essentialMatrix(F8, K1, K2))
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'])
print(err)
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), 'epipolarCorrespondence returns x & y coordinates'
np.savez('../results/q4_1.npz', F=F8, pts1=data['pts1'], pts2=data['pts2'])
helper.epipolarMatchGUI(im1, im2, F8)
print('Format check passed.')
K2 = data['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
print(f"C1:\n{C1}")
# Triangulate
with np.load(q3_1_file) as data:
E = data['E']
print(f"E:\n{E}")
M2s = camera2(E)
for i in range(4):
M2 = M2s[:, :, i]
C2 = K2 @ M2
X, err = 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(X[:, -1] > 0):
print(f"M2 found for i={i+1}")
print(f"C1:\n{C1}\nC2:\n{C2}\nM2:\n{M2}")
break
np.savez(q4_2_file, F=F, M1=M1, C1=C1, M2=M2, C2=C2)
# sanity check
with np.load(q4_2_file) as data:
F_ = data['F']
C1_ = data['C1']
M1_ = data['M1']
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)
if np.all(W[:, 2] > 0):
M2 = M2_prev[:, :, i]
break
np.savez('q3_3.npz', M2=M2, C2=C2, P=W)
F = sub.eightpoint(pts1, pts2, M)
# estimate the essential matrix E
intrinsic = np.load('../data/intrinsics.npz')
K1, K2 = intrinsic['K1'], intrinsic['K2']
E = sub.essentialMatrix(F, K1, K2)
# get the 4 candidates of M2s
M2s = helper.camera2(E)
# calculate C1 for camera 1
M1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
C1 = np.dot(K1, M1)
M2 = None
P = None
# find the correct M2
for ind in range(M2s.shape[-1]):
M2_tmp = M2s[:, :, ind]
C2_tmp = np.dot(K2, M2_tmp)
w, err = sub.triangulate(C1, pts1, C2_tmp, pts2)
if np.min(w[:, -1]) > 0:
M2 = M2_tmp
P = w
break
C2 = np.dot(K2, M2)
np.savez('q3_3.npz', M2=M2, C2=C2, P=P)
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)
C1 = np.dot(K1, M1)
err_val = np.inf
for i in range(M2_all.shape[2]):
C2 = np.dot(K2, M2_all[:, :, i])
w, err = submission.triangulate(C1, temple_pts, C2, temple_pts2)
if (err < err_val and np.min(w[:, 2]) >= 0):
err_val = err
M2 = M2_all[:, :, i]
C2_best = C2
C2 = C2_best
P_best = w
np.savez('q4_2.npz', F=F, M1=M1, M2=M2, C1=C1, C2=C2)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim3d(np.min(P_best[:, 0]), np.max(P_best[:, 0]))
ax.set_ylim3d(np.min(P_best[:, 1]), np.max(P_best[:, 1]))
ax.set_zlim3d(np.min(P_best[:, 2]), np.max(P_best[:, 2]))
x1 = temple_Coords['x1']
y1 = temple_Coords['y1']
pts1 = np.zeros([len(x1), 2])
pts2 = np.zeros([len(x1), 2])
pts1[:, 0] = x1.flatten()
pts1[:, 1] = y1.flatten()
q2_1 = np.load('../jingruwu/data/q2_1.npz')
F = q2_1['F']
for i in range(len(x1)):
[pts2[i, 0],
pts2[i, 1]] = sub.epipolarCorrespondence(im1, im2, F, x1[i], y1[i])
# get C1 C2
intrinsics = np.load('../jingruwu/data/intrinsics.npz')
K1 = intrinsics['K1']
K2 = intrinsics['K2']
M1 = np.eye(3, 4)
C1 = np.dot(K1, M1)
findC2 = np.load('../jingruwu/data/q3_3.npz')
C2 = findC2['C2']
M2 = findC2['M2']
#save the matrix F, matrices M1, M2, C1, C2 which you used to generate the screenshots to the file q4 2.npz.
# np.savez('../jingruwu/data/q4_2.npz',F=F,M1=M1, M2=M2, C1=C1, C2=C2)
[P_3d, err] = sub.triangulate(C1, pts1, C2, pts2)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
print(P_3d.shape)
# plot 3d scatter
ax.scatter(P_3d[:, 0], P_3d[:, 1], P_3d[:, 2], s=2)
plt.show()