├── README.md ├── camera.py ├── cube_reconstruction.py ├── example.py ├── features.py ├── processor.py ├── requirements.txt ├── structure.py ├── testsets ├── 3d_to_2d_projection.png ├── dino_2d_points.png ├── dino_3d_reconstructed.png ├── house.p3d ├── house1.jpg └── house_3d.png └── transformers.py /README.md: -------------------------------------------------------------------------------- 1 | # 3D reconstruction 2 | 3 | 3D reconstruction from 2D images pipeline 4 | 5 | Steps: 6 | 1. Detect 2D points 7 | 2. Match 2D points across 2 images 8 | 3. Epipolar geometry 9 | 3a. If both intrinsic and extrinsic camera parameters are known, reconstruct with projection matrices. 10 | 3b. If only the intrinsic parameters are known, normalize coordinates and calculate the essential matrix. 11 | 3c. If neither intrinsic nor extrinsic parameters are known, calculate the fundamental matrix. 12 | 4. With fundamental or essential matrix, assume P1 = [I 0] and calulate parameters of camera 2. 13 | 5. Triangulate knowing that x1 = P1 * X and x2 = P2 * X. 14 | 6. Bundle adjustment to minimize reprojection errors and refine the 3D coordinates. 15 | 16 | Note: Steps and code in this repo is my hobby / learning exercise. Ie, its probably not very efficient. If you wish to use a more production-ready library, check out [OpenCV's SFM module](https://github.com/opencv/opencv_contrib/tree/master/modules/sfm). I have a docker environment for it at: https://github.com/alyssaq/reconstruction 17 | 18 | ## Prerequisites 19 | * Python 3.5+ 20 | * Install [OpenCV](http://opencv.org/): [Mac installation steps](https://gist.github.com/alyssaq/f60393545173379e0f3f) 21 | * pip install -r requirements.txt 22 | 23 | ## Example 3D cube reconstruction 24 | ```sh 25 | $ python3 cube_reconstruction.py 26 | ``` 27 | 28 | ## Example Dino 3D reconstruction from 2D images 29 | Download images from and place into `imgs/dinos` 30 | ```sh 31 | $ python3 example.py 32 | ``` 33 | 34 | Detected points and matched across 2 images. 35 | ![](testsets/dino_2d_points.png?raw=true) 36 | 37 | 3D reconstructed dino with essential matrix 38 | ![](testsets/dino_3d_reconstructed.png?raw=true) 39 | 40 | ## 3D to 2D Projection 41 | ```sh 42 | $ python3 camera.py 43 | ``` 44 | 45 | 3D points of model house from Oxford University VGG datasets. 46 | ![](testsets/house_3d.png?raw=true) 47 | 48 | Projected points 49 | ![](testsets/3d_to_2d_projection.png?raw=true) 50 | ## Datasets 51 | * Oxford University, Visual Geometry Group: http://www.robots.ox.ac.uk/~vgg/data/data-mview.html 52 | * EPFL computer vision lab: http://cvlabwww.epfl.ch/data/multiview/knownInternalsMVS.html 53 | 54 | ## References 55 | * [Eight point algorithm](http://ece631web.groups.et.byu.net/Lectures/ECEn631%2013%20-%208%20Point%20Algorithm.pdf) 56 | * [Multiple View Geometry in Computer Vision (Hartley & Zisserman)](http://www.robots.ox.ac.uk/~vgg/hzbook/) 57 | 58 | ## License 59 | [MIT](https://alyssaq.github.io/mit-license) 60 | -------------------------------------------------------------------------------- /camera.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import processor 3 | import transformers 4 | 5 | 6 | class Camera(object): 7 | """ Class for representing pin-hole camera """ 8 | 9 | def __init__(self, P=None, K=None, R=None, t=None): 10 | """ P = K[R|t] camera model. (3 x 4) 11 | Must either supply P or K, R, t """ 12 | if P is None: 13 | try: 14 | self.extrinsic = np.hstack([R, t]) 15 | P = np.dot(K, self.extrinsic) 16 | except TypeError as e: 17 | print('Invalid parameters to Camera. Must either supply P or K, R, t') 18 | raise 19 | 20 | self.P = P # camera matrix 21 | self.K = K # intrinsic matrix 22 | self.R = R # rotation 23 | self.t = t # translation 24 | self.c = None # camera center 25 | 26 | def project(self, X): 27 | """ Project 3D homogenous points X (4 * n) and normalize coordinates. 28 | Return projected 2D points (2 x n coordinates) """ 29 | x = np.dot(self.P, X) 30 | x[0, :] /= x[2, :] 31 | x[1, :] /= x[2, :] 32 | 33 | return x[:2, :] 34 | 35 | def qr_to_rq_decomposition(self): 36 | """ Convert QR to RQ decomposition with numpy. 37 | Note that this could be done by passing in a square matrix with scipy: 38 | K, R = scipy.linalg.rq(self.P[:, :3]) """ 39 | Q, R = np.linalg.qr(np.flipud(self.P).T) 40 | R = np.flipud(R.T) 41 | return R[:, ::-1], Q.T[::-1, :] 42 | 43 | def factor(self): 44 | """ Factorize the camera matrix P into K,R,t with P = K[R|t] 45 | using RQ-factorization """ 46 | if self.K is not None and self.R is not None: 47 | return self.K, self.R, self.t # Already been factorized or supplied 48 | 49 | K, R = self.qr_to_rq_decomposition() 50 | # make diagonal of K positive 51 | T = np.diag(np.sign(np.diag(K))) 52 | if np.linalg.det(T) < 0: 53 | T[1, 1] *= -1 54 | 55 | self.K = np.dot(K, T) 56 | self.R = np.dot(T, R) # T is its own inverse 57 | self.t = np.dot(np.linalg.inv(self.K), self.P[:, 3]) 58 | 59 | return self.K, self.R, self.t 60 | 61 | def center(self): 62 | """ Compute and return the camera center. """ 63 | if self.c is not None: 64 | return self.c 65 | elif self.R: 66 | # compute c by factoring 67 | self.c = -np.dot(self.R.T, self.t) 68 | else: 69 | # P = [M|−MC] 70 | self.c = np.dot(-np.linalg.inv(self.c[:, :3]), self.c[:, -1]) 71 | return self.c 72 | 73 | 74 | def test(): 75 | import matplotlib.pyplot as plt 76 | from mpl_toolkits.mplot3d import Axes3D 77 | import cv2 78 | 79 | # load points 80 | points = processor.read_matrix('testsets/house.p3d').T # 3 x n 81 | points = processor.cart2hom(points) # 4 x n 82 | 83 | img = cv2.imread('testsets/house1.jpg') 84 | height, width, ch = img.shape 85 | 86 | K = np.array([ 87 | [width * 30, 0, width / 2], 88 | [0, height * 30, height / 2], 89 | [0, 0, 1]]) 90 | R = np.array([ # No rotation 91 | [1, 0, 0], 92 | [0, 1, 0], 93 | [0, 0, 1] 94 | ]) 95 | t = np.array([[0], [0], [100]]) # t != 0 to be away from image plane 96 | 97 | # Setup cameras 98 | cam = Camera(K=K, R=R, t=t) 99 | x = cam.project(points) 100 | 101 | rotation_angle = 20 102 | rotation_mat = transformers.rotation_3d_from_angles(rotation_angle) 103 | cam = Camera(K=K, R=rotation_mat, t=t) 104 | x2 = cam.project(points) 105 | 106 | # Plot actual 3d points 107 | fig = plt.figure() 108 | ax = fig.gca(projection='3d') 109 | ax.set_aspect('equal') 110 | ax.plot(points[0], points[1], points[2], 'b.') 111 | ax.set_xlabel('x axis') 112 | ax.set_ylabel('y axis') 113 | ax.set_zlabel('z axis') 114 | ax.view_init(elev=140, azim=0) 115 | 116 | # Plot 3d to 2d projection 117 | f, ax = plt.subplots(2, sharex=True, sharey=True) 118 | plt.subplots_adjust(left=0.08, bottom=0.08, right=0.99, 119 | top=0.95, wspace=0, hspace=0.01) 120 | ax[0].set_aspect('equal') 121 | ax[0].set_title( 122 | '3D to 2D projection. Bottom x-axis rotated by {0}°'.format(rotation_angle)) 123 | ax[0].plot(x[0], x[1], 'k.') 124 | ax[1].plot(x2[0], x2[1], 'k.') 125 | plt.show() 126 | 127 | 128 | if __name__ == '__main__': 129 | test() 130 | -------------------------------------------------------------------------------- /cube_reconstruction.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pylab as plt 3 | from mpl_toolkits.mplot3d import Axes3D 4 | import camera 5 | import processor 6 | import structure 7 | import transformers 8 | 9 | 10 | def plot_projections(points): 11 | num_images = len(points) 12 | 13 | plt.figure() 14 | plt.suptitle('3D to 2D Projections', fontsize=16) 15 | for i in range(num_images): 16 | plt.subplot(1, num_images, i+1) 17 | ax = plt.gca() 18 | ax.set_aspect('equal') 19 | ax.plot(points[i][0], points[i][1], 'r.') 20 | 21 | 22 | def plot_cube(points3d, title=''): 23 | fig = plt.figure() 24 | fig.suptitle(title, fontsize=16) 25 | ax = fig.gca(projection='3d') 26 | ax.set_aspect('equal') 27 | ax.plot(points3d[0], points3d[1], points3d[2], 'b.') 28 | ax.set_xlabel('x axis') 29 | ax.set_ylabel('y axis') 30 | ax.set_zlabel('z axis') 31 | ax.view_init(elev=135, azim=90) 32 | return ax 33 | 34 | def extrinsic_from_camera_pose(m_c1_wrt_c2): 35 | # Inverse to get extrinsic matrix from camera to world view 36 | # http://ksimek.github.io/2012/08/22/extrinsic/ 37 | # Returns homogenous 4x4 extrinsic camera matrix 38 | # Alternatively, R = R^t, T = -RC 39 | # Rct = m_c1_wrt_c2[:3, :3].T 40 | # t = -np.dot(Rct, m_c1_wrt_c2[:3, 3]) 41 | H_m = np.vstack([m_c1_wrt_c2, [0, 0, 0, 1]]) 42 | ext = np.linalg.inv(H_m) 43 | return ext 44 | 45 | 46 | def camera_corners(camera, dist=0.25): 47 | d = dist 48 | x, y, z = np.ravel(camera.t) 49 | corners = np.array([ 50 | [x-d, y+d, z], 51 | [x+d, y+d, z], 52 | [x+d, y-d, z], 53 | [x-d, y-d, z], 54 | [x-d, y+d, z] 55 | ]).T 56 | 57 | return np.asarray(np.dot(camera.R, corners)) 58 | 59 | 60 | size = 300 # size of image in pixels 61 | center = size / 2 62 | intrinsic = np.array([ 63 | [size, 0, center], 64 | [0, size, center], 65 | [0, 0, 1] 66 | ]) 67 | 68 | # Points of on the surface of the cube 69 | points3d = np.array([ 70 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 1, 2, 1, 2], 71 | [2, 1, 0, 2, 1, 0, 2, 1, 0, 0, 0, 0, 0, 0, 0], 72 | [0, 0, 0, -1, -1, -1, -2, -2, -2, 0, 0, -1, -1, -2, -2], 73 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 74 | ]) 75 | 76 | # Define pose of cube with respect to camera1 in world view 77 | rotation_mat = transformers.rotation_3d_from_angles(120, 0, 60) 78 | translation_mat = np.matrix([0, 0, 5]).T 79 | c = camera.Camera(K=intrinsic, R=rotation_mat, t=translation_mat) 80 | 81 | # Project 3d points to camera1 on the left 82 | points1 = c.project(points3d) 83 | points1 = processor.cart2hom(points1) 84 | 85 | # Get 4x4 homogenous extrinsic parameters of camera 1 86 | H_c1 = np.vstack([c.extrinsic, [0, 0, 0, 1]]) 87 | 88 | # Define rotation of camera1 wrt camera2 and 89 | # translation of camera2 wrt camera1 90 | rotation_mat_wrt_c1 = transformers.rotation_3d_from_angles(0, -25, 0) 91 | translation_mat_wrt_c1 = np.matrix([3, 0, 1]).T 92 | H_c2_c1 = np.hstack([rotation_mat_wrt_c1, translation_mat_wrt_c1]) 93 | H_c1_c2 = extrinsic_from_camera_pose(H_c2_c1) 94 | 95 | # Calculate pose of model wrt to camera2 in world view 96 | H_c2 = np.dot(H_c1_c2, H_c1) 97 | 98 | # Project 3d points to camera 2 on the right 99 | c2 = camera.Camera(K=intrinsic, R=H_c2[:3, :3], t=H_c2[:3, 3]) 100 | points2 = c2.project(points3d) 101 | points2 = processor.cart2hom(points2[:2]) 102 | 103 | # True essential matrix E = [t]R 104 | true_E = np.dot(structure.skew(translation_mat_wrt_c1), rotation_mat_wrt_c1) 105 | print('True essential matrix:', true_E) 106 | 107 | # Calculate essential matrix with 2d points. 108 | # Result will be up to a scale 109 | # First, normalize points 110 | points1n = np.dot(np.linalg.inv(intrinsic), points1) 111 | points2n = np.dot(np.linalg.inv(intrinsic), points2) 112 | E = structure.compute_essential_normalized(points1n, points2n) 113 | print('Computed essential matrix:', (-E / E[0][1])) 114 | 115 | # True fundamental matrix F = K^-t E K^-1 116 | true_F = np.dot(np.dot(np.linalg.inv(intrinsic).T, true_E), np.linalg.inv(intrinsic)) 117 | F = structure.compute_fundamental_normalized(points1, points2) 118 | print('True fundamental matrix:', true_F) 119 | print('Computed fundamental matrix:', (F * true_F[2][2])) 120 | 121 | # Given we are at camera 1, calculate the parameters for camera 2 122 | # Using the essential matrix returns 4 possible camera paramters 123 | P1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]]) 124 | P2s = structure.compute_P_from_essential(E) 125 | 126 | ind = -1 127 | for i, P2 in enumerate(P2s): 128 | # Find the correct camera parameters 129 | d1 = structure.reconstruct_one_point(points1n[:, 0], points2n[:, 0], P1, P2) 130 | P2_homogenous = extrinsic_from_camera_pose(P2) 131 | d2 = np.dot(P2_homogenous[:3, :4], d1) 132 | 133 | if d1[2] > 0 and d2[2] > 0: 134 | ind = i 135 | 136 | print('True pose of c2 wrt c1: ', H_c1_c2) 137 | P2 = np.linalg.inv(np.vstack([P2s[ind], [0, 0, 0, 1]]))[:3, :4] 138 | P2f = structure.compute_P_from_fundamental(F) 139 | print('Calculated camera 2 parameters:', P2, P2f) 140 | 141 | tripoints3d = structure.reconstruct_points(points1n, points2n, P1, P2) 142 | tripoints3d = structure.linear_triangulation(points1n, points2n, P1, P2) 143 | 144 | structure.plot_epipolar_lines(points1n, points2n, E) 145 | plot_projections([points1, points2]) 146 | 147 | ax = plot_cube(points3d, 'Original') 148 | cam_corners1 = camera_corners(c) 149 | cam_corners2 = camera_corners(c2) 150 | ax.plot(cam_corners1[0], cam_corners1[1], cam_corners1[2], 'g-') 151 | ax.plot(cam_corners2[0], cam_corners2[1], cam_corners2[2], 'r-') 152 | 153 | plot_cube(tripoints3d, '3D reconstructed') 154 | plt.show() 155 | -------------------------------------------------------------------------------- /example.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from mpl_toolkits.mplot3d import Axes3D 3 | import numpy as np 4 | import cv2 5 | 6 | from camera import Camera 7 | import structure 8 | import processor 9 | import features 10 | 11 | # Download images from http://www.robots.ox.ac.uk/~vgg/data/data-mview.html 12 | 13 | def house(): 14 | input_path = 'imgs/house/' 15 | camera_filepath = 'imgs/house/3D/house.00{0}.P' 16 | 17 | cameras = [Camera(processor.read_matrix(camera_filepath.format(i))) 18 | for i in range(9)] 19 | [c.factor() for c in cameras] 20 | 21 | points3d = processor.read_matrix(input_path + '3D/house.p3d').T # 3 x n 22 | points4d = np.vstack((points3d, np.ones(points3d.shape[1]))) # 4 x n 23 | points2d = [processor.read_matrix( 24 | input_path + '2D/house.00' + str(i) + '.corners') for i in range(9)] 25 | 26 | index1 = 2 27 | index2 = 4 28 | img1 = cv2.imread(input_path + 'house.00' + str(index1) + '.pgm') # left image 29 | img2 = cv2.imread(input_path + 'house.00' + str(index2) + '.pgm') 30 | 31 | # fig = plt.figure() 32 | # ax = fig.gca(projection='3d') 33 | # ax.plot(points3d[0], points3d[1], points3d[2], 'b.') 34 | # ax.set_xlabel('x axis') 35 | # ax.set_ylabel('y axis') 36 | # ax.set_zlabel('z axis') 37 | # ax.view_init(elev=135, azim=90) 38 | 39 | # x = cameras[index2].project(points4d) 40 | # plt.figure() 41 | # plt.plot(x[0], x[1], 'b.') 42 | # plt.show() 43 | 44 | corner_indexes = processor.read_matrix( 45 | input_path + '2D/house.nview-corners', np.int) 46 | corner_indexes1 = corner_indexes[:, index1] 47 | corner_indexes2 = corner_indexes[:, index2] 48 | intersect_indexes = np.intersect1d(np.nonzero( 49 | [corner_indexes1 != -1]), np.nonzero([corner_indexes2 != -1])) 50 | corner_indexes1 = corner_indexes1[intersect_indexes] 51 | corner_indexes2 = corner_indexes2[intersect_indexes] 52 | points1 = processor.cart2hom(points2d[index1][corner_indexes1].T) 53 | points2 = processor.cart2hom(points2d[index2][corner_indexes2].T) 54 | 55 | height, width, ch = img1.shape 56 | intrinsic = np.array([ # for imgs/house 57 | [2362.12, 0, width / 2], 58 | [0, 2366.12, height / 2], 59 | [0, 0, 1]]) 60 | 61 | return points1, points2, intrinsic 62 | 63 | 64 | def dino(): 65 | # Dino 66 | img1 = cv2.imread('imgs/dinos/viff.003.ppm') 67 | img2 = cv2.imread('imgs/dinos/viff.001.ppm') 68 | pts1, pts2 = features.find_correspondence_points(img1, img2) 69 | points1 = processor.cart2hom(pts1) 70 | points2 = processor.cart2hom(pts2) 71 | 72 | fig, ax = plt.subplots(1, 2) 73 | ax[0].autoscale_view('tight') 74 | ax[0].imshow(cv2.cvtColor(img1, cv2.COLOR_BGR2RGB)) 75 | ax[0].plot(points1[0], points1[1], 'r.') 76 | ax[1].autoscale_view('tight') 77 | ax[1].imshow(cv2.cvtColor(img2, cv2.COLOR_BGR2RGB)) 78 | ax[1].plot(points2[0], points2[1], 'r.') 79 | fig.show() 80 | 81 | height, width, ch = img1.shape 82 | intrinsic = np.array([ # for dino 83 | [2360, 0, width / 2], 84 | [0, 2360, height / 2], 85 | [0, 0, 1]]) 86 | 87 | return points1, points2, intrinsic 88 | 89 | 90 | points1, points2, intrinsic = dino() 91 | 92 | # Calculate essential matrix with 2d points. 93 | # Result will be up to a scale 94 | # First, normalize points 95 | points1n = np.dot(np.linalg.inv(intrinsic), points1) 96 | points2n = np.dot(np.linalg.inv(intrinsic), points2) 97 | E = structure.compute_essential_normalized(points1n, points2n) 98 | print('Computed essential matrix:', (-E / E[0][1])) 99 | 100 | # Given we are at camera 1, calculate the parameters for camera 2 101 | # Using the essential matrix returns 4 possible camera paramters 102 | P1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]]) 103 | P2s = structure.compute_P_from_essential(E) 104 | 105 | ind = -1 106 | for i, P2 in enumerate(P2s): 107 | # Find the correct camera parameters 108 | d1 = structure.reconstruct_one_point( 109 | points1n[:, 0], points2n[:, 0], P1, P2) 110 | 111 | # Convert P2 from camera view to world view 112 | P2_homogenous = np.linalg.inv(np.vstack([P2, [0, 0, 0, 1]])) 113 | d2 = np.dot(P2_homogenous[:3, :4], d1) 114 | 115 | if d1[2] > 0 and d2[2] > 0: 116 | ind = i 117 | 118 | P2 = np.linalg.inv(np.vstack([P2s[ind], [0, 0, 0, 1]]))[:3, :4] 119 | #tripoints3d = structure.reconstruct_points(points1n, points2n, P1, P2) 120 | tripoints3d = structure.linear_triangulation(points1n, points2n, P1, P2) 121 | 122 | fig = plt.figure() 123 | fig.suptitle('3D reconstructed', fontsize=16) 124 | ax = fig.gca(projection='3d') 125 | ax.plot(tripoints3d[0], tripoints3d[1], tripoints3d[2], 'b.') 126 | ax.set_xlabel('x axis') 127 | ax.set_ylabel('y axis') 128 | ax.set_zlabel('z axis') 129 | ax.view_init(elev=135, azim=90) 130 | plt.show() 131 | -------------------------------------------------------------------------------- /features.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | 4 | 5 | def find_correspondence_points(img1, img2): 6 | sift = cv2.xfeatures2d.SIFT_create() 7 | 8 | # find the keypoints and descriptors with SIFT 9 | kp1, des1 = sift.detectAndCompute( 10 | cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY), None) 11 | kp2, des2 = sift.detectAndCompute( 12 | cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY), None) 13 | 14 | # Find point matches 15 | FLANN_INDEX_KDTREE = 0 16 | index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) 17 | search_params = dict(checks=50) 18 | flann = cv2.FlannBasedMatcher(index_params, search_params) 19 | matches = flann.knnMatch(des1, des2, k=2) 20 | 21 | # Apply Lowe's SIFT matching ratio test 22 | good = [] 23 | for m, n in matches: 24 | if m.distance < 0.8 * n.distance: 25 | good.append(m) 26 | 27 | src_pts = np.asarray([kp1[m.queryIdx].pt for m in good]) 28 | dst_pts = np.asarray([kp2[m.trainIdx].pt for m in good]) 29 | 30 | # Constrain matches to fit homography 31 | retval, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 100.0) 32 | mask = mask.ravel() 33 | 34 | # We select only inlier points 35 | pts1 = src_pts[mask == 1] 36 | pts2 = dst_pts[mask == 1] 37 | 38 | return pts1.T, pts2.T 39 | -------------------------------------------------------------------------------- /processor.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def read_matrix(path, astype=np.float64): 5 | """ Reads a file containing a matrix where each line represents a point 6 | and each point is tab or space separated. * are replaced with -1. 7 | :param path: path to the file 8 | :parama astype: type to cast the numbers. Default: np.float64 9 | :returns: array of array of numbers 10 | """ 11 | with open(path, 'r') as f: 12 | arr = [] 13 | for line in f: 14 | arr.append([(token if token != '*' else -1) 15 | for token in line.strip().split()]) 16 | return np.asarray(arr).astype(astype) 17 | 18 | 19 | def cart2hom(arr): 20 | """ Convert catesian to homogenous points by appending a row of 1s 21 | :param arr: array of shape (num_dimension x num_points) 22 | :returns: array of shape ((num_dimension+1) x num_points) 23 | """ 24 | if arr.ndim == 1: 25 | return np.hstack([arr, 1]) 26 | return np.asarray(np.vstack([arr, np.ones(arr.shape[1])])) 27 | 28 | 29 | def hom2cart(arr): 30 | """ Convert homogenous to catesian by dividing each row by the last row 31 | :param arr: array of shape (num_dimension x num_points) 32 | :returns: array of shape ((num_dimension-1) x num_points) iff d > 1 33 | """ 34 | # arr has shape: dimensions x num_points 35 | num_rows = len(arr) 36 | if num_rows == 1 or arr.ndim == 1: 37 | return arr 38 | 39 | return np.asarray(arr[:num_rows - 1] / arr[num_rows - 1]) 40 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | matplotlib 3 | -------------------------------------------------------------------------------- /structure.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | 4 | def reconstruct_points(p1, p2, m1, m2): 5 | num_points = p1.shape[1] 6 | res = np.ones((4, num_points)) 7 | 8 | for i in range(num_points): 9 | res[:, i] = reconstruct_one_point(p1[:, i], p2[:, i], m1, m2) 10 | 11 | return res 12 | 13 | 14 | def reconstruct_one_point(pt1, pt2, m1, m2): 15 | """ 16 | pt1 and m1 * X are parallel and cross product = 0 17 | pt1 x m1 * X = pt2 x m2 * X = 0 18 | """ 19 | A = np.vstack([ 20 | np.dot(skew(pt1), m1), 21 | np.dot(skew(pt2), m2) 22 | ]) 23 | U, S, V = np.linalg.svd(A) 24 | P = np.ravel(V[-1, :4]) 25 | 26 | return P / P[3] 27 | 28 | 29 | def linear_triangulation(p1, p2, m1, m2): 30 | """ 31 | Linear triangulation (Hartley ch 12.2 pg 312) to find the 3D point X 32 | where p1 = m1 * X and p2 = m2 * X. Solve AX = 0. 33 | :param p1, p2: 2D points in homo. or catesian coordinates. Shape (3 x n) 34 | :param m1, m2: Camera matrices associated with p1 and p2. Shape (3 x 4) 35 | :returns: 4 x n homogenous 3d triangulated points 36 | """ 37 | num_points = p1.shape[1] 38 | res = np.ones((4, num_points)) 39 | 40 | for i in range(num_points): 41 | A = np.asarray([ 42 | (p1[0, i] * m1[2, :] - m1[0, :]), 43 | (p1[1, i] * m1[2, :] - m1[1, :]), 44 | (p2[0, i] * m2[2, :] - m2[0, :]), 45 | (p2[1, i] * m2[2, :] - m2[1, :]) 46 | ]) 47 | 48 | _, _, V = np.linalg.svd(A) 49 | X = V[-1, :4] 50 | res[:, i] = X / X[3] 51 | 52 | return res 53 | 54 | 55 | def compute_epipole(F): 56 | """ Computes the (right) epipole from a 57 | fundamental matrix F. 58 | (Use with F.T for left epipole.) 59 | """ 60 | # return null space of F (Fx=0) 61 | U, S, V = np.linalg.svd(F) 62 | e = V[-1] 63 | return e / e[2] 64 | 65 | 66 | def plot_epipolar_lines(p1, p2, F, show_epipole=False): 67 | """ Plot the points and epipolar lines. P1' F P2 = 0 """ 68 | plt.figure() 69 | plt.suptitle('Epipolar lines', fontsize=16) 70 | 71 | plt.subplot(1, 2, 1, aspect='equal') 72 | # Plot the epipolar lines on img1 with points p2 from the right side 73 | # L1 = F * p2 74 | plot_epipolar_line(p1, p2, F, show_epipole) 75 | plt.subplot(1, 2, 2, aspect='equal') 76 | # Plot the epipolar lines on img2 with points p1 from the left side 77 | # L2 = F' * p1 78 | plot_epipolar_line(p2, p1, F.T, show_epipole) 79 | 80 | 81 | def plot_epipolar_line(p1, p2, F, show_epipole=False): 82 | """ Plot the epipole and epipolar line F*x=0 83 | in an image given the corresponding points. 84 | F is the fundamental matrix and p2 are the point in the other image. 85 | """ 86 | lines = np.dot(F, p2) 87 | pad = np.ptp(p1, 1) * 0.01 88 | mins = np.min(p1, 1) 89 | maxes = np.max(p1, 1) 90 | 91 | # epipolar line parameter and values 92 | xpts = np.linspace(mins[0] - pad[0], maxes[0] + pad[0], 100) 93 | for line in lines.T: 94 | ypts = np.asarray([(line[2] + line[0] * p) / (-line[1]) for p in xpts]) 95 | valid_idx = ((ypts >= mins[1] - pad[1]) & (ypts <= maxes[1] + pad[1])) 96 | plt.plot(xpts[valid_idx], ypts[valid_idx], linewidth=1) 97 | plt.plot(p1[0], p1[1], 'ro') 98 | 99 | if show_epipole: 100 | epipole = compute_epipole(F) 101 | plt.plot(epipole[0] / epipole[2], epipole[1] / epipole[2], 'r*') 102 | 103 | 104 | def skew(x): 105 | """ Create a skew symmetric matrix *A* from a 3d vector *x*. 106 | Property: np.cross(A, v) == np.dot(x, v) 107 | :param x: 3d vector 108 | :returns: 3 x 3 skew symmetric matrix from *x* 109 | """ 110 | return np.array([ 111 | [0, -x[2], x[1]], 112 | [x[2], 0, -x[0]], 113 | [-x[1], x[0], 0] 114 | ]) 115 | 116 | 117 | def compute_P(p2d, p3d): 118 | """ Compute camera matrix from pairs of 119 | 2D-3D correspondences in homog. coordinates. 120 | """ 121 | n = p2d.shape[1] 122 | if p3d.shape[1] != n: 123 | raise ValueError('Number of points do not match.') 124 | 125 | # create matrix for DLT solution 126 | M = np.zeros((3 * n, 12 + n)) 127 | for i in range(n): 128 | M[3 * i, 0:4] = p3d[:, i] 129 | M[3 * i + 1, 4:8] = p3d[:, i] 130 | M[3 * i + 2, 8:12] = p3d[:, i] 131 | M[3 * i:3 * i + 3, i + 12] = -p2d[:, i] 132 | 133 | U, S, V = np.linalg.svd(M) 134 | return V[-1, :12].reshape((3, 4)) 135 | 136 | 137 | def compute_P_from_fundamental(F): 138 | """ Compute the second camera matrix (assuming P1 = [I 0]) 139 | from a fundamental matrix. 140 | """ 141 | e = compute_epipole(F.T) # left epipole 142 | Te = skew(e) 143 | return np.vstack((np.dot(Te, F.T).T, e)).T 144 | 145 | 146 | def compute_P_from_essential(E): 147 | """ Compute the second camera matrix (assuming P1 = [I 0]) 148 | from an essential matrix. E = [t]R 149 | :returns: list of 4 possible camera matrices. 150 | """ 151 | U, S, V = np.linalg.svd(E) 152 | 153 | # Ensure rotation matrix are right-handed with positive determinant 154 | if np.linalg.det(np.dot(U, V)) < 0: 155 | V = -V 156 | 157 | # create 4 possible camera matrices (Hartley p 258) 158 | W = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) 159 | P2s = [np.vstack((np.dot(U, np.dot(W, V)).T, U[:, 2])).T, 160 | np.vstack((np.dot(U, np.dot(W, V)).T, -U[:, 2])).T, 161 | np.vstack((np.dot(U, np.dot(W.T, V)).T, U[:, 2])).T, 162 | np.vstack((np.dot(U, np.dot(W.T, V)).T, -U[:, 2])).T] 163 | 164 | return P2s 165 | 166 | 167 | def correspondence_matrix(p1, p2): 168 | p1x, p1y = p1[:2] 169 | p2x, p2y = p2[:2] 170 | 171 | return np.array([ 172 | p1x * p2x, p1x * p2y, p1x, 173 | p1y * p2x, p1y * p2y, p1y, 174 | p2x, p2y, np.ones(len(p1x)) 175 | ]).T 176 | 177 | return np.array([ 178 | p2x * p1x, p2x * p1y, p2x, 179 | p2y * p1x, p2y * p1y, p2y, 180 | p1x, p1y, np.ones(len(p1x)) 181 | ]).T 182 | 183 | 184 | def compute_image_to_image_matrix(x1, x2, compute_essential=False): 185 | """ Compute the fundamental or essential matrix from corresponding points 186 | (x1, x2 3*n arrays) using the 8 point algorithm. 187 | Each row in the A matrix below is constructed as 188 | [x'*x, x'*y, x', y'*x, y'*y, y', x, y, 1] 189 | """ 190 | A = correspondence_matrix(x1, x2) 191 | # compute linear least square solution 192 | U, S, V = np.linalg.svd(A) 193 | F = V[-1].reshape(3, 3) 194 | 195 | # constrain F. Make rank 2 by zeroing out last singular value 196 | U, S, V = np.linalg.svd(F) 197 | S[-1] = 0 198 | if compute_essential: 199 | S = [1, 1, 0] # Force rank 2 and equal eigenvalues 200 | F = np.dot(U, np.dot(np.diag(S), V)) 201 | 202 | return F 203 | 204 | 205 | def scale_and_translate_points(points): 206 | """ Scale and translate image points so that centroid of the points 207 | are at the origin and avg distance to the origin is equal to sqrt(2). 208 | :param points: array of homogenous point (3 x n) 209 | :returns: array of same input shape and its normalization matrix 210 | """ 211 | x = points[0] 212 | y = points[1] 213 | center = points.mean(axis=1) # mean of each row 214 | cx = x - center[0] # center the points 215 | cy = y - center[1] 216 | dist = np.sqrt(np.power(cx, 2) + np.power(cy, 2)) 217 | scale = np.sqrt(2) / dist.mean() 218 | norm3d = np.array([ 219 | [scale, 0, -scale * center[0]], 220 | [0, scale, -scale * center[1]], 221 | [0, 0, 1] 222 | ]) 223 | 224 | return np.dot(norm3d, points), norm3d 225 | 226 | 227 | def compute_normalized_image_to_image_matrix(p1, p2, compute_essential=False): 228 | """ Computes the fundamental or essential matrix from corresponding points 229 | using the normalized 8 point algorithm. 230 | :input p1, p2: corresponding points with shape 3 x n 231 | :returns: fundamental or essential matrix with shape 3 x 3 232 | """ 233 | n = p1.shape[1] 234 | if p2.shape[1] != n: 235 | raise ValueError('Number of points do not match.') 236 | 237 | # preprocess image coordinates 238 | p1n, T1 = scale_and_translate_points(p1) 239 | p2n, T2 = scale_and_translate_points(p2) 240 | 241 | # compute F or E with the coordinates 242 | F = compute_image_to_image_matrix(p1n, p2n, compute_essential) 243 | 244 | # reverse preprocessing of coordinates 245 | # We know that P1' E P2 = 0 246 | F = np.dot(T1.T, np.dot(F, T2)) 247 | 248 | return F / F[2, 2] 249 | 250 | 251 | def compute_fundamental_normalized(p1, p2): 252 | return compute_normalized_image_to_image_matrix(p1, p2) 253 | 254 | 255 | def compute_essential_normalized(p1, p2): 256 | return compute_normalized_image_to_image_matrix(p1, p2, compute_essential=True) 257 | -------------------------------------------------------------------------------- /testsets/3d_to_2d_projection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alyssaq/3Dreconstruction/3513c24e58fdb3d3b23250f407624e96515dacbc/testsets/3d_to_2d_projection.png -------------------------------------------------------------------------------- /testsets/dino_2d_points.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alyssaq/3Dreconstruction/3513c24e58fdb3d3b23250f407624e96515dacbc/testsets/dino_2d_points.png -------------------------------------------------------------------------------- /testsets/dino_3d_reconstructed.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alyssaq/3Dreconstruction/3513c24e58fdb3d3b23250f407624e96515dacbc/testsets/dino_3d_reconstructed.png -------------------------------------------------------------------------------- /testsets/house.p3d: -------------------------------------------------------------------------------- 1 | -1.4889 1.09408 -5.89588 2 | -1.13932 0.935522 -5.62824 3 | 1.09404 0.738447 -4.53106 4 | -1.26433 0.278026 -5.4722 5 | -1.55242 -0.132491 -5.84788 6 | -1.51218 -0.18739 -5.84725 7 | -1.85213 -0.532959 -5.65752 8 | 0.293132 -0.218916 -4.61308 9 | 0.895004 -1.2123 -4.02492 10 | -0.0146579 -1.00935 -4.60085 11 | 0.713014 -1.31943 -3.80594 12 | 0.0912043 1.51508 -5.06884 13 | 0.0993179 1.42352 -5.02601 14 | 0.684799 -0.0545285 -4.19282 15 | -1.78248 0.851829 -5.8902 16 | -0.872425 0.115185 -5.24536 17 | -1.15975 0.109602 -5.44714 18 | 1.36203 -1.38883 -3.64542 19 | 1.11979 -1.27727 -3.89339 20 | -2.0112 0.419228 -5.94718 21 | -1.44655 0.487362 -8.40095 22 | 0.907698 -1.47904 -3.29647 23 | 0.640547 -1.33443 -3.61227 24 | 0.465669 -1.32336 -3.65725 25 | 0.261172 -1.24743 -4.08401 26 | -0.435926 -1.02678 -4.59736 27 | -1.51714 -0.620722 -5.4707 28 | -1.57106 -0.607631 -5.50251 29 | -0.695753 -1.19816 -4.14679 30 | -2.24934 -0.212354 -6.392 31 | -1.10977 1.01642 -5.65255 32 | -1.8212 0.4482 -5.81623 33 | -1.33151 0.279304 -5.51438 34 | -0.901036 0.250231 -5.21143 35 | -0.894737 0.12486 -5.2635 36 | -1.5168 -0.0890313 -5.80468 37 | -1.21639 -0.106916 -5.64918 38 | 0.301939 -0.387427 -4.64264 39 | 0.995774 -0.373109 -4.17863 40 | 0.807874 -0.534481 -4.36206 41 | -0.541635 -0.661158 -5.33643 42 | 0.622488 -1.2804 -3.86391 43 | -1.41363 1.00318 -5.81977 44 | -1.90735 0.464245 -5.86977 45 | -2.01472 0.321393 -6.01722 46 | -1.73079 0.267305 -5.89559 47 | -1.83288 0.172635 -6.01072 48 | -1.17618 0.0367353 -5.58061 49 | 0.139957 -0.175823 -4.67159 50 | -1.22892 -0.0647634 -5.62415 51 | 0.856755 -0.548027 -4.35341 52 | -1.99991 -0.210606 -6.21358 53 | -2.14651 -0.206087 -6.31923 54 | -0.378751 -0.42568 -5.12839 55 | -1.44987 -0.328803 -5.85816 56 | -0.934762 -0.484919 -5.53544 57 | -1.43622 1.00903 -5.83723 58 | -1.94716 0.474285 -5.89868 59 | 0.374654 -0.327275 -4.58155 60 | 0.29363 -0.673017 -4.77454 61 | -2.05237 0.00894829 -6.16176 62 | -0.963473 -0.764631 -5.13498 63 | -1.70784 1.51558 -6.24952 64 | -0.0934386 1.07183 -5.24338 65 | -1.5184 1.02419 -5.86117 66 | 0.784481 0.669019 -4.64253 67 | -0.973353 0.715619 -5.41389 68 | -0.349621 0.538184 -5.02563 69 | -0.256725 0.519855 -4.97284 70 | -0.355471 0.506877 -5.00943 71 | 0.730735 0.400854 -4.4913 72 | -0.107047 0.436644 -4.87683 73 | -1.34843 0.450174 -5.44908 74 | 0.710369 0.369238 -4.48523 75 | -1.13715 0.392774 -5.33028 76 | -1.03787 0.3676 -5.26156 77 | -0.855013 0.321093 -5.15242 78 | -0.968972 0.307987 -5.23442 79 | -1.29388 0.322006 -5.45661 80 | -1.12805 0.284322 -5.36586 81 | -0.860506 0.27123 -5.16336 82 | -1.41394 0.301963 -5.6836 83 | -1.88808 0.309102 -6.01353 84 | -0.696153 0.213483 -5.07146 85 | -1.01517 0.219614 -5.31114 86 | -1.83786 0.218367 -5.97371 87 | -0.678168 0.17845 -5.07538 88 | -0.8028 0.167821 -5.17121 89 | -0.982449 0.160539 -5.3045 90 | -1.47143 0.15939 -5.6622 91 | -0.901327 0.150824 -5.25425 92 | -0.519041 0.129537 -4.98125 93 | -0.846964 0.131405 -5.22141 94 | -0.18162 0.0880369 -4.77 95 | -0.534578 0.0902787 -5.01679 96 | -1.84156 0.0904871 -6.0332 97 | -0.134641 0.0538861 -4.7527 98 | -2.07609 0.0485988 -6.173 99 | -1.81693 0.0416879 -6.05396 100 | -1.84609 -0.0356963 -6.09061 101 | 0.66142 -0.0429768 -4.20664 102 | -1.75641 -0.0581408 -6.00774 103 | -0.709101 -0.120018 -5.28929 104 | -0.162482 -0.132878 -4.84564 105 | -0.609391 -0.143894 -5.2233 106 | 0.656255 -0.177672 -4.32422 107 | -1.933 -0.26261 -6.17539 108 | -0.768112 -0.235698 -5.39252 109 | 0.720593 -0.197692 -4.297 110 | -0.122839 -0.228963 -4.92277 111 | -1.49671 -0.297433 -5.87896 112 | -0.611689 -0.274578 -5.28078 113 | -0.76922 -0.317066 -5.43249 114 | -1.0824 -0.328557 -5.59509 115 | -0.709163 -0.333492 -5.38923 116 | -0.340005 -0.366088 -5.06022 117 | -1.08421 -0.448952 -5.64459 118 | -1.19114 -0.459246 -5.74228 119 | -0.906182 -0.503089 -5.52609 120 | 1.04802 -0.385442 -4.15975 121 | 0.590791 -0.450094 -4.54747 122 | -0.124049 -0.509589 -5.04897 123 | -0.823579 -0.562243 -5.49821 124 | -0.371764 -0.544197 -5.16692 125 | 0.970681 -0.511482 -4.25465 126 | 0.102289 -0.619118 -4.86151 127 | -0.466207 -0.670903 -5.27266 128 | -1.81997 -0.686387 -5.27052 129 | 1.20136 -0.58213 -4.1682 130 | -0.202022 -0.705151 -5.11551 131 | 0.587868 -0.689978 -4.6481 132 | -0.00789702 -0.756967 -5.0348 133 | 0.051238 -0.767511 -4.95132 134 | -1.0254 -0.788379 -4.9759 135 | 0.422086 -0.87135 -4.74021 136 | 0.578741 -0.914346 -4.6959 137 | 1.04294 -1.01156 -4.38153 138 | 1.19393 -1.0494 -4.34728 139 | 0.00364241 1.04533 -5.18138 140 | -0.0279571 1.04139 -5.21043 141 | -1.20092 1.11453 -6.23682 142 | -1.20996 0.954149 -5.64668 143 | -1.24627 0.967288 -5.70915 144 | -1.36532 0.993286 -5.78663 145 | -1.7869 0.892131 -5.91728 146 | -1.83127 0.866705 -5.91983 147 | -0.275073 0.51872 -4.98607 148 | -0.214219 0.498915 -4.94956 149 | -0.0954383 0.468374 -4.88391 150 | -0.0773017 0.453817 -4.84867 151 | -2.17436 0.690307 -5.88287 152 | -1.99237 0.565955 -5.88726 153 | -1.61779 0.476778 -5.63681 154 | -1.72513 0.459437 -5.72906 155 | -1.84466 0.483541 -5.80808 156 | -1.78834 0.406909 -5.79314 157 | -1.08737 0.279444 -5.3274 158 | -0.473517 0.0991728 -4.96623 159 | -1.79094 0.284059 -5.93656 160 | -2.04219 0.278749 -6.03848 161 | -1.73731 0.193774 -5.91653 162 | 0.739121 -0.171711 -4.26235 163 | -0.776692 0.0229338 -5.26751 164 | -0.699056 0.00615065 -5.24376 165 | -1.7942 0.163244 -5.98913 166 | -0.596947 -0.0205201 -5.18325 167 | -1.73791 0.149606 -5.9367 168 | -2.05884 0.18762 -6.10274 169 | -1.22281 0.0505528 -5.58694 170 | -0.77691 -0.0584544 -5.3022 171 | -0.686574 -0.0863166 -5.26138 172 | -0.703099 -0.0792604 -5.27889 173 | -1.7433 0.020134 -5.99412 174 | -2.19803 0.0736056 -6.2547 175 | -1.54311 -0.0319296 -5.79344 176 | 0.00810661 -0.262365 -4.87547 177 | -1.49097 -0.058717 -5.76931 178 | -1.29991 -0.0881831 -5.70624 179 | -1.9049 -0.0230636 -6.14596 180 | -0.777719 -0.188795 -5.35987 181 | -0.399282 -0.244788 -5.05538 182 | -1.10133 -0.145989 -5.52466 183 | -0.690437 -0.215169 -5.31578 184 | -1.65623 -0.0983917 -5.90973 185 | -1.33941 -0.196744 -5.78919 186 | -1.29869 -0.209329 -5.76723 187 | -1.22848 -0.231173 -5.7145 188 | -1.18684 -0.239781 -5.69484 189 | -1.51141 -0.217636 -5.8532 190 | -0.603187 -0.362029 -5.3309 191 | -1.52362 -0.242512 -5.8536 192 | -1.55953 -0.234137 -5.88831 193 | -1.05999 -0.314582 -5.55547 194 | 0.765162 -0.601488 -4.41819 195 | 0.601586 -0.593092 -4.61571 196 | -0.0641737 -0.525753 -4.97575 197 | -1.29786 -0.422557 -5.81658 198 | -0.60721 -0.648838 -5.37521 199 | -0.750382 -0.816376 -5.00458 200 | -2.31465 -0.64623 -5.32283 201 | -2.1944 -0.831642 -4.75203 202 | 0.606958 -1.34131 -3.62678 203 | 0.688131 -1.3611 -3.56898 204 | -0.0700479 1.04944 -5.22596 205 | -0.220694 0.94018 -5.24435 206 | -0.162803 0.928199 -5.20306 207 | -1.782 0.848906 -5.86341 208 | -1.03277 0.733851 -5.45519 209 | -0.396476 0.51047 -5.02417 210 | -0.306296 0.490107 -4.94411 211 | -1.92934 0.540768 -5.84992 212 | 0.677332 0.389148 -4.49342 213 | -1.88977 0.495867 -5.84168 214 | -1.81763 0.412949 -5.84383 215 | -1.94459 0.346294 -5.94401 216 | -0.659529 0.271776 -5.0219 217 | -0.453194 0.216776 -4.91302 218 | -0.843304 0.22714 -5.18096 219 | -0.559412 0.205198 -4.985 220 | -0.565647 0.169368 -4.99986 221 | -0.621489 0.161143 -5.04092 222 | -0.489555 0.143607 -4.96661 223 | -1.89028 0.186204 -6.0767 224 | -0.713782 0.147881 -5.1195 225 | -1.06481 0.135266 -5.37291 226 | -0.206963 0.113835 -4.77542 227 | -0.492891 0.0431026 -4.99789 228 | -1.89656 0.055743 -6.13875 229 | -0.428094 -0.00627116 -4.97463 230 | -0.506942 -0.0404667 -5.03932 231 | -1.82337 -0.0451989 -6.08695 232 | -1.5155 -0.0499157 -5.8039 233 | 0.0759409 -0.0746702 -4.66433 234 | -0.301373 -0.0817895 -4.9089 235 | 1.68611 -0.085481 -4.68558 236 | 1.71392 -0.0907234 -4.67348 237 | -0.76285 -0.110473 -5.35792 238 | 0.433497 -0.102911 -4.43634 239 | -0.684999 -0.123966 -5.28228 240 | 0.470934 -0.109233 -4.41295 241 | 0.175052 -0.109502 -4.6078 242 | -2.0634 -0.147086 -6.23291 243 | -0.553855 -0.182669 -5.13722 244 | -1.04693 -0.209063 -5.51184 245 | -0.709584 -0.206343 -5.32522 246 | 0.472308 -0.180244 -4.44379 247 | -1.82813 -0.257041 -6.09741 248 | -0.686227 -0.253861 -5.34274 249 | -0.517626 -0.25578 -5.13349 250 | 0.702627 -0.222608 -4.31827 251 | 0.498212 -0.225357 -4.43716 252 | -0.041273 -0.251659 -4.887 253 | -0.492299 -0.269038 -5.13135 254 | -1.04098 -0.314589 -5.49288 255 | 0.616412 -0.262844 -4.38821 256 | -0.502671 -0.325419 -5.16374 257 | 0.532533 -0.284755 -4.44455 258 | 0.826278 -0.308904 -4.2757 259 | -0.420078 -0.386457 -5.11876 260 | 0.575084 -0.343087 -4.44033 261 | -0.456475 -0.399414 -5.16507 262 | -2.72274 -0.462531 -5.83964 263 | -0.0433415 -0.409674 -4.95197 264 | -0.499435 -0.448876 -5.22302 265 | 0.512111 -0.430641 -4.5827 266 | -0.336357 -0.491596 -5.11396 267 | -0.781909 -0.572354 -5.46823 268 | 1.12074 -0.447652 -4.13227 269 | 1.09425 -0.471293 -4.16962 270 | 0.582662 -0.544483 -4.57299 271 | 0.470182 -0.560764 -4.65837 272 | 0.5096 -0.568844 -4.63892 273 | 0.53029 -0.571079 -4.62854 274 | -1.66222 -0.677357 -5.35116 275 | 1.06129 -0.545695 -4.21105 276 | 0.935904 -0.59571 -4.30769 277 | 0.472819 -0.65771 -4.70493 278 | 0.509356 -0.669476 -4.67955 279 | 0.807204 -0.64916 -4.39968 280 | 0.263117 -0.736599 -4.79924 281 | 0.794959 -0.843612 -4.48116 282 | 0.74305 -0.880972 -4.53255 283 | 0.661187 -0.922627 -4.59399 284 | 0.854048 -0.906692 -4.4735 285 | -0.448413 -0.971484 -4.66001 286 | 0.843535 -0.951615 -4.47972 287 | -1.72344 -1.0622 -4.17882 288 | 0.783439 -1.28653 -4.18809 289 | 1.48565 -1.31869 -3.69337 290 | -0.238594 -1.31915 -3.71389 291 | -1.69461 1.94127 -6.17648 292 | -1.73882 1.57108 -6.35837 293 | -1.86623 1.40272 -6.27014 294 | -0.120754 1.0817 -5.26546 295 | 0.187054 1.09301 -5.3888 296 | -1.42487 1.06355 -5.81146 297 | -1.33294 1.04503 -5.7576 298 | -1.33353 0.982095 -5.71715 299 | -1.27135 1.05276 -6.18895 300 | 0.765699 0.679306 -4.68465 301 | -2.02932 0.582479 -5.91012 302 | -0.241155 0.470839 -4.93532 303 | -1.62621 0.362431 -5.70411 304 | -0.535161 0.238795 -4.95884 305 | -0.901849 0.216543 -5.25142 306 | -1.19931 0.192529 -5.43798 307 | -0.348373 0.164322 -4.83784 308 | -1.89446 0.209454 -6.07461 309 | -1.81477 0.16989 -6.0032 310 | -0.0590296 0.115569 -4.68237 311 | -1.35924 0.108886 -5.66224 312 | -1.30213 0.0708663 -5.60377 313 | -1.2343 0.0687969 -5.62186 314 | -0.329904 0.0500412 -4.88728 315 | -0.823365 0.0539447 -5.26158 316 | -0.192841 0.0413577 -4.82408 317 | 0.138497 0.0301242 -4.57745 318 | 0.368606 0.018301 -4.43178 319 | -1.93986 -0.0556024 -6.22009 320 | 0.15196 -0.0772676 -4.61324 321 | 0.048522 -0.0845337 -4.66757 322 | 0.359847 -0.0934253 -4.46453 323 | 1.25365 -0.117284 -4.53344 324 | 1.29455 -0.124432 -4.52964 325 | -2.0811 -0.213182 -6.30606 326 | -0.523926 -0.174335 -5.10776 327 | 0.545091 -0.159403 -4.38482 328 | -1.36273 -0.222965 -5.81806 329 | 0.367906 -0.17959 -4.49979 330 | -0.383553 -0.215448 -5.03578 331 | -0.613718 -0.228049 -5.23255 332 | 0.560362 -0.206416 -4.39566 333 | -0.950808 -0.251024 -5.46695 334 | 0.665094 -0.212157 -4.32467 335 | -0.484983 -0.243661 -5.13009 336 | -0.61055 -0.251542 -5.25324 337 | -1.4627 -0.286136 -5.82212 338 | -1.01698 -0.284217 -5.52856 339 | 0.643171 -0.227576 -4.34268 340 | 0.560085 -0.238736 -4.40478 341 | 0.692139 -0.2485 -4.32216 342 | 0.633858 -0.245262 -4.34491 343 | 0.751562 -0.283754 -4.29868 344 | -0.132137 -0.344427 -4.93918 345 | 0.0635505 -0.333806 -4.7649 346 | 0.497766 -0.323036 -4.47407 347 | -0.0192147 -0.37215 -4.86816 348 | 0.672534 -0.337095 -4.3661 349 | 0.69114 -0.354332 -4.36398 350 | 0.865793 -0.35593 -4.25701 351 | -0.352693 -0.434241 -5.12174 352 | -2.10602 -0.478588 -5.65437 353 | 0.473451 -0.423488 -4.62003 354 | 0.53011 -0.433773 -4.56966 355 | -1.08309 -0.541118 -5.67844 356 | 0.299879 -0.468356 -4.66944 357 | -0.482865 -0.547972 -5.25318 358 | 0.0590956 -0.511662 -4.84765 359 | 1.29784 -0.441123 -4.04224 360 | 0.845562 -0.470089 -4.30162 361 | 1.16664 -0.464452 -4.11048 362 | 0.508372 -0.52629 -4.61285 363 | 0.548977 -0.534553 -4.60247 364 | 0.310151 -0.542023 -4.68749 365 | -1.55607 -0.644092 -5.42739 366 | -1.35665 -0.639883 -5.29105 367 | 2.67709 -0.693154 -5.48598 368 | 1.29632 -0.518868 -4.06158 369 | 0.264866 -0.647915 -4.75916 370 | 2.98861 -0.764589 -5.39978 371 | 2.77616 -0.747254 -5.3618 372 | 0.131121 -0.703582 -4.86995 373 | 0.864217 -0.658043 -4.36409 374 | -1.04344 -0.781559 -5.08943 375 | 0.970638 -0.662214 -4.29588 376 | 2.94821 -0.840267 -5.32485 377 | 0.111726 -0.768497 -4.89852 378 | 0.97772 -0.681867 -4.28558 379 | 0.865443 -0.695089 -4.36577 380 | -1.93529 -0.799023 -4.96062 381 | -0.67864 -0.833532 -4.99274 382 | 1.24842 -0.705212 -4.1346 383 | -0.936304 -0.850723 -4.88685 384 | -1.82526 -0.846647 -4.81743 385 | 1.28139 -0.778581 -4.1511 386 | 0.869148 -0.851413 -4.41671 387 | 0.649693 -0.896196 -4.58786 388 | 1.27439 -0.82733 -4.16302 389 | -0.22888 -0.941547 -4.69871 390 | 0.6298 -0.931687 -4.60748 391 | 2.59905 -1.01053 -4.85641 392 | -1.83521 -0.958673 -4.50427 393 | -2.32555 -0.942409 -4.40895 394 | 1.27618 -0.906473 -4.20583 395 | 1.24848 -1.04055 -4.2512 396 | -1.68825 -1.07439 -4.21345 397 | -1.91194 -1.13173 -4.06895 398 | 0.471362 -1.18423 -4.1538 399 | -0.352258 -1.17728 -4.06384 400 | -1.66697 -1.17693 -3.91964 401 | -1.52969 -1.18717 -3.87652 402 | 0.907712 -1.23651 -3.93333 403 | 1.09055 -1.2559 -3.9579 404 | 1.12676 -1.2634 -3.9171 405 | 0.432554 -1.30953 -3.64426 406 | 1.41958 -1.35765 -3.59914 407 | -1.74681 1.88904 -6.0267 408 | -1.67951 1.49542 -6.17723 409 | -1.78956 1.42515 -6.18733 410 | -1.82351 1.38767 -6.18115 411 | -1.26527 1.03382 -5.7289 412 | -1.21572 1.02175 -5.70409 413 | -0.0335646 0.485748 -4.81967 414 | -0.0514981 0.477153 -4.841 415 | -0.267947 0.47623 -4.93958 416 | -2.07622 0.622135 -5.93011 417 | -2.34119 0.593059 -5.39001 418 | -2.05654 0.585878 -5.91896 419 | -1.36449 0.372214 -5.47542 420 | -1.25809 0.345176 -5.43092 421 | -1.90723 0.43652 -5.86869 422 | -1.22999 0.31965 -5.4319 423 | -0.669652 0.22524 -5.05457 424 | -1.32767 0.324911 -5.4897 425 | 0.0214208 0.0939148 -4.64067 426 | 0.227311 0.045366 -4.52958 427 | -1.04113 0.246269 -5.29866 428 | -1.6558 0.346539 -5.72755 429 | -0.642134 0.15992 -5.65169 430 | 0.745308 -0.0851398 -4.23943 431 | -0.357156 0.0848803 -4.88041 432 | -1.02271 0.185883 -5.33922 433 | -1.63073 0.237005 -5.74269 434 | -0.474286 0.0431499 -4.96997 435 | -1.89111 0.234888 -6.04882 436 | -1.6321 0.196238 -5.76466 437 | -2.15484 0.25449 -6.13854 438 | -1.62217 0.16727 -5.77772 439 | -1.39024 0.117745 -5.63376 440 | -2.02377 0.204051 -6.03275 441 | -0.68227 0.0024801 -5.22635 442 | -1.89647 0.105651 -6.09811 443 | -1.6207 0.0495203 -5.83069 444 | -1.74599 0.0660705 -5.95166 445 | -0.601493 -0.103691 -5.21065 446 | -0.487531 -0.127952 -5.06779 447 | -0.514122 -0.122938 -5.078 448 | 0.3062 -0.23947 -4.58934 449 | -1.58242 -0.0165492 -5.83381 450 | -1.19101 -0.0761754 -5.5976 451 | -0.164698 -0.220861 -4.94476 452 | -0.426652 -0.191678 -5.05548 453 | -1.33047 -0.0824171 -5.75896 454 | 0.296167 -0.301901 -4.59784 455 | -0.516105 -0.196508 -5.11017 456 | 0.984934 -0.423499 -4.20995 457 | -1.60888 -0.0875583 -5.86319 458 | -0.555775 -0.251846 -5.15206 459 | -0.165027 -0.335628 -4.99071 460 | -0.481854 -0.32989 -5.15653 461 | -0.167051 -0.37816 -5.00593 462 | 1.29378 -0.57613 -4.07675 463 | -1.99134 -0.165408 -6.16899 464 | -0.690646 -0.34121 -5.37517 465 | -1.13729 -0.275267 -5.58214 466 | -2.03654 -0.187437 -6.23222 467 | -2.06473 -0.181939 -6.23488 468 | -0.486591 -0.399698 -5.17243 469 | -0.407458 -0.425201 -5.13306 470 | -1.98251 -0.249846 -6.2124 471 | -1.87332 -0.277423 -6.16527 472 | -0.166777 -0.497571 -5.06508 473 | -2.79063 -0.179701 -6.69678 474 | -1.48808 -0.345006 -5.89275 475 | 0.000651974 -0.543371 -4.98733 476 | -1.53005 -0.399527 -5.934 477 | 0.128408 -0.616679 -4.84605 478 | 0.548925 -0.676649 -4.65937 479 | -0.560327 -0.535191 -5.29564 480 | 0.290367 -0.643867 -4.74779 481 | -0.589002 -0.585487 -5.33475 482 | -2.32384 -0.423845 -5.9931 483 | -0.279161 -0.931826 -4.71095 484 | -1.1629 -0.812587 -5.00566 485 | -1.86856 -0.784519 -5.02474 486 | -1.86441 -0.99152 -4.43073 487 | -1.65594 -1.15311 -4.02434 488 | -1.04817 -1.29536 -3.76944 489 | 0.0818358 1.06825 -5.19175 490 | -1.17415 1.02145 -5.70599 491 | -1.29361 1.05076 -5.79446 492 | -1.38326 1.06779 -5.85433 493 | -1.16882 1.06866 -6.11305 494 | -1.2109 0.989437 -5.71635 495 | -1.36771 1.03942 -5.8241 496 | -1.38423 1.04607 -5.83382 497 | -1.45556 1.034 -5.86553 498 | -0.0493274 0.466859 -5.14743 499 | 0.711881 -0.0769301 -4.23703 500 | -1.47537 0.478962 -5.51856 501 | -0.776256 0.24979 -5.10533 502 | -0.924313 0.297125 -5.20361 503 | -1.80323 0.54285 -5.77194 504 | 1.23791 -0.428919 -4.00298 505 | -0.680463 0.135273 -5.10015 506 | 0.112175 -0.205255 -4.70722 507 | -0.0376326 -0.170874 -4.74015 508 | -1.1615 0.17561 -5.42624 509 | 0.577039 -0.367793 -4.43814 510 | -1.20476 0.171121 -5.4586 511 | -0.948964 0.0394555 -5.33296 512 | -1.82231 0.293855 -5.96134 513 | 0.42219 -0.404174 -4.55792 514 | -1.04174 0.00535952 -5.41988 515 | -1.27885 0.0788248 -5.62033 516 | -0.249464 -0.24582 -4.99094 517 | -1.79346 0.207433 -5.97278 518 | -2.07615 0.289917 -6.07812 519 | 1.21319 -0.754215 -4.14248 520 | 0.980509 -0.681287 -4.30323 521 | -0.071571 -0.359193 -4.89428 522 | -1.82678 0.192537 -5.99997 523 | -1.96745 0.285728 -7.93217 524 | -0.0919694 -0.376275 -5.00971 525 | 1.27078 -0.830391 -4.17814 526 | -1.58688 0.0396765 -5.79619 527 | -0.701967 -0.250991 -5.35754 528 | -2.23587 0.261013 -8.15549 529 | -1.0824 -0.14732 -5.50509 530 | -1.17581 -0.119697 -5.65128 531 | -1.79591 0.0774018 -6.03239 532 | -2.10252 0.204072 -7.64819 533 | 0.529353 -0.678701 -4.67227 534 | -1.83457 0.0444426 -6.06518 535 | -0.677424 -0.340259 -5.35479 536 | -0.550335 -0.392837 -5.22478 537 | -0.993449 -0.263038 -5.48196 538 | 0.967211 -0.941952 -4.41213 539 | -0.943241 -0.345224 -5.4874 540 | -1.03042 -0.319469 -5.55173 541 | -0.291544 -0.60362 -5.13536 542 | -1.45996 -0.225178 -5.79928 543 | 1.28416 -1.12143 -4.301 544 | 0.607204 -0.909528 -4.59398 545 | -0.0962132 -0.724298 -5.05511 546 | -1.02228 -0.42903 -5.578 547 | -1.82418 -0.207005 -6.08464 548 | -1.56148 -0.359191 -5.94473 549 | -1.39308 -0.426785 -5.83883 550 | -1.05493 -0.548029 -5.74974 551 | -2.12049 -0.219742 -6.30901 552 | 1.06934 -1.25417 -3.93982 553 | 0.84483 -1.21472 -4.01468 554 | -0.493271 -0.880403 -4.86675 555 | -0.599726 -0.844768 -4.89177 556 | -0.699044 -0.817151 -4.94594 557 | -1.36817 -0.642548 -5.31987 558 | -2.71944 -0.222508 -6.29001 559 | -1.92063 -0.513771 -5.65656 560 | -2.11667 -0.47512 -5.86597 561 | -1.30935 -0.743376 -5.09459 562 | -1.65879 -0.693811 -5.18239 563 | -1.75035 -0.745808 -5.05162 564 | 0.295288 -1.39711 -3.39497 565 | -1.89445 -0.791182 -4.93087 566 | -1.85286 -0.864717 -4.67569 567 | 0.108042 1.58902 -5.29949 568 | 0.0846304 1.50567 -5.39049 569 | -0.0305278 1.08408 -5.22924 570 | -0.479125 1.47827 -6.62029 571 | -0.169793 0.929213 -5.20593 572 | -1.21355 1.02946 -5.71839 573 | -1.26034 1.02653 -5.74 574 | -1.35716 1.06246 -5.80836 575 | -1.16938 0.949078 -5.66458 576 | -1.21066 0.956529 -5.6843 577 | 0.766974 -0.218806 -4.26318 578 | 0.976661 -0.468739 -4.21972 579 | 1.03768 -0.650152 -4.31278 580 | -1.90203 0.502688 -5.85488 581 | -1.1164 0.0898183 -5.43261 582 | 1.48051 -1.32864 -3.70481 583 | -1.02672 0.485678 -8.6377 584 | 0.632807 -0.952633 -4.61831 585 | -1.38496 0.543077 -8.65592 586 | 0.905791 -1.26458 -3.86294 587 | 0.699666 -1.24699 -4.05826 588 | 0.491108 -1.18761 -4.14844 589 | -1.41951 -0.0710077 -5.75383 590 | 0.533952 -1.37235 -3.59342 591 | -1.45244 -0.29464 -5.85334 592 | 0.222291 -1.42549 -3.44142 593 | -1.16672 -0.478343 -5.73857 594 | -1.97703 -0.0578408 -6.09876 595 | -0.8468 -0.796995 -5.06489 596 | -2.15924 -0.0786145 -6.27202 597 | -1.83224 -0.331454 -6.12819 598 | -1.87125 -0.315637 -6.14457 599 | -1.50072 -0.651735 -5.37738 600 | -1.21098 -1.13535 -4.24198 601 | -1.75409 1.91535 -6.08145 602 | -0.0448276 0.413915 -4.83752 603 | -1.31692 1.05468 -5.77461 604 | -1.83288 1.38915 -6.46483 605 | -1.83984 1.26383 -6.54124 606 | -1.94195 1.18735 -6.39892 607 | -1.87303 1.15404 -6.5487 608 | -1.78054 1.1747 -6.73336 609 | -1.95694 1.10876 -6.34374 610 | -1.74714 1.16052 -6.83499 611 | -1.69935 1.16129 -6.90978 612 | -1.96657 1.07292 -6.38881 613 | -0.807467 0.223671 -5.19275 614 | -0.577749 0.0507105 -5.06361 615 | -1.62668 1.06684 -7.17364 616 | -1.74877 1.0392 -6.92958 617 | -1.82197 1.02754 -6.78425 618 | -2.00133 0.978182 -6.33708 619 | -1.93704 0.9802 -6.46091 620 | -2.00794 0.951551 -6.32733 621 | -1.83706 0.960228 -6.72735 622 | -1.98219 0.9372 -6.43248 623 | -1.61931 0.972856 -7.20895 624 | -1.78197 0.917079 -6.87078 625 | 1.34971 -1.65592 -2.99528 626 | -1.72184 0.899297 -7.0716 627 | -0.58441 0.752536 -9.16479 628 | 0.637056 -1.39024 -3.56794 629 | -1.99998 0.725242 -6.48003 630 | 0.482782 -1.40303 -3.52009 631 | -2.0631 0.642986 -6.38253 632 | -2.03192 0.622561 -6.50092 633 | -0.943116 0.553953 -8.67556 634 | -0.365715 -0.997503 -3.6236 635 | 0.251788 -1.21678 -4.1343 636 | 0.192266 -1.25854 -3.92033 637 | 0.118726 -1.18279 -4.24509 638 | -1.67183 0.538575 -7.33605 639 | -2.15941 0.522553 -6.19912 640 | -2.09794 0.520692 -6.36079 641 | -2.11761 0.477105 -6.30147 642 | -0.351157 -0.998134 -4.65665 643 | -0.514414 -0.874756 -4.86667 644 | -1.9415 0.41071 -6.79731 645 | -2.0528 0.389302 -6.56835 646 | -0.297488 -1.24135 -4.05292 647 | -1.57436 0.434699 -8.35018 648 | -1.82955 0.293648 -7.14007 649 | -2.22528 0.333331 -6.18442 650 | -1.117 -0.513164 -5.67134 651 | -1.82788 0.436493 -8.14193 652 | -2.021 0.2471 -6.67895 653 | -1.05374 -0.729644 -5.17397 654 | -2.14729 0.220881 -6.42846 655 | -1.02549 -0.877404 -4.91293 656 | -2.13301 0.163952 -6.47553 657 | -2.1498 0.14109 -6.4391 658 | -1.35132 -0.655996 -5.36393 659 | -2.02666 -0.0863327 -6.15864 660 | -2.06497 0.220175 -7.69643 661 | -2.2175 0.0754273 -6.3491 662 | -0.884729 -1.36994 -3.69256 663 | -1.3045 -0.900386 -4.82491 664 | -1.52009 -0.648461 -5.44275 665 | -2.2376 0.282442 -7.91382 666 | -1.26875 -1.21591 -4.03855 667 | -1.67429 -0.795422 -5.11857 668 | -2.35692 0.00998355 -7.06962 669 | -1.58633 -1.16857 -4.0561 670 | -2.49859 -0.117172 -6.76897 671 | -2.51002 -0.144305 -6.7266 672 | -1.59536 -1.37243 -3.78376 673 | -------------------------------------------------------------------------------- /testsets/house1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alyssaq/3Dreconstruction/3513c24e58fdb3d3b23250f407624e96515dacbc/testsets/house1.jpg -------------------------------------------------------------------------------- /testsets/house_3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alyssaq/3Dreconstruction/3513c24e58fdb3d3b23250f407624e96515dacbc/testsets/house_3d.png -------------------------------------------------------------------------------- /transformers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | """ 4 | Helper functions to create various matrices 5 | """ 6 | 7 | def rotation_3d_from_angles(x_angle, y_angle=0, z_angle=0): 8 | """ Creates a 3D rotation matrix given angles in degrees. 9 | Positive angles rotates anti-clockwise. 10 | :params x_angle, y_angle, z_angle: x, y, z angles between 0 to 360 11 | :returns: 3x3 rotation matrix """ 12 | ax = np.deg2rad(x_angle) 13 | ay = np.deg2rad(y_angle) 14 | az = np.deg2rad(z_angle) 15 | 16 | # Rotation matrix around x-axis 17 | rx = np.array([ 18 | [1, 0, 0], 19 | [0, np.cos(ax), -np.sin(ax)], 20 | [0, np.sin(ax), np.cos(ax)] 21 | ]) 22 | # Rotation matrix around y-axis 23 | ry = np.array([ 24 | [np.cos(ay), 0, np.sin(ay)], 25 | [0, 1, 0], 26 | [-np.sin(ay), 0, np.cos(ay)] 27 | ]) 28 | # Rotation matrix around z-axis 29 | rz = np.array([ 30 | [np.cos(az), -np.sin(az), 0], 31 | [np.sin(az), np.cos(az), 0], 32 | [0, 0, 1] 33 | ]) 34 | 35 | return np.dot(np.dot(rx, ry), rz) 36 | --------------------------------------------------------------------------------