1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
|
import numpy as np, cv2 as cv, matplotlib.pyplot as plt, time, sys, os
from mpl_toolkits.mplot3d import axes3d, Axes3D
def getEpipolarError(F, pts1_, pts2_, inliers):
pts1 = np.concatenate((pts1_.T, np.ones((1, pts1_.shape[0]))))[:,inliers]
pts2 = np.concatenate((pts2_.T, np.ones((1, pts2_.shape[0]))))[:,inliers]
lines2 = np.dot(F , pts1)
lines1 = np.dot(F.T, pts2)
return np.median((np.abs(np.sum(pts1 * lines1, axis=0)) / np.sqrt(lines1[0,:]**2 + lines1[1,:]**2) +
np.abs(np.sum(pts2 * lines2, axis=0)) / np.sqrt(lines2[0,:]**2 + lines2[1,:]**2))/2)
if __name__ == '__main__':
if len(sys.argv) < 3:
print("Path to data file and directory to image files are missing!\nData file must have"
" format:\n--------------\n image_name_1\nimage_name_2\nk11 k12 k13\n0 k22 k23\n"
"0 0 1\n--------------\nIf image_name_{1,2} are not in the same directory as "
"the data file then add argument with directory to image files.\nFor example: "
"python essential_mat_reconstr.py essential_mat_data.txt ./")
exit(1)
else:
data_file = sys.argv[1]
image_dir = sys.argv[2]
if not os.path.isfile(data_file):
print('Incorrect path to data file!')
exit(1)
with open(data_file, 'r') as f:
image1 = cv.imread(image_dir+f.readline()[:-1]) # remove '\n'
image2 = cv.imread(image_dir+f.readline()[:-1])
K = np.array([[float(x) for x in f.readline().split(' ')],
[float(x) for x in f.readline().split(' ')],
[float(x) for x in f.readline().split(' ')]])
if image1 is None or image2 is None:
print('Incorrect directory to images!')
exit(1)
if K.shape != (3,3):
print('Intrinsic matrix has incorrect format!')
exit(1)
print('find keypoints and compute descriptors')
detector = cv.SIFT_create(nfeatures=20000)
keypoints1, descriptors1 = detector.detectAndCompute(cv.cvtColor(image1, cv.COLOR_BGR2GRAY), None)
keypoints2, descriptors2 = detector.detectAndCompute(cv.cvtColor(image2, cv.COLOR_BGR2GRAY), None)
matcher = cv.FlannBasedMatcher(dict(algorithm=0, trees=5), dict(checks=32))
print('match with FLANN, size of descriptors', descriptors1.shape, descriptors2.shape)
matches_vector = matcher.knnMatch(descriptors1, descriptors2, k=2)
print('find good keypoints')
pts1 = []; pts2 = []
for m in matches_vector:
# compare best and second match using Lowe ratio test
if m[0].distance / m[1].distance < 0.75:
pts1.append(keypoints1[m[0].queryIdx].pt)
pts2.append(keypoints2[m[0].trainIdx].pt)
pts1 = np.array(pts1); pts2 = np.array(pts2)
print('points size', pts1.shape[0])
print('Essential matrix RANSAC')
start = time.time()
E, inliers = cv.findEssentialMat(pts1, pts2, K, cv.RANSAC, 0.999, 1.0)
print('RANSAC time', time.time() - start, 'seconds')
print('Median error to epipolar lines', getEpipolarError
(np.dot(np.linalg.inv(K).T, np.dot(E, np.linalg.inv(K))), pts1, pts2, inliers.squeeze()),
'number of inliers', inliers.sum())
print('Decompose essential matrix')
R1, R2, t = cv.decomposeEssentialMat(E)
# Assume relative pose. Fix the first camera
P1 = np.concatenate((K, np.zeros((3,1))), axis=1) # K [I | 0]
P2s = [np.dot(K, np.concatenate((R1, t), axis=1)), # K[R1 | t]
np.dot(K, np.concatenate((R1, -t), axis=1)), # K[R1 | -t]
np.dot(K, np.concatenate((R2, t), axis=1)), # K[R2 | t]
np.dot(K, np.concatenate((R2, -t), axis=1))] # K[R2 | -t]
obj_pts_per_cam = []
# enumerate over all P2 projection matrices
for cam_idx, P2 in enumerate(P2s):
obj_pts = []
for i, (pt1, pt2) in enumerate(zip(pts1, pts2)):
if not inliers[i]:
continue
# find object point by triangulation of image points by projection matrices
obj_pt = cv.triangulatePoints(P1, P2, pt1, pt2)
obj_pt /= obj_pt[3]
# check if reprojected point has positive depth
if obj_pt[2] > 0:
obj_pts.append([obj_pt[0], obj_pt[1], obj_pt[2]])
obj_pts_per_cam.append(obj_pts)
best_cam_idx = np.array([len(obj_pts_per_cam[0]),len(obj_pts_per_cam[1]),
len(obj_pts_per_cam[2]),len(obj_pts_per_cam[3])]).argmax()
max_pts = len(obj_pts_per_cam[best_cam_idx])
print('Number of object points', max_pts)
# filter object points to have reasonable depth
MAX_DEPTH = 6.
obj_pts = []
for pt in obj_pts_per_cam[best_cam_idx]:
if pt[2] < MAX_DEPTH:
obj_pts.append(pt)
obj_pts = np.array(obj_pts).reshape(len(obj_pts), 3)
# visualize image points
for i, (pt1, pt2) in enumerate(zip(pts1, pts2)):
if inliers[i]:
cv.circle(image1, (int(pt1[0]), int(pt1[1])), 7, (255,0,0), -1)
cv.circle(image2, (int(pt2[0]), int(pt2[1])), 7, (255,0,0), -1)
# concatenate two images
image1 = np.concatenate((image1, image2), axis=1)
# resize concatenated image
new_img_size = 1200. * 800.
image1 = cv.resize(image1, (int(np.sqrt(image1.shape[1] * new_img_size / image1.shape[0])),
int(np.sqrt (image1.shape[0] * new_img_size / image1.shape[1]))))
# plot object points
fig = plt.figure(figsize=(13.0, 11.0))
ax = fig.add_subplot(111, projection='3d')
ax.set_aspect('equal')
ax.scatter(obj_pts[:,0], obj_pts[:,1], obj_pts[:,2], c='r', marker='o', s=3)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('depth')
ax.view_init(azim=-80, elev=110)
# save figures
cv.imshow("matches", image1)
cv.imwrite('matches_E.png', image1)
plt.savefig('reconstruction_3D.png')
cv.waitKey(0)
plt.show()
|
