# custom_undistort_v3

Nov 7th, 2019
279
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. import numpy as np
2. import time
3.
4.
5. def _distort_z_1(x, y, step_size, k1, k2, k3, k4, k5, k6, p1, p2):
6.     col_count = x.shape[0]
7.     row_count = y.shape[0]
8.
9.     xx = np.multiply(x, x)
10.     yy = np.multiply(y, y)
11.
12.     two_p1 = 2 * p1
13.     two_p2 = 2 * p2
14.     two_xx = 2 * xx
15.
16.     x_distorted = np.zeros((row_count, col_count), np.float32)
17.     y_distorted = np.zeros((row_count, col_count), np.float32)
18.
19.     for i in range(0, row_count, step_size):
20.         i_e = min(row_count, i + step_size)
21.         yv = y[i:i_e,np.newaxis]
22.         yyv = yy[i:i_e,np.newaxis]
23.
24.         xy = x * yv
25.
27.         r4 = np.multiply(r2, r2)
28.         r6 = np.multiply(r4, r2)
29.
30.         radial_a = (1 + k1 * r2 + k2 * r4 + k3 * r6)
31.         radial_b = (1 + k4 * r2 + k5 * r4 + k6 * r6)
33.
34.         tangential_x = two_p1 * xy + p2 * (r2 + two_xx)
35.         tangential_y = p1 * (r2 + 2 * yyv) + two_p2 * xy
36.
37.         x_distorted[i:i_e,:] = x * radial + tangential_x
38.         y_distorted[i:i_e,:] = yv * radial + tangential_y
39.
40.     return x_distorted, y_distorted
41.
42.
43. def _get_undistort_rectify_maps(distortion_coefficients, camera_matrix, image_width, image_height, step_size = 20):
44.     start = time.time()
45.     coords_x = np.arange(image_width, dtype=np.float32)
46.     coords_y = np.arange(image_height, dtype=np.float32)
47.
48.     camera_matrix_inv = np.linalg.inv(camera_matrix)
49.     fx_inv = camera_matrix_inv[0,0]
50.     cx_inv = camera_matrix_inv[0,2]
51.     fy_inv = camera_matrix_inv[1,1]
52.     cy_inv = camera_matrix_inv[1,2]
53.
54.     coords_x[:] = fx_inv * coords_x + cx_inv
55.     coords_y[:] = fy_inv * coords_y + cy_inv
56.
57.     end = time.time()
58.     print("END: camera_matrix_inv * (x,y) equivalent", end - start)
59.
60.
61.     start = time.time()
62.     x_distorted, y_distorted = _distort_z_1(
63.         coords_x
64.         , coords_y
65.         , step_size
66.         , **distortion_coefficients)
67.     end = time.time()
68.     print("END: _distort_z_1", end - start)
69.
70.
71.     start = time.time()
72.     fx = camera_matrix[0,0]
73.     cx = camera_matrix[0,2]
74.     fy = camera_matrix[1,1]
75.     cy = camera_matrix[1,2]
76.
77.     result_x = fx * x_distorted + cx
78.     result_y = fy * y_distorted + cy
79.     end = time.time()
80.     print("END: camera_matrix * (x_d,y_d) equivalent", end - start)
81.
82.     return (result_x, result_y)
83.
84.
85. if __name__ == "__main__":
86.     image_width = 4032
87.     image_height = 3024
88.
89.     distortion_coefficients = {
90.         "k1": np.float32(0)
91.         , "k2": np.float32(0)
92.         , "k3": np.float32(0)
93.         , "k4": np.float32(0)
94.         , "k5": np.float32(0)
95.         , "k6": np.float32(0)
96.         , "p1": np.float32(0)
97.         , "p2": np.float32(0)
98.         }
99.
100.     camera_matrix = np.array([
101.         [1000, 0, 2016],
102.         [0, 1000, 1512],
103.         [0, 0, 1]], dtype=np.float32)
104.
105.     start = time.time()
106.     map_x, map_y = _get_undistort_rectify_maps(
107.         distortion_coefficients
108.         , camera_matrix
109.         , image_width
110.         , image_height
111.         , 20)
112.     end = time.time()
113.     print("END: _get_undistort_rectify_maps", end - start)
114.
115.     #np.savetxt("custund_mapx.csv", map_x, delimiter=",")
116.     #np.savetxt("custund_mapy.csv", map_y, delimiter=",")