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 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 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 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, 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 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 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 q3_1():
with np.load(q2_1_file) as data:
F = data['F']
print(f"F:\n{F}")
with np.load("../data/intrinsics.npz") as data:
K1 = data['K1']
K2 = data['K2']
print(f"K1:\n{K1}\nK2:\n{K2}")
E = essentialMatrix(F=F, K1=K1, K2=K2)
np.savez(q3_1_file, E=E)
# sanity check
with np.load(q3_1_file) as data:
E = data['E']
assert E.shape == (3, 3), f"E shape is {E.shape} instead of (3, 3)"
print(f"E:\n{E}")
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
import numpy as np
import matplotlib.pyplot as plt
import submission as sub
import helper as hp
data = np.load('../data/some_corresp.npz')
im1 = plt.imread('../data/im1.png')
im2 = plt.imread('../data/im2.png')
K_matrices = np.load('../data/intrinsics.npz')
N = data['pts1'].shape[0]
M = 640
# 2.1
#F8 = sub.eightpoint(data['pts1'], data['pts2'], M)
#hp.displayEpipolarF(im1,im2,F8)
#F = F8
#2.2
F7 = sub.sevenpoint(data['pts1'][21:28, :], data['pts2'][21:28, :], M)
hp.displayEpipolarF(im1, im2, F7)
E = sub.essentialMatrix(F, K_matrices['K1'], K_matrices['K2'])
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)
# 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')
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'])
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)
if np.all(W[:, 2] > 0):
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)
C1 = np.dot(K1, M1)
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)
'''
Q4.2:
1. Integrating everything together.
2. Loads necessary files from ../data/ and visualizes 3D reconstruction using scatter
'''
from submission import essentialMatrix
import numpy as np
res = np.load("../results/q2_1.npz")
ks = np.load('../data/intrinsics.npz')
k1 = ks['K1']
k2 = ks['K2']
E = essentialMatrix(res['F'], k1, k2)
E
F7 = sub.sevenpoint(pts1, pts2, 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'
# 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)
errorvalue = []
for i in range(0, 3):
least_error = np.sum(np.square(np.subtract(F, F_seven_temp[i])))
errorvalue.append(least_error)
error_index = np.argmin(errorvalue)
F_seven = F_seven_temp[error_index]
helper.displayEpipolarF(image1, image2, F_seven)
#intrinsic parameters
intrinsic = np.load(
'/home/geekerlink/Desktop/Computer Vision/Homeworks/hw4/hw4/data/intrinsics.npz'
)
b = intrinsic.files
k1 = intrinsic[b[0]]
k2 = intrinsic[b[1]]
#calculating essential matrix
E = submission.essentialMatrix(F, k1, k2)
#matching epipolar correspondences
epipolarMatchGUI(image1, image2, F)
#loading image 1 coordinates
points1 = np.load(
'/home/geekerlink/Desktop/Computer Vision/Homeworks/hw4/hw4/data/templeCoords.npz'
)
x1 = points1['x1']
y1 = points1['y1']
number_points = len(x1)
x2 = np.zeros((number_points, 1))
y2 = np.zeros((number_points, 1))
#finding corresponding points in image 2
for i in range(0, number_points):
[x2_temp,
y2_temp] = submission.epipolarCorrespondence(image1, image2, F, x1[i],
'''
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])
P_calc, err_calc = submission.triangulate(C1, data['pts1'], C2,
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
from helper import camera2
import submission as sub
#load correspondences
corrs = np.load('../data/some_corresp.npz')
pts1 = corrs['pts1']
pts2 = corrs['pts2']
#load intrinsic camera matrices
ks = np.load('../data/intrinsics.npz')
k1 = ks['K1']
k2 = ks['K2']
#calculate F and E
M = np.max(np.hstack((pts1, pts2)))
F = sub.eightpoint(pts1, pts2, M)
E = sub.essentialMatrix(F, k1, k2)
M1 = np.hstack((np.eye(3), np.zeros((3, 1))))
C1 = k1 @ M1
M2s = camera2(E)
for ind in range(M2s.shape[2]):
M2 = M2s[:, :, ind]
C2 = k2 @ M2
P, err = sub.triangulate(C1, pts1, C2, pts2)
if np.all(P[:, 2] > 0):
#This means we one where the points are infront of the camera
np.savez("../results/q3_3", M2=M2, C2=C2, P=P)
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])
pts2 = np.zeros(pts1.shape)
for i in range(x1.shape[0]):
x = pts1[i, 0]
y = pts1[i, 1]
# print(Fs.shape)
# im1 = cv2.imread('../data/im1.png')
# im2 = cv2.imread('../data/im2.png')
# if Fs.ndim == 3:
# for F in Fs:
# helper.displayEpipolarF(im1, im2, F)
# else:
# helper.displayEpipolarF(im1, im2, Fs)
# 3.1
K = np.load('../data/intrinsics.npz')
K1 = K['K1']
K2 = K['K2']
data = np.load('q2_1.npz')
F = data['F']
E = src.essentialMatrix(F, K1, K2)
print(E)
# 3.2
# M1 = np.zeros((3, 4))
# M1[0, 0] = 1
# M1[1, 1] = 0
# M1[2, 2] = 0
# C1 = K1 @ M1
# C2 = K2 @ M2
# P, err = src.triangulate(C1, pts1, C2, pts2)
# print(err)
# 4.1
# im1 = cv2.imread('../data/im1.png')
# im2 = 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)
print(f"E:\n{E}")
# Triangulate
q3_3_file = "q3_3.npz"
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]])
M2s = camera2(E)
C1 = K1 @ M1
for i in range(4):
M2 = M2s[:, :, i]
C2 = K2 @ M2
P, err = triangulate(C1=C1, pts1=pts1, C2=C2, pts2=pts2)
# Valid solution would be the one where z is positive. Thus, the points