helper_functions.py
Go to the documentation of this file.
1 ##################################################################################################
2 ## License: Apache 2.0. See LICENSE file in root directory. ####
3 ##################################################################################################
4 ## Box Dimensioner with multiple cameras: Helper files ####
5 ##################################################################################################
6 
7 # Opencv helper functions and class
8 import cv2
9 import numpy as np
10 
11 """
12  _ _ _ _____ _ _
13  | | | | ___ | | _ __ ___ _ __ | ___|_ _ _ __ ___ | |_ (_) ___ _ __ ___
14  | |_| | / _ \| || '_ \ / _ \| '__| | |_ | | | || '_ \ / __|| __|| | / _ \ | '_ \ / __|
15  | _ || __/| || |_) || __/| | | _| | |_| || | | || (__ | |_ | || (_) || | | |\__ \
16  |_| |_| \___||_|| .__/ \___||_| |_| \__,_||_| |_| \___| \__||_| \___/ |_| |_||___/
17  _|
18 """
19 
20 
21 def calculate_rmsd(points1, points2, validPoints=None):
22  """
23  calculates the root mean square deviation between to point sets
24 
25  Parameters:
26  -------
27  points1, points2: numpy matrix (K, N)
28  where K is the dimension of the points and N is the number of points
29 
30  validPoints: bool sequence of valid points in the point set.
31  If it is left out, all points are considered valid
32  """
33  assert(points1.shape == points2.shape)
34  N = points1.shape[1]
35 
36  if validPoints == None:
37  validPoints = [True]*N
38 
39  assert(len(validPoints) == N)
40 
41  points1 = points1[:,validPoints]
42  points2 = points2[:,validPoints]
43 
44  N = points1.shape[1]
45 
46  dist = points1 - points2
47  rmsd = 0
48  for col in range(N):
49  rmsd += np.matmul(dist[:,col].transpose(), dist[:,col]).flatten()[0]
50 
51  return np.sqrt(rmsd/N)
52 
53 
54 def get_chessboard_points_3D(chessboard_params):
55  """
56  Returns the 3d coordinates of the chessboard corners
57  in the coordinate system of the chessboard itself.
58 
59  Returns
60  -------
61  objp : array
62  (3, N) matrix with N being the number of corners
63  """
64  assert(len(chessboard_params) == 3)
65  width = chessboard_params[0]
66  height = chessboard_params[1]
67  square_size = chessboard_params[2]
68  objp = np.zeros((width * height, 3), np.float32)
69  objp[:,:2] = np.mgrid[0:width,0:height].T.reshape(-1,2)
70  return objp.transpose() * square_size
71 
72 
73 def cv_find_chessboard(depth_frame, infrared_frame, chessboard_params):
74  """
75  Searches the chessboard corners using the set infrared image and the
76  checkerboard size
77 
78  Returns:
79  -----------
80  chessboard_found : bool
81  Indicates wheather the operation was successful
82  corners : array
83  (2,N) matrix with the image coordinates of the chessboard corners
84  """
85  assert(len(chessboard_params) == 3)
86  infrared_image = np.asanyarray(infrared_frame.get_data())
87  criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
88  chessboard_found = False
89  chessboard_found, corners = cv2.findChessboardCorners(infrared_image, (
90  chessboard_params[0], chessboard_params[1]))
91 
92  if chessboard_found:
93  corners = cv2.cornerSubPix(infrared_image, corners, (11,11),(-1,-1), criteria)
94  corners = np.transpose(corners, (2,0,1))
95  return chessboard_found, corners
96 
97 
98 
99 def get_depth_at_pixel(depth_frame, pixel_x, pixel_y):
100  """
101  Get the depth value at the desired image point
102 
103  Parameters:
104  -----------
105  depth_frame : rs.frame()
106  The depth frame containing the depth information of the image coordinate
107  pixel_x : double
108  The x value of the image coordinate
109  pixel_y : double
110  The y value of the image coordinate
111 
112  Return:
113  ----------
114  depth value at the desired pixel
115 
116  """
117  return depth_frame.as_depth_frame().get_distance(round(pixel_x), round(pixel_y))
118 
119 
120 
121 def convert_depth_pixel_to_metric_coordinate(depth, pixel_x, pixel_y, camera_intrinsics):
122  """
123  Convert the depth and image point information to metric coordinates
124 
125  Parameters:
126  -----------
127  depth : double
128  The depth value of the image point
129  pixel_x : double
130  The x value of the image coordinate
131  pixel_y : double
132  The y value of the image coordinate
133  camera_intrinsics : The intrinsic values of the imager in whose coordinate system the depth_frame is computed
134 
135  Return:
136  ----------
137  X : double
138  The x value in meters
139  Y : double
140  The y value in meters
141  Z : double
142  The z value in meters
143 
144  """
145  X = (pixel_x - camera_intrinsics.ppx)/camera_intrinsics.fx *depth
146  Y = (pixel_y - camera_intrinsics.ppy)/camera_intrinsics.fy *depth
147  return X, Y, depth
148 
149 
150 
151 def convert_depth_frame_to_pointcloud(depth_image, camera_intrinsics ):
152  """
153  Convert the depthmap to a 3D point cloud
154 
155  Parameters:
156  -----------
157  depth_frame : rs.frame()
158  The depth_frame containing the depth map
159  camera_intrinsics : The intrinsic values of the imager in whose coordinate system the depth_frame is computed
160 
161  Return:
162  ----------
163  x : array
164  The x values of the pointcloud in meters
165  y : array
166  The y values of the pointcloud in meters
167  z : array
168  The z values of the pointcloud in meters
169 
170  """
171 
172  [height, width] = depth_image.shape
173 
174  nx = np.linspace(0, width-1, width)
175  ny = np.linspace(0, height-1, height)
176  u, v = np.meshgrid(nx, ny)
177  x = (u.flatten() - camera_intrinsics.ppx)/camera_intrinsics.fx
178  y = (v.flatten() - camera_intrinsics.ppy)/camera_intrinsics.fy
179 
180  z = depth_image.flatten() / 1000;
181  x = np.multiply(x,z)
182  y = np.multiply(y,z)
183 
184  x = x[np.nonzero(z)]
185  y = y[np.nonzero(z)]
186  z = z[np.nonzero(z)]
187 
188  return x, y, z
189 
190 
191 def convert_pointcloud_to_depth(pointcloud, camera_intrinsics):
192  """
193  Convert the world coordinate to a 2D image coordinate
194 
195  Parameters:
196  -----------
197  pointcloud : numpy array with shape 3xN
198 
199  camera_intrinsics : The intrinsic values of the imager in whose coordinate system the depth_frame is computed
200 
201  Return:
202  ----------
203  x : array
204  The x coordinate in image
205  y : array
206  The y coordiante in image
207 
208  """
209 
210  assert (pointcloud.shape[0] == 3)
211  x_ = pointcloud[0,:]
212  y_ = pointcloud[1,:]
213  z_ = pointcloud[2,:]
214 
215  m = x_[np.nonzero(z_)]/z_[np.nonzero(z_)]
216  n = y_[np.nonzero(z_)]/z_[np.nonzero(z_)]
217 
218  x = m*camera_intrinsics.fx + camera_intrinsics.ppx
219  y = n*camera_intrinsics.fy + camera_intrinsics.ppy
220 
221  return x, y
222 
223 
224 
225 def get_boundary_corners_2D(points):
226  """
227  Get the minimum and maximum point from the array of points
228 
229  Parameters:
230  -----------
231  points : array
232  The array of points out of which the min and max X and Y points are needed
233 
234  Return:
235  ----------
236  boundary : array
237  The values arranged as [minX, maxX, minY, maxY]
238 
239  """
240  padding=0.05
241  if points.shape[0] == 3:
242  assert (len(points.shape)==2)
243  minPt_3d_x = np.amin(points[0,:])
244  maxPt_3d_x = np.amax(points[0,:])
245  minPt_3d_y = np.amin(points[1,:])
246  maxPt_3d_y = np.amax(points[1,:])
247 
248  boudary = [minPt_3d_x-padding, maxPt_3d_x+padding, minPt_3d_y-padding, maxPt_3d_y+padding]
249 
250  else:
251  raise Exception("wrong dimension of points!")
252 
253  return boudary
254 
255 
256 
257 def get_clipped_pointcloud(pointcloud, boundary):
258  """
259  Get the clipped pointcloud withing the X and Y bounds specified in the boundary
260 
261  Parameters:
262  -----------
263  pointcloud : array
264  The input pointcloud which needs to be clipped
265  boundary : array
266  The X and Y bounds
267 
268  Return:
269  ----------
270  pointcloud : array
271  The clipped pointcloud
272 
273  """
274  assert (pointcloud.shape[0]>=2)
275  pointcloud = pointcloud[:,np.logical_and(pointcloud[0,:]<boundary[1], pointcloud[0,:]>boundary[0])]
276  pointcloud = pointcloud[:,np.logical_and(pointcloud[1,:]<boundary[3], pointcloud[1,:]>boundary[2])]
277  return pointcloud
278 
279 
280 
281 
282 
helper_functions.cv_find_chessboard
def cv_find_chessboard(depth_frame, infrared_frame, chessboard_params)
Definition: helper_functions.py:73
helper_functions.convert_depth_pixel_to_metric_coordinate
def convert_depth_pixel_to_metric_coordinate(depth, pixel_x, pixel_y, camera_intrinsics)
Definition: helper_functions.py:121
helper_functions.get_chessboard_points_3D
def get_chessboard_points_3D(chessboard_params)
Definition: helper_functions.py:54
helper_functions.convert_pointcloud_to_depth
def convert_pointcloud_to_depth(pointcloud, camera_intrinsics)
Definition: helper_functions.py:191
helper_functions.get_clipped_pointcloud
def get_clipped_pointcloud(pointcloud, boundary)
Definition: helper_functions.py:257
helper_functions.convert_depth_frame_to_pointcloud
def convert_depth_frame_to_pointcloud(depth_image, camera_intrinsics)
Definition: helper_functions.py:151
helper_functions.calculate_rmsd
def calculate_rmsd(points1, points2, validPoints=None)
Definition: helper_functions.py:21
helper_functions.get_boundary_corners_2D
def get_boundary_corners_2D(points)
Definition: helper_functions.py:225
helper_functions.get_depth_at_pixel
def get_depth_at_pixel(depth_frame, pixel_x, pixel_y)
Definition: helper_functions.py:99

librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:16