├── LICENSE ├── README.md ├── mapper.py └── requirements.txt /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License Copyright (c) 2021 KonstantinosAng 2 | 3 | Permission is hereby granted, 4 | free of charge, to any person obtaining a copy of this software and associated 5 | documentation files (the "Software"), to deal in the Software without 6 | restriction, including without limitation the rights to use, copy, modify, merge, 7 | publish, distribute, sublicense, and/or sell copies of the Software, and to 8 | permit persons to whom the Software is furnished to do so, subject to the 9 | following conditions: 10 | 11 | The above copyright notice and this permission notice 12 | (including the next paragraph) shall be included in all copies or substantial 13 | portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 16 | ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 17 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO 18 | EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR 19 | OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PyKinect2 Mapper Functions 2 | *** 3 | 4 | ## Description 5 | Documentation and description on how to use the ICoordinateMapper of Microsoft Kinect 2 using [PyKinect2](https://github.com/Kinect/PyKinect2) and [python](https://www.python.org). 6 | 7 | ## Instructions 8 | 9 | Simply import the mapper.py in your file and use the functions. The reason I have the 10 | kinect instance as the first argument is to avoid any segmentation errors by multiple access points to 11 | the the kinect device. Most of the function work pretty well, just remember to give time to kinect to open 12 | and have at least one depth frame and color frame to use most of the functions without errors. Feel free to modify and use 13 | and if you like it give it a star. Most of the documentation is inside the mapper.py file, if you cannot understand how to use 14 | any of the function feel free to open an issue and ask me. 15 | 16 | ## Requirements 17 | Install all requirements from requirements.txt using the following command 18 | ``` 19 | pip install -r requirements.txt --no-index --find-links file:///tmp/packages 20 | ``` 21 | Full list of **all** requirements 22 | ``` 23 | * ctypes 24 | * numpy==1.18.1 25 | * pykinect2==0.1.0 26 | ``` 27 | -------------------------------------------------------------------------------- /mapper.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Konstantinos Angelopoulos 3 | Date: 04/02/2020 4 | All rights reserved. 5 | Feel free to use and modify and if you like it give it a star. 6 | """ 7 | 8 | 9 | def subscribe_coordinate_mapping_changed(kinect): 10 | return kinect._mapper.SubscribeCoordinateMappingChanged() 11 | 12 | 13 | def unsubscribe_coordinate_mapping_changed(kinect, waitableHandle_id): 14 | """ 15 | The waitableHandle_id is returned by the subscribe_coordinate_mapping_changed function 16 | So use that function first to get the id and pass it to this function 17 | """ 18 | return kinect._mapper.UnsubscribeCoordinateMappingChanged(waitableHandle_id) 19 | 20 | 21 | def get_coordinate_mapping_changed_event_data(kinect, waitableHandle_id): 22 | """ 23 | The waitableHandle_id is returned by the subscribe_coordinate_mapping_changed function 24 | So use that function first to get the id and pass it to this function 25 | """ 26 | return kinect._mapper.GetCoordinateMappingChangedEventData(waitableHandle_id) 27 | 28 | 29 | # Map Depth Space to Color Space (Image) 30 | def depth_2_color_space(kinect, depth_space_point, depth_frame_data, show=False, return_aligned_image=False): 31 | """ 32 | 33 | :param kinect: kinect class 34 | :param depth_space_point: _DepthSpacePoint from PyKinectV2 35 | :param depth_frame_data: kinect._depth_frame_data 36 | :param show: shows the aligned image 37 | :return: return the mapped color frame to depth frame 38 | """ 39 | # Import here to optimize 40 | import numpy as np 41 | import ctypes 42 | import cv2 43 | # Map Color to Depth Space 44 | color2depth_points_type = depth_space_point * np.int(1920 * 1080) 45 | color2depth_points = ctypes.cast(color2depth_points_type(), ctypes.POINTER(depth_space_point)) 46 | kinect._mapper.MapColorFrameToDepthSpace(ctypes.c_uint(512 * 424), depth_frame_data, ctypes.c_uint(1920 * 1080), color2depth_points) 47 | # Where color_point = [xcolor, ycolor] 48 | # color_x = color2depth_points[depth_point[1] * 1920 + color_point[0] - 1].x 49 | # color_y = color2depth_points[depth_point[1] * 1920 + color_point[0] - 1].y 50 | depthXYs = np.copy(np.ctypeslib.as_array(color2depth_points, shape=(kinect.color_frame_desc.Height*kinect.color_frame_desc.Width,))) # Convert ctype pointer to array 51 | depthXYs = depthXYs.view(np.float32).reshape(depthXYs.shape + (-1,)) # Convert struct array to regular numpy array https://stackoverflow.com/questions/5957380/convert-structured-array-to-regular-numpy-array 52 | depthXYs += 0.5 53 | depthXYs = depthXYs.reshape(kinect.color_frame_desc.Height, kinect.color_frame_desc.Width, 2).astype(np.int) 54 | depthXs = np.clip(depthXYs[:, :, 0], 0, kinect.depth_frame_desc.Width - 1) 55 | depthYs = np.clip(depthXYs[:, :, 1], 0, kinect.depth_frame_desc.Height - 1) 56 | if (show or return_aligned_image): 57 | depth_frame = kinect.get_last_depth_frame() 58 | depth_img = depth_frame.reshape((kinect.depth_frame_desc.Height, kinect.depth_frame_desc.Width, 1)).astype(np.uint16) 59 | align_depth_img = np.zeros((1080, 1920, 4), dtype=np.uint16) 60 | align_depth_img[:, :] = depth_img[depthYs, depthXs, :] 61 | if show: 62 | cv2.imshow('Aligned Image', cv2.resize(cv2.flip(align_depth_img, 1), (int(1920 / 2.0), int(1080 / 2.0)))) 63 | cv2.waitKey(3000) 64 | if return_aligned_image: 65 | return align_depth_img 66 | return depthXs, depthYs 67 | 68 | 69 | # Map Color Space to Depth Space (Image) 70 | def color_2_depth_space(kinect, color_space_point, depth_frame_data, show=False, return_aligned_image=False): 71 | """ 72 | 73 | :param kinect: kinect class 74 | :param color_space_point: _ColorSpacePoint from PyKinectV2 75 | :param depth_frame_data: kinect._depth_frame_data 76 | :param show: shows aligned image with color and depth 77 | :return: mapped depth to color frame 78 | """ 79 | import numpy as np 80 | import ctypes 81 | import cv2 82 | # Map Depth to Color Space 83 | depth2color_points_type = color_space_point * np.int(512 * 424) 84 | depth2color_points = ctypes.cast(depth2color_points_type(), ctypes.POINTER(color_space_point)) 85 | kinect._mapper.MapDepthFrameToColorSpace(ctypes.c_uint(512 * 424), depth_frame_data, kinect._depth_frame_data_capacity, depth2color_points) 86 | # depth_x = depth2color_points[color_point[0] * 1920 + color_point[0] - 1].x 87 | # depth_y = depth2color_points[color_point[0] * 1920 + color_point[0] - 1].y 88 | colorXYs = np.copy(np.ctypeslib.as_array(depth2color_points, shape=(kinect.depth_frame_desc.Height * kinect.depth_frame_desc.Width,))) # Convert ctype pointer to array 89 | colorXYs = colorXYs.view(np.float32).reshape(colorXYs.shape + (-1,)) # Convert struct array to regular numpy array https://stackoverflow.com/questions/5957380/convert-structured-array-to-regular-numpy-array 90 | colorXYs += 0.5 91 | colorXYs = colorXYs.reshape(kinect.depth_frame_desc.Height, kinect.depth_frame_desc.Width, 2).astype(np.int) 92 | colorXs = np.clip(colorXYs[:, :, 0], 0, kinect.color_frame_desc.Width - 1) 93 | colorYs = np.clip(colorXYs[:, :, 1], 0, kinect.color_frame_desc.Height - 1) 94 | if show or return_aligned_image: 95 | color_frame = kinect.get_last_color_frame() 96 | color_img = color_frame.reshape((kinect.color_frame_desc.Height, kinect.color_frame_desc.Width, 4)).astype(np.uint8) 97 | align_color_img = np.zeros((424, 512, 4), dtype=np.uint8) 98 | align_color_img[:, :] = color_img[colorYs, colorXs, :] 99 | if show: 100 | cv2.imshow('img', cv2.flip(align_color_img, 1)) 101 | cv2.waitKey(3000) 102 | if return_aligned_image: 103 | return align_color_img 104 | return colorXs, colorYs 105 | 106 | 107 | # Map Color Points to Depth Points 108 | def color_point_2_depth_point(kinect, depth_space_point, depth_frame_data, color_point): 109 | """ 110 | 111 | :param kinect: kinect class 112 | :param depth_space_point: _DepthSpacePoint from PyKinectV2 113 | :param depth_frame_data: kinect._depth_frame_data 114 | :param color_point: color_point pixel location as [x, y] 115 | :return: depth point of color point 116 | """ 117 | # Import here to optimize 118 | import numpy as np 119 | import ctypes 120 | # Map Color to Depth Space 121 | # Make sure that the kinect was able to obtain at least one color and depth frame, else the dept_x and depth_y values will go to infinity 122 | color2depth_points_type = depth_space_point * np.int(1920 * 1080) 123 | color2depth_points = ctypes.cast(color2depth_points_type(), ctypes.POINTER(depth_space_point)) 124 | kinect._mapper.MapColorFrameToDepthSpace(ctypes.c_uint(512 * 424), depth_frame_data, ctypes.c_uint(1920 * 1080), color2depth_points) 125 | # Where color_point = [xcolor, ycolor] 126 | depth_x = color2depth_points[color_point[1] * 1920 + color_point[0] - 1].x 127 | depth_y = color2depth_points[color_point[1] * 1920 + color_point[0] - 1].y 128 | return [int(depth_x) if depth_x != float('-inf') and depth_x != float('inf') else 0, int(depth_y) if depth_y != float('-inf') and depth_y != float('inf') else 0] 129 | 130 | 131 | # Return depth of object given the depth map coordinates 132 | def depth_space_2_world_depth(depth_map, x, y): 133 | """ 134 | 135 | :param depth_map: kinect.get_last_depth_frame 136 | :param x: depth pixel x 137 | :param y: depth pixel y 138 | :return: depth z of object 139 | """ 140 | if int(y) * 512 + int(x) <= 512 * 424: 141 | return float(depth_map[int(y) * 512 + int(x)]) # mm 142 | else: 143 | # If it exceeds return the last value to catch overflow 144 | return float(depth_map[512*424]) 145 | 146 | 147 | # Find the transformation from color to depth 148 | def transform_color_2_depth(color_points, depth_points): 149 | """ 150 | https://www.comp.nus.edu.sg/~cs4340/lecture/imorph.pdf 151 | Modules = import numpy as np 152 | TODO: Find 3 sets of (x, y) in color image and their (u, v) coordinates in depth image 153 | TODO: Use the sets to solve the following equations and find the coefficients 154 | Equation1: u = a11*x + a12*y + a13 155 | Equation2: v = a21*x + a22*y + a23 156 | :return: matrix with transformation coefficients [[a11, a12, a13], [a21, a22, a23]] as nparray 157 | 158 | [ depth_u ] = [a11 a12 a13] * [ color_x ] 159 | [ depth_v ] [a21 a22 a23] * [ color_y ] 160 | [ 1 ] 161 | """ 162 | # Import library here to optimize 163 | import numpy as np 164 | import json 165 | ret = True 166 | matrix = [] # transformation matrix 167 | # Solve the first 3 equations to find coefficients for u coordinates 168 | color = np.array([[color_points[0][0], color_points[0][1], 1], 169 | [color_points[1][0], color_points[1][1], 1], 170 | [color_points[2][0], color_points[2][1], 1]]) 171 | depth_u = np.array([depth_points[0][0], depth_points[1][0], depth_points[2][0]]) 172 | depth_v = np.array([depth_points[0][1], depth_points[1][1], depth_points[2][1]]) 173 | try: 174 | # Solve for u 175 | u_coeffs = np.linalg.solve(color, depth_u) 176 | # Solve for v 177 | v_coeffs = np.linalg.solve(color, depth_v) 178 | # Make matrix 179 | matrix = np.vstack([u_coeffs, v_coeffs]) 180 | # Description for json file 181 | description = 'Transformation matrix to go from Color Coordinates to Depth Coordinates,\nwithout the need of MapColorFrameToDepthSpace from ICoordinateMapper.\nSee mapper.py for more information' 182 | # Write matrix to use in the main file 183 | with open('mapper/matrix.json', 'w', encoding='utf-8') as json_file: 184 | configs = {"Description": description, "Transformation Matrix": matrix.tolist()} 185 | json.dump(configs, json_file, separators=(',', ':'), sort_keys=True, indent=4) 186 | except Exception as e: 187 | print(f"[MAPPER]: Could not solve linear equations \n{e}") 188 | ret = False 189 | 190 | return matrix, ret 191 | 192 | 193 | # Calculate pixel location from color to depth using only image resolutions 194 | def xy2uv_with_res(x, y, color_width, color_height, depth_width, depth_height): 195 | """ 196 | :return go from color pixel to depth pixel by ignoring distortion 197 | works better for center point but is not accurate for edge pixels 198 | """ 199 | # Calculate pixel location assuming that images are the same and only the resolution changes 200 | u = (x / color_width) * depth_width 201 | v = (y / color_height) * depth_height 202 | return [u, v] 203 | 204 | 205 | # Map Depth Frame to World Space 206 | def depth_2_world(kinect, depth_frame_data, camera_space_point, as_array=False): 207 | """ 208 | :param kinect: kinect class 209 | :param depth_frame_data: kinect._depth_frame_data 210 | :param camera_space_point: _CameraSpacePoint 211 | :param as_array: returns the data as a numpy array 212 | :return: returns the DepthFrame mapped to camera space 213 | """ 214 | import numpy as np 215 | import ctypes 216 | depth2world_points_type = camera_space_point * np.int(512 * 424) 217 | depth2world_points = ctypes.cast(depth2world_points_type(), ctypes.POINTER(camera_space_point)) 218 | kinect._mapper.MapDepthFrameToCameraSpace(ctypes.c_uint(512 * 424), depth_frame_data, ctypes.c_uint(512 * 424), depth2world_points) 219 | points = ctypes.cast(depth2world_points, ctypes.POINTER(ctypes.c_float)) 220 | data = np.ctypeslib.as_array(points, shape=(424, 512, 3)) 221 | if not as_array: 222 | return depth2world_points 223 | else: 224 | return data 225 | 226 | 227 | # Map Color Frame to World Space 228 | def color_2_world(kinect, depth_frame_data, camera_space_point, as_array=False): 229 | """ 230 | :param kinect: Class for main file 231 | :param depth_frame_data: kinect._depth_frame_data 232 | :param camera_space_point: _CameraSpacePoint structure from PyKinectV2 233 | :param as_array: returns frame as numpy array 234 | :return: returns mapped color frame to camera space 235 | """ 236 | import numpy as np 237 | import ctypes 238 | color2world_points_type = camera_space_point * np.int(1920 * 1080) 239 | color2world_points = ctypes.cast(color2world_points_type(), ctypes.POINTER(camera_space_point)) 240 | kinect._mapper.MapColorFrameToCameraSpace(ctypes.c_uint(512 * 424), depth_frame_data, ctypes.c_uint(1920 * 1080), color2world_points) 241 | pf_csps = ctypes.cast(color2world_points, ctypes.POINTER(ctypes.c_float)) 242 | data = np.ctypeslib.as_array(pf_csps, shape=(1080, 1920, 3)) 243 | if not as_array: 244 | return color2world_points 245 | else: 246 | return data 247 | 248 | 249 | # Map world/camera point to color space 250 | def world_point_2_color(kinect, camera_space_point, point): 251 | """ 252 | :arg: kinect class from main file 253 | :arg: _CameraSpacePoint structure from PyKinectV2 254 | :arg: world point [x, y, z] in meters 255 | :return: colorPoint = [u, v] pixel coordinates 256 | """ 257 | import ctypes 258 | import numpy as np 259 | world_point_data_type = camera_space_point * np.int(1) 260 | world_point = ctypes.cast(world_point_data_type(), ctypes.POINTER(camera_space_point)) 261 | world_point.contents.x = point[0] 262 | world_point.contents.y = point[1] 263 | world_point.contents.z = point[2] 264 | color_point = kinect._mapper.MapCameraPointToColorSpace(world_point.contents) 265 | return [color_point.x, color_point.y] 266 | 267 | 268 | # Map world/camera point to depth space 269 | def world_point_2_depth(kinect, camera_space_point, point): 270 | """ 271 | :arg: kinect class from main file 272 | :arg: _CameraSpacePoint structure from PyKinectV2 273 | :arg: world point [x, y, z] in meters 274 | :return: depthPoint = [u, v] pixel coordinates 275 | """ 276 | import ctypes 277 | import numpy as np 278 | world_point_data_type = camera_space_point * np.int(1) 279 | world_point = ctypes.cast(world_point_data_type(), ctypes.POINTER(camera_space_point)) 280 | world_point.contents.x = point[0] 281 | world_point.contents.y = point[1] 282 | world_point.contents.z = point[2] 283 | depth_point = kinect._mapper.MapCameraPointToDepthSpace(world_point.contents) 284 | return [depth_point.x, depth_point.y] 285 | 286 | 287 | # Map world/camera points to color space 288 | def world_points_2_color(kinect, camera_space_point, points): 289 | """ 290 | :arg: kinect class from main file 291 | :arg: _CameraSpacePoint structure from PyKinectV2 292 | :arg: world points [[x, y, z], [x, y, z], ..... , [x, y, z]] in meters 293 | :return: colorPoints = [[u, v], [u, v], ...., [u, v]] pixel coordinates 294 | """ 295 | import ctypes 296 | import numpy as np 297 | world_point_data_type = camera_space_point * np.int(1) 298 | world_point = ctypes.cast(world_point_data_type(), ctypes.POINTER(camera_space_point)) 299 | color_points = [] 300 | for i in range(len(points)): 301 | world_point.contents.x = points[i, 0] 302 | world_point.contents.y = points[i, 1] 303 | world_point.contents.z = points[i, 2] 304 | color_point = kinect._mapper.MapCameraPointToColorSpace(world_point.contents) 305 | color_points.append([color_point.x, color_point.y]) 306 | return color_points 307 | 308 | 309 | # Map world/camera points to depth space 310 | def world_points_2_depth(kinect, camera_space_point, points): 311 | """ 312 | :arg: kinect class from main file 313 | :arg: _CameraSpacePoint structure from PyKinectV2 314 | :arg: world points [[x, y, z], [x, y, z], ..... , [x, y, z]] in meters 315 | :return: colorPoints = [[u, v], [u, v], ...., [u, v]] pixel coordinates 316 | """ 317 | import ctypes 318 | import numpy as np 319 | world_point_data_type = camera_space_point * np.int(1) 320 | world_point = ctypes.cast(world_point_data_type(), ctypes.POINTER(camera_space_point)) 321 | depth_points = [] 322 | for i in range(len(points)): 323 | world_point.contents.x = points[i, 0] 324 | world_point.contents.y = points[i, 1] 325 | world_point.contents.z = points[i, 2] 326 | depth_point = kinect._mapper.MapCameraPointToDepthSpace(world_point.contents) 327 | depth_points.append([depth_point.x, depth_point.y]) 328 | return depth_points 329 | 330 | 331 | # Map Depth Points to Camera Space 332 | def depth_points_2_world_points(kinect, depth_space_point, depth_points): 333 | """ 334 | 335 | :param kinect: kinect class 336 | :param depth_space_point: _DepthSpacePoint 337 | :param depth_points: depth points as array [[x, y], [x, y], [x, y].... [x, y]] 338 | :return: return camera space points 339 | """ 340 | import ctypes 341 | import numpy as np 342 | depth2world_point_type = depth_space_point * np.int(1) 343 | depth2world_point = ctypes.cast(depth2world_point_type(), ctypes.POINTER(depth_space_point)) 344 | camera_points = np.ndarray(shape=(len(depth_points), 3), dtype=float) 345 | for i, point in enumerate(depth_points): 346 | depth2world_point.contents.x = point[0] 347 | depth2world_point.contents.y = point[1] 348 | world_point = kinect._mapper.MapDepthPointToCameraSpace(depth2world_point.contents, ctypes.c_ushort(512 * 424)) 349 | camera_points[i] = [world_point.x, world_point.y, world_point.z] 350 | return camera_points # meters 351 | 352 | 353 | # Map depth points to world points faster than above method 354 | def depth_points_2_camera_points(kinect, depth_space_point, camera_space_point, xys, as_array=False): 355 | """ 356 | :param kinect: kinect class 357 | :param depth_space_point: _DepthSpacePoint 358 | :param camera_space_point: _CameraSpacePoint 359 | :return camera space points as camera_points[y*512 + x].x/y/z 360 | """ 361 | import ctypes 362 | import numpy as np 363 | length_of_points = len(xys) 364 | depth_points_type = depth_space_point * np.int(length_of_points) 365 | depth_points = ctypes.cast(depth_points_type(), ctypes.POINTER(depth_space_point)) 366 | camera_points_type = camera_space_point * np.int(length_of_points) 367 | camera_points = ctypes.cast(camera_points_type(), ctypes.POINTER(camera_space_point)) 368 | depths = ctypes.POINTER(ctypes.c_ushort) * np.int(length_of_points) 369 | depths = ctypes.cast(depths(), ctypes.POINTER(ctypes.c_ushort)) 370 | for i, point in enumerate(xys): 371 | depth_points[i].x = point[0] 372 | depth_points[i].y = point[1] 373 | kinect._mapper.MapDepthPointsToCameraSpace(ctypes.c_uint(length_of_points), depth_points, ctypes.c_uint(length_of_points), depths, ctypes.c_uint(length_of_points), camera_points) 374 | if as_array: 375 | camera_points = ctypes.cast(camera_points, ctypes.POINTER(ctypes.c_float)) 376 | camera_points = np.ctypeslib.as_array(camera_points, shape=(length_of_points, 3)) 377 | return camera_points 378 | return camera_points 379 | 380 | 381 | # Map a depth point to world point 382 | def depth_point_2_world_point(kinect, depth_space_point, depthPoint): 383 | """ 384 | 385 | :param kinect: kinect class 386 | :param depth_space_point: _DepthSpacePoint from PyKinectV2 387 | :param depthPoint: depth point as array [x, y] 388 | :return: return the camera space point 389 | """ 390 | # Import here for optimization 391 | import numpy as np 392 | import ctypes 393 | depth_point_data_type = depth_space_point * np.int(1) 394 | depth_point = ctypes.cast(depth_point_data_type(), ctypes.POINTER(depth_space_point)) 395 | depth_point.contents.x = depthPoint[0] 396 | depth_point.contents.y = depthPoint[1] 397 | world_point = kinect._mapper.MapDepthPointToCameraSpace(depth_point.contents, ctypes.c_ushort(512*424)) 398 | return [world_point.x, world_point.y, world_point.z] # meters 399 | 400 | 401 | # Map depth point to color point 402 | def depth_point_2_color(kinect, depth_space_point, depthPoint): 403 | """ 404 | :param kinect: kinect class 405 | :param depth_space_point: _DepthSpacePoint from PyKinectV2 406 | :param depthPoint: depth point as array [x, y] 407 | :return: return the mapped color point 408 | """ 409 | import numpy as np 410 | import ctypes 411 | depth_point_type = depth_space_point * np.int(1) 412 | depth_point = ctypes.cast(depth_point_type(), ctypes.POINTER(depth_space_point)) 413 | depth_point.contents.x = depthPoint[0] 414 | depth_point.contents.y = depthPoint[1] 415 | color_point = kinect._mapper.MapDepthPointToColorSpace(depth_point.contents, ctypes.c_ushort(512*424)) 416 | return [color_point.x, color_point.y] 417 | 418 | 419 | # Get Depth Frame to Camera Space Table 420 | def depth_2_world_table(kinect, depth_space_point, as_array=False): 421 | """ 422 | :param kinect: kinect instance 423 | :param depth_space_point: _DepthSpacePoint from PyKinectV2 424 | :param as_array: returns data as numpy array 425 | :return: return the mapped depth to camera space as frame 426 | """ 427 | # Import here for optimization 428 | import numpy as np 429 | import ctypes 430 | table = depth_space_point * np.int(512 * 424) 431 | table = ctypes.cast(table(), ctypes.POINTER(ctypes.c_ulong)) 432 | table = kinect._mapper.GetDepthFrameToCameraSpaceTable(table) 433 | """ Use table[0].x and table[0].y for the first pixel in kinect.get_last_depth_frame array 434 | """ 435 | if as_array: 436 | """ Returns an array as table[0, 0] = x and table[0, 1] = y for the first pixel in depth frame 437 | """ 438 | table = ctypes.cast(table, ctypes.POINTER(ctypes.c_float)) 439 | table = np.ctypeslib.as_array(table, shape=(kinect.depth_frame_desc.Height * kinect.depth_frame_desc.Width, 2)) 440 | return table 441 | 442 | 443 | # Retrieve the depth camera intrinsics from the kinect's mapper 444 | # and write them at: calibrate/IR/intrinsics_retrieved_from_kinect_mapper.json 445 | def intrinsics(kinect, path='calibrate/IR/intrinsics_retrieved_from_kinect_mapper.json', write=False): 446 | """ 447 | :param kinect: kinect instance 448 | :param path: path to save the intrinsics as a json file 449 | :param write: save or not save the intrinsics 450 | :return: returns the intrinsics matrix 451 | """ 452 | import json 453 | intrinsics_matrix = kinect._mapper.GetDepthCameraIntrinsics() 454 | if write: 455 | with open(path, 'w', encoding='utf-8') as json_file: 456 | configs = {"FocalLengthX": intrinsics_matrix.FocalLengthX, "FocalLengthY": intrinsics_matrix.FocalLengthY, 457 | "PrincipalPointX": intrinsics_matrix.PrincipalPointX, "PrincipalPointY": intrinsics_matrix.PrincipalPointY, 458 | "RadialDistortionFourthOrder": intrinsics_matrix.RadialDistortionFourthOrder, "RadialDistortionSecondOrder": intrinsics_matrix.RadialDistortionSecondOrder, 459 | "RadialDistortionSixthOrder": intrinsics_matrix.RadialDistortionSixthOrder} 460 | json.dump(configs, json_file, separators=(',', ':'), sort_keys=True, indent=4) 461 | return intrinsics_matrix 462 | 463 | 464 | if __name__ == '__main__': 465 | """ 466 | Example of some usages 467 | """ 468 | from pykinect2 import PyKinectV2 469 | from pykinect2.PyKinectV2 import * 470 | from pykinect2 import PyKinectRuntime 471 | import cv2 472 | import numpy as np 473 | 474 | kinect = PyKinectRuntime.PyKinectRuntime(PyKinectV2.FrameSourceTypes_Depth | PyKinectV2.FrameSourceTypes_Color) 475 | 476 | while True: 477 | if kinect.has_new_depth_frame(): 478 | color_frame = kinect.get_last_color_frame() 479 | colorImage = color_frame.reshape((kinect.color_frame_desc.Height, kinect.color_frame_desc.Width, 4)).astype(np.uint8) 480 | colorImage = cv2.flip(colorImage, 1) 481 | cv2.imshow('Test Color View', cv2.resize(colorImage, (int(1920 / 2.5), int(1080 / 2.5)))) 482 | depth_frame = kinect.get_last_depth_frame() 483 | depth_img = depth_frame.reshape((kinect.depth_frame_desc.Height, kinect.depth_frame_desc.Width)).astype(np.uint8) 484 | depth_img = cv2.flip(depth_img, 1) 485 | cv2.imshow('Test Depth View', depth_img) 486 | # print(color_point_2_depth_point(kinect, _DepthSpacePoint, kinect._depth_frame_data, [100, 100])) 487 | # print(depth_points_2_world_points(kinect, _DepthSpacePoint, [[100, 150], [200, 250]])) 488 | # print(intrinsics(kinect).FocalLengthX, intrinsics(kinect).FocalLengthY, intrinsics(kinect).PrincipalPointX, intrinsics(kinect).PrincipalPointY) 489 | # print(intrinsics(kinect).RadialDistortionFourthOrder, intrinsics(kinect).RadialDistortionSecondOrder, intrinsics(kinect).RadialDistortionSixthOrder) 490 | # print(world_point_2_depth(kinect, _CameraSpacePoint, [0.250, 0.325, 1])) 491 | # img = depth_2_color_space(kinect, _DepthSpacePoint, kinect._depth_frame_data, show=False, return_aligned_image=True) 492 | depth_2_color_space(kinect, _DepthSpacePoint, kinect._depth_frame_data, show=True) 493 | # img = color_2_depth_space(kinect, _ColorSpacePoint, kinect._depth_frame_data, show=True, return_aligned_image=True) 494 | 495 | # Quit using q 496 | if cv2.waitKey(1) & 0xff == ord('q'): 497 | break 498 | 499 | cv2.destroyAllWindows() 500 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy==1.18.1 2 | pykinect2==0.1.0 3 | --------------------------------------------------------------------------------