def calc_H():
""" This is to calculate the homography projecting the 3D points onto
2D image space, given the coordinates of 4 pairs of points both in
3D space and 2D space.
Returns:
H: The homography matrix with size (3*3)
"""
p1 = np.array([[483, 1704, 2175, 67], [810, 781, 2217, 2286]])
p2 = np.array([[0.0, 18.2, 18.2, 0.0], [0.0, 0.0, 26.0, 26.0]])
H = computeH(p1, p2)
return H
X1 = np.dot(R, W1) + t
X1 = np.dot(K, X1)
X1 = X1 / X1[2, :]
X2 = np.dot(R, W) + t
X2 = np.dot(K, X2)
X2 = X2 / X2[2, :]
plt.plot(np.squeeze(np.asarray(X1[0, :])),
np.squeeze(np.asarray(X1[1, :])),
'y.',
markersize=1)
plt.show()
return X1
if __name__ == '__main__':
W = np.array([[0.0, 18.2, 18.2, 0.0], [0.0, 0.0, 26.0, 26.0],
[0.0, 0.0, 0.0, 0.0]])
X = np.array([[483, 1704, 2175, 67], [810, 781, 2217, 2286]])
K = np.array([[3043.72, 0.0, 1196.00], [0.0, 3043.72, 1604.00],
[0.0, 0.0, 1.0]])
H = planarH.computeH(X, W[:2, :])
R, t = compute_extrinsics(K, H)
print('R:\n', R)
print('t:\n', t)
W = np.loadtxt('..\data\sphere.txt')
im = skimage.io.imread('..\data\prince_book.jpeg')
plt.imshow(im)
X = project_extrinsics(K, W, R, t)
p[0, :] /= p[2, :]
p[1, :] /= p[2, :]
return p[0:2, :]
W = np.array([[0.0, 18.2, 18.2, 0.0], [0.0, 0.0, 26.0, 26.0],
[0.0, 0.0, 0.0, 0.0], [1, 1, 1, 1]],
dtype=np.float32)
X = np.array([[480, 1704, 2175, 67], [810, 781, 2217, 2286]], dtype=np.float32)
K = np.array([[3043.72, 0, 1196], [0, 3043.72, 1604], [0, 0, 1]],
dtype=np.float32)
pw = W[0:2, :]
pi = X[0:2, :]
Hw2i = computeH(pi, pw)
R, t = compute_extrinsics(K, Hw2i)
print(R)
print(t)
sphere = get_sphere('../data/sphere.txt')
sphere += np.array([[6.5], [20], [6.85]], dtype=np.float32)
#sphere = W[0:3, :]
sphere_projected = project_extrinsics(K, sphere, R, t)
im = cv2.imread('../data/prince_book.jpeg')[:, :, [2, 1, 0]]
print(im.shape)
fig = plt.figure()
plt.imshow(im)
transformed = K.dot(R_t).dot(W)
transformed = transformed / transformed[2]
im = plt.imread(image_name)
implot = plt.imshow(im)
plt.scatter(x=transformed[0, :], y=transformed[1, :], c='y', s=1)
plt.show()
plt.close()
if __name__ == '__main__':
K = np.array([[3043.72, 0.0, 1196.00], [0.0, 3043.72, 1604.00],
[0.0, 0.0, 1]])
W = np.array([[0.0, 18.2, 18.2, 0.0], [0.0, 0.0, 26.0, 26.0]])
X = np.array([[483, 1704, 2175, 67], [810, 781, 2217, 2286]])
H = computeH(X, W)
R, t = compute_extrinsics(K, H)
H_prime = np.linalg.inv(K).dot(H)
with open('../data/sphere.txt', 'r') as f:
pts = f.readlines()
W_new = []
for i in pts:
i = i.strip()
results = list(map(float, i.split(' ')))
results = np.asarray(results)
W_new.append(results)
W_new.append(np.ones(results.shape[0]))
W_new = np.asarray(W_new)
center = np.array([810, 1430, 1])
shift = np.linalg.inv(H).dot(center)
inData = inFile.readlines()
inDataX = inData[0].split(' ')
inDataY = inData[1].split(' ')
inDataZ = inData[2].split(' ')
#print (len(inDataX), len(inDataY), len(inDataZ))
sphere3DPoints = np.array([[],[],[]])
for i in range(1, len(inDataX)):
sphere3DPoints = np.append(sphere3DPoints, np.array([[float(inDataX[i]) + 11], [float(inDataY[i]) + 16], [float(inDataZ[i])]]), axis = 1)
sphere3DPoints = np.append(sphere3DPoints, np.ones((1, sphere3DPoints.shape[1])), axis = 0)
#print (sphere3DPoints)
W_2D = W[0:2, :]
#print (X)
H = computeH(W_2D, X)
#print (H)
R, t = compute_extrinsics(K, H)
np.transpose(t)
#print (R, t)
imagePoints = project_extrinsics(K, sphere3DPoints, R, t)
fig = plt.figure()
plt.imshow(im)
for i in range(imagePoints.shape[1]):
plt.plot(imagePoints[0, i], imagePoints[1, i], 'y.', markersize = 1)
#plt.draw()
#plt.waitforbuttonpress(0)
#plt.close(fig)
plt.savefig('../results/arImage.png')
#plt.show()
return R, t
def project_extrinsics(K, W, R, t):
proj = np.matmul(R, W + np.matrix([6.16, 18.4, -ball_diameter / 2]).T) + t
proj = np.matmul(K, proj)
proj = proj / proj[-1]
return proj
def project_on_image(img, proj):
plt.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
plt.plot(proj[0], proj[1], '.', color='yellow')
plt.show()
if __name__ == '__main__':
# Load data
img = cv2.imread('../data/prince_book.jpeg')
sphere = load_sphere('../data/sphere.txt')
# Compute parameters
H = computeH(X[:2], W[:2])
R, t = compute_extrinsics(K, H)
# Project 3D object on to the image
proj = project_extrinsics(K, sphere, R, t)
project_on_image(img, proj)
W = np.array([[0.0, 18.2, 18.2, 0.0], [0.0, 0.0, 26.0, 26.0],
[0.0, 0.0, 0.0, 0.0]])
# X is the projection of the corner
X = np.array([[483, 1704, 2175, 67], [810, 781, 2217, 2286]])
# K is the intrinsic matrix of the camera
K = np.array([[3043.72, 0.0, 1196.00], [0.0, 3043.72, 1604.00],
[0.0, 0.0, 1.0]])
# 0. Read the txt file as the 3D point of the tennis
tennis = np.loadtxt('../data/sphere.txt')
tennis = np.append(tennis, np.ones((1, tennis.shape[1])), axis=0)
# 1. Compute homography matrix H based on given 3D points W and its projection X
H2to1 = computeH(X, W[0:2])
# 2. Calculate the Rotation matrix R and translation vector t
# based on the intrinsic matrix k and homagraphy matrix H
R, t = compute_extrinsics(K, H2to1)
# 3. Calculate the 2D projection of the tennis points use intrinsic K, extrinsic R,t
shift = np.array([[350], [820]])
tennis_2d = project_extrinsics(K, tennis, R, t) + shift
# 4. visualization
fig = plt.figure()
plt.imshow(im)
for i in range(tennis_2d.shape[1]):
plt.plot(tennis_2d[0, i], tennis_2d[1, i], 'y.', markersize=1)
with open('../data/sphere.txt', "r") as f:
str = f.read()
lines = str.split('\n')
x = lines[0].split(' ')
y = lines[1].split(' ')
z = lines[2].split(' ')
sphere = []
for i in range(1, len(x)):
sphere.append([float(x[i]), float(y[i]), float(z[i])])
W_sphere = np.array(sphere).transpose()
W_sphere = np.append(W_sphere, np.ones((1, W_sphere.shape[1])), axis=0)
W_xy = W[0:2, :]
H = computeH(X, W_xy)
R, t = compute_extrinsics(K, H)
dots = project_extrinsics(K, W_sphere, R, t) + np.array(
(350, 820)).reshape((2, 1))
im = imageio.imread('../data/prince_book.jpeg')
fig = plt.figure()
plt.imshow(im)
# plt.show()
for i in range(dots.shape[1]):
plt.plot(dots[0, i], dots[1, i], 'y.', markersize=1)
plt.draw()
plt.waitforbuttonpress(0)
plt.close(fig)
def display_points(K,R,t):
W_new = np.loadtxt('../data/sphere.txt')
shift = [5.2,11.2,-6.85/2]
# shift = [0,0,0]
W_new[0,:] += shift[0]
W_new[1,:] += shift[1]
W_new[2,:] += shift[2]
X = project_extrinsics(K,W_new,R,t)
sphere_x, sphere_y = [], []
for i in range(X.shape[1]):
sphere_x.append(int(X[0,i]))
sphere_y.append(int(X[1,i]))
im = cv2.imread('../data/prince_book.jpeg')
im = cv2.cvtColor(im, cv2.COLOR_BGR2RGB)
implot = plt.imshow(im)
plt.plot(sphere_x, sphere_y, '-', color='yellow')
plt.axis('off')
plt.savefig("../results/ar.png", bbox_inches='tight')
# plt.show()
if __name__ == "__main__":
K = np.array([[3043.72,0,1196],[0,3043.72,1604],[0,0,1]])
W = np.array([[0,18.2,18.2,0],[0,0,26,26],[0,0,0,0]])
X = np.array([[483, 1704, 2175, 67], [810, 781, 2217, 2286]])
H = computeH(X, W[:2,:])
R,t = compute_extrinsics(K,H)
X_projected = project_extrinsics(K,W,R,t)
display_points(K,R,t)
print(np.shape(t))
X = np.matmul(R,W)
X= X+t
X = np.matmul(K,X)
lam = X[-1, :]
X = X / lam
# print(np.shape(lam))
print(X)
# print(np.shape(X))
return(X)
if __name__ == '__main__':
X2 = np.array([[0,18.2,18.2,0],[0,0,26,26]])
X1 = np.array([[483,1704,2175,67],[810,781,2217,2286]])
H = planarH.computeH(X1,X2)
K = np.array([[3043.72,0.0,1196.0],[0.0,3043.72,1604.0],[0.0,0.0,1.0]])
print(K)
R,t = compute_extrinsics(K,H)
textfile = open('../data/sphere.txt','r')
text = textfile.readlines()
W = np.array([], dtype=np.int64).reshape(0, 961)
print(np.shape(text))
for line in text:
# coor = re.findall(r"[-+]?\d*\.\d+|d+",line)
# coor = list(map(float,coor))
# coor = np.array(coor)
number_str = line.split()
coor = np.array([float(x) for x in number_str])
coor = coor.reshape(1,np.shape(coor)[0])
W = np.vstack((W,coor))
book_img = mpimg.imread('../data/prince_book.jpeg')
plt.imshow(book_img)
plt.scatter(X_list, Y_list, c='y', s=1)
plt.show()
if __name__ == '__main__':
W = np.asarray([[0.0, 18.2, 18.2, 0.0], [0.0, 0.0, 26.0, 26.0],
[0.0, 0.0, 0.0, 0.0]])
X = np.asarray([[483, 1704, 2175, 67], [810, 781, 2217, 2286]])
K = np.asarray([[3043.72, 0.0, 1196.0], [0.0, 3043.72, 1604.0],
[0.0, 0.0, 1.0]])
homography_approx = computeH(X, W[0:2, :])
print('homography_approx: ', homography_approx)
R, t = compute_extrinsics(K, homography_approx)
f = open('../data/sphere.txt', 'r')
points_sphere = f.readlines()
f.close()
# print(len(points_sphere))
print('Raw points sphere: ', points_sphere)
X_arr = str.split(points_sphere[0])
X_arr = [float(i) for i in X_arr]
print('X_arr: ', X_arr)
book_im = cv2.imread('../data/prince_book.jpeg')
# plt.imshow(im, cmap='gray')
# plt.show()
#############################
# TO DO ...
# perform required operations and plot sphere
# Four corners of book in real life
W = np.array([[0, 18.2, 18.2, 0], [0, 0, 26, 26], [0, 0, 0, 0]])
# Four corners of the book on image
X = np.array([[483, 1704, 2175, 67], [810, 781, 2217, 2286]])
# Intrinsic matrix for image
K = np.array([[3043.72, 0, 1196], [0, 3043.72, 1604], [0, 0, 1]])
H = computeH(X, W[0:2, :])
R, t = compute_extrinsics(K, H)
# Open sphere file
file = open('../data/sphere.txt', "r")
str = file.read()
str_array = str.split('\n')
sphere_x = str_array[0].split(' ')
sphere_y = str_array[1].split(' ')
sphere_z = str_array[2].split(' ')
tmp_coord_list = []
for i in range(1, len(sphere_x)):
tmp_coord_list.append([float(sphere_x[i]), float(sphere_y[i]), float(sphere_z[i])])
W_sphere = np.array(tmp_coord_list).T
W_sphere = np.concatenate((W_sphere, np.ones((1, W_sphere.shape[1]))), axis=0)
X = np.matmul(K, np.matmul(R, w) + t)
return X
if __name__ == "__main__":
# image
im = cv2.imread('../data/prince_book.jpeg')
#############################
# TO DO ...
# perform required operations and plot sphere
W = np.array([[0.0, 18.2, 18.2, 0], [0.0, 0.0, 26.0, 26.0],
[0.0, 0.0, 0.0, 0.0]])
K = np.array([[3043.72, 0.0, 1196.00], [0.0, 3043.72, 1604.00],
[0.0, 0.0, 1.0]])
X = np.array([[483, 1704, 2175, 67], [810, 781, 2217, 2286]])
H = computeH(W[:2, :], X)
R, t, scale = compute_extrinsics(K, H)
X = project_extrinsics(K, W, R, t)
X = X / X[2, :]
# plt.imshow(im)
# for i in range(X.shape[1]):
# plt.plot(X[0][i]+400,X[1][i]+600, 'y.', markersize=1)
# plt.show()
y = a[1].split(' ')[1:]
z = a[2].split(' ')[1:]
x_vals = [float(i) for i in x]
y_vals = [float(i) for i in y]
z_vals = [float(i) for i in z]
x_vals = np.array(x_vals)
y_vals = np.array(y_vals)
z_vals = np.array(z_vals)
X = np.array([[483, 1704, 2175, 67], [810, 781, 2217, 2286]])
K = np.array([[3043.72, 0.0, 1196.0], [0.0, 3043.72, 1604.00],
[0.0, 0.0, 1.0]])
W = np.array([[0.0, 18.2, 18.2, 0.0], [0.0, 0.0, 26.0, 26.0],
[0.0, 0.0, 0.0, 0.0]])
H = computeH(X, W)
H_inv = computeH(W[:2], X)
o_xy = np.reshape(np.array([830, 1640, 1]), (3, 1))
o_world = H_inv @ o_xy
o_world = o_world / o_world[2]
# print(o_world)
z_vals = z_vals + np.min(z_vals)
x_vals = x_vals + o_world[0]
y_vals = y_vals + o_world[1]
points = np.vstack((x_vals, y_vals, z_vals, np.ones((1, len(x_vals)))))
# print('Homography')
# print(H)
R, t = compute_extrinsics(K, H)
# print('Translation')
