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 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 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 ComputeExtrinsic( K1, K2, E, pts1, pts2 ):
"""
TODO 2.5
Compute P1 and P2 and the error of reprojection
This function should call triangulate(...)
[I] K1, camera matrix 1 (3x3 matrix)
K2, camera matrix 2 (3x3 matrix)
E, the essential matrix (3x3 matrix)
pts1, points in image 1 (Nx2 matrix)
pts2, points in image 2 (Nx2 matrix)
[O] R1, camera 1 rotation matrix (3x3 matrix)
t1, camera 1 translation vector (3x1 matrix)
R2, camera 2 rotation matrix (3x3 matrix)
t2, camera 2 translation vector (3x1 matrix)
pts3d, 3D points in space (Nx3 matrix)
err, reprojection error
"""
R1 = None
t1 = None
R2 = None
t2 = None
pts3d = None
err = None
R2t2 = hlp.camera2(E) # R2t2[i] (avec i = 0..3) sont les quatre matrices extrinsèques possibles
# TODO-BLOC-DEBUT
raise NotImplementedError("TODO 2.5 : dans submission.py non implémenté")
# TODO-BLOC-FIN
return R1, t1, R2, t2, pts3d, err
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 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 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 extrinsic_calibration(img1, img2, K1, K2):
'''
Input: 1. img1, img2 - Two images
2. K1, K2 - Camera intrinsics
Output: M1, M2 - Camera extrinsic matrix
Description: Computes the camera extrinsics using two images and their intrinsics
by obtaining matching points and triangulation
'''
pts1, pts2 = feature_matching(img1, img2, show_matches=False)
M = max(img1.shape[0], img1.shape[1])
F = find_F(pts1, pts2, M)
# hp.displayEpipolarF(img1,img2,F)
E = find_E(F, K1, K2)
C1 = np.concatenate([np.eye(3), np.zeros([3, 1])], axis=1)
M1 = np.dot(K1, C1)
C2s = hp.camera2(E)
C2 = find_best_C2(pts1, pts2, C2s, K2, M1)
M2 = np.dot(K2, C2)
# pts_3D, error = triangulate(M1, pts1, M2, pts2)
# plot_3D_points(pts_3D)
return M1, M2
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 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 extra_credits():
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))
F,inliers = ransacF(data['pts1'], data['pts2'], M)
print("after RANSAC")
print('F :',F)
E = essentialMatrix(F, K1, K2)
print("E is:",E)
M1 = np.hstack(((np.eye(3)), np.zeros((3, 1))))
M2s = helper.camera2(E)
row, col, num = np.shape(M2s)
C1 = np.matmul(K1, M1)
for i in range(num):
M2 = M2s[:, :, i]
C2 = np.matmul(K2, M2)
P, err = 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 = 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')
plt.show()
#ans=extra_credits()
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 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 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
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])
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]):
# put a blue dot at (10, 20)
plt.scatter([10], [20])
# put a red dot, size 40, at 2 locations:
plt.scatter(x=pts2[:, 0], y=pts2[:, 1], c='b', s=40)
plt.show()
Intrinsic = np.load('../data/intrinsics.npz')
P1 = np.hstack([Intrinsic['K1'], np.zeros([3, 1])])
# 6. Use camera2 to get 4 camera projection matrices P2
E = sub.essential_matrix(F, Intrinsic['K1'], Intrinsic['K2'])
P2s = hlp.camera2(E)
#pdb.set_trace()
# 7. Run triangulate using the projection matrices
bestWorldPts, bestNumInfront = None, 0
bestI = None
for i in range(4):
worldPts, numInfrontBoth = sub.triangulate(P1, pts1,
Intrinsic['K2'] @ P2s[:, :, i],
pts2)
if bestNumInfront < numInfrontBoth:
bestWorldPts = worldPts
bestNumInfront = numInfrontBoth
bestI = i
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)
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)
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 = 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)
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
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)
pts2Valid = pts2[np.where(inliers == True)]
numPoints = pts1.size // 2
randPerm = np.random.permutation(numPoints)
randPoints = randPerm[0:7]
Farray = sevenpoint(pts1[randPoints,:], pts2[randPoints,:], M)
helper.displayEpipolarF(im1, im2, Farray[0])
outFile = 'q2_2.npz'
np.savez(outFile, F = Farray[0], pts1 = pts1[randPoints,:], pts2 = pts2[randPoints,:], M = M)
'''
KIntrinsic = np.load('../data/intrinsics.npz')
KIntrinsic1 = KIntrinsic['K1']
KIntrinsic2 = KIntrinsic['K2']
E = essentialMatrix(FRansac, 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_init = np.zeros((pts1Valid.shape[0], 3))
for i in range(ExtrinsicMList.shape[2]):
M1 = ProjMatrix1
M2 = ExtrinsicMList[:, :, i]
[W1, err1] = triangulate(np.matmul(KIntrinsic1, M1), pts1Valid,
np.matmul(KIntrinsic2, M2), pts2Valid)
zIndicesCam1 = W1[:, 2]
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'
# hlpr.displayEpipolarF(im1, im2, f7)
# np.savez('q2_2.npz', F=F7, M=M, pts1=pts1, pts2=pts2)
print("q2-2:\n", F7)
# 3.1
intrinsics_data = np.load('../data/intrinsics.npz')
# 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
import matplotlib.pyplot as plt
import submission as sub
import helper
data = np.load('../data/some_corresp.npz')
M = 640
F8 = sub.eightpoint(data['pts1'], data['pts2'], M)
assert F8.shape == (3, 3), 'eightpoint returns 3x3 matrix'
intrinsics = np.load('../data/intrinsics.npz')
K1, K2 = intrinsics.f.K1 , intrinsics.f.K2
E = sub.essentialMatrix(F8, K1, K2)
M2 = helper.camera2(E) #M2 has the shape of [4,4,3] so be careful
C1 = np.concatenate([np.eye(3), np.zeros([3,1])], axis = 1)
C1 = np.matmul(K1, C1)
err_min = 10000
for i in range(4):
C2 = np.matmul(K2, M2[:,:,i])
P, err = sub.triangulate(C1, data['pts1'], C2, data['pts2'])
if np.all(P[:,2] > 0):
sol = M2[:,:,i]
C2 = np.matmul(K2, M2[:,:,i])
break
M2 = sol
np.savez('q3_3.npz', M2 = M2, C2 = C2, P = P)
# C1 = np.matmul(K1, C1)
def main():
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 = eightpoint(pts1, pts2, M)
intrinsic = np.load('../data/intrinsics.npz')
K1, K2 = intrinsic['K1'], intrinsic['K2']
E = essentialMatrix(F, K1, K2)
coords = np.load('../data/templeCoords.npz')
x1, y1 = coords['x1'], coords['y1']
x2, y2 = [], []
for i in range(x1.shape[0]):
coor = epipolarCorrespondence(im1, im2, F, x1[i][0], y1[i][0])
x2.append(coor[0])
y2.append(coor[1])
x2 = np.array(x2).reshape(-1, 1)
y2 = np.array(y2).reshape(-1, 1)
M2_ = helper.camera2(E)
M1 = np.eye(4)[0:3]
C1 = np.dot(K1, M1)
temple_pts1 = np.hstack((x1, y1))
temple_pts2 = np.hstack((x2, y2))
plt.scatter(temple_pts1[:, 0], temple_pts1[:, 1])
plt.show()
plt.scatter(temple_pts2[:, 0], temple_pts2[:, 1])
plt.show()
err_min = np.inf
w_ = None
M2_best = None
for i in range(M2_.shape[-1]):
M2 = M2_[:, :, i]
C2 = np.dot(K2, M2)
w, err = triangulate(C1, np.hstack((x1, y1)), C2, np.hstack((x2, y2)))
if np.min(w[:, -1]) > 0:
M2_best = M2
w_ = w
# break
# if err_min > err:
# err_min = err
# x_best = M2
# w_ = w
C2 = np.dot(K2, M2_best)
np.savez('q4_2.npz', F=F, M1=M1, M2=M2_best, C1=C1, C2=C2)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(w_[:, 0], w_[:, 1], w_[:, 2])
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
ptsT1 = np.stack((xT1[:, 0], yT1[:, 0]))
ptsT1 = np.moveaxis(ptsT1, 0, -1)
ptsT2 = np.stack((xT2, yT2))
ptsT2 = np.moveaxis(ptsT2, 0, -1)
M1 = M1 = np.zeros((3, 4))
M1[0, 0] = 1
M1[1, 1] = 1
M1[2, 2] = 1
C1 = K1 @ M1
tee = 0
err_final = 10000
M2_ = helper.camera2(E)
for i in range(0, 4):
print(i)
M2 = M2_[:, :, i]
C2 = K2 @ M2
#print(C2.shape)
W, err = submission.triangulate(C1, ptsT1, C2, ptsT2)
z = W[:, 2]
#print(W[:,2])
tee = z[z < 0]
#print(tee)
y1 = templeCoords["y1"]
x2 = np.zeros(np.size(x1))
y2 = np.zeros(np.size(y1))
for i in range(np.size(x1)):
x2[i], y2[i] = submission.epipolarCorrespondence(im1, im2, F, x1[i][0],
y1[i][0])
x2 = x2.reshape(np.size(x2), 1)
y2 = y2.reshape(np.size(y2), 1)
M1 = np.eye(4)[0:3, :]
C1 = K1 @ M1
M2all = helper.camera2(E)
num = np.size(M2all, axis=2)
pts1 = np.hstack((x1, y1))
pts2 = np.hstack((x2, y2))
for i in range(num):
M2 = M2all[:, :, i]
C2 = K2 @ M2
w, err = submission.triangulate(C1, pts1, C2, pts2)
# print("Error = " ,err)
zmin = np.min(w[:, -1])
# print("zmin = ", zmin)
if (zmin > 0):
break
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
im2 = plt.imread('../data/im2.png')
M = np.max(im1.shape)
F = eightpoint(pts1, pts2, M)
print(F)
# helper.displayEpipolarF(im1, im2, F)
np.savez('q2_1.npz', F=F, M=M)
# Q3.1
intrinsics = np.load('../data/intrinsics.npz')
K1, K2 = intrinsics['K1'], intrinsics['K2']
E = essentialMatrix(F, K1, K2)
print(E)
# Q3.2
C1 = np.eye(4)[:3]
C2 = helper.camera2(E)[1]
print(C1, C2)
w, e = triangulate(C1, pts1, C2, pts2)
print(w, e)
# Q4.1
# helper.epipolarMatchGUI(im1, im2, F)
np.savez('q4_1.npz', F=F, pts1=pts1, pts2=pts2)
# Q5.1
pts = np.load('../data/some_corresp_noisy.npz')
pts1, pts2 = pts['pts1'], pts['pts2']
nIters = 50
tol = 1
print(pts1.shape)
print(pts2.shape)
