F7 = sub.sevenpoint(data['pts1'][:7, :], data['pts2'][:7, :], 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'
# 3.1
C1 = np.concatenate([np.random.rand(3, 3), np.ones([3, 1])], axis=1)
C2 = np.concatenate([np.random.rand(3, 3), np.ones([3, 1])], axis=1)
P, err = sub.triangulate(C1, data['pts1'], C2, data['pts2'])
assert P.shape == (N, 3), 'triangulate returns Nx3 matrix P'
assert np.isscalar(err), 'triangulate returns scalar err'
# 4.1
x2, y2 = sub.epipolarCorrespondence(im1, im2, F8, data['pts1'][0, 0],
data['pts1'][0, 1])
assert np.isscalar(x2) & np.isscalar(
y2), 'epipolarCoorespondence returns x & y coordinates'
# 5.1
# F, inliers = sub.ransacF(data['pts1'], data['pts2'], M)
# assert F.shape == (3, 3), 'ransacF returns 3x3 matrix'
# 5.2
r = np.ones([3, 1])
R = sub.rodrigues(r)
assert R.shape == (3, 3), 'rodrigues returns 3x3 matrix'
R = np.eye(3)
r = sub.invRodrigues(R)
assert (r.shape == (3, )) | (r.shape
# Finding points in first image
N = 288
x1_selected = selected_points['x1']
y1_selected = selected_points['y1']
selected_points_1 = np.zeros([N, 2])
for i in range(N):
selected_points_1[i][0] = x1_selected[i]
selected_points_1[i][1] = y1_selected[i]
selected_points_1 = np.asarray(selected_points_1)
# Finding corresponding points
N = len(x1_selected)
x2_selected = []
y2_selected = []
for i in range(N):
x_t, y_t = sub.epipolarCorrespondence(im1, im2, F8, int(x1_selected[i]),
int(y1_selected[i]))
x2_selected.append(x_t)
y2_selected.append(y_t)
x2_selected = np.asarray(x2_selected)
y2_selected = np.asarray(y2_selected)
selected_points_2 = np.zeros([N, 2])
for i in range(N):
selected_points_2[i][0] = x2_selected[i]
selected_points_2[i][1] = y2_selected[i]
selected_points_2 = np.asarray(selected_points_2)
# Finding 3D points in space
M2, C2, P = findM2.find(M2s, C1, selected_points_1, selected_points_2, K2)
# np.savez('../results/q4_2.npz', M1 = M1, M2 = M2, C1 = C1, C2 = C2)
I1 = plt.imread('../data/im1.png')
I2 = plt.imread('../data/im2.png')
pts1 = corresp["pts1"]
pts2 = corresp["pts2"]
x1 = templeCoords["x1"].reshape(-1)
y1 = templeCoords["y1"].reshape(-1)
x2 = x1.copy()
y2 = y1.copy()
M = np.max(I1.shape)
F = submission.eightpoint(pts1, pts2, M)
for i in range(len(x1)):
x2[i], y2[i] = submission.epipolarCorrespondence(I1, I2, F, x1[i], y1[i])
pointset1 = np.concatenate((x1.reshape((-1, 1)), y1.reshape((-1, 1))), axis=1)
pointset2 = np.concatenate((x2.reshape((-1, 1)), y2.reshape((-1, 1))), axis=1)
K1 = K['K1']
K2 = K['K2']
E = submission.essentialMatrix(F, K1, K2)
M2s = helper.camera2(E)
M1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
C1 = K1 @ M1
index = 0
error_now = 10000000
C2_save = 0
intrinsics = np.load('../data/intrinsics.npz')
K1 = intrinsics['K1']
K2 = intrinsics['K2']
M1 = np.array([ [ 1,0,0,0 ],
[ 0,1,0,0 ],
[ 0,0,1,0 ] ])
C1 = K1.dot(M1)
# FIND EPIPOLAR PTS2 CORRESPONDANCES
pts1_new = []
pts2_new = []
for i in range(x1.shape[0]):
x2, y2 = sub.epipolarCorrespondence(im1, im2, F, x1[i], y1[i])
if x2 is not None:
pts1_new.append([ x1[i], y1[i] ])
pts2_new.append([ x2, y2 ])
pts1_new = np.asarray(pts1_new)
pts2_new = np.asarray(pts2_new)
# print('pts1_new shape: ', pts1_new.shape)
# print('pts2_new shape: ', pts2_new.shape)
# FIND 3D POINTS USING TRIANGULATION
P_best, C2_best, M2_best, err_best = findM2.bestM2(pts1_new, pts2_new, F, K1, K2)
np.savez('../data/q4_2.npz', F=F, M1=M1, M2=M2_best, C1=C1 , C2=C2_best )
print(np.load('../data/q4_2.npz').files)
K1 = intrinsics['K1']
K2 = intrinsics['K2']
im1 = plt.imread('../data/im1.png')
im2 = plt.imread('../data/im2.png')
M = max(im1.shape)
F = submission.eightpoint(pts1, pts2, M)
E = submission.essentialMatrix(F, K1, K2)
X = np.zeros((len(x1), 1))
Y = np.zeros((len(x1), 1))
for i in range(len(x1)):
X[i], Y[i] = submission.epipolarCorrespondence(im1, im2, F, x1[i], y1[i])
p1 = np.hstack((x1, y1))
p2 = np.hstack((X, Y))
M1 = np.array([[1.0, 0, 0, 0], [0, 1.0, 0, 0], [0, 0, 1.0, 0]])
M2s = helper.camera2(E)
C1 = K1.dot(M1)
min_error = np.inf
M_final = np.array([])
P_final = 0
C_final = 0
for i in range(M2s.shape[2]):
C2 = K2.dot(M2s[:, :, i])
use_temple = False
if use_temple:
templeCoords = np.load('../data/templeCoords.npz')
x1s = templeCoords['x1']
y1s = templeCoords['y1']
x2s, y2s = list(), list()
# get x2 y2 from x1 y1
for i in range(x1s.shape[0]):
x1, y1 = x1s[i, 0], y1s[i, 0]
x2, y2 = epipolarCorrespondence(im1, im2, F, x1, y1)
x2s.append(x2)
y2s.append(y2)
x2s, y2s = np.array(x2s).reshape(-1, 1), np.array(y2s).reshape(-1, 1)
pts1 = np.concatenate((x1s, y1s), axis=1)
pts2 = np.concatenate((x2s, y2s), axis=1)
N = pts2.shape[0]
ax = plt.subplot()
ax.imshow(im2)
for i in range(N):
x, y = pts2[i, :]
ax.text(x, y, ('%d' % i), fontsize=10, color=(0, 1, 0))
plt.show()
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])
correspPoint = submission.epipolarCorrespondence(im1, im2, FEight, x, y)
#print (correspPoint)
#correspPoint = np.reshape(correspPoint, (1,2))
if i == 0:
im2Array = correspPoint
else:
im2Array = np.vstack((im2Array, correspPoint))
Points3D, error = submission.triangulate(C1, templePoints, C2, im2Array)
#print (error)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
xmin, xmax = np.min(Points3D[:, 0]), np.max(Points3D[:, 0])
ymin, ymax = np.min(Points3D[:, 1]), np.max(Points3D[:, 1])
zmin, zmax = np.min(Points3D[:, 2]), np.max(Points3D[:, 2])
def epipolarMatchGUI(I1, I2, F):
e1, e2 = _epipoles(F)
sy, sx, _ = I2.shape
f, [ax1, ax2] = plt.subplots(1, 2, figsize=(12, 9))
ax1.imshow(I1)
ax1.set_title('Select a point in this image')
ax1.set_axis_off()
ax2.imshow(I2)
ax2.set_title('Verify that the corresponding point \n is on the epipolar line in this image')
ax2.set_axis_off()
pts1 = []
pts2 = []
while True and len(pts1)<9:
plt.sca(ax1)
inpt = plt.ginput(1, mouse_stop=2)
if not inpt:
break
x, y = inpt[0]
xc = int(x)
yc = int(y)
pts1.append([xc,yc])
v = np.array([xc, yc, 1])
l = F.dot(v)
s = np.sqrt(l[0]**2+l[1]**2)
if s == 0:
print('ERROR: Zero line vector in displayEpipolar')
break
l = l/s
if l[0] != 0:
ye = sy-1
ys = 0
xe = -(l[1] * ye + l[2])/l[0]
xs = -(l[1] * ys + l[2])/l[0]
else:
xe = sx-1
xs = 0
ye = -(l[0] * xe + l[2])/l[1]
ys = -(l[0] * xs + l[2])/l[1]
# plt.plot(x,y, '*', 'MarkerSize', 6, 'LineWidth', 2)
ax1.plot(x, y, '*', MarkerSize=6, linewidth=2)
ax2.plot([xs, xe], [ys, ye], linewidth=2)
# draw points
x2, y2 = sub.epipolarCorrespondence(I1, I2, F, xc, yc)
ax2.plot(x2, y2, 'ro', MarkerSize=8, linewidth=2)
plt.draw()
pts2.append([x2,y2])
pts1 = np.vstack(pts1)
pts2 = np.vstack(pts2)
np.savez('../results/q4_1.npz',F=F,pts1=pts1,pts2=pts2)
# 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
print(data_select['x1'][0], data_select['y1'][0])
x2, y2 = sub.epipolarCorrespondence(im1, im2, F8, data_select['x1'][0],
data_select['y1'][0])
assert np.isscalar(x2) & np.isscalar(
y2), 'epipolarCorrespondence returns x & y coordinates'
# pts1, pts2 = hlpr.epipolarMatchGUI(im1, im2, F8)
# np.savez('q4_1.npz', F=F8, pts1=pts1, pts2=pts2)
# np.savez('q4_1.npz', F=F8, pts1=data['pts1'], pts2=data['pts2'])
print('Format check passed.')
im2 = plt.imread('../data/im2.png')
intrinsics_data = np.load('../data/intrinsics.npz')
N = data['pts1'].shape[0]
M = 640
F8 = sub.eightpoint(data['pts1'], data['pts2'], M)
E = sub.essentialMatrix(F8, intrinsics_data["K1"], intrinsics_data["K2"])
M2s = hlpr.camera2(E)
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
pts1 = np.hstack([data_select['x1'], data_select['y1']])
pts2 = np.zeros_like(pts1)
for i, pt in enumerate(pts1):
x2, y2 = sub.epipolarCorrespondence(im1, im2, F8, pt[0], pt[1])
pts2[i] = np.array([x2, y2])
P, err = sub.triangulate(C1, pts1, C2, pts2)
ax = plt.axes(projection='3d')
ax.scatter(P[:, 0], P[:, 1], P[:, 2], c=P[:, 2])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.show()
# np.savez('q4_2.npz', F=F8, M1=M1, M2=M2, C1=C1, C2=C2)
def epipolarMatchGUI(I1, I2, F):
e1, e2 = _epipoles(F)
sy, sx, _ = I2.shape
f, [ax1, ax2] = plt.subplots(1, 2, figsize=(12, 9))
ax1.imshow(I1)
ax1.set_title('Select a point in this image')
ax1.set_axis_off()
ax2.imshow(I2)
ax2.set_title(
'Verify that the corresponding point \n is on the epipolar line in this image'
)
ax2.set_axis_off()
pts1 = []
pts2 = []
while True:
plt.sca(ax1)
x, y = plt.ginput(1, mouse_stop=2)[0]
xc = int(x)
yc = int(y)
v = np.array([xc, yc, 1])
l = F.dot(v)
s = np.sqrt(l[0]**2 + l[1]**2)
if s == 0:
error('Zero line vector in displayEpipolar')
l = l / s
if l[0] != 0:
ye = sy - 1
ys = 0
xe = -(l[1] * ye + l[2]) / l[0]
xs = -(l[1] * ys + l[2]) / l[0]
else:
xe = sx - 1
xs = 0
ye = -(l[0] * xe + l[2]) / l[1]
ys = -(l[0] * xs + l[2]) / l[1]
# plt.plot(x,y, '*', 'MarkerSize', 6, 'LineWidth', 2);
ax1.plot(x, y, '*', MarkerSize=6, linewidth=2)
ax2.plot([xs, xe], [ys, ye], linewidth=2)
# draw points
x2, y2 = sub.epipolarCorrespondence(I1, I2, F, xc, yc)
ax2.plot(x2, y2, 'ro', MarkerSize=8, linewidth=2)
pts1.append([xc, yc])
pts2.append([x2, y2])
'''
#debug
x2, y2, sbegin, send = sub.epipolarCorrespondence(I1, I2, F, xc, yc)
ax2.plot(x2, y2, 'ro', MarkerSize=8, linewidth=2)
bx, by = sbegin[0, 0], sbegin[1, 0]
ex, ey = send[0, 0], send[1, 0]
ax2.plot(bx, by, 'go', MarkerSize=4, linewidth=2)
ax2.plot(ex, ey, 'bo', MarkerSize=4, linewidth=2)
'''
pts1_o = np.array(pts1)
pts2_o = np.array(pts2)
np.savez("../results/q4_1.npz", F=F, pts1=pts1_o, pts2=pts2_o)
print("yes")
plt.draw()
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)
def epipolarMatchGUI(I1, I2, F):
e1, e2 = _epipoles(F)
sy, sx, _ = I2.shape
f, [ax1, ax2] = plt.subplots(1, 2, figsize=(12, 9))
ax1.imshow(I1)
ax1.set_title('Select a point in this image')
ax1.set_axis_off()
ax2.imshow(I2)
ax2.set_title('Verify that the corresponding point \n is on the epipolar line in this image')
ax2.set_axis_off()
# Exit the plot after choosing num_points points
num_points = 10
i = 0
pts1, pts2 = [], []
while i < num_points:
plt.sca(ax1)
x, y = plt.ginput(1, mouse_stop=2)[0]
xc = int(x)
yc = int(y)
v = np.array([xc, yc, 1])
l = F.dot(v)
s = np.sqrt(l[0]**2+l[1]**2)
if s == 0:
print('Zero line vector in displayEpipolar')
l = l / s;
if l[0] != 0:
ye = sy-1
ys = 0
xe = -(l[1] * ye + l[2])/l[0]
xs = -(l[1] * ys + l[2])/l[0]
else:
xe = sx-1
xs = 0
ye = -(l[0] * xe + l[2])/l[1]
ys = -(l[0] * xs + l[2])/l[1]
# plt.plot(x,y, '*', 'MarkerSize', 6, 'LineWidth', 2);
ax1.plot(x, y, '*', MarkerSize=6, linewidth=2)
ax2.plot([xs, xe], [ys, ye], linewidth=2)
# draw points
x2, y2 = sub.epipolarCorrespondence(I1, I2, F, xc, yc)
ax2.plot(x2, y2, 'ro', MarkerSize=8, linewidth=2)
plt.draw()
pts1.append([xc, yc])
pts2.append([x2, y2])
i += 1
pts1 = np.array(pts1)
pts2 = np.array(pts2)
plt.show()
np.savez('q4_1.npz', F=F, pts1=pts1, pts2=pts2)
