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 q5_1(I1, I2, M):
with np.load("../data/some_corresp_noisy.npz") as data:
pts1 = data['pts1']
pts2 = data['pts2']
inliers = len(pts1)
# F = eightpoint(pts1, pts2, M)
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")
I1 = I1[::, ::, ::-1]
I2 = I2[::, ::, ::-1]
displayEpipolarF(I1, I2, F)
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 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 helper
import submission as sub
from mpl_toolkits.mplot3d import Axes3D
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])
# 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 = 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)
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'
epipolarMatchGUI(im1, im2, F8)
# 5.1
"""
You can opt to uncomment this if extra credit q5 is implemented. Note this only checks formatting.
"""
F, inliers = sub.ransacF(data['pts1'], data['pts2'], M, tol=12)
assert F.shape == (3, 3), 'ransacF returns 3x3 matrix'
displayEpipolarF(im1, im2, F)
# 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)
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')
pts1=data['pts1']
pts2=data['pts2']
M=650
[F, inliers]=sub.ransacF(pts1,pts2,M)
print('best F:\n',F)
# print('inliers=',inliers)
pts1_inliers=pts1[inliers,:]
pts2_inliers=pts2[inliers,:]
F = sub.eightpoint(pts1_inliers, pts2_inliers, M)
helper.displayEpipolarF(im1, im2, F)
# # directly run eightpoint on noisy correspondance
# F = sub.eightpoint(pts1, pts2, M)
# helper.displayEpipolarF(im1, im2, F)
import numpy as np
import cv2
from helper import displayEpipolarF
from submission import eightpoint, ransacF
im1 = cv2.imread('../data/im1.png')[:, :, ::-1]
im2 = cv2.imread('../data/im2.png')[:, :, ::-1]
corresp = np.load('../data/some_corresp_noisy.npz')
pts1 = corresp['pts1']
pts2 = corresp['pts2']
M = np.max(im1.shape)
print('======no RANSAC=======')
F1 = eightpoint(pts1, pts2, M)
print(F1)
displayEpipolarF(im1, im2, F1)
print('======RANSAC=======')
F2, _ = ransacF(pts1, pts2, M)
print(F2)
displayEpipolarF(im1, im2, F2)
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'
print('3.1 done')
# 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'
print('4.1 done')
# 5.1
F, inliers = sub.ransacF(data['pts1'], data['pts2'], M, 5, 0.9)
assert F.shape == (3, 3), 'ransacF returns 3x3 matrix'
print('5.1 done')
# 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'
print('5.2 done')
# 5.3
from submission import eightpoint, ransacF, epipolarCorrespondence, triangulate, essentialMatrix, bundleAdjustment
from helper import camera2
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
im1 = cv2.imread('../data/im1.png')[:, :, ::-1]
im2 = cv2.imread('../data/im2.png')[:, :, ::-1]
corresp = np.load('../data/some_corresp_noisy.npz')
pts1 = corresp['pts1']
pts2 = corresp['pts2']
M = np.max(im1.shape)
print('%d corresps' % pts1.shape[0])
F, inliers = ransacF(pts1, pts2, M)
pts1, pts2 = pts1[inliers, :], pts2[inliers, :]
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
helper.displayEpipolarF(im1, im2, Farray[1])
np.savez('q2_2.npz', F=Farray[1], M=M, pts1=pts1, pts2=pts2)
# 3.1
intrinsic = np.load('../data/intrinsics.npz')
K1, K2 = intrinsic['K1'], intrinsic['K2']
E = sub.essentialMatrix(F8, K1, K2)
print(E)
# 4.1
selected_pts1, selected_pts2 = helper.epipolarMatchGUI(im1, im2, F8)
#np.savez('q4_1.npz', F=F8, pts1=selected_pts1, pts2=selected_pts2)
# 5.1
noise_data = np.load('../data/some_corresp_noisy.npz')
F, inliers = sub.ransacF(noise_data['pts1'], noise_data['pts2'], M)
np.savez('tmpnew.npz', F=F, inliers=inliers)
# helper.displayEpipolarF(im1, im2, F)
# F_compare = sub.eightpoint(noise_data['pts1'], noise_data['pts2'], M)
# helper.displayEpipolarF(im1, im2, F_compare)
# 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
# np.savez('q4_1.npz', F = F, pts1 = pts1, pts2 = pts2)
# sel_pts1 , sel_pts2 = helper.epipolarMatchGUI(im1, im2, F)
# # 5.1
pts = np.load('../data/some_corresp_noisy.npz')
pts1 = pts['pts1']
pts2 = pts['pts2']
# # Without RANSAC
F = submission.eightpoint(pts1, pts2, M)
# # helper.displayEpipolarF(im1,im2,F)
# # RANSAC
nIters = 110
tol = 0.85
F, inliers = submission.ransacF(pts1, pts2, M, nIters, tol)
# print("Acccuracy of Ransac: ", (np.count_nonzero(inliers)/len(inliers)))
F = submission.eightpoint(pts1[inliers, :], pts2[inliers, :], M)
# np.savez('F_ransac.npz',F=F, inliers = inliers)
E = submission.essentialMatrix(F, K1, K2)
# # helper.displayEpipolarF(im1, im2, F)
# # 5.3
M1 = np.eye(3)
M1 = np.hstack((M1, np.zeros([3, 1])))
M2_all = helper.camera2(E)
C1 = np.dot(K1, M1)
# Q4.1
# np.savez('../results/q4_1.npz', F = F8, pts1 = data['pts1'], pts2 = data['pts2'])
# helper.epipolarMatchGUI(im1, im2, F8)
# Code edited in helper function for saving pts1 and pts2
# Q4.2
# See viualize.py
# Q5.1
data2 = np.load('../data/some_corresp_noisy.npz')
# for n,v in data2.items():
# print (n)
F, inlier_1, inlier_2 = sub.ransacF(data2['pts1'], data2['pts2'], M)
# helper.displayEpipolarF(im1, im2, F)
# Computing F8 for noisy points
# F8_noisy = sub.eightpoint(data2['pts1'], data2['pts2'], M)
# helper.displayEpipolarF(im1, im2, F8_noisy)
# Q5.2
# a)
# r = np.ones([3, 1])
# R = sub.rodrigues(r)
# '''
# helper.displayEpipolarF(I1, I2, F_ransac)
'''
Q5_3
'''
print("Running Question 5_3")
corresp = np.load("../data/some_corresp_noisy.npz")
pts1 = corresp["pts1"]
pts2 = corresp["pts2"]
I1 = plt.imread('../data/im1.png')
I2 = plt.imread('../data/im2.png')
M = np.maximum(I1.shape[0],I1.shape[1])
F, pts1_in, pts2_in = submission.ransacF(pts1, pts2, M)
# np.savez("../data/F_ransac.npz", F=F, pts1_in=pts1_in, pts2_in=pts2_in)
# F = np.load("../data/F_ransac.npz")["F"]
# pts1_in = np.load("../data/F_ransac.npz")["pts1_in"]
# pts2_in = np.load("../data/F_ransac.npz")["pts2_in"]
K = np.load("../data/intrinsics.npz")
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