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 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 q4_1(I1, I2):
if os.path.exists(q4_1_file):
with np.load(q4_1_file) as data:
F = data['F']
assert F.shape == (3, 3), f"F shape is {F.shape} instead of (3, 3)"
pts1 = data['pts1']
pts2 = data['pts2']
assert pts1.shape == pts2.shape, \
f"pt1s shape {pts1.shape} != pts2 size {pts2.shape}"
print(f"F:\n{F} matched {len(pts1)} points")
else:
if os.path.exists(q2_1_file):
with np.load(q2_1_file, allow_pickle=True) as data:
F = data['F']
else:
with np.load("../data/some_corresp.npz") as data:
pts1 = data['pts1']
pts2 = data['pts2']
M = max(I1.shape[0], I1.shape[1])
F = eightpoint(pts1=pts1, pts2=pts2, M=M)
np.savez(q2_1_file, F=F, M=M)
# Epipolar matching
I1 = I1[::, ::, ::-1]
I2 = I2[::, ::, ::-1]
epipolarMatchGUI(I1=I1, I2=I2, F=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')
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 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():
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 q2_1(I1, I2, M):
# Find the fundamental matrix
with np.load("../data/some_corresp.npz") as data:
pts1 = data['pts1']
pts2 = data['pts2']
F = eightpoint(pts1=pts1, pts2=pts2, M=M)
# np.savez(q2_1_file, F=F, M=M)
# sanity check
with np.load(q2_1_file) as data:
F = data['F']
assert F.shape == (3, 3), f"F shape is {F.shape} instead of (3, 3)"
M = data['M']
print(f"F:\n{F}\nM:{M}")
# Display epipolar lines
I1 = I1[::, ::, ::-1]
I2 = I2[::, ::, ::-1]
displayEpipolarF(I1, I2, F)
def ransacF(pts1, pts2, M):
num_iter=5000
tol=10e-3
#create a set of random index to match points
randIdx=np.random.randint(0,pts1.shape[0]-1,size=num_iter*8,dtype=int)
randIdx=randIdx.reshape(num_iter,8)
max_inliers=0
for i in range(num_iter):
index=randIdx[i,:]
P1=pts1[index,:]
P2=pts2[index,:]
F=sub.eightpoint(P1,P2,M)
num_inliers=0
inliers_points=np.repeat(False,len(pts1))
for m in range(len(pts1)):
p2=np.vstack([pts2[m,0],pts2[m,1],1])
epipolar_line = np.dot(F,p2)
# a = epipolar_line[0]
# b = epipolar_line[1]
# c = epipolar_line[2]
p1=np.hstack([pts1[m,0],pts1[m,1],1])
diff= abs(np.dot(p1,epipolar_line))
if diff<tol:
num_inliers=num_inliers+1
inliers_points[m]=True
if num_inliers>max_inliers:
max_inliers=num_inliers
inliers=inliers_points
F_best=F
print('times:',i)
print('max num of inlier points',max_inliers)
num_iter=5000
tol=3*10e-4
#create a set of random index to match points
randIdx=np.random.randint(0,pts1.shape[0]-1,size=num_iter*7,dtype=int)
randIdx=randIdx.reshape(num_iter,7)
max_inliers=0
for i in range(num_iter):
print('times:',i)
index=randIdx[i,:]
P1=pts1[index,:]
P2=pts2[index,:]
Farray=sub.sevenpoint(P1,P2,M)
inliers_points=np.repeat(False,len(pts1))
for j in range(Farray.shape[2]):
num_inliers=0
F=Farray[:,:,j]
for m in range(len(pts1)):
p2=np.hstack([pts2[m,0],pts2[m,1],1])
# a = epipolar_line[0]
# b = epipolar_line[1]
# c = epipolar_line[2]
p1=np.vstack([pts1[m,0],pts1[m,1],1])
epipolar_line = np.dot(F,p1)
diff= abs(np.dot(p2,epipolar_line))
if diff<tol:
num_inliers=num_inliers+1
inliers_points[m]=True
if num_inliers>max_inliers:
max_inliers=num_inliers
inliers=inliers_points
F_best=F
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')
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])
for i in range(M2s.shape[2]):
C2 = np.dot(K1,M2s[:, :, i])
[P, err] = sub.triangulate(C1,pts1_inliers,C2,pts2_inliers)
K1 = intrinsics['K1']
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)
pts = np.load('../data/templeCoords.npz')
xPoints = pts['x1']
yPoints = pts['y1']
templePoints = np.append(xPoints, yPoints, axis=1)
im1 = plt.imread('../data/im1.png')
im2 = plt.imread('../data/im2.png')
M = max(im1.shape)
pts = np.load('../data/some_corresp.npz')
pts1 = pts['pts1']
pts2 = pts['pts2']
intrinsics = np.load('../data/intrinsics.npz')
FEight = submission.eightpoint(pts1, pts2, M)
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])
(255, 0, 255), (255, 0, 255), (255, 0, 255), (255, 0, 255)]
colors = [
'blue', 'blue', 'blue', 'blue', 'red', 'magenta', 'green', 'green',
'green', 'green', 'red', 'red', 'red', 'red', 'magenta', 'magenta',
'magenta', 'magenta'
]
# 2.1
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 = submission.eightpoint(pts1, pts2, M) # EightPoint algrithm to find F
# F7 = submission.sevenpoint(pts1, pts2, M) # EightPoint algrithm to find F
# np.savez('q2_1.npz', F=F, M=M)
# # helper.displayEpipolarF(im1, im2, F) # Visualize result
# # # 3.1
# # import camera instrinsics
K = np.load('../data/intrinsics.npz')
K1 = K['K1']
K2 = K['K2']
E = submission.essentialMatrix(F, K1, K2)
# # # 4.1
# np.savez('q4_1.npz', F = F, pts1 = pts1, pts2 = pts2)
# sel_pts1 , sel_pts2 = helper.epipolarMatchGUI(im1, im2, F)
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)
# Plot the 3D points
plot3D(points3D)
'''
Q4.2:
1. Integrating everything together.
2. Loads necessary files from ../data/ and visualizes 3D reconstruction using scatter3
'''
if __name__ == '__main__':
# Load data points
points = np.load('../data/templeCoords.npz')
some_corresp = np.load('../data/some_corresp.npz')
intrinsics = np.load('../data/intrinsics.npz')
# Load images
im1 = scipy.ndimage.imread('../data/im1.png')
im2 = scipy.ndimage.imread('../data/im2.png')
# Compute fundamental matrix
F = sub.eightpoint(some_corresp['pts1'], some_corresp['pts2'], 640)
# Get epipolar data
C1, C2, M1, M2, P = findM2(some_corresp['pts1'], some_corresp['pts2'], F,
intrinsics['K1'], intrinsics['K2'])
pts1 = np.hstack((points['x1'], points['y1'])).astype('int')
# Run correspondence and visualize point cloud
visualize(pts1, F, C1, C2, im1, im2)
im1 = plt.imread('../data/im1.png')
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)
3. Save the correct M2, C2, and P to q3_3.npz
'''
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])
import submission as sub
import helper
data = np.load('../data/some_corresp.npz')
im1 = plt.imread('../data/im1.png')
im2 = plt.imread('../data/im2.png')
intr = np.load('../data/intrinsics.npz')
K1 = intr['K1']
K2 = intr['K2']
N = data['pts1'].shape[0]
M = 640
# 2.1
F8 = sub.eightpoint(data['pts1'], data['pts2'], M)
assert F8.shape == (3, 3), 'eightpoint returns 3x3 matrix'
print('F8')
print(F8)
#helper.displayEpipolarF(im1, im2, F8)
# 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')
# Find the fundamental matrix
with np.load("../data/some_corresp.npz") as data:
pts1 = data['pts1']
pts2 = data['pts2']
N = len(pts1)
F_file = "q2-1.npz"
if os.path.exists(F_file):
with np.load(F_file, allow_pickle=True) as data:
F = data['F']
else:
I1 = cv2.imread("../data/im1.png")
I2 = 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)
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)
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
'''
import numpy as np
import cv2
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
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])
points = np.load("../data/some_corresp.npz")
intrinsics = np.load("../data/intrinsics.npz")
K1 = intrinsics['K1']
K2 = intrinsics['K2']
p1 = points['pts1']
p2 = points['pts2']
h, w, c = img1.shape
pointsTemple = np.load("../data/templeCoords.npz")
xT1 = pointsTemple['x1']
yT1 = pointsTemple['y1']
xT2 = []
yT2 = []
M = max(h, w)
F = submission.eightpoint(p1, p2, M)
E = submission.essentialMatrix(F, K1, K2)
for i in range(len(xT1)):
#print(xT1[0])
x2, y2 = submission.epipolarCorrespondence(img1, img2, F, xT1[i, 0],
yT1[i, 0])
xT2.append(x2)
yT2.append(y2)
points = [xT2, yT2]
#print(xT2,yT2)
ptsT1 = np.stack((xT1[:, 0], yT1[:, 0]))
ptsT1 = np.moveaxis(ptsT1, 0, -1)
