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()
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()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(P_init[:,0], P_init[:,1], P_init[:,2],s=2,c='b')
ax.scatter(P_optimized[:,0], P_optimized[:,1], P_optimized[:,2],s=2,c='r')
plt.show()
F = 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
== (3, 1)), 'invRodrigues returns 3x1 vector'
# 5.3
K1 = np.random.rand(3, 3)
K2 = np.random.rand(3, 3)
M1 = np.concatenate([np.random.rand(3, 3), np.ones([3, 1])], axis=1)
M2 = np.concatenate([np.random.rand(3, 3), np.ones([3, 1])], axis=1)
r2 = np.ones(3)
t2 = np.ones(3)
x = np.concatenate([P.reshape([-1]), r2, t2])
residuals = sub.rodriguesResidual(K1, M1, data['pts1'], K2, data['pts1'], x)
assert residuals.shape == (4 * N,
1), 'rodriguesResidual returns vector of size 4Nx1'
M2, P = sub.bundleAdjustment(K1, M1, data['pts1'], K2, M2, data['pts1'], P)
assert M2.shape == (3, 4), 'bundleAdjustment returns 3x4 matrix M'
assert P.shape == (N, 3), 'bundleAdjustment returns Nx3 matrix P'
print('Format check passed.')
break
M2 = sol
C2 = np.matmul(K2, M2)
P, err = triangulate(C1, pts1_inliers, C2, pts2_inliers)
# print(P)
R2 = M2[:, 0:3]
t2 = M2[:, 3]
print('The Initial value of M2')
#change the below latter
r2 = invRodrigues(R2)
r2 = np.reshape(r2, [1, 3])
# print(M2)
x = np.concatenate([P.reshape([-1]), r2[-1], t2])
residuals = sub.rodriguesResidual(K1, M1, pts1_inliers, K2, pts2_inliers,
x)
M2, P2 = sub.bundleAdjustment(K1, M1, pts1_inliers, K2, M2, pts2_inliers,
P)
# print('The Optimized value of M2')
# print(P2)
#print(P2)
fig = plt.figure(1)
#ax = Axes3D(fig)
ax = fig.add_subplot(111, projection='3d')
x = P2[:, 0]
y = P2[:, 1]
z = P2[:, 2]
plt.gca().set_aspect('equal', adjustable='box')
# ax.scatter(x,y,z, color='blue')
for i in range(P.shape[0]):
if np.min(P[:, 2]) > 0:
chose_M2_idx = i
Ps.append(P)
print('Reprojection error of M2_%d: %f' % (i, cur_err))
# choose a best M2
# chose_M2_idx = 2
print('chosen M2 idx: %d' % chose_M2_idx)
M2 = M2s[:, :, chose_M2_idx]
P = Ps[chose_M2_idx]
C2 = C2s[chose_M2_idx]
# find M2 ============
# bundle adjustment
M2, P = bundleAdjustment(K1, M1, pts1, K2, M2, pts2, P)
C2 = K2 @ M2
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
xmin, xmax = np.min(P[:, 0]), np.max(P[:, 0])
ymin, ymax = np.min(P[:, 1]), np.max(P[:, 1])
zmin, zmax = np.min(P[:, 2]), np.max(P[:, 2])
ax.set_xlim3d(xmin, xmax)
ax.set_ylim3d(ymin, ymax)
ax.set_zlim3d(zmin, zmax)
import numpy as np
import matplotlib.pyplot as plt
import submission as sub
from helper import displayEpipolarF, chooseSevenPoints, epipolarMatchGUI, getBaParams
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
data_noisy = np.load('../data/some_corresp_noisy.npz')
K1, K2, M1, M2, P, inliers = getBaParams(data_noisy, M)
M2, P = sub.bundleAdjustment(K1, M1, data_noisy['pts1'][inliers], K2, M2,
data_noisy['pts2'][inliers], P)
M2s = helper.camera2(E)
M2_init = 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, p1, C2_tmp, p2)
if np.min(w[:, -1]) > 0:
M2_init = M2_tmp
C2_init = np.dot(K2, M2_init)
P_init, err = sub.triangulate(C1, p1, C2_init, p2)
print(err)
M2, P = sub.bundleAdjustment(K1, M1, p1, K2, M2_init, p2, P_init)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(P_init[:, 0], P_init[:, 1], P_init[:, 2], c='r', marker='.')
ax.scatter(P[:, 0], P[:, 1], P[:, 2], c='b', marker='.')
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
C2 = np.dot(K2, M2)
W = np.hstack((P, np.ones((P.shape[0], 1))))
err2 = 0
for i in range(p1.shape[0]):
proj1 = np.dot(C1, np.transpose(W[i, :]))
err_val = np.inf
for i in range(M2_all.shape[2]):
C2 = np.dot(K2, M2_all[:, :, i])
w, err = submission.triangulate(C1, pts1, C2, pts2)
if err < err_val:
err_val = err
M2 = M2_all[:, :, i]
C2_best = C2
w_best = w
P_init, err = submission.triangulate(C1, pts1[inliers, :], C2_best,
pts2[inliers, :])
M2_opt, P2 = submission.bundleAdjustment(K1, M1, pts1[inliers, :], K2, M2,
pts2[inliers, :], P_init)
# fig = plt.figure()
# ax = fig.add_subplot(111, projection = '3d')
# ax.set_xlim3d(np.min(P_init[:,0]),np.max(P_init[:,0]))
# ax.set_ylim3d(np.min(P_init[:,1]),np.max(P_init[:,1]))
# ax.set_zlim3d(np.min(P_init[:,2]),np.max(P_init[:,2]))
# ax.set_xlabel('X')
# ax.set_ylabel('Y')
# ax.set_zlabel('Z')
# ax.scatter(P_init[:,0],P_init[:,1],P_init[:,2])
# plt.show()
# fig = plt.figure()
# ax = fig.add_subplot(111, projection = '3d')
# ax.set_xlim3d(np.min(P2[:,0]),np.max(P2[:,0]))
# Good reprojection error and plot
M2, C2, P = findM2.find(M2s, C1, inlier_1, inlier_2, K2)
P, err = sub.triangulate(C1, inlier_1, C2, inlier_2)
print ('Error after bundle adjustment = ', err)
# plot P
# Bad reprojection error and plot
M2, P = sub.bundleAdjustment(K1, M1, inlier_1, K2, M2, inlier_2, P)
C2 = np.matmul(K2, M2)
P, err = sub.triangulate(C1, inlier_1, C2, inlier_2)
print ("error after bundle adjustment = ", err)
# plot
# Plotting
fig = plt.figure()
ax = fig.add_subplot(111, projection = '3d')
x = P[:,0]
y = P[:,1]
error_now = 10000000
C2_save = 0
w_save = 0
for i in range(4):
C2 = K2 @ M2s[:, :, i] # To be modified
w, error = submission.triangulate(C1, pts1_in, C2, pts2_in)
if error<error_now:
if np.min(w[:,2])>0:
error_now = error
index = i
C2_save = C2
w_save = w
print('Reprojection error before Bundle Adjustment: %f' % error_now)
M2 = M2s[:, :, index]
M2_final, w_final = submission.bundleAdjustment(K1, M1, pts1_in, K2, M2, pts1_in, w_save[:,0:3])
P = w_final
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
xmin, xmax = np.min(P[:, 0]), np.max(P[:, 0])
ymin, ymax = np.min(P[:, 1]), np.max(P[:, 1])
zmin, zmax = np.min(P[:, 2]), np.max(P[:, 2])
ax.set_xlim3d(xmin, xmax)
ax.set_ylim3d(ymin, ymax)
ax.set_zlim3d(zmin, zmax)
ax.scatter(P[:, 0], P[:, 1], P[:, 2], c='b', marker='o')