├── requirements.txt ├── data └── bench │ ├── left.png │ ├── right.png │ ├── depth_map.png │ ├── left_points.npy │ └── right_points.npy ├── README.md ├── .gitignore ├── stereo_utils.py └── utils.py /requirements.txt: -------------------------------------------------------------------------------- 1 | pytransform3d 2 | ipympl -------------------------------------------------------------------------------- /data/bench/left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wingedrasengan927/Stereo-Geometry/HEAD/data/bench/left.png -------------------------------------------------------------------------------- /data/bench/right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wingedrasengan927/Stereo-Geometry/HEAD/data/bench/right.png -------------------------------------------------------------------------------- /data/bench/depth_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wingedrasengan927/Stereo-Geometry/HEAD/data/bench/depth_map.png -------------------------------------------------------------------------------- /data/bench/left_points.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wingedrasengan927/Stereo-Geometry/HEAD/data/bench/left_points.npy -------------------------------------------------------------------------------- /data/bench/right_points.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wingedrasengan927/Stereo-Geometry/HEAD/data/bench/right_points.npy -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | A comprehensive tutorial on Stereo Geometry and Stereo Rectification with Examples. 2 | ## Setting up 3 | Assuming you've anaconda installed, create a virtual environment and install dependencies. 4 | 5 | ### Create Virtual Environment 6 | ``` 7 | conda create -n stereo-geometry python=3.6 anaconda 8 | conda activate stereo-geometry 9 | ``` 10 | ### Clone and Install dependencies 11 | ``` 12 | git clone https://github.com/wingedrasengan927/Stereo-Geometry.git 13 | cd Stereo-Geometry 14 | pip install -r requirements.txt 15 | ``` 16 | There are two main libraries we'll be using: 17 |
[**pytransform3d**](https://github.com/rock-learning/pytransform3d): This library has great functions for visualizations and transformations in the 3D space. 18 |
[**ipympl**](https://github.com/matplotlib/ipympl): It makes the matplotlib plot interactive allowing us to perform pan, zoom, and rotation in real time within the notebook which is really helpful when working with 3D plots. 19 | 20 | **Note:** If you're using Jupyter Lab, please install the Jupyter Lab extension of ipympl from [here](https://github.com/matplotlib/ipympl) 21 | 22 | ipympl can be accessed in the notebook by including the magic command `%matplotlib widget` 23 | 24 | ### Article 25 | The code follows [**this article**](https://medium.com/p/7f368b09924a) 26 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | #.idea/ -------------------------------------------------------------------------------- /stereo_utils.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | 5 | 6 | ### ESSENTIAL MATRIX ### 7 | 8 | def get_cross_product_matrix(vector): 9 | ''' 10 | The cross product of two vectors can be represented as a matrix multiplication. 11 | a x b = [a']b, 12 | where 13 | a = [a1, a2, a3] and 14 | a' = [[0, -a3, a2], 15 | [a3, 0, -a1], 16 | [-a2, a1, 0]] 17 | ''' 18 | A = np.zeros((3, 3)) 19 | a1, a2, a3 = vector 20 | A[0][1] = -a3 21 | A[0][2] = a2 22 | A[1][0] = a3 23 | A[1][2] = -a1 24 | A[2][0] = -a2 25 | A[2][1] = a1 26 | 27 | return A 28 | 29 | def to_hg_coords(points): 30 | ''' 31 | Convert the points from euclidean coordinates to homogeneous coordinates 32 | ''' 33 | points = np.concatenate((points, np.ones((1, points.shape[1]))), axis=0) 34 | return points 35 | 36 | def to_eucld_coords(points_hg): 37 | ''' 38 | Convert the points from homogeneous coordinates to euclidean coordinates 39 | ''' 40 | z = points_hg[-1,:] 41 | points = points_hg[:2,:]/z 42 | return points 43 | 44 | def is_vectors_close(v1, v2): 45 | ''' 46 | check if two vectors are close to each other 47 | ''' 48 | v1 = v1.reshape(-1) 49 | v2 = v2.reshape(-1) 50 | assert len(v1) == len(v2) 51 | assert np.isclose(v1, v2).sum() == len(v1) 52 | 53 | def plot_line(coeffs, xlim): 54 | ''' 55 | Given the coefficients a, b, c of the ax + by + c = 0, 56 | plot the line within the given x limits. 57 | ax + by + c = 0 => y = (-ax - c) / b 58 | ''' 59 | a, b, c = coeffs 60 | x = np.linspace(xlim[0], xlim[1], 100) 61 | y = (a * x + c) / -b 62 | return x, y 63 | 64 | ### FUNDAMENTAL MATRIX ### 65 | 66 | def show_matching_result(img1, img2, img1_pts, img2_pts): 67 | ''' 68 | plot the images and their corresponding matching points 69 | ''' 70 | fig = plt.figure(figsize=(8, 8)) 71 | plt.imshow(np.hstack((img1, img2)), cmap="gray") 72 | for p1, p2 in zip(img1_pts, img2_pts): 73 | plt.scatter(p1[0], p1[1], s=35, edgecolors='r', facecolors='none') 74 | plt.scatter(p2[0] + img1.shape[1], p2[1], s=35, edgecolors='r', facecolors='none') 75 | plt.plot([p1[0], p2[0] + img1.shape[1]], [p1[1], p2[1]]) 76 | plt.show() 77 | 78 | def compute_fundamental_matrix_normalized(points1, points2): 79 | ''' 80 | Normalize points by calculating the centroid, subtracting 81 | it from the points and scaling the points such that the distance 82 | from the origin is sqrt(2) 83 | 84 | Parameters 85 | ------------ 86 | points1, points2 - array with shape [n, 3] 87 | corresponding points in images represented as 88 | homogeneous coordinates 89 | ''' 90 | # validate points 91 | assert points1.shape[0] == points2.shape[0], "no. of points don't match" 92 | 93 | # compute centroid of points 94 | c1 = np.mean(points1, axis=0) 95 | c2 = np.mean(points2, axis=0) 96 | 97 | # compute the scaling factor 98 | s1 = np.sqrt(2 / np.mean(np.sum((points1 - c1) ** 2, axis=1))) 99 | s2 = np.sqrt(2 / np.mean(np.sum((points2 - c2) ** 2, axis=1))) 100 | 101 | # compute the normalization matrix for both the points 102 | T1 = np.array([ 103 | [s1, 0, -s1 * c1[0]], 104 | [0, s1, -s1 * c1[1]], 105 | [0, 0 ,1] 106 | ]) 107 | T2 = np.array([ 108 | [s2, 0, -s2 * c2[0]], 109 | [0, s2, -s2 * c2[1]], 110 | [0, 0, 1] 111 | ]) 112 | 113 | # normalize the points 114 | points1_n = T1 @ points1.T 115 | points2_n = T2 @ points2.T 116 | 117 | # compute the normalized fundamental matrix 118 | F_n = compute_fundamental_matrix(points1_n.T, points2_n.T) 119 | 120 | # de-normalize the fundamental 121 | return T2.T @ F_n @ T1 122 | 123 | def compute_fundamental_matrix(points1, points2): 124 | ''' 125 | Compute the fundamental matrix given the point correspondences 126 | 127 | Parameters 128 | ------------ 129 | points1, points2 - array with shape [n, 3] 130 | corresponding points in images represented as 131 | homogeneous coordinates 132 | ''' 133 | # validate points 134 | assert points1.shape[0] == points2.shape[0], "no. of points don't match" 135 | 136 | u1 = points1[:, 0] 137 | v1 = points1[:, 1] 138 | u2 = points2[:, 0] 139 | v2 = points2[:, 1] 140 | one = np.ones_like(u1) 141 | 142 | # construct the matrix 143 | # A = [u2.u1, u2.v1, u2, v2.u1, v2.v1, v2, u1, v1, 1] for all the points 144 | # stack columns 145 | A = np.c_[u1 * u2, v1 * u2, u2, u1 * v2, v1 * v2, v2, u1, v1, one] 146 | 147 | # peform svd on A and find the minimum value of |Af| 148 | U, S, V = np.linalg.svd(A, full_matrices=True) 149 | f = V[-1, :] 150 | F = f.reshape(3, 3) # reshape f as a matrix 151 | 152 | # constrain F 153 | # make rank 2 by zeroing out last singular value 154 | U, S, V = np.linalg.svd(F, full_matrices=True) 155 | S[-1] = 0 # zero out the last singular value 156 | F = U @ np.diag(S) @ V # recombine again 157 | return F 158 | 159 | def plot_epipolar_lines(img1, img2, points1, points2, show_epipole=False): 160 | ''' 161 | Given two images and their corresponding points, compute the fundamental matrix 162 | and plot epipole and epipolar lines 163 | 164 | Parameters 165 | ------------ 166 | img1, img2 - array with shape (height, width) 167 | grayscale images with only two channels 168 | points1, points2 - array with shape [n, 3] 169 | corresponding points in images represented as 170 | homogeneous coordinates 171 | show_epipole - boolean 172 | whether to compute and plot the epipole or not 173 | ''' 174 | 175 | # get image size 176 | h, w = img1.shape 177 | n = points1.shape[0] 178 | # validate points 179 | if points2.shape[0] != n: 180 | raise ValueError("No. of points don't match") 181 | 182 | # compute the fundamental matrix 183 | F = compute_fundamental_matrix_normalized(points1, points2) 184 | 185 | # configure figure 186 | nrows = 2 187 | ncols = 1 188 | fig, axes = plt.subplots(nrows=nrows, ncols=ncols, figsize=(6, 8)) 189 | 190 | # plot image 1 191 | ax1 = axes[0] 192 | ax1.set_title("Image 1") 193 | ax1.imshow(img1, cmap="gray") 194 | 195 | # plot image 2 196 | ax2 = axes[1] 197 | ax2.set_title("Image 2") 198 | ax2.imshow(img2, cmap="gray") 199 | 200 | # plot epipolar lines 201 | for i in range(n): 202 | p1 = points1.T[:, i] 203 | p2 = points2.T[:, i] 204 | 205 | # Epipolar line in the image of camera 1 given the points in the image of camera 2 206 | coeffs = p2.T @ F 207 | x, y = plot_line(coeffs, (-1500, w)) # limit hardcoded for this image. please change 208 | ax1.plot(x, y, color="orange") 209 | ax1.scatter(*p1.reshape(-1)[:2], color="blue") 210 | 211 | # Epipolar line in the image of camera 2 given the points in the image of camera 1 212 | coeffs = F @ p1 213 | x, y = plot_line(coeffs, (0, 2800)) # limit hardcoded for this image. please change 214 | ax2.plot(x, y, color="orange") 215 | ax2.scatter(*p2.reshape(-1)[:2], color="blue") 216 | 217 | if show_epipole: 218 | # compute epipole 219 | e1 = compute_epipole(F) 220 | e2 = compute_epipole(F.T) 221 | # plot epipole 222 | ax1.scatter(*e1.reshape(-1)[:2], color="red") 223 | ax2.scatter(*e2.reshape(-1)[:2], color="red") 224 | else: 225 | # set axes limits 226 | ax1.set_xlim(0, w) 227 | ax1.set_ylim(h, 0) 228 | ax2.set_xlim(0, w) 229 | ax2.set_ylim(h, 0) 230 | 231 | plt.tight_layout() 232 | 233 | def compute_epipole(F): 234 | ''' 235 | Compute epipole using the fundamental matrix. 236 | pass F.T as argument to compute the other epipole 237 | ''' 238 | U, S, V = np.linalg.svd(F) 239 | e = V[-1, :] 240 | e = e / e[2] 241 | return e 242 | 243 | def compute_matching_homographies(e2, F, im2, points1, points2): 244 | ''' 245 | Compute the matching homography matrices 246 | ''' 247 | h, w = im2.shape 248 | # create the homography matrix H2 that moves the epipole to infinity 249 | 250 | # create the translation matrix to shift to the image center 251 | T = np.array([[1, 0, -w/2], [0, 1, -h/2], [0, 0, 1]]) 252 | e2_p = T @ e2 253 | e2_p = e2_p / e2_p[2] 254 | e2x = e2_p[0] 255 | e2y = e2_p[1] 256 | # create the rotation matrix to rotate the epipole back to X axis 257 | if e2x >= 0: 258 | a = 1 259 | else: 260 | a = -1 261 | R1 = a * e2x / np.sqrt(e2x ** 2 + e2y ** 2) 262 | R2 = a * e2y / np.sqrt(e2x ** 2 + e2y ** 2) 263 | R = np.array([[R1, R2, 0], [-R2, R1, 0], [0, 0, 1]]) 264 | e2_p = R @ e2_p 265 | x = e2_p[0] 266 | # create matrix to move the epipole to infinity 267 | G = np.array([[1, 0, 0], [0, 1, 0], [-1/x, 0, 1]]) 268 | # create the overall transformation matrix 269 | H2 = np.linalg.inv(T) @ G @ R @ T 270 | 271 | # create the corresponding homography matrix for the other image 272 | e_x = np.array([[0, -e2[2], e2[1]], [e2[2], 0, -e2[0]], [-e2[1], e2[0], 0]]) 273 | M = e_x @ F + e2.reshape(3,1) @ np.array([[1, 1, 1]]) 274 | points1_t = H2 @ M @ points1.T 275 | points2_t = H2 @ points2.T 276 | points1_t /= points1_t[2, :] 277 | points2_t /= points2_t[2, :] 278 | b = points2_t[0, :] 279 | a = np.linalg.lstsq(points1_t.T, b, rcond=None)[0] 280 | H_A = np.array([a, [0, 1, 0], [0, 0, 1]]) 281 | H1 = H_A @ H2 @ M 282 | return H1, H2 283 | 284 | -------------------------------------------------------------------------------- /utils.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | import pytransform3d.rotations as pr 5 | from pytransform3d.plot_utils import plot_vector 6 | 7 | import matplotlib.pyplot as plt 8 | 9 | # constants 10 | 11 | origin = np.array([0, 0, 0]) 12 | 13 | # basis vectors 14 | x = np.array([1, 0, 0]) 15 | y = np.array([0, 1, 0]) 16 | z = np.array([0, 0, 1]) 17 | 18 | # basis vectors as homogeneous coordinates 19 | xh = np.array([1, 0, 0, 1]) 20 | yh = np.array([0, 1, 0, 1]) 21 | zh = np.array([0, 0, 1, 1]) 22 | 23 | # helper functions 24 | def get_rot_x(angle): 25 | ''' 26 | transformation matrix that rotates a point about the standard X axis 27 | ''' 28 | Rx = np.zeros(shape=(3, 3)) 29 | Rx[0, 0] = 1 30 | Rx[1, 1] = np.cos(angle) 31 | Rx[1, 2] = -np.sin(angle) 32 | Rx[2, 1] = np.sin(angle) 33 | Rx[2, 2] = np.cos(angle) 34 | 35 | return Rx 36 | 37 | def get_rot_y(angle): 38 | ''' 39 | transformation matrix that rotates a point about the standard Y axis 40 | ''' 41 | Ry = np.zeros(shape=(3, 3)) 42 | Ry[0, 0] = np.cos(angle) 43 | Ry[0, 2] = -np.sin(angle) 44 | Ry[2, 0] = np.sin(angle) 45 | Ry[2, 2] = np.cos(angle) 46 | Ry[1, 1] = 1 47 | 48 | return Ry 49 | 50 | def get_rot_z(angle): 51 | ''' 52 | transformation matrix that rotates a point about the standard Z axis 53 | ''' 54 | Rz = np.zeros(shape=(3, 3)) 55 | Rz[0, 0] = np.cos(angle) 56 | Rz[0, 1] = -np.sin(angle) 57 | Rz[1, 0] = np.sin(angle) 58 | Rz[1, 1] = np.cos(angle) 59 | Rz[2, 2] = 1 60 | 61 | return Rz 62 | 63 | def create_rotation_transformation_matrix(angles, order): 64 | ''' 65 | Create a matrix that rotates a vector through the given angles in the given order 66 | wrt the standard global axes (extrinsic rotation) 67 | Note: The rotation is carried out anti-clockwise in a left handed axial system 68 | 69 | Parameters 70 | ----------- 71 | angles - list 72 | list of angles in radians 73 | order - string 74 | the order in which to rotate 75 | 76 | Returns 77 | -------- 78 | net - np.ndarray, shape - (3, 3) 79 | The transformation matrix which carries out the given rotations 80 | ''' 81 | fn_mapping = {'x': get_rot_x, 'y': get_rot_y, 'z': get_rot_z} 82 | net = np.identity(3) 83 | for angle, axis in list(zip(angles, order))[::-1]: 84 | if fn_mapping.get(axis) is None: 85 | raise ValueError("Invalid axis") 86 | R = fn_mapping.get(axis) 87 | net = np.matmul(net, R(angle)) 88 | 89 | return net 90 | 91 | def create_translation_matrix(offset): 92 | ''' 93 | Create a transformation matrix that translates a vetor by the given offset 94 | 95 | Parameters 96 | ----------- 97 | offset - np.ndarray, shape - (3,) 98 | The translation offset 99 | 100 | Returns 101 | ---------- 102 | T - np.ndarray, shape - (4, 4) 103 | The translation matrix 104 | ''' 105 | T = np.identity(4) 106 | T[:3, 3] = offset 107 | return T 108 | 109 | make_line = lambda u, v: np.vstack((u, v)).T 110 | 111 | def create_rotation_change_of_basis_matrix(transformation_matrix): 112 | ''' 113 | Creates a rotation change of basis matrix 114 | ''' 115 | 116 | xt = transformation_matrix.T @ x.reshape(3, 1) 117 | yt = transformation_matrix.T @ y.reshape(3, 1) 118 | zt = transformation_matrix.T @ z.reshape(3, 1) 119 | 120 | return np.hstack((xt, yt, zt)) 121 | 122 | def create_image_grid(f, img_size): 123 | ''' 124 | Create an image grid of the given size parallel to the XY plane 125 | at a distance f from the camera center (origin) 126 | ''' 127 | h, w = img_size 128 | xx, yy = np.meshgrid(range(-(h // 2), w // 2 + 1), range(-(h // 2), w // 2 + 1)) 129 | Z = np.ones(shape=img_size) * f 130 | 131 | return xx, yy, Z 132 | 133 | def convert_grid_to_homogeneous(xx, yy, Z, img_size): 134 | ''' 135 | Extract coordinates from a grid and convert them to homogeneous coordinates 136 | ''' 137 | h, w = img_size 138 | pi = np.ones(shape=(4, h*w)) 139 | c = 0 140 | for i in range(h): 141 | for j in range(w): 142 | x = xx[i, j] 143 | y = yy[i, j] 144 | z = Z[i, j] 145 | point = np.array([x, y, z]) 146 | pi[:3, c] = point 147 | c += 1 148 | return pi 149 | 150 | def convert_homogeneous_to_grid(pts, img_size): 151 | ''' 152 | Convert a set of homogeneous points to a grid 153 | ''' 154 | xxt = pts[0, :].reshape(img_size) 155 | yyt = pts[1, :].reshape(img_size) 156 | Zt = pts[2, :].reshape(img_size) 157 | 158 | return xxt, yyt, Zt 159 | 160 | def create_same_plane_points(n_points, xlim, ylim, elevation): 161 | ''' 162 | Create points that lie on the same plane within the given limits at the given elevation 163 | ''' 164 | x = np.linspace(xlim[0], xlim[1], n_points) 165 | y = np.linspace(ylim[0], ylim[1], n_points) 166 | xxs, yys = np.meshgrid(x, y) 167 | zzs = elevation * np.ones(shape=(n_points, n_points)) 168 | same_plane_points = np.ones(shape=(3, n_points * n_points)) 169 | c = 0 170 | for i in range(n_points): 171 | for j in range(n_points): 172 | xs = xxs[i, j] 173 | ys = yys[i, j] 174 | zs = zzs[i, j] 175 | same_plane_points[:, c] = np.array([xs, ys, zs]) 176 | c += 1 177 | return same_plane_points 178 | 179 | def compute_intrinsic_parameter_matrix(f, s, a, cx, cy): 180 | K = np.identity(3) 181 | K[0, 0] = f 182 | K[0, 1] = s 183 | K[0, 2] = cx 184 | K[1, 1] = a * f 185 | K[1, 2] = cy 186 | 187 | return K 188 | 189 | def compute_image_projection(points, K): 190 | ''' 191 | Compute projection of points onto the image plane 192 | 193 | Parameters 194 | ----------- 195 | points - np.ndarray, shape - (3, n_points) 196 | points we want to project onto the image plane 197 | the points should be represented in the camera coordinate system 198 | K - np.ndarray, shape - (3, 3) 199 | camera intrinsic matrix 200 | 201 | Returns 202 | ------- 203 | points_i - np.ndarray, shape - (2, n_points) 204 | the projected points on the image 205 | ''' 206 | 207 | h_points_i = K @ points 208 | 209 | h_points_i[0, :] = h_points_i[0, :] / h_points_i[2, :] 210 | h_points_i[1, :] = h_points_i[1, :] / h_points_i[2, :] 211 | 212 | points_i = h_points_i[:2, :] 213 | 214 | return points_i 215 | 216 | def generate_random_points(n_points, xlim, ylim, zlim): 217 | ''' 218 | Generate random points in the given limits 219 | ''' 220 | x = np.random.randint(xlim[0], xlim[1], size=n_points) 221 | y = np.random.randint(ylim[0], ylim[1], size=n_points) 222 | z = np.random.randint(zlim[0], zlim[1], size=n_points) 223 | 224 | return np.vstack((x, y, z)) 225 | 226 | def compute_coordniates_wrt_camera(world_points, E, is_homogeneous=False): 227 | ''' 228 | Performs a change of basis operation from the world coordinate system 229 | to the camera coordinate system 230 | 231 | Parameters 232 | ------------ 233 | world_points - np.ndarray, shape - (3, n_points) or (4, n_points) 234 | points in the world coordinate system 235 | E - np.ndarray, shape - (3, 4) 236 | the camera extrinsic matrix 237 | is_homogeneous - boolean 238 | whether the coordinates are represented in their homogeneous form 239 | if False, an extra dimension will be added for computation 240 | 241 | Returns 242 | ---------- 243 | points_c - np.ndarray, shape - (3, n_points) 244 | points in the camera coordinate system 245 | ''' 246 | if not is_homogeneous: 247 | # convert to homogeneous coordinates 248 | points_h = np.vstack((world_points, np.ones(world_points.shape[1]))) 249 | 250 | points_c = E @ points_h 251 | return points_c 252 | 253 | def create_algebraic_matrix(world_points, projections): 254 | ''' 255 | Create the algebraic matrix A for camera calibration 256 | 257 | Parameters 258 | ----------- 259 | world points - np.ndarray, shape - (3, n_points) 260 | points in the world coordinate system 261 | 262 | projections - np.ndarray, shape - (3, n_points) 263 | projections of the above points in the image 264 | 265 | Returns 266 | ---------- 267 | A - np.ndarray, shape - (2 * n_points, 12) 268 | the algebraic matrix used for camera calibration 269 | ''' 270 | 271 | assert world_points.shape[1] == projections.shape[1] 272 | n_points = world_points.shape[1] 273 | A = np.ones(shape=(2*n_points, 12)) 274 | 275 | c = 0 276 | 277 | for i in range(n_points): 278 | 279 | w = world_points[:, i] 280 | p = projections[:, i] 281 | 282 | X, Y, Z = w 283 | u, v = p 284 | rows = np.zeros(shape=(2, 12)) 285 | 286 | rows[0, 0], rows[0, 1], rows[0, 2], rows[0, 3] = X, Y, Z, 1 287 | rows[0, 8], rows[0, 9], rows[0, 10], rows[0, 11] = -u * X, -u * Y, -u * Z, -u 288 | 289 | rows[1, 4], rows[1, 5], rows[1, 6], rows[1, 7] = X, Y, Z, 1 290 | rows[1, 8], rows[1, 9], rows[1, 10], rows[1, 11] = -v * X, -v * Y, -v * Z, -v 291 | 292 | A[c:c+2, :] = rows 293 | c += 2 294 | 295 | return A 296 | 297 | def compute_world2img_projection(world_points, M, is_homogeneous=False): 298 | ''' 299 | Given a set of points in the world and the overall camera matrix, 300 | compute the projection of world points onto the image 301 | 302 | Parameters 303 | ----------- 304 | world_points - np.ndarray, shape - (3, n_points) 305 | points in the world coordinate system 306 | 307 | M - np.ndarray, shape - (3, 4) 308 | The overall camera matrix which is a composition of the extrinsic and intrinsic matrix 309 | 310 | is_homogeneous - boolean 311 | whether the coordinates are represented in their homogeneous form 312 | if False, an extra dimension will be added for computation 313 | 314 | Returns 315 | ---------- 316 | projections - np.ndarray, shape - (2, n_points) 317 | projections of the world points onto the image 318 | ''' 319 | if not is_homogeneous: 320 | # convert to homogeneous coordinates 321 | world_points = np.vstack((world_points, np.ones(world_points.shape[1]))) 322 | 323 | h_points_i = M @ world_points 324 | 325 | h_points_i[0, :] = h_points_i[0, :] / h_points_i[2, :] 326 | h_points_i[1, :] = h_points_i[1, :] / h_points_i[2, :] 327 | 328 | points_i = h_points_i[:2, :] 329 | 330 | return points_i 331 | 332 | def geometric_error(m, world_points, projections): 333 | ''' 334 | compute the geometric error wrt the 335 | prediction projections and the groundtruth projections 336 | 337 | Parameters 338 | ------------ 339 | m - np.ndarray, shape - (12) 340 | an 12-dim vector which is to be updated 341 | world_points - np.ndarray, shape - (3, n) 342 | points in the world coordinate system 343 | projections - np.ndarray(2, n) 344 | projections of the points in the image 345 | 346 | Returns 347 | -------- 348 | error - float 349 | the geometric error 350 | ''' 351 | assert world_points.shape[1] == projections.shape[1] 352 | error = 0 353 | n_points = world_points.shape[1] 354 | for i in range(n_points): 355 | X, Y, Z = world_points[:, i] 356 | u, v = projections[:, i] 357 | u_ = m[0] * X + m[1] * Y + m[2] * Z + m[3] 358 | v_ = m[4] * X + m[5] * Y + m[6] * Z + m[7] 359 | d = m[8] * X + m[9] * Y + m[10] * Z + m[11] 360 | u_ = u_/d 361 | v_ = v_/d 362 | error += np.sqrt(np.square(u - u_) + np.square(v - v_)) 363 | return error --------------------------------------------------------------------------------