dan-masek

custom_undistort_v3

Nov 7th, 2019
279
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 3.27 KB | None | 0 0
  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.  
  26.         r2 = np.add(xx, yyv)
  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)
  32.         radial = radial_a / radial_b
  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=",")
Add Comment
Please, Sign In to add comment