· 4 years ago · Jan 05, 2021, 05:10 PM
1import cv2
2import numpy as np
3import math
4import os
5import glob
6
7ply_header = '''ply
8format ascii 1.0
9element vertex %(vert_num)d
10property float x
11property float y
12property float z
13property uchar red
14property uchar green
15property uchar blue
16end_header
17'''
18
19
20class PLY:
21 def __init__(self, results_dir):
22 self.dir = results_dir
23 self.name = None
24
25 def insert_header(self, point_cloud_size, Name):
26 self.name = self.dir + Name + '.ply'
27
28 with open(self.name, 'wb') as file:
29 file.write((ply_header % dict(vert_num=point_cloud_size+1)).encode('utf-8'))
30 file.write('0 0 0 255 0 0\n'.encode('utf-8'))
31
32 def insert_point(self, x, y, z, b, g, r):
33
34 with open(self.name, 'ab') as file:
35 file.write((str(x[0]) + ' ').encode('utf-8'))
36 file.write((str(y[0]) + ' ').encode('utf-8'))
37 file.write((str(z[0]) + ' ').encode('utf-8'))
38 file.write((str(r) + ' ').encode('utf-8'))
39 file.write((str(g) + ' ').encode('utf-8'))
40 file.write((str(b) + '\n').encode('utf-8'))
41
42class PointCloudTable:
43 def __init__(self):
44 self.table = []
45 self.entry_num = 0
46
47 def init(self):
48 self.table = []
49 self.entry_num = 0
50
51 def add_all_entries(self, two_d, three_d):
52 size2d = len(two_d)
53 three_d_start = len(three_d) - size2d
54 print(size2d)
55 print(len(three_d))
56 print(three_d_start)
57
58 for i in range(size2d):
59 _two_d = (two_d[i].pt[0], two_d[i].pt[1])
60 print(three_d[three_d_start+i])
61 _three_d = (three_d[three_d_start + i][0],
62 three_d[three_d_start + i][1],
63 three_d[three_d_start + i][2])
64
65 self.add_entry(_three_d, _two_d)
66
67 def add_entry(self, three_d, two_d):
68 e = (two_d, three_d)
69 self.table.append(e)
70 self.entry_num += 1
71
72 def find_3d(self, two_d):
73 for i in range(self.entry_num):
74 if self.table[i][0][0] == two_d[0] and self.table[i][0][1] == two_d[1]:
75 p = self.table[i][1]
76 return p
77
78 return None
79
80 def copy(self):
81 ret = PointCloudTable()
82 ret.entry_num = self.entry_num
83 for entry in self.table:
84 ret.table.append(entry)
85
86 return ret
87
88 def table_size(self):
89 return self.entry_num
90
91input_dir = "../Resources/Fountain/"
92output_dir = "F:/Projects/Github/Major-Project-2020/Output/abc"
93format_img = ".jpg"
94
95def KeyPointsToPoints(keypoints):
96 out = []
97 for kp in keypoints:
98 out.append([[kp.pt[0], kp.pt[1]]])
99 res = np.array(out, dtype=np.float32)
100 return res
101
102def PointMatchingSURF(img1, img2, save = False, filename1 = None, filename2 = None):
103 #Matches using SURF
104 surf = cv2.xfeatures2d.SURF_create()
105 keypoints1, descriptors1 = surf.detectAndCompute(img1, None)
106 keypoints2, descriptors2 = surf.detectAndCompute(img2, None)
107 img4 = cv2.drawKeypoints(img1, keypoints1, None)
108 cv2.imwrite("KP.jpg", img4)
109 bf = cv2.BFMatcher_create()
110 matches = bf.match(descriptors1, descriptors2)
111 img3 = cv2.drawMatches(img1, keypoints1, img2, keypoints2, matches[:200], None, flags=2)
112 if save:
113 cv2.imwrite(img1.filename+"_x_"+img2.filename+".jpg", img3)
114 return keypoints1, keypoints2,matches
115
116def PointMatchingOpticalFlow(img1, img2, save=False, filename1 = None, filename2 = None):
117 #matches using optical flow
118 ffd = cv2.FastFeatureDetector_create()
119 left_keypoints = ffd.detect(img1, None)
120 right_keypoints = ffd.detect(img2, None)
121 left_points = KeyPointsToPoints(left_keypoints)
122 right_points = np.zeros_like(left_points)
123
124 #Checking if images are in greyscale
125 prevgray = img1
126 gray = img2
127 if len(img1.shape) == 3:
128 prevgray = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
129 gray = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
130
131 right_points, vstatus, verror = cv2.calcOpticalFlowPyrLK(prevgray,gray,left_points,right_points)
132 #Filterout points with high error
133 right_points_to_find = []
134 right_points_to_find_back_index = []
135 for i in range(0, len(vstatus)):
136 if(vstatus[i] and verror[i] < 12.0):
137 right_points_to_find_back_index.append(i)
138 right_points_to_find.append(right_points[i])
139 else:
140 vstatus[i] = 0
141
142 found_in_imgpts_j = []
143 right_points_to_find_flat = np.array(right_points_to_find).reshape(len(right_points_to_find), 2)
144 right_features = KeyPointsToPoints(right_keypoints)
145 right_features_flat = right_features.reshape(len(right_features), 2)
146
147 #Look around each of point in right image for any features that were detected in its area and make a match
148 matcher = cv2.BFMatcher_create(cv2.NORM_L2)
149 nearest_neighbours = matcher.radiusMatch(right_points_to_find_flat, right_features_flat, 2.0)#THIS IS THE NEW LINE added(Sarthak)
150 #nearest_neighbours = cv2.BFMatcher().radiusMatch(right_features_flat, right_points_to_find_flat, 2.0)
151 matches = []
152 # print(len(nearest_neighbours))
153
154 for i in range(0, len(nearest_neighbours)):
155 _m = None
156 if len(nearest_neighbours[i]) == 1:
157 _m = nearest_neighbours[i][0]
158 elif len(nearest_neighbours[i]) > 1:
159 if (nearest_neighbours[i][0].distance / nearest_neighbours[i][1].distance) < 0.7:
160 _m = nearest_neighbours[i][0]
161 else:
162 #did not pass ratio test
163 pass
164 else:
165 #no match
166 pass
167
168 #prevent duplicates
169 if _m != None:
170 if found_in_imgpts_j.count(_m.trainIdx) == 0:
171 #back to original indexing of points for <i_idx>
172 _m.queryIdx = right_points_to_find_back_index[_m.queryIdx]
173 matches.append(_m)
174 right_points_to_find_back_index.append(_m.trainIdx) #Added this LINE(Sarthak)
175
176 img3 = cv2.drawMatches(img1, left_keypoints, img2, right_keypoints, matches, None)
177 if save:
178 cv2.imwrite(filename1+"_x_"+filename2+".jpg", img3)
179 return left_keypoints, right_keypoints, matches
180
181def findCalibrationMat():
182 with open(input_dir+'intrinsic.txt') as f:
183 lines = f.readlines()
184 return np.array(
185 [l.strip().split(' ') for l in lines],
186 dtype=np.float32
187 )
188 # # For Fountain dataset
189 # # K = np.float32([[2759.48, 0, 1520.69],
190 # # [0, 12764.16, 1006.81],
191 # # [0, 0, 1]])
192
193 # #For Palace dataset
194 # # K = np.float32([[2780.1700000000000728, 0, 1539.25],
195 # # [0, 2773.5399999999999636, 1001.2699999999999818],
196 # # [0, 0, 1]])
197
198 # #For Rubics and Rubics_Vertices
199 # K = np.float32([[2666.6667, 0, 960],
200 # [0, 2250.0000, 540],
201 # [0, 0, 1]])
202 #return K
203
204def FindEssentialMat(kp1, kp2, matches, K):
205 imgpts1 = []
206 imgpts2 = []
207 for i in range(0, len(matches)):
208 imgpts1.append([[kp1[matches[i].queryIdx].pt[0], kp1[matches[i].queryIdx].pt[1]]])
209 imgpts2.append([[kp2[matches[i].trainIdx].pt[0], kp2[matches[i].trainIdx].pt[1]]])
210
211 F = cv2.findFundamentalMat(np.array(imgpts1, dtype=np.float32), np.array(imgpts2, dtype=np.float32), method=cv2.FM_RANSAC, ransacReprojThreshold=0.1, confidence=0.99)
212 E = np.matmul(np.matmul(np.transpose(K), F[0]), K)
213 return E
214
215def checkCoherentRotation(R):
216 if(math.fabs(np.linalg.det(R))-1.0 > 1e-07):
217 print("Not a coherent rotational Matrix")
218 return False
219 else:
220 print("Coherent Rotational Matrix found")
221 return True
222
223def FindPMat(E):
224 _, u, vt = cv2.SVDecomp(E, flags=cv2.SVD_MODIFY_A)
225 W = np.float32([[0,-1,0],
226 [1,0,0],
227 [0,0,1]])
228 R = np.matmul(np.matmul(u, W), vt)
229 t = u[:, 2]
230
231 P = np.float32([])
232 if checkCoherentRotation(R):
233 P = np.float32([[R[0][0], R[0][1], R[0][2], t[0]],
234 [R[1][0], R[1][1], R[1][2], t[1]],
235 [R[2][0], R[2][1], R[2][2], t[2]]])
236 else:
237 P = None
238 return P
239
240def LinearLSTriangulation(u, P, u1, P1):
241 A = np.float32([[u[0]*P[2][0]-P[0][0], u[0]*P[2][1]-P[0][1], u[0]*P[2][2]-P[0][2]],
242 [u[1]*P[2][0]-P[1][0], u[1]*P[2][1]-P[1][1], u[1]*P[2][2]-P[1][2]],
243 [u1[0]*P1[2][0]-P1[0][0], u1[0]*P1[2][1]-P1[0][1], u1[0]*P1[2][2]-P[0][2]],
244 [u1[1]*P1[2][0]-P1[1][0], u1[1]*P1[2][1]-P1[1][1], u1[1]*P1[2][2]-P1[1][2]]])
245 A = np.reshape(A, [4, 3])
246 B = np.float32([[-(u[0]*P[2][3]-P[0][3])],
247 [-(u[1]*P[2][3]-P[1][3])],
248 [-(u1[0]*P1[2][3]-P1[0][3])],
249 [-(u1[1]*P1[2][3]-P1[1][3])]])
250 B = np.reshape(B, [4, 1])
251 _, X = cv2.solve(A,B,flags=cv2.DECOMP_SVD)
252 return X
253
254def TraingulatePoints(pt_set1, pt_set2, matches, K, P, P1, img1, ply, _current = None):
255 Kinv = np.linalg.inv(K)
256 reproj_error = []
257 for i in range(0, len(matches)):
258 kp = pt_set1[matches[i].queryIdx].pt
259 u = np.float32([[[kp[0]], [kp[1]], [1]]])
260 um = np.matmul(Kinv, u)
261 u = um[0]
262
263 kp1 = pt_set2[matches[i].trainIdx].pt
264 u1 = np.float32([[[kp1[0]], [kp1[1]], [1]]])
265 um1 = np.matmul(Kinv, u1)
266 u1 = um1[0]
267
268 #Triangulate
269 X = LinearLSTriangulation(u, P, u1, P1)
270
271 #Calculate reprojection error
272 X1 = [[X[0][0]],
273 [X[1][0]],
274 [X[2][0]],
275 [1]]
276 xPt_img = np.matmul(np.matmul(K, P1), X1)
277
278 xPt_img_ = np.float32([[xPt_img[0]/xPt_img[2], xPt_img[1]/xPt_img[2]]])
279 reproj_error.append(np.linalg.norm(xPt_img_-kp1))
280 reproj_error.append(1.0)
281
282 #print(kp[0], kp[1])
283 bgr = img1[int(kp[1]),int(kp[0])]
284
285 if _current is not None:
286 _current.add_entry((X[0],X[1],X[2]), (kp1[0], kp1[1]))
287
288 #x = X[0] y = Y[1] z = X[2]
289 ply.append([X[0],X[1],X[2],bgr[0],bgr[1],bgr[2]])
290 me = np.mean(reproj_error)
291 return me, ply
292
293def find_second_camera_matrix(p1, new_kp, old_kp, matches, current, prev, K):
294 found_points2D = []
295 found_points3D = []
296
297 kp1 = []
298 kp2 = []
299
300 for i in range(0, len(matches)):
301 kp1.append(new_kp[matches[i].trainIdx])
302 kp2.append(old_kp[matches[i].queryIdx])
303
304 for i in range(len(kp2)):
305 found = prev.find_3d(kp2[i].pt)
306 if found is not None:
307 new_point = (found[0], found[1], found[2])
308 new_point2 = (kp1[i].pt[0], kp1[i].pt[1])
309
310 found_points3D.append(new_point)
311 found_points2D.append(new_point2)
312 #current.add_entry(new_point, new_point2)
313
314 print('Matches found in table: ' + str(len(found_points2D)))
315
316 size = len(found_points3D)
317
318 found3d_points = np.zeros([size, 3], dtype=np.float32)
319 found2d_points = np.zeros([size, 2], dtype=np.float32)
320
321 for i in range(size):
322 found3d_points[i, 0] = found_points3D[i][0]
323 found3d_points[i, 1] = found_points3D[i][1]
324 found3d_points[i, 2] = found_points3D[i][2]
325
326 found2d_points[i, 0] = found_points2D[i][0]
327 found2d_points[i, 1] = found_points2D[i][1]
328
329 p_tmp = p1.copy()
330
331 r = np.float32(p_tmp[0:3, 0:3])
332 t = np.float32(p_tmp[0:3, 3:4])
333
334 r_rog, _ = cv2.Rodrigues(r)
335
336 _dc = np.float32([0, 0, 0, 0])
337
338 _, r_rog, t = cv2.solvePnP(found3d_points, found2d_points, K, _dc, r_rog, t)
339 t1 = np.float32(t)
340
341 R1, _ = cv2.Rodrigues(r_rog)
342
343 camera = np.float32([
344 [R1[0, 0], R1[0, 1], R1[0, 2], t1[0]],
345 [R1[1, 0], R1[1, 1], R1[1, 2], t1[1]],
346 [R1[2, 0], R1[2, 1], R1[2, 2], t1[2]]
347 ])
348
349 return camera
350
351def PairStructureFromMotion():
352 img1 = cv2.imread("../Resources/Fountain/im1.jpg")
353 img2 = cv2.imread("../Resources/Fountain/im2.jpg")
354 #kp1, kp2, matches = PointMatchingSURF(img1, img2)
355 kp1, kp2, matches = PointMatchingOpticalFlow(img1, img2)
356 K = findCalibrationMat()
357 E = FindEssentialMat(kp1, kp2, matches, K)
358
359 P0 = np.float32([[1,0,0,0],
360 [0,1,0,0],
361 [0,0,1,0]])
362
363 P1 = FindPMat(E)
364 print(P1)
365
366 ply = []
367 error, ply = TraingulatePoints(kp1, kp2, matches, K, P0, P1, img1, ply)
368 print("Mean Error = ", error)
369
370 # out = PLY("Pair_Output/")
371 # out.insert_header(len(ply), "Palace")
372 # for i in range(0,len(ply)):
373 # out.insert_point(ply[i][0],ply[i][1],ply[i][2],ply[i][3],ply[i][4],ply[i][5])
374
375
376 cv2.destroyAllWindows()
377
378def MultiViewStructureFromMotion():
379 #Create Output directory if does not exists
380 try:
381 os.mkdir(output_dir)
382 except FileExistsError:
383 pass
384 out = PLY(output_dir+'/')
385
386 #Number of frames present in the input directory
387 number_of_images = len(glob.glob1(input_dir, '*' + format_img))
388
389 current = PointCloudTable()
390 prev = PointCloudTable()
391
392 file_number = 0
393
394 picture_number1 = 0
395 picture_number2 = 1
396
397 image_name1 = input_dir + 'im0' + format_img
398 image_name2 = input_dir + 'im1' + format_img
399
400 frame1 = cv2.imread(image_name1)
401 frame2 = cv2.imread(image_name2)
402
403 point_cloud = []
404 p1 = np.zeros([3, 4], dtype=np.float32)
405 p2 = np.zeros([3, 4], dtype=np.float32)
406
407 initial_3d_model = True
408
409 factor = 1
410 count = 0
411
412 K = findCalibrationMat()
413
414 while file_number < number_of_images - 1:
415 print('Using ' + str(image_name1) + ' and ' + str(image_name2))
416 print('Matching...')
417 kp1, kp2, matches = PointMatchingOpticalFlow(frame1, frame2, True, "im" + str(picture_number1), "im" + str(picture_number2))
418
419 if len(matches) >= 8:
420 if initial_3d_model:
421 E = FindEssentialMat(kp1, kp2, matches, K)
422 p1 = np.float32([[1,0,0,0],
423 [0,1,0,0],
424 [0,0,1,0]])
425 p2 = FindPMat(E)
426
427 print('Creating initial 3D model...')
428 error, point_cloud = TraingulatePoints(kp1, kp2, matches, K, p1, p2, frame1, point_cloud, current)
429 print("Mean Error = ", error)
430 print('Initial lookup table size is: ' + str(current.table_size()))
431 initial_3d_model = False
432 else:
433 prev.init()
434 prev = current.copy()
435
436 current.init()
437
438 print('LookupTable size is: ' + str(prev.table_size()))
439
440 p1 = p2.copy()
441 p2 = find_second_camera_matrix(p2, kp2, kp1, matches, current, prev, K)
442
443 #print('New table size after adding known 3D points: ' + str(current.table_size()))
444 print('Triangulating...')
445 error, point_cloud = TraingulatePoints(kp1, kp2, matches, K, p1, p2, frame1, point_cloud, current)
446 print("Mean Error = ", error)
447
448 print('Start writing points to file...')
449 out.insert_header(len(point_cloud), str(file_number))
450 for i in range(len(point_cloud)):
451 out.insert_point(point_cloud[i][0], point_cloud[i][1], point_cloud[i][2],
452 point_cloud[i][3], point_cloud[i][4], point_cloud[i][5])
453 point_cloud = []
454 file_number += 1
455
456 else:
457 print("Not enough matches...")
458
459 picture_number1 = picture_number2 % number_of_images
460 picture_number2 = (picture_number2 + factor) % number_of_images
461
462 count += 1
463 if count % number_of_images == number_of_images - 1:
464 picture_number2 += 1
465 factor += 1
466
467 image_name1 = input_dir + 'im' + str(picture_number1) + format_img
468 image_name2 = input_dir + 'im' + str(picture_number2) + format_img
469 frame1 = cv2.imread(image_name1)
470 frame2 = cv2.imread(image_name2)
471
472 print("\n\n")
473 print("Done")
474
475#PairStructureFromMotion()
476MultiViewStructureFromMotion()