├── modeler ├── __init__.py ├── SpacePoint.py ├── VideoToImages.py ├── PLY_Manip.py ├── PointCloudTable.py ├── ImagePair.py ├── Matcher.py ├── Triangulation.py └── SfM.py ├── dist ├── modeler-0.0.1.tar.gz └── modeler-0.0.1-py3-none-any.whl ├── tests └── test.py ├── setup.py ├── LICENSE └── README.md /modeler/__init__.py: -------------------------------------------------------------------------------- 1 | from .SfM import SfM -------------------------------------------------------------------------------- /dist/modeler-0.0.1.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ItsWajdy/modeler/HEAD/dist/modeler-0.0.1.tar.gz -------------------------------------------------------------------------------- /dist/modeler-0.0.1-py3-none-any.whl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ItsWajdy/modeler/HEAD/dist/modeler-0.0.1-py3-none-any.whl -------------------------------------------------------------------------------- /tests/test.py: -------------------------------------------------------------------------------- 1 | from modeler import SfM 2 | 3 | 4 | sfm = SfM('results/', True, 'videos/vid1.mp4', 27, debug_mode=False) 5 | print('Done') 6 | # sfm.find_structure_from_motion() 7 | -------------------------------------------------------------------------------- /modeler/SpacePoint.py: -------------------------------------------------------------------------------- 1 | class SpacePoint: 2 | def __init__(self, x=0, y=0, z=0, r=0, g=0, b=0): 3 | self.x = x 4 | self.y = y 5 | self.z = z 6 | self.r = r 7 | self.g = g 8 | self.b = b 9 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name='modeler', 5 | version='0.0.1', 6 | packages=['modeler'], 7 | url='https://github.com/Wajdy759/modeler', 8 | license='mit', 9 | author='Wajdy', 10 | author_email='itswajdy@gmail.com', 11 | description='Reconstruct 3D models of objects from multiple images' 12 | ) 13 | -------------------------------------------------------------------------------- /modeler/VideoToImages.py: -------------------------------------------------------------------------------- 1 | import cv2 as cv 2 | import os 3 | 4 | 5 | class VideoToImages: 6 | def __init__(self, video_path, images_dir, sampling_rate, debug_mode=False): 7 | self.video_path = video_path 8 | self.images_dir = images_dir 9 | self.sampling_rate = sampling_rate 10 | self.debug_mode = debug_mode 11 | 12 | try: 13 | os.mkdir(self.images_dir) 14 | except FileExistsError: 15 | pass 16 | 17 | def convert(self): 18 | print('Converting video to images...\n') 19 | cap = cv.VideoCapture(self.video_path) 20 | counter = 0 21 | name_counter = 0 22 | 23 | while True: 24 | ret, frame = cap.read() 25 | if not ret: 26 | break 27 | 28 | if counter % self.sampling_rate == 0: 29 | if self.debug_mode: 30 | print('Writing image #' + str(name_counter)) 31 | cv.imwrite(self.images_dir + 'im' + str(name_counter) + '.jpg', frame) 32 | name_counter += 1 33 | 34 | counter += 1 35 | 36 | cap.release() 37 | 38 | print('Done converting!\n\n') 39 | return name_counter 40 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2018 The Python Packaging Authority 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. -------------------------------------------------------------------------------- /modeler/PLY_Manip.py: -------------------------------------------------------------------------------- 1 | ply_header = '''ply 2 | format ascii 1.0 3 | element vertex %(vert_num)d 4 | property float x 5 | property float y 6 | property float z 7 | property uchar red 8 | property uchar green 9 | property uchar blue 10 | end_header 11 | ''' 12 | 13 | 14 | class PLY_Manip: 15 | def __init__(self, results_dir): 16 | self.dir = results_dir 17 | 18 | def insert_header(self, point_cloud_size, index): 19 | number = str(index) 20 | name = self.dir + 'out' + number + '.ply' 21 | 22 | with open(name, 'wb') as file: 23 | file.write((ply_header % dict(vert_num=point_cloud_size+1)).encode('utf-8')) 24 | file.write('0 0 0 255 0 0\n'.encode('utf-8')) 25 | 26 | def insert_point(self, x, y, z, b, g, r, index): 27 | number = str(index) 28 | name = self.dir + 'out' + number + '.ply' 29 | 30 | with open(name, 'ab') as file: 31 | file.write((str(x[0]) + ' ').encode('utf-8')) 32 | file.write((str(y[0]) + ' ').encode('utf-8')) 33 | file.write((str(z[0]) + ' ').encode('utf-8')) 34 | file.write((str(b) + ' ').encode('utf-8')) 35 | file.write((str(g) + ' ').encode('utf-8')) 36 | file.write((str(r) + '\n').encode('utf-8')) 37 | -------------------------------------------------------------------------------- /modeler/PointCloudTable.py: -------------------------------------------------------------------------------- 1 | class PointCloudTable: 2 | def __init__(self): 3 | self.table = [] 4 | self.entry_num = 0 5 | 6 | def init(self): 7 | # TODO check if correct behaviour 8 | self.table = [] 9 | self.entry_num = 0 10 | 11 | def add_all_entries(self, two_d, three_d): 12 | # TODO check this method for correct behaviour 13 | size2d = len(two_d) 14 | three_d_start = len(three_d) - size2d 15 | 16 | for i in range(size2d): 17 | _two_d = (two_d[i].pt[0], two_d[i].pt[1]) 18 | 19 | _three_d = (three_d[three_d_start + i].x, 20 | three_d[three_d_start + i].y, 21 | three_d[three_d_start + i].z) 22 | 23 | self.add_entry(_three_d, _two_d) 24 | 25 | def add_entry(self, three_d, two_d): 26 | e = (two_d, three_d) 27 | self.table.append(e) 28 | self.entry_num += 1 29 | 30 | def find_3d(self, two_d): 31 | for i in range(self.entry_num): 32 | if self.table[i][0][0] == two_d[0] and self.table[i][0][1] == two_d[1]: 33 | p = self.table[i][1] 34 | return p 35 | 36 | return None 37 | 38 | def copy(self): 39 | ret = PointCloudTable() 40 | ret.entry_num = self.entry_num 41 | for entry in self.table: 42 | ret.table.append(entry) 43 | 44 | return ret 45 | 46 | def table_size(self): 47 | return self.entry_num 48 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # modeler 2 | A simple package to reconstruct a 3D model of an object from a video taken for that object 3 | 4 | ## Prerequisites 5 | This package uses Python 3.x 6 | 7 | Also, you'll need OpenCV and numpy which can be downloaded using the following commands 8 | ``` 9 | pip install numpy 10 | pip install opencv-python 11 | ``` 12 | 13 | ## Installation 14 | To install modeler simply run this command from the command line: 15 | ``` 16 | pip install modeler 17 | ``` 18 | Or 19 | ``` 20 | pip install git+git://github.com/Wajdy759/modeler.git#egg=modeler 21 | ``` 22 | Then you can import it using: 23 | ``` 24 | import modeler 25 | ``` 26 | 27 | ## Usage 28 | modeler has a module called SfM which handles wraps everything you'll need 29 | ``` 30 | sfm = SfM(results_dir, video_already_converted, video_path, video_sampling_rate) 31 | ``` 32 | Where: 33 | - `results_dir`: the directory where the module outputs the .ply file 34 | - `video_already_converted`: a boolean set to true if the video used was already converted to images in 'input_images' 35 | - `video_path`: in case video_already_converted is False, the module uses the video in 'video_path' as input 36 | - `video_sampling_rate`: the frequency at which the module extracts images from the video to use in the actual reconstruction 37 | 38 | To actually run the algorithm and get the 3D model you'd use 39 | ``` 40 | sfm.find_structure_from_motion() 41 | ``` 42 | 43 | ## Example 44 | ``` 45 | from modeler import SfM 46 | 47 | sfm = SfM('results/', False, 'videos/vid1.mp4', 27) 48 | sfm.structure_from_motion() 49 | ``` 50 | When the progrm terminates you'll get a set of .ply files in the folder named results 51 | 52 | ## References 53 | This project was heavily inspired by this article https://www.researchgate.net/publication/265190880_3D_reconstruction_from_multiple_images 54 | -------------------------------------------------------------------------------- /modeler/ImagePair.py: -------------------------------------------------------------------------------- 1 | import cv2 as cv 2 | import numpy as np 3 | from .Matcher import Matcher 4 | 5 | 6 | class ImagePair: 7 | def __init__(self, image1, image2, debug_mode=False): 8 | self.image1 = image1 9 | self.image2 = image2 10 | self.debug_mode = debug_mode 11 | 12 | good_matcher = Matcher() 13 | good_matcher.set_confidence_level(0.98) 14 | good_matcher.set_min_distance_to_epipolar(1.0) 15 | good_matcher.set_ratio(0.65) 16 | surf = cv.xfeatures2d_SURF.create() 17 | good_matcher.set_detector(surf) 18 | 19 | self.kp1 = [] 20 | self.kp2 = [] 21 | self.colors = [] 22 | self.fundamental_matrix = np.float32([]) 23 | 24 | self.matches = [] 25 | self.full_kp1 = [] 26 | self.full_kp2 = [] 27 | self.enough_matches = False 28 | 29 | self.enough_matches, self.matches, self.full_kp1, self.full_kp2 = good_matcher.match(image1, image2) 30 | self.fundamental_matrix = good_matcher.get_fundamental_matrix() 31 | 32 | for match in self.matches: 33 | self.kp1.append(self.full_kp1[match.queryIdx]) 34 | self.kp2.append(self.full_kp2[match.trainIdx]) 35 | 36 | def get_colors(self, image): 37 | print('Image dimensions: ', image.shape[1], ', ', image.shape[0]) 38 | 39 | colors = [] 40 | 41 | for kp in self.kp1: 42 | y = int(kp.pt[0] + 0.5) 43 | x = int(kp.pt[1] + 0.5) 44 | 45 | # TODO change (everywhere in project) the way to deal with colors and make it BGR not RGB or BRG 46 | blue = image[x, y, 0] 47 | green = image[x, y, 1] 48 | red = image[x, y, 2] 49 | 50 | colors.append((blue, green, red)) 51 | self.colors = colors 52 | return colors 53 | 54 | def display_and_save_matches_image(self): 55 | out_img = 0 56 | out_img = cv.drawMatches(self.image1, self.full_kp1, self.image2, self.full_kp2, self.matches, outImg=out_img) 57 | cv.imwrite('output.jpg', out_img) 58 | 59 | if self.debug_mode: 60 | cv.imshow('Matches', out_img) 61 | cv.waitKey(0) 62 | cv.destroyAllWindows() 63 | 64 | def has_enough_matches(self): 65 | return self.enough_matches 66 | 67 | # Getters 68 | def get_fundamental_matrix(self): 69 | return self.fundamental_matrix 70 | 71 | def get_keypoints_image1(self): 72 | return self.kp1 73 | 74 | def get_keypoints_image2(self): 75 | return self.kp2 76 | 77 | -------------------------------------------------------------------------------- /modeler/Matcher.py: -------------------------------------------------------------------------------- 1 | import cv2 as cv 2 | import numpy as np 3 | 4 | 5 | class Matcher: 6 | def __init__(self): 7 | self.ratio = 0.65 8 | self.refineF = True 9 | self.confidence = 0.99 10 | self.distance = 3.0 11 | self.detector = cv.xfeatures2d_SURF.create() 12 | self.fundamental_matrix = None 13 | 14 | def get_fundamental_matrix(self): 15 | return self.fundamental_matrix 16 | 17 | def set_detector(self, detector): 18 | self.detector = detector 19 | 20 | def set_confidence_level(self, confidence_level): 21 | self.confidence = confidence_level 22 | 23 | def set_min_distance_to_epipolar(self, distance): 24 | self.distance = distance 25 | 26 | def set_ratio(self, ratio): 27 | self.ratio = ratio 28 | 29 | def match(self, image1, image2): 30 | enough_matches = False 31 | 32 | kp1, desc1 = self.detector.detectAndCompute(image1, None) 33 | kp2, desc2 = self.detector.detectAndCompute(image2, None) 34 | 35 | # TODO try cv.DIST_L2 instead 36 | matcher = cv.BFMatcher(cv.NORM_L2) 37 | 38 | matches1 = matcher.knnMatch(desc1, desc2, k=2) 39 | matches2 = matcher.knnMatch(desc2, desc1, k=2) 40 | 41 | _, matches1 = self.ratio_test(matches1) 42 | _, matches2 = self.ratio_test(matches2) 43 | 44 | sym_matches = self.symmetry_test(matches1, matches2) 45 | matches = None 46 | 47 | if len(sym_matches) < 30: 48 | enough_matches = False 49 | else: 50 | enough_matches = True 51 | self.fundamental_matrix, matches = self.ransac_test(sym_matches, kp1, kp2) 52 | if len(matches) < 25: 53 | enough_matches = False 54 | 55 | return enough_matches, matches, kp1, kp2 56 | 57 | def ratio_test(self, matches): 58 | removed = 0 59 | for match in matches: 60 | if len(match) > 1: 61 | if match[0].distance/match[1].distance > self.ratio: 62 | match.clear() 63 | removed += 1 64 | else: 65 | match.clear() 66 | removed += 1 67 | 68 | return removed, matches 69 | 70 | def symmetry_test(self, matches1, matches2): 71 | symmetric_matches = [] 72 | for match1 in matches1: 73 | if len(match1) < 2: 74 | continue 75 | for match2 in matches2: 76 | if len(match2) < 2: 77 | continue 78 | 79 | # Symmetry test 80 | if match1[0].queryIdx == match2[0].trainIdx and match2[0].queryIdx == match1[0].trainIdx: 81 | symmetric_matches.append(cv.DMatch(match1[0].queryIdx, match1[0].trainIdx, match1[0].distance)) 82 | break 83 | 84 | return symmetric_matches 85 | 86 | def ransac_test(self, matches, kp1, kp2): 87 | ret_matches = [] 88 | points1, points2 = [], [] 89 | for match in matches: 90 | points1.append(kp1[match.queryIdx].pt) 91 | points2.append(kp2[match.trainIdx].pt) 92 | 93 | fundamental, inliers = cv.findFundamentalMat(np.float32(points1), np.float32(points2), method=cv.FM_RANSAC, param1=self.distance, param2=self.confidence) 94 | 95 | for inlier, match in zip(inliers, matches): 96 | if inlier[0] > 0: 97 | ret_matches.append(match) 98 | 99 | if self.refineF: 100 | points1 = [] 101 | points2 = [] 102 | for match in ret_matches: 103 | points1.append(kp1[match.queryIdx].pt) 104 | points2.append(kp2[match.trainIdx].pt) 105 | 106 | if len(points1) > 0 and len(points2) > 0: 107 | fundamental, _ = cv.findFundamentalMat(np.float32(points1), np.float32(points2), method=cv.FM_8POINT) 108 | 109 | return fundamental, ret_matches 110 | -------------------------------------------------------------------------------- /modeler/Triangulation.py: -------------------------------------------------------------------------------- 1 | import cv2 as cv 2 | import numpy as np 3 | 4 | from .SpacePoint import SpacePoint 5 | 6 | 7 | class Triangulation: 8 | def __init__(self): 9 | self. K = np.float32([]) 10 | 11 | def calculate_essential_matrix(self, F): 12 | return np.matmul(np.matmul(np.transpose(self.K), F), self.K) 13 | 14 | def decompose_E_to_R_and_T(self, E): 15 | # TODO recheck for correctness 16 | _, u, vt = cv.SVDecomp(E, flags=cv.SVD_MODIFY_A) 17 | 18 | W = np.float32([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) 19 | Wt = np.float32([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]) 20 | 21 | R1 = np.matmul(np.matmul(u, W), vt) 22 | R2 = np.matmul(np.matmul(u, Wt), vt) 23 | t1 = u[:, 2] 24 | t2 = -u[:, 2] 25 | return R1, R2, t1, t2 26 | 27 | def find_matrix_K(self, image): 28 | px = image.shape[1] / 2 29 | py = image.shape[0] / 2 30 | 31 | self.K = np.float32([[1000, 0, px], [0, 1000, py], [0, 0, 1]]) 32 | return self.K 33 | 34 | def find_camera_matrices(self, F): 35 | E = self.calculate_essential_matrix(F) 36 | R1, R2, t1, t2 = self.decompose_E_to_R_and_T(E) 37 | 38 | p_temp = np.float32([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]]) 39 | p1 = p_temp.copy() 40 | 41 | p2 = np.float32([ 42 | [R1[0, 0], R1[0, 1], R1[0, 2], t2[0]], 43 | [R1[1, 0], R1[1, 1], R1[1, 2], t2[1]], 44 | [R1[2, 0], R1[2, 1], R1[2, 2], t2[2]] 45 | ]) 46 | 47 | return p1, p2 48 | 49 | # TODO try cv.triangulatePoints instead 50 | def triangulate(self, kp1, kp2, K, p1, p2, point_cloud): 51 | inv_k = np.linalg.inv(K) 52 | 53 | temp_cloud = point_cloud.copy() 54 | 55 | for k1, k2 in zip(kp1, kp2): 56 | # TODO check if getting right x and y from kp (sometimes they need to be flipped) 57 | x = k1.pt[0] 58 | y = k1.pt[1] 59 | tmp = np.float32([[x], [y], [1]]) 60 | mapping1 = np.matmul(inv_k, tmp) 61 | point3D1 = [mapping1[0], mapping1[1], mapping1[2]] 62 | 63 | x = k2.pt[0] 64 | y = k2.pt[1] 65 | tmp = np.float32([[x], [y], [1]]) 66 | mapping2 = np.matmul(inv_k, tmp) 67 | point3D2 = [mapping2[0], mapping2[1], mapping2[2]] 68 | 69 | X = self.iterative_triangulation(point3D1, p1, point3D2, p2) 70 | 71 | temp_cloud.append(SpacePoint(x=X[0], y=X[1], z=X[2])) 72 | 73 | # point_cloud = temp_cloud.copy() 74 | return temp_cloud 75 | 76 | def linear_ls_triangulation(self, u, p, u1, p1): 77 | tmpA = [ 78 | [u[0]*p[2, 0]-p[0, 0], u[0]*p[2, 1]-p[0, 1], u[0]*p[2, 2]-p[0, 2]], 79 | [u[1]*p[2, 0]-p[1, 0], u[1]*p[2, 1]-p[1, 1], u[1]*p[2, 2]-p[1, 2]], 80 | [u1[0]*p1[2, 0]-p1[0, 0], u1[0]*p1[2, 1]-p1[0, 1], u1[0]*p1[2, 2]-p1[0, 2]], 81 | [u1[1]*p1[2, 0]-p1[1, 0], u1[1]*p1[2, 1]-p1[1, 1], u1[1]*p1[2, 2]-p1[1, 2]] 82 | ] 83 | A = np.float32(tmpA) 84 | A = np.reshape(A, [4, 3]) 85 | 86 | B = np.float32([ 87 | [-(u[0]*p[2, 3] - p[0, 3])], 88 | [-(u[1]*p[2, 3] - p[1, 3])], 89 | [-(u1[0]*p1[2, 3] - p1[0, 3])], 90 | [-(u1[1]*p1[2, 3] - p1[1, 3])] 91 | ]) 92 | B = np.reshape(B, [4, 1]) 93 | 94 | X = np.zeros([]) 95 | _, X = cv.solve(A, B, dst=X, flags=cv.DECOMP_SVD) 96 | return X 97 | 98 | def iterative_triangulation(self, u, p, u1, p1): 99 | wi = 1 100 | wi1 = 1 101 | X = np.zeros([4, 1], dtype=np.float32) 102 | 103 | iterations = 10 104 | 105 | for i in range(iterations): 106 | X_ = self.linear_ls_triangulation(u, p, u1, p1) 107 | X[0] = X_[0] 108 | X[1] = X_[1] 109 | X[2] = X_[2] 110 | X[3] = 1.0 111 | 112 | p2x = (np.matmul(np.float32(p)[2, :], X))[0] 113 | p2x1 = (np.matmul(np.float32(p1)[2, :], X))[0] 114 | 115 | wi = p2x 116 | wi1 = p2x1 117 | 118 | A = np.float32([ 119 | [(u[0]*p[2, 0]-p[0, 0])/wi, (u[0]*p[2, 1]-p[0, 1])/wi, (u[0]*p[2, 2]-p[0, 2])/wi], 120 | [(u[1]*p[2, 0]-p[1, 0])/wi, (u[1]*p[2, 1]-p[1, 1])/wi, (u[1]*p[2, 2]-p[1, 2])/wi], 121 | [(u1[0]*p1[2, 0]-p1[0, 0])/wi1, (u1[0]*p1[2, 1]-p1[0, 1])/wi1, (u1[0]*p1[2, 2]-p1[0, 2])/wi1], 122 | [(u1[1]*p1[2, 0]-p1[1, 0])/wi1, (u1[1]*p1[2, 1]-p1[1, 1])/wi1, (u1[1]*p1[2, 2]-p1[1, 2])/wi1] 123 | ]) 124 | A = np.reshape(A, [4, 3]) 125 | 126 | B = np.float32([ 127 | [-(u[0]*p[2, 3] - p[0, 3])/wi], 128 | [-(u[1]*p[2, 3] - p[1, 3])/wi], 129 | [-(u1[0]*p1[2, 3] - p1[0, 3])/wi1], 130 | [-(u1[1]*p1[2, 3] - p1[1, 3])/wi1] 131 | ]) 132 | B = np.reshape(B, [4, 1]) 133 | 134 | _, X_ = cv.solve(A, B, flags=cv.DECOMP_SVD) 135 | X[0] = X_[0] 136 | X[1] = X_[1] 137 | X[2] = X_[2] 138 | X[3] = 1.0 139 | 140 | return X 141 | -------------------------------------------------------------------------------- /modeler/SfM.py: -------------------------------------------------------------------------------- 1 | import cv2 as cv 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import os 5 | import glob 6 | 7 | from .VideoToImages import VideoToImages as Converter 8 | from .ImagePair import ImagePair 9 | from .PLY_Manip import PLY_Manip 10 | from .Triangulation import Triangulation 11 | from .PointCloudTable import PointCloudTable 12 | 13 | 14 | class SfM: 15 | def __init__(self, results_dir, video_already_converted, video_path=None, video_sampling_rate=None, debug_mode=False): 16 | self.images_dir = 'input_images/' 17 | self.results_dir = results_dir 18 | 19 | try: 20 | os.mkdir(self.results_dir) 21 | except FileExistsError: 22 | pass 23 | 24 | if video_already_converted: 25 | self.number_of_images = len(glob.glob1(self.images_dir, "*.jpg")) 26 | else: 27 | converter = Converter(video_path, self.images_dir, video_sampling_rate, debug_mode) 28 | self.number_of_images = converter.convert() 29 | 30 | self.debug_mode = debug_mode 31 | 32 | self.triangulation = Triangulation() 33 | self.ply = PLY_Manip(self.results_dir) 34 | 35 | self.table1 = PointCloudTable() 36 | self.table2 = PointCloudTable() 37 | 38 | self.table1.init() 39 | self.table2.init() 40 | 41 | self.current = self.table1.copy() 42 | self.prev = self.table2.copy() 43 | 44 | @staticmethod 45 | def downsample(image): 46 | max_rows = 1800 47 | max_cols = 1600 48 | modify_image = image.copy() 49 | height = modify_image.shape[0] 50 | width = modify_image.shape[1] 51 | 52 | if height % 2 != 0: 53 | height -= 1 54 | if width % 2 != 0: 55 | width -= 1 56 | 57 | down_size = modify_image.copy() 58 | 59 | while True: 60 | tmp_height = down_size.shape[0] 61 | tmp_width = down_size.shape[1] 62 | 63 | if tmp_height % 2 != 0: 64 | tmp_height -= 1 65 | if tmp_width % 2 != 0: 66 | tmp_width -= 1 67 | 68 | even_size = down_size[0:tmp_height, 0:tmp_width] 69 | down_size = cv.pyrDown(even_size, dst=down_size, dstsize=(int(tmp_width / 2), int(tmp_height / 2))) 70 | 71 | if tmp_width * tmp_height <= max_cols * max_rows: 72 | break 73 | 74 | return down_size 75 | 76 | @staticmethod 77 | def find_second_camera_matrix(p1, new_kp, old_kp, current, prev, K): 78 | found_points2D = [] 79 | found_points3D = [] 80 | 81 | for i in range(len(old_kp)): 82 | found = prev.find_3d(old_kp[i].pt) 83 | if found is not None: 84 | new_point = (found[0], found[1], found[2]) 85 | new_point2 = (new_kp[i].pt[0], new_kp[i].pt[1]) 86 | 87 | found_points3D.append(new_point) 88 | found_points2D.append(new_point2) 89 | current.add_entry(new_point, new_point2) 90 | 91 | print('Matches found in table: ' + str(len(found_points2D))) 92 | 93 | size = len(found_points3D) 94 | 95 | found3d_points = np.zeros([size, 3], dtype=np.float32) 96 | found2d_points = np.zeros([size, 2], dtype=np.float32) 97 | 98 | for i in range(size): 99 | found3d_points[i, 0] = found_points3D[i][0] 100 | found3d_points[i, 1] = found_points3D[i][1] 101 | found3d_points[i, 2] = found_points3D[i][2] 102 | 103 | found2d_points[i, 0] = found_points2D[i][0] 104 | found2d_points[i, 1] = found_points2D[i][1] 105 | 106 | p_tmp = p1.copy() 107 | 108 | r = np.float32(p_tmp[0:3, 0:3]) 109 | t = np.float32(p_tmp[0:3, 3:4]) 110 | 111 | r_rog, _ = cv.Rodrigues(r) 112 | 113 | _dc = np.float32([0, 0, 0, 0]) 114 | 115 | _, r_rog, t = cv.solvePnP(found3d_points, found2d_points, K, _dc, useExtrinsicGuess=False) 116 | t1 = np.float32(t) 117 | 118 | R1, _ = cv.Rodrigues(r_rog) 119 | 120 | camera = np.float32([ 121 | [R1[0, 0], R1[0, 1], R1[0, 2], t1[0]], 122 | [R1[1, 0], R1[1, 1], R1[1, 2], t1[1]], 123 | [R1[2, 0], R1[2, 1], R1[2, 2], t1[2]] 124 | ]) 125 | 126 | return camera 127 | 128 | def find_structure_from_motion(self): 129 | file_number = 0 130 | 131 | picture_number1 = 0 132 | picture_number2 = 1 133 | 134 | image_name1 = self.images_dir + 'im0.jpg' 135 | image_name2 = self.images_dir + 'im1.jpg' 136 | 137 | # TODO check what -1 means in OpenCV 138 | frame1 = cv.imread(image_name1) 139 | frame2 = cv.imread(image_name2) 140 | 141 | point_cloud = [] 142 | p1 = np.zeros([3, 4], dtype=np.float32) 143 | p2 = np.zeros([3, 4], dtype=np.float32) 144 | 145 | prev_number_of_points_added = 0 146 | initial_3d_model = True 147 | 148 | factor = 1 149 | count = 0 150 | 151 | while file_number < self.number_of_images - 1: 152 | frame1 = SfM.downsample(frame1) 153 | frame2 = SfM.downsample(frame2) 154 | 155 | print('Using ' + str(image_name1) + ' and ' + str(image_name2)) 156 | 157 | if self.debug_mode: 158 | plt.subplot('121') 159 | plt.imshow(frame1) 160 | 161 | plt.subplot('122') 162 | plt.imshow(frame2) 163 | plt.show() 164 | 165 | print('Matching...') 166 | 167 | robust_matcher = ImagePair(frame1, frame2) 168 | kp1 = robust_matcher.get_keypoints_image1() 169 | kp2 = robust_matcher.get_keypoints_image2() 170 | colors = robust_matcher.get_colors(frame1) 171 | 172 | if robust_matcher.has_enough_matches(): 173 | robust_matcher.display_and_save_matches_image() 174 | print('Enough Matches!') 175 | 176 | K = self.triangulation.find_matrix_K(frame1) 177 | if initial_3d_model: 178 | print('Calculating initial camera matrices...') 179 | p1, p2 = self.triangulation.find_camera_matrices(robust_matcher.get_fundamental_matrix()) 180 | 181 | print('Creating initial 3D model...') 182 | point_cloud = self.triangulation.triangulate(kp1, kp2, K, p1, p2, point_cloud) 183 | self.current.add_all_entries(kp2, point_cloud) 184 | 185 | if self.debug_mode: 186 | print('Initial lookup table size is: ' + str(self.current.table_size())) 187 | initial_3d_model = False 188 | else: 189 | self.prev.init() 190 | # TODO one might have to call .copy() on current to eliminate unwanted behaviours 191 | self.prev = self.current.copy() 192 | 193 | if self.current == self.table2: 194 | self.current = self.table1.copy() 195 | elif self.current == self.table1: 196 | self.current = self.table2.copy() 197 | 198 | if self.debug_mode: 199 | print('LookupTable size is: ' + str(self.prev.table_size())) 200 | print('New Table size is: ' + str(self.current.table_size())) 201 | 202 | p1 = p2.copy() 203 | p2 = SfM.find_second_camera_matrix(p2, kp2, kp1, self.current, self.prev, K) 204 | 205 | if self.debug_mode: 206 | print('New table size after adding known 3D points: ' + str(self.current.table_size())) 207 | 208 | print('Triangulating...') 209 | point_cloud = self.triangulation.triangulate(kp1, kp2, K, p1, p2, point_cloud) 210 | self.current.add_all_entries(kp2, point_cloud) 211 | 212 | number_of_points_added = len(kp1) 213 | 214 | print('Start writing points to file...') 215 | self.ply.insert_header(len(point_cloud), file_number) 216 | 217 | for i in range(prev_number_of_points_added): 218 | point = point_cloud[i] 219 | blue = point_cloud[i].b 220 | red = point_cloud[i].r 221 | green = point_cloud[i].g 222 | self.ply.insert_point(point.x, point.y, point.z, red, green, blue, file_number) 223 | 224 | for i in range(number_of_points_added): 225 | point = point_cloud[i + prev_number_of_points_added] 226 | point_color = colors[i] 227 | point_cloud[i + prev_number_of_points_added].b = point_color[0] 228 | point_cloud[i + prev_number_of_points_added].g = point_color[1] 229 | point_cloud[i + prev_number_of_points_added].r = point_color[2] 230 | 231 | self.ply.insert_point(point.x, point.y, point.z, point_color[0], point_color[1], point_color[2], 232 | file_number) 233 | 234 | file_number += 1 235 | prev_number_of_points_added = number_of_points_added + prev_number_of_points_added 236 | 237 | else: 238 | print('Not enough matches') 239 | 240 | picture_number1 = picture_number2 % self.number_of_images 241 | picture_number2 = (picture_number2 + factor) % self.number_of_images 242 | 243 | count += 1 244 | if count % self.number_of_images == self.number_of_images - 1: 245 | picture_number2 += 1 246 | factor += 1 247 | 248 | image_name1 = self.images_dir + 'im' + str(picture_number1) + '.jpg' 249 | image_name2 = self.images_dir + 'im' + str(picture_number2) + '.jpg' 250 | frame1 = cv.imread(image_name1) 251 | frame2 = cv.imread(image_name2) 252 | 253 | print('\n\n') 254 | 255 | print('Done') 256 | 257 | --------------------------------------------------------------------------------