├── LICENSE ├── README.md ├── benchmarks ├── body_tracking_speed.py ├── pointcloud_speed.py └── videoacq_speed.py ├── calibration.cpp ├── calibration.h ├── demos ├── body_tracking_demo.py ├── calibration-demo.py ├── cameras_sensors_demo.py ├── map_color_to_depth_and_3d_demo.py ├── map_depth_to_color_and_3d_demo.py └── pointcloud_demo.py ├── environment.yaml ├── kinect.cpp ├── kinect.h ├── pybinder.cpp ├── resources ├── 0002-color_vis.jpg ├── 0002-depth_vis.png ├── 0004-color_vis.jpg ├── 0004-depth_vis.png ├── nfov.png └── wfov.png ├── setup.py ├── utils.cpp └── utils.h /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2020, Juan Terven 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # KinZ for Python A library for Azure Kinect 2 | At the time of this writing, there are no official python bindings for Kinect for Azure. 3 | This library allows to use Azure Kinect directly in Python. 4 | 5 | 6 | ## Installation: 7 | 1. Install the Azure Kinect SDK as described [here](https://docs.microsoft.com/en-us/azure/kinect-dk/sensor-sdk-download) where it says Microsoft installer. For Windows, Download the .exe and follow the steps. For Ubuntu use the *sudo apt install* commands shown in the same page. 8 | 2. For Body tracking functionality (optional), you need an NVIDIA GPU and install CUDA. Download from [here](https://developer.nvidia.com/cuda-downloads?/). 9 | 3. For Body tracking functionality (optional), install the Azure Kinect Body Tracking SDK. For Windows, download the msi installer from [here](https://docs.microsoft.com/en-us/azure/kinect-dk/body-sdk-download). For Ubuntu simply run the *sudo apt install* command provided in the webpage. 10 | 4. Before compiling the code for Matlab, make sure the Kinect works correctly using the viewers provided my Microsoft, e.g. *C:\Program Files\Azure Kinect SDK v1.4.1\tools\k4aviewer.exe* and *C:\Program Files\Azure Kinect Body Tracking SDK\tools\k4abt_simple_3d_viewer.exe*. In Linux just type *k4aviewer* or *k4abt_simple_3d_viewer* in the terminal. 11 | 5. Install the Python requirements (see environment.yaml). 12 | To create a fully functional conda environment run: 13 | ```sh 14 | conda env create --file environment.yaml 15 | ``` 16 | 6. Install the library with: 17 | ```sh 18 | pip install . 19 | ``` 20 | 7. To install KinZ with body tracking functionality run: 21 | ```sh 22 | CFLAGS="-l:libk4abt.so -DBODY" pip install . 23 | ``` 24 | 25 | 26 | ## Demos 27 | Inside demos directory, you'll find demos showing all the features of the library. 28 | Currently, there are only 6 demos: 29 | - **cameras_sensors_demo**: shows how to get color, depth IR, and IMU sensors. 30 | - **calibration-demo.py**: shows how to extract camera calibration values. 31 | - **map_depth_to_color_and_3d_demo.py**: shows how to map sparse points from depth to color and from depth to 3D. 32 | - **map_color_to_depth_and_3d_demo.py**: shows how to map sparse points from color to depth and from color to 3D. 33 | - **pointcloud_demo**: shows how to get the colored pointcloud and visualize it with open3D. 34 | - **body_tracking_demo**: shows how to get body tracking information and visualize it. 35 | 36 | ## Streaming Speed 37 | The streaming speeds in Python are the following: 38 | 39 | | Resolution | FPS Unaligned | FPS Aligned Depth2Color | 40 | |------------|---------------|-------------------------| 41 | | 1280 x 720 | 30 | 30 | 42 | | 1920 x 1080 | 30 | 30 | 43 | | 2560 x 1440 | 30 | 22 | 44 | | 2048 x 1536 | 30 | 30 | 45 | | 3840 x 2160 | 30 | 16 | 46 | | 4096 x 3072 | 15 | 9 | 47 | 48 | 49 | ## Uninstall: 50 | ```sh 51 | pip uninstall kinz 52 | ``` 53 | 54 | ## Coming soon ... 55 | ### Hand pose estimation 56 | Color image keypoints 57 | ![alt text](https://github.com/jrterven/KinZ-Python/blob/master/resources/0002-color_vis.jpg "RGB Image keypoints") 58 | 59 | Depth image keypoints 60 | ![alt text](https://github.com/jrterven/KinZ-Python/blob/master/resources/0002-depth_vis.png "Depth Image keypoints") 61 | 62 | 63 | For more details checkout the [wiki pages](https://github.com/jrterven/KinZ-Python/wiki). 64 | 65 | ## Citation 66 | ``` 67 | @article{terven_cordova_kinz, 68 | title = {KinZ an azure kinect toolkit for Python and Matlab}, 69 | journal = {Science of Computer Programming}, 70 | pages = {102702}, 71 | year = {2021}, 72 | author = {Juan R. Terven and Diana M. Córdova-Esparza}, 73 | keywords = {Azure Kinect, Python, Matlab}} 74 | ``` 75 | [Link to paper](https://www.sciencedirect.com/science/article/pii/S0167642321000952) -------------------------------------------------------------------------------- /benchmarks/body_tracking_speed.py: -------------------------------------------------------------------------------- 1 | """ 2 | Kinect for Azure Color, Depth and Infrared streaming in Python 3 | 4 | Supported resolutions: 5 | 720: 1280 x 720 @ 30 FPS binned depth 6 | 1080: 1920 x 1080 @ 30 FPS binned depth 7 | 1440: 2560 x 1440 @ 30 FPS binned depth 8 | 1535: 2048 x 1536 @ 30 FPS binned depth 9 | 2160: 3840 x 2160 @ 30 FPS binned depth 10 | 3072: 4096 x 3072 @ 15 FPS binned depth 11 | """ 12 | import numpy as np 13 | import cv2 14 | import kinz 15 | 16 | # Create Kinect object and initialize 17 | kin = kinz.Kinect(resolution=720, wfov=False, binned=True, framerate=30, 18 | imu_sensors=False, body_tracking=True) 19 | 20 | # initialize fps counter 21 | t = cv2.getTickCount() 22 | fps_count = 0 23 | fps = 0 24 | 25 | try: 26 | while True: 27 | if fps_count==0: 28 | t = cv2.getTickCount() 29 | 30 | # read kinect frames. If frames available return 1 31 | if kin.get_frames(get_color=True, get_depth=True, get_ir=False, 32 | get_sensors=False, get_body=True, get_body_index=True): 33 | color_data = kin.get_color_data() 34 | depth_data = kin.get_depth_data(align=False) 35 | bodies = kin.get_bodies() 36 | body_index_data = kin.get_body_index_map(returnId=True) 37 | 38 | # extract frames to np arrays 39 | depth_image = np.array(depth_data.buffer, copy=True) 40 | color_image = np.array(color_data.buffer, copy=True) # image is BGRA 41 | color_image = cv2.cvtColor(color_image, cv2.COLOR_BGRA2BGR) # to BGR 42 | body_index_image = np.array(body_index_data.buffer, copy=True) 43 | 44 | # increment frame counter and calculate FPS 45 | fps_count = fps_count + 1 46 | if (fps_count == 30): 47 | t = (cv2.getTickCount() - t)/cv2.getTickFrequency() 48 | fps = 30.0/t 49 | fps_count = 0 50 | print(f"FPS:{fps:.2f}") 51 | finally: 52 | print("Closing Kinect") 53 | kin.close() 54 | 55 | -------------------------------------------------------------------------------- /benchmarks/pointcloud_speed.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | import kinz 4 | 5 | # Create Kinect object and initialize 6 | kin = kinz.Kinect(resolution=720, wfov=True, binned=False, framerate=30) 7 | 8 | # initialize fps counter 9 | t = cv2.getTickCount() 10 | fps_count = 0 11 | fps = 0 12 | 13 | try: 14 | count = 0 15 | while True: 16 | if fps_count==0: 17 | t = cv2.getTickCount() 18 | 19 | # read kinect frames. If frames available return 1 20 | if kin.get_frames(get_color=True, get_depth=True, get_ir=False): 21 | count += 1 22 | 23 | # Get the pointcloud data and convert to Numpy (512 x 512 x 3, int16) 24 | pointcloud_data = kin.get_pointcloud() 25 | pointcloud_np = np.array(pointcloud_data, copy = True) 26 | 27 | # Get the pointcloud color and conver to Numpy (512 x 512 x 3, uint8) 28 | pointcloudcolor_data = kin.get_pointcloud_color() 29 | pointcloudcolor_np = np.array(pointcloudcolor_data, copy = True) 30 | 31 | # increment frame counter and calculate FPS 32 | fps_count = fps_count + 1 33 | if (fps_count == 30): 34 | t = (cv2.getTickCount() - t)/cv2.getTickFrequency() 35 | fps = 30.0/t 36 | fps_count = 0 37 | print(f"FPS:{fps:.2f}") 38 | 39 | finally: 40 | print("Closing Kinect") 41 | kin.close() 42 | 43 | -------------------------------------------------------------------------------- /benchmarks/videoacq_speed.py: -------------------------------------------------------------------------------- 1 | """ 2 | Kinect for Azure Color, Depth and Infrared streaming in Python 3 | 4 | Supported resolutions: 5 | 720: 1280 x 720 @ 30 FPS binned depth 6 | 1080: 1920 x 1080 @ 30 FPS binned depth 7 | 1440: 2560 x 1440 @ 30 FPS binned depth 8 | 1535: 2048 x 1536 @ 30 FPS binned depth 9 | 2160: 3840 x 2160 @ 30 FPS binned depth 10 | 3072: 4096 x 3072 @ 15 FPS binned depth 11 | """ 12 | import numpy as np 13 | import cv2 14 | import kinz 15 | 16 | # Create Kinect object and initialize 17 | kin = kinz.Kinect(resolution=4096, wfov=True, binned=False, framerate=30, imu_sensors=False) 18 | 19 | # Get depth aligned with color? 20 | align_frames = False 21 | 22 | # initialize fps counter 23 | t = cv2.getTickCount() 24 | fps_count = 0 25 | fps = 0 26 | 27 | while True: 28 | if fps_count==0: 29 | t = cv2.getTickCount() 30 | 31 | # read kinect frames. If frames available return 1 32 | if kin.get_frames(get_color=True, get_depth=True, get_ir=True, get_sensors=True): 33 | color_data = kin.get_color_data() 34 | depth_data = kin.get_depth_data(align=align_frames) 35 | ir_data = kin.get_ir_data() 36 | 37 | # extract frames to np arrays 38 | depth_image = np.array(depth_data.buffer, copy = True) 39 | color_image = np.array(color_data.buffer, copy = True) # image is BGRA 40 | color_image = cv2.cvtColor(color_image, cv2.COLOR_BGRA2BGR) # to BGR 41 | ir_image = np.array(ir_data.buffer, copy = True) 42 | 43 | 44 | # increment frame counter and calculate FPS 45 | fps_count = fps_count + 1 46 | if (fps_count == 30): 47 | t = (cv2.getTickCount() - t)/cv2.getTickFrequency() 48 | fps = 30.0/t 49 | fps_count = 0 50 | print(f"FPS:{fps:.2f}") 51 | 52 | kin.close() # close Kinect 53 | cv2.destroyAllWindows() 54 | -------------------------------------------------------------------------------- /calibration.cpp: -------------------------------------------------------------------------------- 1 | #include "calibration.h" 2 | 3 | py::list Calibration::get_size() { 4 | py::list res; 5 | res.append(width); 6 | res.append(height); 7 | return res; 8 | } 9 | 10 | py::array_t Calibration::get_intrinsics_matrix(bool extended=false) { 11 | int msize = 3; 12 | if(extended) 13 | msize = 4; 14 | size_t size = msize*msize; 15 | 16 | double *intrinsics = new double[size]; 17 | for(unsigned int i=0; i(f); 39 | delete[] intrinsics; 40 | }); 41 | 42 | return py::array_t( 43 | {msize, msize}, // shape 44 | {msize*8, 8}, // C-style contiguous strides for double 45 | intrinsics, // the data pointer 46 | free_when_done); // numpy array references this parent 47 | } 48 | 49 | py::array_t Calibration::get_distortion_params() { 50 | constexpr size_t size = 1*8; 51 | double *dist_params = new double[size]; 52 | dist_params[0] = k1; 53 | dist_params[1] = k2; 54 | dist_params[2] = p1; 55 | dist_params[3] = p2; 56 | dist_params[4] = k3; 57 | dist_params[5] = k4; 58 | dist_params[6] = k5; 59 | dist_params[7] = k6; 60 | 61 | // Python object that frees the allocated memory when destroyed 62 | py::capsule free_when_done(dist_params, [](void *f) { 63 | double *dist_params = reinterpret_cast(f); 64 | delete[] dist_params; 65 | }); 66 | 67 | return py::array_t( 68 | {1, 8}, // shape 69 | {1*8, 8}, // strides 70 | dist_params, // data pointer 71 | free_when_done); // numpy array references this parent 72 | } 73 | 74 | py::array_t Calibration::get_rotation_matrix() { 75 | constexpr size_t size = 3*3; 76 | double *rot = new double[size]; 77 | for(unsigned int i=0; i(f); 83 | delete[] rot; 84 | }); 85 | 86 | return py::array_t( 87 | {3, 3}, // shape 88 | {3*8, 8}, // strides 89 | rot, // data pointer 90 | free_when_done); // numpy array references this parent 91 | } 92 | 93 | py::array_t Calibration::get_translation_vector() { 94 | constexpr size_t size = 3*1; 95 | double *t = new double[size]; 96 | for(unsigned int i=0; i(f); 102 | delete[] t; 103 | }); 104 | 105 | return py::array_t( 106 | {3, 1}, // shape 107 | {1*8, 8}, // strides 108 | t, // data pointer 109 | free_when_done); // numpy array references this parent 110 | } 111 | 112 | py::array_t Calibration::get_camera_pose() { 113 | constexpr size_t size = 4*4; 114 | double *pose = new double[size]; 115 | for(unsigned int i=0; i(f); 127 | delete[] pose; 128 | }); 129 | 130 | return py::array_t( 131 | {4, 4}, // shape 132 | {4*8, 8}, // strides 133 | pose, // data pointer 134 | free_when_done); // numpy array references this parent 135 | } 136 | -------------------------------------------------------------------------------- /calibration.h: -------------------------------------------------------------------------------- 1 | #ifndef CALIBRATION_H 2 | #define CALIBRATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace py = pybind11; 9 | 10 | class Calibration { 11 | public: 12 | int width, height; 13 | float cx, cy; 14 | float fx, fy; 15 | float k1, k2, k3, k4, k5, k6; 16 | float p1, p2; 17 | float codx, cody; 18 | float metric_radius; 19 | float rotation[9]; 20 | float translation[3]; 21 | 22 | py::list get_size(); 23 | py::array_t get_intrinsics_matrix(bool); 24 | py::array_t get_distortion_params(); 25 | py::array_t get_rotation_matrix(); 26 | py::array_t get_translation_vector(); 27 | py::array_t get_camera_pose(); 28 | }; 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /demos/body_tracking_demo.py: -------------------------------------------------------------------------------- 1 | """ 2 | Kinect for Azure Color, Depth and Infrared streaming in Python 3 | 4 | Supported resolutions: 5 | 720: 1280 x 720 @ 30 FPS binned depth 6 | 1080: 1920 x 1080 @ 30 FPS binned depth 7 | 1440: 2560 x 1440 @ 30 FPS binned depth 8 | 1535: 2048 x 1536 @ 30 FPS binned depth 9 | 2160: 3840 x 2160 @ 30 FPS binned depth 10 | 3072: 4096 x 3072 @ 15 FPS binned depth 11 | """ 12 | import numpy as np 13 | import cv2 14 | import kinz 15 | import cmapy 16 | 17 | 18 | def main(): 19 | # Create Kinect object and initialize 20 | kin = kinz.Kinect(resolution=1080, wfov=True, binned=True, framerate=30, 21 | imu_sensors=False, body_tracking=True) 22 | 23 | # Get depth aligned with color? 24 | align_frames = False 25 | image_scale = 0.5 # visualized image scale 26 | 27 | # initialize fps counter 28 | t = cv2.getTickCount() 29 | fps_count = 0 30 | fps = 0 31 | 32 | while True: 33 | if fps_count==0: 34 | t = cv2.getTickCount() 35 | 36 | # read kinect frames. If frames available return 1 37 | if kin.get_frames(get_color=True, get_depth=True, get_ir=False, 38 | get_sensors=False, get_body=True, get_body_index=True, 39 | align_depth=align_frames): 40 | 41 | color_data = kin.get_color_data() 42 | depth_data = kin.get_depth_data() 43 | bodies = kin.get_bodies() 44 | body_index_data = kin.get_body_index_map(returnId=True, inColor=False) 45 | 46 | print("bodies:", bodies) 47 | 48 | # extract frames to np arrays 49 | depth_image = np.array(depth_data.buffer, copy=True) 50 | color_image = np.array(color_data.buffer, copy=True) # image is BGRA 51 | color_image = cv2.cvtColor(color_image, cv2.COLOR_BGRA2BGR) # to BGR 52 | body_index_image = np.array(body_index_data.buffer, copy=True) 53 | print(body_index_image.shape) 54 | 55 | # Apply colormap on depth image (image must be converted to 8-bit per pixel first) 56 | depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET) 57 | 58 | # Apply colormap on body index image 59 | body_index_image = cv2.applyColorMap(body_index_image*10, cmapy.cmap('tab20')) 60 | 61 | # Draw bodies on the RGB image 62 | draw_keypoints(color_image, bodies, img_type='rgb') 63 | 64 | # Draw bodies on the depth image 65 | draw_keypoints(depth_colormap, bodies, img_type='depth') 66 | 67 | # Resize images 68 | if align_frames: 69 | depth_colormap = cv2.resize(depth_colormap, None, fx=image_scale, fy=image_scale) 70 | body_index_image = cv2.resize(body_index_image, None, fx=image_scale, fy=image_scale) 71 | 72 | color_small = cv2.resize(color_image, None, fx=image_scale, fy=image_scale) 73 | size = color_small.shape[0:2] 74 | cv2.putText(color_small, "{0:.2f}-FPS".format(fps), (20, size[0]-20), cv2.FONT_HERSHEY_COMPLEX, 0.8, (0, 0, 255), 2) 75 | 76 | cv2.imshow('Depth', depth_colormap) 77 | cv2.imshow('Color', color_small) 78 | cv2.imshow('Body index', body_index_image) 79 | 80 | k = cv2.waitKey(1) & 0xFF 81 | if k == 27: 82 | break 83 | elif k == ord('s'): 84 | cv2.imwrite("color.jpg", color_image) 85 | print("Image saved") 86 | 87 | # increment frame counter and calculate FPS 88 | fps_count = fps_count + 1 89 | if (fps_count == 30): 90 | t = (cv2.getTickCount() - t)/cv2.getTickFrequency() 91 | fps = 30.0/t 92 | fps_count = 0 93 | 94 | kin.close() # close Kinect 95 | cv2.destroyAllWindows() 96 | 97 | 98 | def draw_keypoints(img, bodies, img_type='rgb', size=5): 99 | colors = [ 100 | [255, 10, 0], [255, 85, 0], [255, 170, 0], [255, 255, 0], [170, 255, 0], 101 | [85, 255, 0], [0, 255, 0], [0, 255, 85], [0, 255, 170], [0, 255, 255], 102 | [0, 170, 255], [0, 85, 255], [0, 0, 255], [85, 0, 255], [170, 0, 255], 103 | [255, 0, 255], [255, 0, 170], [255, 0, 85], [255, 0, 0], [170, 0, 255], 104 | [0, 170, 255], [0, 85, 255], [0, 0, 255], [85, 0, 255], [170, 0, 255], 105 | [0, 170, 255], [0, 85, 255], [0, 0, 255], [85, 0, 255], [170, 0, 255]] 106 | 107 | body_parts = ["Pelvis", "Spine_navel", "Spine_chest", 108 | "Neck", "Clavicle_left", "Shoulder_left", 109 | "Elbow_left", "Wrist_left", "Hand_left", 110 | "Handtip_left", "Thumb_left", "Clavicle_right", 111 | "Shoulder_right", "Elbow_right", "Wrist_right", 112 | "Hand_right", "Handtip_right", "Thumb_right", 113 | "Hip_left", "Knee_left", "Ankle_left", 114 | "Foot_left", "Hip_right", "Knee_right", 115 | "Ankle_right", "Foot_right", "Head", 116 | "Nose", "Eye_left", "Ear_left", 117 | "Eye_right", "Ear_right"] 118 | body_limbs = [("Head", "Neck"), ("Neck", "Spine_chest"), 119 | ("Spine_chest", "Spine_navel"), ("Spine_navel", "Pelvis"), 120 | ("Neck", "Clavicle_left"), ("Neck", "Clavicle_right"), 121 | ("Clavicle_left", "Shoulder_left"), ("Clavicle_right", "Shoulder_right"), 122 | ("Shoulder_left", "Elbow_left"), ("Shoulder_right", "Elbow_right"), 123 | ("Elbow_left", "Wrist_left"), ("Elbow_right", "Wrist_right"), 124 | ("Wrist_left", "Thumb_left"), ("Wrist_right", "Thumb_right"), 125 | ("Wrist_left", "Hand_left"), ("Wrist_right", "Hand_right"), 126 | ("Hand_left", "Handtip_left"), ("Hand_right", "Handtip_right"), 127 | ("Pelvis", "Hip_left"), ("Pelvis", "Hip_right"), 128 | ("Hip_left", "Knee_left"), ("Hip_right", "Knee_right"), 129 | ("Knee_left", "Ankle_left"), ("Knee_right", "Ankle_right"), 130 | ("Ankle_left", "Foot_left"), ("Ankle_right", "Foot_right")] 131 | #color = [255, 255, 255] 132 | if img_type == 'rgb': 133 | p_type = 'position2d-rgb' 134 | else: 135 | p_type = 'position2d-depth' 136 | for body in bodies: 137 | # Get the body ID to select a unique color 138 | body_id = body['id'] 139 | color = colors[body_id] 140 | 141 | # for each body part 142 | for part in body_parts: 143 | # Get the prediction confidence 144 | confidence = body[part]['confidence'] 145 | 146 | # if the confidence is not None or Low, draw the body keypoint 147 | if confidence > 1: 148 | x = body[part][p_type]['x'] 149 | y = body[part][p_type]['y'] 150 | cv2.circle(img, (x, y), size, color, -1) 151 | 152 | # Now draw the body limbs 153 | for limb in body_limbs: 154 | conf1 = body[limb[0]]['confidence'] 155 | conf2 = body[limb[1]]['confidence'] 156 | # if the confidence is not None or Low, draw the body keypoint 157 | if conf1 >1 and conf2 > 1 : 158 | xy1 = (body[limb[0]][p_type]['x'],body[limb[0]][p_type]['y']) 159 | xy2 = (body[limb[1]][p_type]['x'],body[limb[1]][p_type]['y']) 160 | cv2.line(img, xy1, xy2, color, size-1) 161 | 162 | 163 | if __name__ == "__main__": 164 | main() -------------------------------------------------------------------------------- /demos/calibration-demo.py: -------------------------------------------------------------------------------- 1 | """ 2 | Calibration demo 3 | 4 | Shows how to extract the instrinsic and extrinsic parameters. 5 | """ 6 | import numpy as np 7 | import cv2 8 | import kinz 9 | 10 | # Create Kinect object and initialize 11 | kin = kinz.Kinect(resolution=1080, wfov=True, binned=True) 12 | 13 | # get calibration objects 14 | depth_calib = kin.get_depth_calibration() 15 | color_calib = kin.get_color_calibration() 16 | 17 | # extract calibration parameters 18 | depth_size = depth_calib.get_size() # image size 19 | color_size = color_calib.get_size() 20 | 21 | # Intrinsics is a 3x3 if extended=False or 4x4 if extended=True 22 | depth_intrinsics = depth_calib.get_intrinsics_matrix(extended=False) 23 | color_intrinsics = color_calib.get_intrinsics_matrix(extended=False) 24 | 25 | # Distortion is a 8x1 numpy vector: k1,k2,p1,p2,k3,k4,k5,k6 26 | depth_dist = depth_calib.get_distortion_params() 27 | color_dist = color_calib.get_distortion_params() 28 | 29 | # Rotation is a 3x3 Rotation matrix wrt depth camera 30 | depth_rot = depth_calib.get_rotation_matrix() 31 | color_rot = color_calib.get_rotation_matrix() 32 | 33 | # Translation is a 3x1 translation vector wrt depth camera 34 | depth_trans = depth_calib.get_translation_vector() 35 | color_trans = color_calib.get_translation_vector() 36 | 37 | # Pose is a 4x4 pose matrix wrt depth camera 38 | depth_pose = depth_calib.get_camera_pose() 39 | color_pose = color_calib.get_camera_pose() 40 | 41 | print("----- DEPTH CALIBRATION ------") 42 | print("depth_size:", type(depth_size), depth_size) 43 | print("depth_intrinsics:", type(depth_intrinsics)) 44 | print(depth_intrinsics) 45 | print("depth_dist:", type(depth_dist), depth_dist) 46 | print("depth_rot:", type(depth_rot)) 47 | print(depth_rot) 48 | print("depth_trans:", type(depth_trans)) 49 | print(depth_trans) 50 | print("depth_pose:", type(depth_pose)) 51 | print(depth_pose) 52 | print("--------------------------------") 53 | 54 | print("----- COLOR CALIBRATION ------") 55 | print("color_size:", type(color_size), color_size) 56 | print("color_intrinsics:", type(color_intrinsics)) 57 | print(color_intrinsics) 58 | print("color_dist:", type(color_dist), color_dist) 59 | print("color_rot:", type(color_rot)) 60 | print(color_rot) 61 | print("color_trans:", type(color_trans)) 62 | print(color_trans) 63 | print("color_pose:", type(color_pose)) 64 | print(color_pose) 65 | print("--------------------------------") 66 | 67 | kin.close() -------------------------------------------------------------------------------- /demos/cameras_sensors_demo.py: -------------------------------------------------------------------------------- 1 | """ 2 | Kinect for Azure Color, Depth and Infrared streaming in Python 3 | 4 | Supported resolutions: 5 | 720: 1280 x 720 @ 30 FPS binned depth 6 | 1080: 1920 x 1080 @ 30 FPS binned depth 7 | 1440: 2560 x 1440 @ 30 FPS binned depth 8 | 1535: 2048 x 1536 @ 30 FPS binned depth 9 | 2160: 3840 x 2160 @ 30 FPS binned depth 10 | 3072: 4096 x 3072 @ 15 FPS binned depth 11 | """ 12 | import numpy as np 13 | import cv2 14 | import kinz 15 | 16 | # Create Kinect object and initialize 17 | kin = kinz.Kinect(resolution=720, wfov=True, binned=True, framerate=30, imu_sensors=True) 18 | 19 | # Get depth aligned with color? 20 | align_frames = False 21 | image_scale = 0.5 # visualized image scale 22 | 23 | # initialize fps counter 24 | t = cv2.getTickCount() 25 | fps_count = 0 26 | fps = 0 27 | 28 | while True: 29 | if fps_count==0: 30 | t = cv2.getTickCount() 31 | 32 | # read kinect frames. If frames available return 1 33 | if kin.get_frames(get_color=True, get_depth=True, get_ir=True, get_sensors=True, 34 | align_depth=False): 35 | color_data = kin.get_color_data() 36 | depth_data = kin.get_depth_data() 37 | ir_data = kin.get_ir_data() 38 | sensor_data = kin.get_sensor_data() 39 | 40 | # extract frames to np arrays 41 | depth_image = np.array(depth_data.buffer, copy = True) 42 | color_image = np.array(color_data.buffer, copy = True) # image is BGRA 43 | color_image = cv2.cvtColor(color_image, cv2.COLOR_BGRA2BGR) # to BGR 44 | ir_image = np.array(ir_data.buffer, copy = True) 45 | 46 | print('Depth shape, type, timestamp:', depth_image.shape, depth_image.dtype, depth_data.timestamp_nsec) 47 | print('Color shape, type, timestamp:', color_image.shape, color_image.dtype, color_data.timestamp_nsec) 48 | print('IR shape, type, timestamp:', ir_image.shape, ir_image.dtype, ir_data.timestamp_nsec) 49 | print('Depth range values:', np.amin(depth_image), np.amax(depth_image)) 50 | print('IR range values:', np.amin(ir_image), np.amax(ir_image)) 51 | print('Temperature: {:.1f} Celsius'.format(sensor_data.temperature)) 52 | print('AccX, AccY, AccZ, AccTime = {:.3f}, {:.3f}, {:.3f}, {:d}'.format(sensor_data.acc_x, 53 | sensor_data.acc_y, 54 | sensor_data.acc_z, 55 | sensor_data.acc_timestamp_usec)) 56 | print('GyroX, GyroY, GyroZ. GyroTime = {:.3f}, {:.3f}, {:.3f}, {:d}'.format(sensor_data.gyro_x, 57 | sensor_data.gyro_y, 58 | sensor_data.gyro_z, 59 | sensor_data.gyro_timestamp_usec)) 60 | 61 | # Apply colormap on depth image (image must be converted to 8-bit per pixel first) 62 | depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET) 63 | 64 | # Reescale IR image values 65 | ir_image = cv2.convertScaleAbs(ir_image, alpha=0.04) 66 | 67 | # Resize images 68 | if align_frames: 69 | depth_colormap = cv2.resize(depth_colormap, None, fx=image_scale, fy=image_scale) 70 | 71 | color_small = cv2.resize(color_image, None, fx=image_scale, fy=image_scale) 72 | size = color_small.shape[0:2] 73 | cv2.putText(color_small, "{0:.2f}-FPS".format(fps), (20, size[0]-20), cv2.FONT_HERSHEY_COMPLEX, 0.8, (0, 0, 255), 2) 74 | 75 | cv2.imshow('Depth', depth_colormap) 76 | cv2.imshow('Color', color_small) 77 | cv2.imshow('IR', ir_image) 78 | 79 | k = cv2.waitKey(1) & 0xFF 80 | if k == 27: 81 | break 82 | elif k == ord('s'): 83 | cv2.imwrite("color.jpg", color_image) 84 | cv2.imwrite("depth.png", depth_image) 85 | print("Image saved") 86 | 87 | # increment frame counter and calculate FPS 88 | fps_count = fps_count + 1 89 | if (fps_count == 30): 90 | t = (cv2.getTickCount() - t)/cv2.getTickFrequency() 91 | fps = 30.0/t 92 | fps_count = 0 93 | 94 | kin.close() # close Kinect 95 | cv2.destroyAllWindows() 96 | -------------------------------------------------------------------------------- /demos/map_color_to_depth_and_3d_demo.py: -------------------------------------------------------------------------------- 1 | """ 2 | Mapping from color to depth and 3D demo 3 | 4 | How to use: 5 | - Clic on the color image to show the corresponding point on depth image 6 | and the 3D coordinates 7 | """ 8 | import numpy as np 9 | import cv2 10 | import kinz 11 | 12 | color_coords = [] 13 | 14 | def mouse_event(event, x, y, flags, param): 15 | global color_coords 16 | if event == cv2.EVENT_LBUTTONDOWN: 17 | color_coords.append([x, y]) 18 | 19 | def main(): 20 | global color_coords 21 | # Create Kinect object and initialize 22 | kin = kinz.Kinect(resolution=720, wfov=True, binned=True) 23 | 24 | depth_window_name = 'Mapped coordinates to Depth image' 25 | color_window_name = 'Click on the Color image' 26 | cv2.namedWindow(color_window_name, cv2.WINDOW_AUTOSIZE) 27 | cv2.namedWindow(depth_window_name, cv2.WINDOW_AUTOSIZE) 28 | cv2.setMouseCallback(color_window_name, mouse_event) 29 | 30 | points_3d = [] 31 | prev_points_3d = [] 32 | 33 | while True: 34 | if kin.get_frames(get_color=True, get_depth=True, get_ir=False, 35 | align_depth=True): 36 | # get buffer data from Kinect 37 | color_data = kin.get_color_data() 38 | depth_data = kin.get_depth_data() 39 | ir_data = kin.get_ir_data() 40 | sensor_data = kin.get_sensor_data() 41 | 42 | # convert buffers to numpy arrays 43 | depth_image = np.array(depth_data.buffer, copy = True) 44 | color_image = np.array(color_data.buffer, copy = True) 45 | color_image = cv2.cvtColor(color_image, cv2.COLOR_BGRA2BGR) 46 | 47 | # Apply colormap on depth image 48 | depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs( 49 | depth_image, alpha=0.03), cv2.COLORMAP_JET) 50 | 51 | print("Color coords:", color_coords) 52 | # Project color image points to depth image 53 | # color_coords is a list of value pairs [[xc1,yc1], [xc2,yc2], ...] 54 | # depth_coords have the same format as color_coords 55 | # depth_coords return -1 if there is no depth information 56 | # at the desire location, 57 | # e.g. if at [x2, y2] the depth is 0 depth_coords = [[xd1, yd1], [-1, -1]] 58 | # we select -1 because there can not be negative pixel coordinates 59 | depth_coords = kin.map_coords_color_to_depth(color_coords) 60 | print("Depth coords:", depth_coords) 61 | 62 | # Backproject color image points to 3D depth camera space 63 | # points_3d is a list of triplets of X, Y, Z points in 64 | # the depth camera reference or color camera reference 65 | # if color_coords is [[xc1,yc1], [xc2,yc2], ...] 66 | # points_3d is [[X1, Y1, Z1], [X2, Y2, Z2], ...] 67 | # points_3d contains [0, 0, 0] if for desired color coordinates 68 | # there is not depth available. 69 | # E.g. if at [x2, y2] the depth is 0, points_3d = [[X1, Y1, Z1], [0, 0, 0]] 70 | # we select 0 because depth = 0 means no depth data. 71 | points_3d = kin.map_coords_color_to_3D(color_coords, 72 | depth_reference=False) 73 | 74 | #if points_3d != prev_points_3d: 75 | print("3D points:", points_3d) 76 | 77 | # Project 3D points image points to depth image and color image 78 | # points_3d is a list of value triplets [[X1, Y1, Z1], [X2, Y2, Z2], ...] 79 | # depth_coords2 and color_coords2 have the same format: [[x1, y1], [x2, y2], ...] 80 | # depth_coords return -1 if there is no depth information 81 | # at the desire location, 82 | # e.g. if at [x2, y2] the depth is 0 depth_coords = [[xd1, yd1], [-1, -1]] 83 | # we select -1 because there can not be negative pixel coordinates 84 | depth_coords2 = kin.map_coords_3d_to_depth(points_3d, depth_reference=False) 85 | color_coords2 = kin.map_coords_3d_to_color(points_3d, depth_reference=False) 86 | print("Color coords2:", color_coords2) 87 | print("Depth coords2:", depth_coords2) 88 | 89 | ## Visualization 90 | # Draw color image points 91 | for p in color_coords: 92 | cv2.circle(color_image,(p[0], p[1]), 8, (0,0,255), -1) 93 | 94 | # Draw depth points 95 | for p in depth_coords: 96 | cv2.circle(depth_colormap,(p[0], p[1]), 5, (0,0,255), -1) 97 | 98 | cv2.imshow(depth_window_name, depth_colormap) 99 | cv2.imshow(color_window_name, color_image) 100 | 101 | prev_points_3d = points_3d 102 | 103 | k = cv2.waitKey(10) & 0xFF 104 | if k == ord('c'): 105 | color_coords = [] 106 | if k ==27: 107 | break 108 | 109 | kin.close() # close Kinect 110 | cv2.destroyAllWindows() 111 | 112 | if __name__ == '__main__': 113 | main() -------------------------------------------------------------------------------- /demos/map_depth_to_color_and_3d_demo.py: -------------------------------------------------------------------------------- 1 | """ 2 | Mapping from depth to color demo 3 | 4 | How to use: 5 | - Clic on the depth image to show the corresponding point on color image 6 | and the 3D coordinates 7 | """ 8 | import numpy as np 9 | import cv2 10 | import kinz 11 | 12 | depth_points = [] 13 | 14 | def mouse_event(event, x, y, flags, param): 15 | global depth_points 16 | if event == cv2.EVENT_LBUTTONDOWN: 17 | depth_points.append((x, y)) 18 | 19 | def main(): 20 | global depth_points 21 | # Create Kinect object and initialize 22 | kin = kinz.Kinect(resolution=720, wfov=True, binned=True) 23 | 24 | depth_window_name = 'Click on the Depth image' 25 | color_window_name = 'Mapped coordinates in Color' 26 | cv2.namedWindow(color_window_name, cv2.WINDOW_AUTOSIZE) 27 | cv2.namedWindow(depth_window_name, cv2.WINDOW_AUTOSIZE) 28 | cv2.setMouseCallback(depth_window_name, mouse_event) 29 | 30 | points_3d = [] 31 | prev_points_3d = [] 32 | 33 | while True: 34 | if kin.get_frames(get_color=True, get_depth=True, get_ir=False, 35 | align_depth=True): 36 | color_data = kin.get_color_data() 37 | depth_data = kin.get_depth_data() 38 | 39 | depth_image = np.array(depth_data.buffer, copy = True) 40 | color_image = np.array(color_data.buffer, copy = True) 41 | color_image = cv2.cvtColor(color_image, cv2.COLOR_BGRA2BGR) 42 | 43 | # Project depth image points to color image points 44 | # depth_points is a list of value pairs [[xd1,yd1], [xd2,yd2], ...] 45 | # color_points have the same format as depth_points 46 | # map_coords_depth_to_color return [-1, -1] if there is no depth information 47 | # at the desire location, or the transformation failed. 48 | color_points = kin.map_coords_depth_to_color(depth_points) 49 | 50 | # Backproject depth image points to 3D depth camera space 51 | # points_3d is a list of triplets of X, Y, Z points in 52 | # the depth camera reference 53 | # if depth_coords is [[x1,y1], [x2,y2], ...] 54 | # points_3d is [[X1, Y1, Z1], [X2, Y2, Z2], ...] 55 | # points_3d contains [0, 0, 0] if for desired depth coordinates 56 | # there is not depth available. 57 | # E.g. if at [x2, y2] the depth is 0, points_3d = [[X1, Y1, Z1], [0, 0, 0]] 58 | # we select 0 because depth = 0 means no depth data. 59 | points_3d = kin.map_coords_depth_to_3D(depth_points) 60 | 61 | if points_3d != prev_points_3d: 62 | print("3D point:", points_3d) 63 | 64 | # Draw mapped points in color image 65 | for p in color_points: 66 | cv2.circle(color_image,(p[0], p[1]), 8, (0,0,255), -1) 67 | 68 | # Apply colormap on depth image for visualization 69 | depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET) 70 | 71 | # Draw depth points 72 | for p in depth_points: 73 | cv2.circle(depth_colormap,(p[0], p[1]), 5, (0,0,255), -1) 74 | 75 | # Resize color image for visualization purposes 76 | color_small = cv2.resize(color_image, None, fx=0.5, fy=0.5) 77 | 78 | cv2.imshow(depth_window_name, depth_colormap) 79 | cv2.imshow(color_window_name, color_small) 80 | prev_points_3d = points_3d 81 | 82 | k = cv2.waitKey(1) & 0xFF 83 | if k == ord('c'): 84 | depth_points = [] 85 | if k ==27: 86 | break 87 | 88 | kin.close() # close Kinect 89 | cv2.destroyAllWindows() 90 | 91 | if __name__ == '__main__': 92 | main() -------------------------------------------------------------------------------- /demos/pointcloud_demo.py: -------------------------------------------------------------------------------- 1 | """ 2 | Kinect for Azure Pointcloud demo 3 | 4 | This demo uses the Open3D library to visualize the pointcloud. 5 | It saves a Pointcloud in a PLY file. 6 | """ 7 | from datetime import datetime 8 | import numpy as np 9 | import os 10 | import kinz 11 | import open3d as o3d 12 | 13 | # Output directory to save the pointcloud 14 | POINTCLOUD_OUTPUT_DIR = "." 15 | 16 | # Create Kinect object and initialize 17 | kin = kinz.Kinect(resolution=1080, wfov=True, binned=True, framerate=30) 18 | 19 | try: 20 | count = 0 21 | geometry_added = False 22 | vis = o3d.visualization.Visualizer() 23 | vis.create_window("PointCloud") 24 | pcd = o3d.geometry.PointCloud() 25 | 26 | while True: 27 | dt0 = datetime.now() 28 | 29 | # read kinect frames. If frames available return 1 30 | if kin.get_frames(get_color=True, get_depth=True, get_ir=False, 31 | align_depth=False): 32 | count += 1 33 | 34 | # Get the pointcloud data and convert to Numpy (512 x 512 x 3, int16) 35 | pointcloud_data = kin.get_pointcloud() 36 | pointcloud_np = np.array(pointcloud_data, copy = True) 37 | 38 | # Get the pointcloud color and conver to Numpy (512 x 512 x 3, uint8) 39 | pointcloudcolor_data = kin.get_pointcloud_color() 40 | pointcloudcolor_np = np.array(pointcloudcolor_data, copy = True) 41 | 42 | # Every 10 pointclouds, print the statistics 43 | if count % 10 == 0: 44 | print('Pointcloud shape and type:', pointcloud_np.shape, pointcloud_np.dtype) 45 | print('PC min:{:d}, max:{:d}'.format(np.amin(pointcloud_np), np.amax(pointcloud_np))) 46 | 47 | print('Pointcloudcolor shape and type:', pointcloudcolor_np.shape, pointcloudcolor_np.dtype) 48 | 49 | # Prepare the Pointcloud data for Open3D pointcloud data 50 | xyz = np.zeros((pointcloud_np.shape[0]*pointcloud_np.shape[1], 3), dtype=np.float32) 51 | x = pointcloud_np[:, :, 0] # extract the X, Y, Z matrices 52 | y = pointcloud_np[:, :, 1] 53 | z = pointcloud_np[:, :, 2] 54 | xyz[:, 0] = np.reshape(x, -1) # convert to vectors 55 | xyz[:, 1] = np.reshape(y, -1) 56 | xyz[:, 2] = np.reshape(z, -1) 57 | pcd.points = o3d.utility.Vector3dVector(xyz) 58 | 59 | # Prepare the Pointcloud colors for Open3D pointcloud colors 60 | colors = np.zeros((pointcloudcolor_np.shape[0]*pointcloudcolor_np.shape[1], 3), dtype=np.float32) 61 | blue = pointcloudcolor_np[:, :, 0] # extract the blue, green , and red components 62 | green = pointcloudcolor_np[:, :, 1] 63 | red = pointcloudcolor_np[:, :, 2] 64 | colors[:, 0] = np.reshape(red, -1) / 255.0 # convert to vectors and scale to 0-1 65 | colors[:, 1] = np.reshape(green, -1) / 255.0 66 | colors[:, 2] = np.reshape(blue, -1) / 255.0 67 | pcd.colors = o3d.utility.Vector3dVector(colors) 68 | 69 | # Render the pointcloud 70 | if not geometry_added: 71 | vis.add_geometry(pcd) 72 | geometry_added = True 73 | 74 | vis.update_geometry(pcd) 75 | vis.poll_events() 76 | vis.update_renderer() 77 | 78 | process_time = datetime.now() - dt0 79 | if count % 10 == 0: 80 | print("FPS = {0}".format(1/process_time.total_seconds())) 81 | 82 | if count == 30: 83 | pointcloud_ouput_path = os.path.join(POINTCLOUD_OUTPUT_DIR, "pointcloud_{:d}.ply".format(count)) 84 | print("Saving Pointcloud in {}".format(pointcloud_ouput_path)) 85 | 86 | kin.save_pointcloud(pointcloud_ouput_path) 87 | break 88 | 89 | finally: 90 | print("Closing Kinect") 91 | kin.close() 92 | 93 | -------------------------------------------------------------------------------- /environment.yaml: -------------------------------------------------------------------------------- 1 | name: kinz 2 | dependencies: 3 | - python=3.6 4 | - numpy 5 | - pyyaml 6 | - pip 7 | - pip: 8 | - opencv-contrib-python 9 | - open3d 10 | 11 | -------------------------------------------------------------------------------- /kinect.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file kinect.cpp 3 | * 4 | * @brief Implementation of kinect K4 wrapper for python bindings 5 | * 6 | * @author Juan Terven 7 | * Contact: juan@aifi.io 8 | * 9 | */ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "kinect.h" 16 | #include 17 | #include 18 | 19 | Kinect::Kinect(uint8_t deviceIndex, int resolution, bool wfov, bool binned, 20 | uint8_t framerate, bool sensor_color, bool sensor_depth, 21 | bool sensor_ir, bool imu_sensors, bool body_tracking, 22 | bool body_index) { 23 | initialize(deviceIndex, resolution, wfov, binned, framerate, 24 | sensor_color, sensor_depth, sensor_ir, imu_sensors, 25 | body_tracking, body_index); 26 | } 27 | 28 | Kinect::~Kinect() 29 | { 30 | close(); 31 | } 32 | 33 | /** 34 | * Available resolutions are: 35 | * 720: 1280 x 720 @ 25 FPS, Aligned 22 FPS 36 | * 1080: 1920 x 1080 @ 24 FPS, Aligned 18 FPS 37 | * 1440: 2560 x 1440 @ 22 FPS, Aligned 14 FPS 38 | * 1536: 2048 x 1536 @ 23 FPS, Aligned 16 FPS 39 | * 2160: 3840 x 2160 @ 20 FPS, Aligned 10 FPS 40 | * 3072: 4096 x 3072 @ 12 FPS, Aligned 7 FPS 41 | */ 42 | int Kinect::initialize(uint8_t deviceIndex, int resolution, bool wfov, bool binned, uint8_t framerate, 43 | bool sensor_color, bool sensor_depth, bool sensor_ir, bool imu_sensors, bool body_tracking, 44 | bool body_index) 45 | { 46 | // Values initialization 47 | // Color resolution 48 | this->res = resolution; 49 | // General configuration of the device 50 | k4a_device_configuration_t m_config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL; 51 | // Sensors sync 52 | m_config.synchronized_images_only = false; 53 | // Color OFF 54 | m_config.color_resolution = K4A_COLOR_RESOLUTION_OFF; 55 | // depth mode 56 | m_config.depth_mode = K4A_DEPTH_MODE_OFF; 57 | 58 | // Devices available 59 | uint32_t m_device_count = k4a_device_get_installed_count(); 60 | 61 | if (m_device_count == 0) 62 | { 63 | printf("No K4A m_devices found\n"); 64 | return 1; 65 | } 66 | 67 | // Open Kinect device 68 | if (K4A_RESULT_SUCCEEDED != k4a_device_open(deviceIndex, &m_device)) { 69 | printf("Failed to open m_device\n"); 70 | if (m_device != NULL) { 71 | k4a_device_close(m_device); 72 | m_device = NULL; 73 | return 1; 74 | } 75 | } 76 | 77 | // Get serial number 78 | size_t serial_size = 0; 79 | k4a_device_get_serialnum(m_device, NULL, &serial_size); 80 | 81 | // Allocate memory for the serial, then acquire it 82 | char *serial = (char*)(malloc(serial_size)); 83 | k4a_device_get_serialnum(m_device,serial,&serial_size); 84 | // printf("Opened device SN: %s\n",serial); 85 | this->serial_number = serial; 86 | free(serial); 87 | 88 | // Turn image sync if RGB or IR is present 89 | if (m_config.color_resolution != K4A_COLOR_RESOLUTION_OFF && m_config.depth_mode != K4A_DEPTH_MODE_OFF ) 90 | m_config.synchronized_images_only = true; 91 | 92 | // Color Configuration 93 | if (sensor_color) 94 | { 95 | // Consider K4A_IMAGE_FORMAT_COLOR_MJPG. It is less CPU expensive 96 | m_config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32; 97 | // m_config.color_format = K4A_IMAGE_FORMAT_COLOR_MJPG; 98 | 99 | switch(resolution) { 100 | case 720: 101 | m_config.color_resolution = K4A_COLOR_RESOLUTION_720P; 102 | break; 103 | case 1080: 104 | m_config.color_resolution = K4A_COLOR_RESOLUTION_1080P; 105 | break; 106 | case 1440: 107 | m_config.color_resolution = K4A_COLOR_RESOLUTION_1440P; 108 | break; 109 | case 2160: 110 | m_config.color_resolution = K4A_COLOR_RESOLUTION_2160P; 111 | break; 112 | case 1536: 113 | m_config.color_resolution = K4A_COLOR_RESOLUTION_1536P; 114 | break; 115 | case 3072: 116 | m_config.color_resolution = K4A_COLOR_RESOLUTION_3072P; 117 | break; 118 | default: 119 | m_config.color_resolution = K4A_COLOR_RESOLUTION_1080P; 120 | break; 121 | } 122 | } 123 | 124 | // Configure Depth sensor 125 | if (!sensor_ir && !sensor_depth) 126 | { 127 | m_config.depth_mode = K4A_DEPTH_MODE_OFF; 128 | } 129 | else if (sensor_ir && !sensor_depth) 130 | { 131 | m_config.depth_mode = K4A_DEPTH_MODE_PASSIVE_IR; 132 | } 133 | else if ((sensor_ir && sensor_depth) || (!sensor_ir && sensor_depth)) 134 | { 135 | if (wfov) 136 | { 137 | if (binned) 138 | { 139 | m_config.depth_mode = K4A_DEPTH_MODE_WFOV_2X2BINNED; 140 | } else { 141 | m_config.depth_mode = K4A_DEPTH_MODE_WFOV_UNBINNED; 142 | } 143 | } else { 144 | if (binned) 145 | { 146 | m_config.depth_mode = K4A_DEPTH_MODE_NFOV_2X2BINNED; 147 | } else { 148 | m_config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED; 149 | } 150 | } 151 | } 152 | 153 | 154 | // Final FPS configuration 155 | // FPS 156 | m_config.camera_fps = K4A_FRAMES_PER_SECOND_30; 157 | // For resolution 4096x3072 only 0,5,15 FPS 158 | int maxFPS = 30; 159 | if (m_config.color_resolution==K4A_COLOR_RESOLUTION_3072P) 160 | //m_config.camera_fps = framerate<=5?K4A_FRAMES_PER_SECOND_5:K4A_FRAMES_PER_SECOND_15; 161 | maxFPS = 15; 162 | 163 | if (m_config.depth_mode==K4A_DEPTH_MODE_WFOV_UNBINNED) 164 | maxFPS = 15; 165 | 166 | if (framerate>maxFPS) 167 | { 168 | if (maxFPS==15) 169 | m_config.camera_fps = K4A_FRAMES_PER_SECOND_15; 170 | } else { 171 | if (framerate<=5) 172 | m_config.camera_fps = K4A_FRAMES_PER_SECOND_5; 173 | // else if (framerate<=10) 174 | // m_config.camera_fps = K4A_FRAMES_PER_SECOND_10; 175 | else if (framerate<=15) 176 | m_config.camera_fps = K4A_FRAMES_PER_SECOND_15; 177 | } 178 | 179 | // Get calibration 180 | if (K4A_RESULT_SUCCEEDED != 181 | k4a_device_get_calibration(m_device, m_config.depth_mode, m_config.color_resolution, &m_calibration)) { 182 | printf("Failed to get calibration\n"); 183 | if (m_device) { 184 | k4a_device_close(m_device); 185 | m_device = NULL; 186 | return 1; 187 | } 188 | } 189 | update_calibration(m_depth_calib, true); 190 | update_calibration(m_color_calib, false); 191 | 192 | // get transformation to map from depth to color 193 | m_transformation = k4a_transformation_create(&m_calibration); 194 | 195 | if (K4A_RESULT_SUCCEEDED != k4a_device_start_cameras(m_device, &m_config)) { 196 | printf("Failed to start m_device\n"); 197 | if (m_device) { 198 | k4a_device_close(m_device); 199 | m_device = NULL; 200 | return 1; 201 | } 202 | } 203 | else 204 | printf("Kinect started successfully!!\n"); 205 | 206 | if (K4A_RESULT_SUCCEEDED != k4a_device_set_color_control(m_device, 207 | K4A_COLOR_CONTROL_POWERLINE_FREQUENCY, 208 | K4A_COLOR_CONTROL_MODE_MANUAL, 2)) { 209 | printf("Failed to change power frequency\n"); 210 | } 211 | 212 | // Start IMU sensors 213 | m_imu_sensors_available = false; 214 | if(imu_sensors) { 215 | if(k4a_device_start_imu(m_device) == K4A_RESULT_SUCCEEDED) { 216 | printf("IMU sensors started succesfully.\n"); 217 | m_imu_sensors_available = true; 218 | } 219 | else { 220 | printf("IMU SENSORES FAILED INITIALIZATION!\n"); 221 | m_imu_sensors_available = false; 222 | } 223 | } 224 | 225 | #ifdef BODY 226 | // Start tracker 227 | m_body_tracking_available = false; 228 | m_num_bodies = 0; 229 | if (body_tracking || body_index) { 230 | k4abt_tracker_configuration_t tracker_config = K4ABT_TRACKER_CONFIG_DEFAULT; 231 | if(k4abt_tracker_create(&m_calibration, tracker_config, &m_tracker) == K4A_RESULT_SUCCEEDED) { 232 | printf("Body tracking started succesfully.\n"); 233 | m_body_tracking_available = true; 234 | } 235 | else { 236 | printf("BODY TRACKING FAILED TO INITIALIZE!\n"); 237 | m_body_tracking_available = false; 238 | } 239 | } 240 | #endif 241 | 242 | 243 | return 0; 244 | } // initialize 245 | 246 | 247 | void Kinect::update_calibration(Calibration &calib_struct, bool depth) { 248 | k4a_calibration_camera_t calib; 249 | 250 | if(depth) 251 | calib = m_calibration.depth_camera_calibration; 252 | else 253 | calib = m_calibration.color_camera_calibration; 254 | 255 | calib_struct.width = calib.resolution_width; 256 | calib_struct.height = calib.resolution_height; 257 | calib_struct.cx = calib.intrinsics.parameters.param.cx; 258 | calib_struct.cy = calib.intrinsics.parameters.param.cy; 259 | calib_struct.fx = calib.intrinsics.parameters.param.fx; 260 | calib_struct.fy = calib.intrinsics.parameters.param.fy; 261 | calib_struct.k1 = calib.intrinsics.parameters.param.k1; 262 | calib_struct.k2 = calib.intrinsics.parameters.param.k2; 263 | calib_struct.k3 = calib.intrinsics.parameters.param.k3; 264 | calib_struct.k4 = calib.intrinsics.parameters.param.k4; 265 | calib_struct.k5 = calib.intrinsics.parameters.param.k5; 266 | calib_struct.k6 = calib.intrinsics.parameters.param.k6; 267 | calib_struct.p1 = calib.intrinsics.parameters.param.p1; 268 | calib_struct.p2 = calib.intrinsics.parameters.param.p2; 269 | calib_struct.codx = calib.intrinsics.parameters.param.codx; 270 | calib_struct.cody = calib.intrinsics.parameters.param.cody; 271 | for(int i=0; i<9; i++) 272 | calib_struct.rotation[i] = calib.extrinsics.rotation[i]; 273 | for(int i=0; i<3; i++) 274 | calib_struct.translation[i] = calib.extrinsics.translation[i]; 275 | 276 | } 277 | 278 | Calibration Kinect::get_depth_calibration() { 279 | return m_depth_calib; 280 | } 281 | 282 | Calibration Kinect::get_color_calibration() { 283 | return m_color_calib; 284 | } 285 | 286 | 287 | const int Kinect::get_frames(bool get_color, bool get_depth, 288 | bool get_ir, bool get_sensors, 289 | bool get_body, bool get_body_index, 290 | bool align_depth) { 291 | m_align_depth = align_depth; 292 | bool good_color = true, good_depth = true, good_ir = true; 293 | if (this->res==0) 294 | get_color = false; 295 | 296 | // Release images before next acquisition 297 | if (m_capture) { 298 | k4a_capture_release(m_capture); 299 | m_capture = NULL; 300 | } 301 | if (m_image_c) { 302 | k4a_image_release(m_image_c); 303 | m_image_c = NULL; 304 | } 305 | if (m_image_d) { 306 | k4a_image_release(m_image_d); 307 | m_image_d = NULL; 308 | } 309 | if (m_image_d_org) { 310 | k4a_image_release(m_image_d_org); 311 | m_image_d_org = NULL; 312 | } 313 | 314 | if (m_image_ir) { 315 | k4a_image_release(m_image_ir); 316 | m_image_ir = NULL; 317 | } 318 | 319 | #ifdef BODY 320 | if (m_body_index) { 321 | k4a_image_release(m_body_index); 322 | m_body_index = NULL; 323 | } 324 | if (m_body_frame) { 325 | k4abt_frame_release(m_body_frame); 326 | m_body_frame = NULL; 327 | } 328 | #endif 329 | 330 | // Get a m_capture 331 | switch (k4a_device_get_capture(m_device, &m_capture, TIMEOUT_IN_MS)) { 332 | case K4A_WAIT_RESULT_SUCCEEDED: 333 | break; 334 | case K4A_WAIT_RESULT_TIMEOUT: 335 | printf("Timed out waiting for a m_capture\n"); 336 | return 0; 337 | break; 338 | case K4A_WAIT_RESULT_FAILED: 339 | printf("Failed to read a m_capture\n"); 340 | printf("Restarting streaming ..."); 341 | k4a_device_stop_cameras (m_device); 342 | if(K4A_RESULT_SUCCEEDED != k4a_device_start_cameras(m_device, &m_config)) { 343 | printf("Failed to restart streaming\n"); 344 | k4a_device_stop_cameras (m_device); 345 | return 0; 346 | } 347 | } 348 | 349 | // Get color image 350 | if(get_color) { 351 | m_image_c = k4a_capture_get_color_image(m_capture); 352 | if (m_image_c == NULL) { 353 | good_color = false; 354 | printf("Could not read color image\n"); 355 | } 356 | } 357 | // Get depth16 image 358 | if(get_depth) { 359 | m_image_d = k4a_capture_get_depth_image(m_capture); 360 | m_image_d_org = k4a_capture_get_depth_image(m_capture); 361 | if (m_image_d == NULL) { 362 | good_depth = false; 363 | printf("Could not read depth image\n"); 364 | } 365 | } 366 | // Get IR image 367 | if(get_ir) { 368 | m_image_ir = k4a_capture_get_ir_image(m_capture); 369 | if (m_image_ir == NULL) { 370 | good_ir = false; 371 | printf("Could not read IR image"); 372 | } 373 | } 374 | 375 | if(get_sensors && m_imu_sensors_available) { 376 | k4a_imu_sample_t imu_sample; 377 | 378 | // Capture a imu sample 379 | k4a_wait_result_t imu_status; 380 | imu_status = k4a_device_get_imu_sample(m_device, &imu_sample, TIMEOUT_IN_MS); 381 | switch (imu_status) 382 | { 383 | case K4A_WAIT_RESULT_SUCCEEDED: 384 | break; 385 | case K4A_WAIT_RESULT_TIMEOUT: 386 | printf("Timed out waiting for a imu sample\n"); 387 | break; 388 | case K4A_WAIT_RESULT_FAILED: 389 | printf("Failed to read a imu sample\n"); 390 | break; 391 | } 392 | 393 | // Access the accelerometer readings 394 | if (imu_status == K4A_WAIT_RESULT_SUCCEEDED) 395 | { 396 | m_imu_data.temperature = imu_sample.temperature; 397 | m_imu_data.acc_x = imu_sample.acc_sample.xyz.x; 398 | m_imu_data.acc_y = imu_sample.acc_sample.xyz.y; 399 | m_imu_data.acc_z = imu_sample.acc_sample.xyz.z; 400 | m_imu_data.acc_timestamp_usec = imu_sample.acc_timestamp_usec; 401 | m_imu_data.gyro_x = imu_sample.gyro_sample.xyz.x; 402 | m_imu_data.gyro_y = imu_sample.gyro_sample.xyz.y; 403 | m_imu_data.gyro_z = imu_sample.gyro_sample.xyz.z; 404 | m_imu_data.gyro_timestamp_usec = imu_sample.gyro_timestamp_usec; 405 | } 406 | } 407 | 408 | #ifdef BODY 409 | if (get_body && m_body_tracking_available) { 410 | // Get body tracking data 411 | k4a_wait_result_t queue_capture_result = k4abt_tracker_enqueue_capture(m_tracker, m_capture, K4A_WAIT_INFINITE); 412 | if (queue_capture_result == K4A_WAIT_RESULT_TIMEOUT) { 413 | // It should never hit timeout when K4A_WAIT_INFINITE is set. 414 | printf("Error! Add capture to tracker process queue timeout!\n"); 415 | } 416 | else if (queue_capture_result == K4A_WAIT_RESULT_FAILED) { 417 | printf("Error! Add capture to tracker process queue failed!\n"); 418 | } 419 | else { 420 | m_body_frame = NULL; 421 | k4a_wait_result_t pop_frame_result = k4abt_tracker_pop_result(m_tracker, &m_body_frame, K4A_WAIT_INFINITE); 422 | if (pop_frame_result == K4A_WAIT_RESULT_SUCCEEDED) { 423 | m_num_bodies = k4abt_frame_get_num_bodies(m_body_frame); 424 | py::list bodies; 425 | 426 | for (uint32_t i = 0; i < m_num_bodies; i++) { 427 | k4abt_body_t body; 428 | if (k4abt_frame_get_body_skeleton(m_body_frame, i, &body.skeleton) == K4A_RESULT_SUCCEEDED) { 429 | body.id = k4abt_frame_get_body_id(m_body_frame, i); 430 | 431 | py::dict body_dict; 432 | body_dict = get_body_data(body); 433 | bodies.append(body_dict); 434 | } 435 | else { 436 | printf("Get body from body frame failed!"); 437 | } 438 | } 439 | m_bodies = bodies; 440 | 441 | if(get_body_index) { 442 | m_body_index = k4abt_frame_get_body_index_map(m_body_frame); 443 | 444 | if (m_body_index == NULL) { 445 | printf("Error: Fail to generate bodyindex map!\n"); 446 | } 447 | } 448 | } 449 | else if (pop_frame_result == K4A_WAIT_RESULT_TIMEOUT) 450 | { 451 | // It should never hit timeout when K4A_WAIT_INFINITE is set. 452 | printf("Error! Pop body frame result timeout!\n"); 453 | } 454 | else 455 | { 456 | printf("Pop body frame result failed!\n"); 457 | } 458 | } 459 | } 460 | #endif 461 | 462 | if(good_color && good_depth && good_ir) 463 | return 1; 464 | else 465 | return 0; 466 | } // get_frames 467 | 468 | Imu_sample Kinect::get_sensor_data() { 469 | return m_imu_data; 470 | } 471 | 472 | ColorData Kinect::get_color_data() { 473 | ColorData color_data; 474 | 475 | if(m_image_c) { 476 | int w = k4a_image_get_width_pixels(m_image_c); 477 | int h = k4a_image_get_height_pixels(m_image_c); 478 | int stride = k4a_image_get_stride_bytes(m_image_c); 479 | uint8_t* dataBuffer = k4a_image_get_buffer(m_image_c); 480 | auto sz = k4a_image_get_size(m_image_c); 481 | void* data = malloc(sz); 482 | memcpy(data, dataBuffer, sz); 483 | BufferColor m((uint8_t *)data, h, w, stride); 484 | 485 | color_data.buffer = m; 486 | color_data.timestamp_nsec = k4a_image_get_system_timestamp_nsec(m_image_c); 487 | return color_data; 488 | } 489 | else { 490 | BufferColor m(NULL, 0, 0, 0); 491 | color_data.buffer = m; 492 | color_data.timestamp_nsec = 0; 493 | return color_data; 494 | } 495 | } 496 | 497 | DepthData Kinect::get_depth_data() { 498 | DepthData depth_data; 499 | 500 | if(m_image_d) { 501 | // align images 502 | if(m_align_depth) { 503 | k4a_image_t image_dc; 504 | if(align_depth_to_color(k4a_image_get_width_pixels(m_image_c), 505 | k4a_image_get_height_pixels(m_image_c), image_dc)) { 506 | k4a_image_release(m_image_d); 507 | m_image_d = NULL; 508 | m_image_d = image_dc; 509 | } 510 | else 511 | printf("Failed to align\n"); 512 | } 513 | 514 | int w = k4a_image_get_width_pixels(m_image_d); 515 | int h = k4a_image_get_height_pixels(m_image_d); 516 | int stride = k4a_image_get_stride_bytes(m_image_d); 517 | uint8_t* dataBuffer = k4a_image_get_buffer(m_image_d); 518 | auto sz = k4a_image_get_size(m_image_d); 519 | void* data = malloc(sz); 520 | memcpy(data, dataBuffer, sz); 521 | BufferDepth m((uint16_t *)data, h, w, stride); 522 | 523 | depth_data.buffer = m; 524 | depth_data.timestamp_nsec = k4a_image_get_system_timestamp_nsec(m_image_d); 525 | return depth_data; 526 | } 527 | else { 528 | BufferDepth m(NULL, 0, 0, 0); 529 | depth_data.buffer = m; 530 | depth_data.timestamp_nsec = 0; 531 | return depth_data; 532 | } 533 | } 534 | 535 | DepthData Kinect::get_ir_data() { 536 | DepthData ir_data; 537 | 538 | if(m_image_ir) { 539 | int w = k4a_image_get_width_pixels(m_image_ir); 540 | int h = k4a_image_get_height_pixels(m_image_ir); 541 | int stride = k4a_image_get_stride_bytes(m_image_ir); 542 | uint8_t* dataBuffer = k4a_image_get_buffer(m_image_ir); 543 | auto sz = k4a_image_get_size(m_image_ir); 544 | void* data = malloc(sz); 545 | memcpy(data, dataBuffer, sz); 546 | BufferDepth m((uint16_t *)data, h, w, stride); 547 | 548 | ir_data.buffer = m; 549 | ir_data.timestamp_nsec = k4a_image_get_system_timestamp_nsec(m_image_ir); 550 | return ir_data; 551 | } 552 | else { 553 | BufferDepth m(NULL, 0, 0, 0); 554 | ir_data.buffer = m; 555 | ir_data.timestamp_nsec = 0; 556 | return ir_data; 557 | } 558 | } 559 | 560 | bool Kinect::align_depth_to_color(int width, int height, k4a_image_t &transformed_depth_image){ 561 | if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16, 562 | width, height, width * (int)sizeof(uint16_t), 563 | &transformed_depth_image)) { 564 | printf("Failed to create transformed depth image\n"); 565 | return false; 566 | } 567 | 568 | if (K4A_RESULT_SUCCEEDED != k4a_transformation_depth_image_to_color_camera(m_transformation, 569 | m_image_d, 570 | transformed_depth_image)) { 571 | printf("Failed to compute transformed depth image\n"); 572 | return false; 573 | } 574 | 575 | return true; 576 | } 577 | 578 | 579 | bool Kinect::align_color_to_depth(k4a_image_t &transformed_color_image){ 580 | int depth_image_width_pixels = k4a_image_get_width_pixels(m_image_d); 581 | int depth_image_height_pixels = k4a_image_get_height_pixels(m_image_d); 582 | 583 | transformed_color_image = NULL; 584 | if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_COLOR_BGRA32, 585 | depth_image_width_pixels, 586 | depth_image_height_pixels, 587 | depth_image_width_pixels * 4 * (int)sizeof(uint8_t), 588 | &transformed_color_image)) 589 | { 590 | printf("Failed to create transformed color image\n"); 591 | return false; 592 | } 593 | 594 | if (K4A_RESULT_SUCCEEDED != k4a_transformation_color_image_to_depth_camera(m_transformation, 595 | m_image_d, 596 | m_image_c, 597 | transformed_color_image)) 598 | { 599 | printf("Failed to compute transformed color image\n"); 600 | return false; 601 | } 602 | 603 | return true; 604 | } 605 | 606 | std::string Kinect::get_serial_number() 607 | { 608 | return this->serial_number; 609 | } 610 | 611 | void Kinect::close(){ 612 | #ifdef BODY 613 | if (m_tracker != NULL) { 614 | k4abt_tracker_shutdown(m_tracker); 615 | k4abt_tracker_destroy(m_tracker); 616 | m_tracker = NULL; 617 | } 618 | if (m_body_index) { 619 | k4a_image_release(m_body_index); 620 | m_body_index = NULL; 621 | } 622 | if (m_body_frame) { 623 | k4abt_frame_release(m_body_frame); 624 | m_body_frame = NULL; 625 | } 626 | #endif 627 | 628 | if (m_device != NULL) { 629 | k4a_device_stop_cameras(m_device); 630 | k4a_device_close(m_device); 631 | m_device = NULL; 632 | } 633 | if (m_image_c != NULL) { 634 | k4a_image_release(m_image_c); 635 | m_image_c = NULL; 636 | } 637 | if (m_image_d != NULL) { 638 | k4a_image_release(m_image_d); 639 | m_image_d = NULL; 640 | } 641 | if (m_image_d_org != NULL) { 642 | k4a_image_release(m_image_d_org); 643 | m_image_d_org = NULL; 644 | } 645 | if (m_image_ir) { 646 | k4a_image_release(m_image_ir); 647 | m_image_ir = NULL; 648 | } 649 | if (m_capture != NULL) { 650 | k4a_capture_release(m_capture); 651 | m_capture = NULL; 652 | } 653 | } 654 | 655 | void Kinect::set_exposure(int exposure) { 656 | if (m_device) { 657 | if (K4A_RESULT_SUCCEEDED != k4a_device_set_color_control(m_device, 658 | K4A_COLOR_CONTROL_EXPOSURE_TIME_ABSOLUTE, 659 | K4A_COLOR_CONTROL_MODE_MANUAL, 660 | (int32_t)exposure)) { 661 | printf("Failed to change exposure time\n"); 662 | } 663 | } 664 | else { 665 | printf("No valid Kinect device. Failed to change exposure time\n"); 666 | } 667 | } 668 | 669 | const int Kinect::get_exposure() { 670 | int exposure = 0; 671 | if(m_image_c) { 672 | exposure = k4a_image_get_exposure_usec(m_image_c); 673 | } 674 | else 675 | printf("Could not get exposure value. Image not valid."); 676 | 677 | return exposure; 678 | } 679 | 680 | void Kinect::set_gain(int gain) { 681 | if (m_device) { 682 | if (K4A_RESULT_SUCCEEDED != k4a_device_set_color_control(m_device, 683 | K4A_COLOR_CONTROL_GAIN, 684 | K4A_COLOR_CONTROL_MODE_MANUAL, 685 | gain)) { 686 | printf("Failed to change gain\n"); 687 | } 688 | } 689 | else { 690 | printf("No valid Kinect device. Failed to change gain\n"); 691 | } 692 | } 693 | 694 | std::vector > Kinect::map_coords_color_to_depth(std::vector > &color_coords) { 695 | k4a_float2_t init_coords, depth_coords; 696 | std::vector > final_coords; 697 | 698 | if (m_align_depth) { 699 | final_coords = color_coords; 700 | return final_coords; 701 | } 702 | int val; 703 | k4a_result_t res; 704 | 705 | for(unsigned int i=0; i < color_coords.size(); i++) { 706 | std::vector d_coords; 707 | init_coords.xy.x = color_coords[i][0]; 708 | init_coords.xy.y = color_coords[i][1]; 709 | res = k4a_calibration_color_2d_to_depth_2d(&m_calibration, &init_coords, m_image_d, &depth_coords, &val); 710 | if(res == K4A_RESULT_SUCCEEDED) { 711 | d_coords.push_back(depth_coords.xy.x); 712 | d_coords.push_back(depth_coords.xy.y); 713 | } 714 | else { 715 | d_coords.push_back(-1); 716 | d_coords.push_back(-1); 717 | } 718 | final_coords.push_back(d_coords); 719 | } 720 | return final_coords; 721 | } 722 | 723 | std::vector > Kinect::map_coords_color_to_3D( 724 | std::vector > &color_coords, 725 | bool depth_reference) { 726 | std::vector > depth_coords; 727 | std::vector > final_coords; 728 | 729 | if (color_coords.size() == 0) 730 | return final_coords; 731 | 732 | k4a_calibration_type_t reference = K4A_CALIBRATION_TYPE_DEPTH; 733 | if (!depth_reference) 734 | reference = K4A_CALIBRATION_TYPE_COLOR; 735 | 736 | k4a_calibration_type_t source_calib; 737 | if (m_align_depth) 738 | source_calib = K4A_CALIBRATION_TYPE_COLOR; 739 | else 740 | source_calib = K4A_CALIBRATION_TYPE_DEPTH; 741 | 742 | // get the depth data 743 | int stride = (int)(k4a_image_get_stride_bytes(m_image_d)/2); 744 | uint16_t *depth_data = (uint16_t *)(void *)k4a_image_get_buffer(m_image_d); 745 | 746 | // from color to depth 747 | depth_coords = this->map_coords_color_to_depth(color_coords); 748 | 749 | // from depth to 3D 750 | for(unsigned int i=0; i < depth_coords.size(); i++) { 751 | std::vector coords3d_vect; 752 | k4a_float2_t coordsdepth; 753 | k4a_float3_t coords3d; 754 | int valid; 755 | if(depth_coords[i][0] != -1 && depth_coords[i][1] != -1) { 756 | coordsdepth.xy.x = depth_coords[i][0]; 757 | coordsdepth.xy.y = depth_coords[i][1]; 758 | // get depth value 759 | float depth = (float)depth_data[stride*depth_coords[i][1] + depth_coords[i][0]]; 760 | k4a_calibration_2d_to_3d(&m_calibration, &coordsdepth, depth, 761 | source_calib, 762 | reference, 763 | &coords3d, &valid); 764 | if(valid == 1) { 765 | coords3d_vect.push_back(coords3d.xyz.x); 766 | coords3d_vect.push_back(coords3d.xyz.y); 767 | coords3d_vect.push_back(coords3d.xyz.z); 768 | } 769 | else { 770 | coords3d_vect.push_back(0); 771 | coords3d_vect.push_back(0); 772 | coords3d_vect.push_back(0); 773 | } 774 | } 775 | else { 776 | coords3d_vect.push_back(0); 777 | coords3d_vect.push_back(0); 778 | coords3d_vect.push_back(0); 779 | } 780 | final_coords.push_back(coords3d_vect); 781 | } 782 | 783 | return final_coords; 784 | } 785 | 786 | std::vector > Kinect::map_coords_depth_to_color(std::vector > &depth_coords) { 787 | k4a_float2_t init_coords, color_coords; 788 | std::vector > final_coords; 789 | int val; 790 | k4a_result_t res; 791 | 792 | k4a_calibration_type_t source_calib = K4A_CALIBRATION_TYPE_DEPTH; 793 | if (m_align_depth) 794 | source_calib = K4A_CALIBRATION_TYPE_COLOR; 795 | 796 | int stride = (int)(k4a_image_get_stride_bytes(m_image_d)/2); 797 | uint16_t *depth_data = (uint16_t *)(void *)k4a_image_get_buffer(m_image_d); 798 | 799 | for(unsigned int i=0; i < depth_coords.size(); i++) { 800 | std::vector c_coords; 801 | init_coords.xy.x = depth_coords[i][0]; 802 | init_coords.xy.y = depth_coords[i][1]; 803 | float depth = (float)depth_data[stride*depth_coords[i][1] + depth_coords[i][0]]; 804 | res = k4a_calibration_2d_to_2d(&m_calibration, &init_coords, depth, 805 | source_calib, K4A_CALIBRATION_TYPE_COLOR, 806 | &color_coords, &val); 807 | if(res == K4A_RESULT_SUCCEEDED) { 808 | c_coords.push_back(color_coords.xy.x); 809 | c_coords.push_back(color_coords.xy.y); 810 | } 811 | else { 812 | c_coords.push_back(-1); 813 | c_coords.push_back(-1); 814 | } 815 | final_coords.push_back(c_coords); 816 | } 817 | return final_coords; 818 | } 819 | 820 | std::vector > Kinect::map_coords_depth_to_3D( 821 | std::vector > &depth_coords, 822 | bool depth_reference) { 823 | std::vector > final_coords; 824 | 825 | if (depth_coords.size() == 0) 826 | return final_coords; 827 | 828 | k4a_calibration_type_t reference = K4A_CALIBRATION_TYPE_DEPTH; 829 | if (!depth_reference || m_align_depth) 830 | reference = K4A_CALIBRATION_TYPE_COLOR; 831 | 832 | k4a_calibration_type_t source_calib; 833 | if (m_align_depth) 834 | source_calib = K4A_CALIBRATION_TYPE_COLOR; 835 | else 836 | source_calib = K4A_CALIBRATION_TYPE_DEPTH; 837 | 838 | // get the depth data 839 | int stride = (int)(k4a_image_get_stride_bytes(m_image_d)/2); 840 | uint16_t *depth_data = (uint16_t *)(void *)k4a_image_get_buffer(m_image_d); 841 | 842 | // from depth to 3D 843 | for(unsigned int i=0; i < depth_coords.size(); i++) { 844 | std::vector coords3d_vect; 845 | k4a_float2_t coordsdepth; 846 | k4a_float3_t coords3d; 847 | int valid; 848 | if(depth_coords[i][0] != -1 && depth_coords[i][1] != -1) { 849 | coordsdepth.xy.x = depth_coords[i][0]; 850 | coordsdepth.xy.y = depth_coords[i][1]; 851 | // get depth value 852 | float depth = (float)depth_data[stride*depth_coords[i][1] + depth_coords[i][0]]; 853 | k4a_calibration_2d_to_3d(&m_calibration, &coordsdepth, depth, 854 | source_calib, 855 | reference, 856 | &coords3d, &valid); 857 | if(valid == 1) { 858 | coords3d_vect.push_back(coords3d.xyz.x); 859 | coords3d_vect.push_back(coords3d.xyz.y); 860 | coords3d_vect.push_back(coords3d.xyz.z); 861 | } 862 | else { 863 | coords3d_vect.push_back(0); 864 | coords3d_vect.push_back(0); 865 | coords3d_vect.push_back(0); 866 | } 867 | } 868 | else { 869 | coords3d_vect.push_back(0); 870 | coords3d_vect.push_back(0); 871 | coords3d_vect.push_back(0); 872 | } 873 | final_coords.push_back(coords3d_vect); 874 | } 875 | 876 | return final_coords; 877 | } 878 | 879 | std::vector > Kinect::map_coords_3d_to_depth( 880 | std::vector > &coords3d, 881 | bool depth_reference) { 882 | std::vector > final_coords; 883 | k4a_result_t res; 884 | 885 | k4a_calibration_type_t reference = K4A_CALIBRATION_TYPE_DEPTH; 886 | k4a_calibration_type_t destination = K4A_CALIBRATION_TYPE_DEPTH; 887 | if (!depth_reference) 888 | reference = K4A_CALIBRATION_TYPE_COLOR; 889 | if (m_align_depth) 890 | destination = K4A_CALIBRATION_TYPE_COLOR; 891 | 892 | for(unsigned int i=0; i < coords3d.size(); i++) { 893 | std::vector coords2d_vect; 894 | k4a_float2_t coords2d; 895 | k4a_float3_t kin_coords3d; 896 | 897 | if(coords3d[i][0] != 0 && coords3d[i][1] != 0 && coords3d[i][2] != 0) { 898 | kin_coords3d.xyz.x = coords3d[i][0]; 899 | kin_coords3d.xyz.y = coords3d[i][1]; 900 | kin_coords3d.xyz.z = coords3d[i][2]; 901 | 902 | int val; 903 | res = k4a_calibration_3d_to_2d(&m_calibration, &kin_coords3d, 904 | reference, destination, 905 | &coords2d, &val); 906 | if(res == K4A_RESULT_SUCCEEDED && val == 1) { 907 | coords2d_vect.push_back(coords2d.xy.x); 908 | coords2d_vect.push_back(coords2d.xy.y); 909 | } 910 | else { 911 | coords2d_vect.push_back(-1); 912 | coords2d_vect.push_back(-1); 913 | } 914 | } 915 | else { 916 | coords2d_vect.push_back(-1); 917 | coords2d_vect.push_back(-1); 918 | } 919 | final_coords.push_back(coords2d_vect); 920 | } 921 | return final_coords; 922 | } 923 | 924 | std::vector > Kinect::map_coords_3d_to_color( 925 | std::vector > &coords3d, 926 | bool depth_reference) { 927 | std::vector > final_coords; 928 | k4a_result_t res; 929 | k4a_calibration_type_t reference = K4A_CALIBRATION_TYPE_DEPTH; 930 | if (!depth_reference) 931 | reference = K4A_CALIBRATION_TYPE_COLOR; 932 | 933 | for(unsigned int i=0; i < coords3d.size(); i++) { 934 | std::vector coords2d_vect; 935 | k4a_float2_t coords2d; 936 | k4a_float3_t kin_coords3d; 937 | 938 | if(coords3d[i][0] != 0 && coords3d[i][1] != 0 && coords3d[i][2] != 0) { 939 | kin_coords3d.xyz.x = coords3d[i][0]; 940 | kin_coords3d.xyz.y = coords3d[i][1]; 941 | kin_coords3d.xyz.z = coords3d[i][2]; 942 | 943 | int val; 944 | res = k4a_calibration_3d_to_2d(&m_calibration, &kin_coords3d, 945 | reference, K4A_CALIBRATION_TYPE_COLOR, 946 | &coords2d, &val); 947 | if(res == K4A_RESULT_SUCCEEDED && val == 1) { 948 | coords2d_vect.push_back(coords2d.xy.x); 949 | coords2d_vect.push_back(coords2d.xy.y); 950 | } 951 | else { 952 | coords2d_vect.push_back(-1); 953 | coords2d_vect.push_back(-1); 954 | } 955 | } 956 | else { 957 | coords2d_vect.push_back(-1); 958 | coords2d_vect.push_back(-1); 959 | } 960 | final_coords.push_back(coords2d_vect); 961 | } 962 | return final_coords; 963 | } 964 | 965 | /** Transforms the depth image into 3 planar images representing X, Y and Z-coordinates of corresponding 3d points. 966 | * Throws error on failure. 967 | * 968 | * \sa k4a_transformation_depth_image_to_point_cloud 969 | */ 970 | bool Kinect::depth_image_to_point_cloud(int width, int height, k4a_image_t &xyz_image) { 971 | if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM, 972 | width, height, 973 | width * 3 * (int)sizeof(int16_t), 974 | &xyz_image)) { 975 | printf("Failed to create transformed xyz image\n"); 976 | return false; 977 | } 978 | 979 | k4a_result_t result = 980 | k4a_transformation_depth_image_to_point_cloud(m_transformation, 981 | m_image_d, 982 | K4A_CALIBRATION_TYPE_DEPTH, 983 | xyz_image); 984 | 985 | if (K4A_RESULT_SUCCEEDED != result) { 986 | printf("Failed to transform depth image to point cloud!"); 987 | return false; 988 | } 989 | return true; 990 | } 991 | 992 | BufferPointCloud Kinect::get_pointcloud() { 993 | if(m_image_d) { 994 | k4a_image_t image_xyz = NULL; 995 | 996 | // Get the point cloud 997 | if(depth_image_to_point_cloud(k4a_image_get_width_pixels(m_image_d), 998 | k4a_image_get_height_pixels(m_image_d), image_xyz)) { 999 | 1000 | int w = k4a_image_get_width_pixels(image_xyz); 1001 | int h = k4a_image_get_height_pixels(image_xyz); 1002 | int stride = k4a_image_get_stride_bytes(image_xyz); 1003 | int16_t *dataBuffer = (int16_t *)(void *)k4a_image_get_buffer(image_xyz); 1004 | 1005 | //uint8_t* dataBuffer = k4a_image_get_buffer(image_xyz); 1006 | auto sz = k4a_image_get_size(image_xyz); 1007 | void* data = malloc(sz); 1008 | memcpy(data, dataBuffer, sz); 1009 | BufferPointCloud m((int16_t *)data, h, w, stride); 1010 | 1011 | return m; 1012 | } 1013 | else { 1014 | printf("Failed to create PointCloud\n"); 1015 | BufferPointCloud m(NULL, 0, 0, 0); 1016 | return m; 1017 | } 1018 | } 1019 | else { 1020 | BufferPointCloud m(NULL, 0, 0, 0); 1021 | return m; 1022 | } 1023 | } 1024 | 1025 | BufferColor Kinect::get_pointcloud_color() { 1026 | if(m_image_d) { 1027 | k4a_image_t transformed_color_image = NULL; 1028 | 1029 | // Get the color image aligned with depth coordinates 1030 | // in transformed_color_image 1031 | if(align_color_to_depth(transformed_color_image)) { 1032 | 1033 | int w = k4a_image_get_width_pixels(transformed_color_image); 1034 | int h = k4a_image_get_height_pixels(transformed_color_image); 1035 | int stride = k4a_image_get_stride_bytes(transformed_color_image); 1036 | uint8_t* dataBuffer = k4a_image_get_buffer(transformed_color_image); 1037 | auto sz = k4a_image_get_size(transformed_color_image); 1038 | void* data = malloc(sz); 1039 | memcpy(data, dataBuffer, sz); 1040 | BufferColor m((uint8_t *)data, h, w, stride); 1041 | k4a_image_release(transformed_color_image); 1042 | return m; 1043 | } 1044 | else { 1045 | BufferColor m(NULL, 0, 0, 0); 1046 | return m; 1047 | } 1048 | } 1049 | else { 1050 | BufferColor m(NULL, 0, 0, 0); 1051 | return m; 1052 | } 1053 | } 1054 | 1055 | void Kinect::save_pointcloud(const char *file_name) { 1056 | if(m_image_d) { 1057 | k4a_image_t image_xyz = NULL; 1058 | k4a_image_t transformed_color_image = NULL; 1059 | 1060 | // Get the color image aligned with depth coordinates 1061 | // in transformed_color_image 1062 | if(align_color_to_depth(transformed_color_image)) { 1063 | 1064 | // Get the point cloud 1065 | if(depth_image_to_point_cloud(k4a_image_get_width_pixels(m_image_d), 1066 | k4a_image_get_height_pixels(m_image_d), image_xyz)) { 1067 | 1068 | write_point_cloud(image_xyz, 1069 | transformed_color_image, 1070 | file_name); 1071 | } 1072 | else { 1073 | printf("Failed to create PointCloud\n"); 1074 | } 1075 | } 1076 | else { 1077 | printf("Failed to get the color image aligned with depth for PointCloud\n"); 1078 | } 1079 | } 1080 | else { 1081 | printf("No depth image available for generating Pointcloud\n"); 1082 | } 1083 | } 1084 | 1085 | #ifdef BODY 1086 | int Kinect::get_num_bodies() { 1087 | return m_num_bodies; 1088 | } 1089 | 1090 | py::list Kinect::get_bodies() { 1091 | return m_bodies; 1092 | } 1093 | 1094 | py::dict Kinect::get_body_data(k4abt_body_t body) { 1095 | py::dict body_data; 1096 | 1097 | // Initialize String Array 1098 | const char *squeleton[(int)K4ABT_JOINT_COUNT] = { "Pelvis", "Spine_navel", "Spine_chest", 1099 | "Neck", "Clavicle_left", "Shoulder_left", 1100 | "Elbow_left", "Wrist_left", "Hand_left", 1101 | "Handtip_left", "Thumb_left", "Clavicle_right", 1102 | "Shoulder_right", "Elbow_right", "Wrist_right", 1103 | "Hand_right", "Handtip_right", "Thumb_right", 1104 | "Hip_left", "Knee_left", "Ankle_left", 1105 | "Foot_left", "Hip_right", "Knee_right", 1106 | "Ankle_right", "Foot_right", "Head", 1107 | "Nose", "Eye_left", "Ear_left", 1108 | "Eye_right", "Ear_right" }; 1109 | 1110 | //const char *confidence_vals[4] = {"None", "Low", "Medium", "High"}; 1111 | 1112 | body_data[py::str("id")] = body.id; 1113 | 1114 | 1115 | for (int i = 0; i < (int)K4ABT_JOINT_COUNT; i++) { 1116 | k4a_float3_t position = body.skeleton.joints[i].position; 1117 | k4a_quaternion_t orientation = body.skeleton.joints[i].orientation; 1118 | k4abt_joint_confidence_level_t confidence_level = body.skeleton.joints[i].confidence_level; 1119 | 1120 | py::dict position3d_dict; 1121 | position3d_dict[py::str("x")] = position.v[0]; 1122 | position3d_dict[py::str("y")] = position.v[1]; 1123 | position3d_dict[py::str("z")] = position.v[2]; 1124 | 1125 | py::dict orientation_dict; 1126 | orientation_dict[py::str("w")] = orientation.v[0]; 1127 | orientation_dict[py::str("x")] = orientation.v[1]; 1128 | orientation_dict[py::str("y")] = orientation.v[2]; 1129 | orientation_dict[py::str("z")] = orientation.v[3]; 1130 | 1131 | // project the 3D coordinates to the color and depth cameras 1132 | k4a_float2_t color_coords, depth_coords; 1133 | int val; 1134 | py::dict position2d_color_dict; 1135 | if (k4a_calibration_3d_to_2d(&m_calibration, &position, 1136 | K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, 1137 | &color_coords, &val) == K4A_RESULT_SUCCEEDED) { 1138 | position2d_color_dict[py::str("x")] = (int)color_coords.xy.x; 1139 | position2d_color_dict[py::str("y")] = (int)color_coords.xy.y; 1140 | } 1141 | else { 1142 | position2d_color_dict[py::str("x")] = -1; 1143 | position2d_color_dict[py::str("y")] = -1; 1144 | } 1145 | 1146 | py::dict position2d_depth_dict; 1147 | if (k4a_calibration_3d_to_2d(&m_calibration, &position, 1148 | K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_DEPTH, 1149 | &depth_coords, &val) == K4A_RESULT_SUCCEEDED) { 1150 | position2d_depth_dict[py::str("x")] = (int)depth_coords.xy.x; 1151 | position2d_depth_dict[py::str("y")] = (int)depth_coords.xy.y; 1152 | } 1153 | else { 1154 | position2d_depth_dict[py::str("x")] = -1; 1155 | position2d_depth_dict[py::str("y")] = -1; 1156 | } 1157 | 1158 | py::dict data; 1159 | data[py::str("position3d")] = position3d_dict; 1160 | data[py::str("position2d-rgb")] = position2d_color_dict; 1161 | data[py::str("position2d-depth")] = position2d_depth_dict; 1162 | data[py::str("orientation")] = orientation_dict; 1163 | data[py::str("confidence")] = (int)confidence_level; 1164 | 1165 | body_data[py::str(squeleton[i])] = data; 1166 | 1167 | //printf("Joint[%d]: Position[mm] ( %f, %f, %f ); Orientation ( %f, %f, %f, %f); Confidence Level (%d) \n", 1168 | // i, position.v[0], position.v[1], position.v[2], orientation.v[0], orientation.v[1], orientation.v[2], orientation.v[3], confidence_level); 1169 | } 1170 | 1171 | return body_data; 1172 | } 1173 | 1174 | BodyIndexData Kinect::get_body_index_map(bool returnId, bool inColor) { 1175 | BodyIndexData bodyIndexMap; 1176 | 1177 | if (inColor) 1178 | m_body_index = convert_body_index_map_to_colour(); 1179 | if(m_body_index) { 1180 | int w = k4a_image_get_width_pixels(m_body_index); 1181 | int h = k4a_image_get_height_pixels(m_body_index); 1182 | int stride = k4a_image_get_stride_bytes(m_body_index); 1183 | uint8_t* dataBuffer = k4a_image_get_buffer(m_body_index); 1184 | 1185 | if (returnId) 1186 | change_body_index_to_body_id(dataBuffer, w, h); 1187 | 1188 | auto sz = k4a_image_get_size(m_body_index); 1189 | void* data = malloc(sz); 1190 | memcpy(data, dataBuffer, sz); 1191 | BufferBodyIndex m((uint8_t *)data, h, w, stride); 1192 | 1193 | bodyIndexMap.buffer = m; 1194 | bodyIndexMap.timestamp_nsec = k4a_image_get_system_timestamp_nsec(m_body_index); 1195 | return bodyIndexMap; 1196 | } 1197 | else { 1198 | BufferBodyIndex m(NULL, 0, 0, 0); 1199 | bodyIndexMap.buffer = m; 1200 | bodyIndexMap.timestamp_nsec = 0; 1201 | return bodyIndexMap; 1202 | } 1203 | } 1204 | 1205 | 1206 | void Kinect::change_body_index_to_body_id(uint8_t* image_data, int width, int height) { 1207 | for(int i=0; i < width*height; i++) { 1208 | uint8_t index = *image_data; 1209 | 1210 | uint32_t body_id = k4abt_frame_get_body_id (m_body_frame, (uint32_t)index); 1211 | *image_data = (uint8_t)body_id; 1212 | image_data++; 1213 | } 1214 | } 1215 | 1216 | k4a_image_t Kinect::convert_body_index_map_to_colour() 1217 | { 1218 | k4a_image_t depth_image_in_colour_space = nullptr; 1219 | 1220 | k4a_result_t result = k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16, 1221 | k4a_image_get_width_pixels(m_image_c), 1222 | k4a_image_get_height_pixels(m_image_c), 1223 | k4a_image_get_width_pixels(m_image_c) * (int)sizeof(uint16_t), &depth_image_in_colour_space); 1224 | 1225 | if (result == K4A_RESULT_FAILED) { 1226 | printf("ERROR: Failed to create depth image in colour space\n"); 1227 | return nullptr; 1228 | } 1229 | 1230 | k4a_image_t body_index_in_colour_space = nullptr; 1231 | result = k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM8, 1232 | k4a_image_get_width_pixels(m_image_c), 1233 | k4a_image_get_height_pixels(m_image_c), 1234 | k4a_image_get_width_pixels(m_image_c) * (int)sizeof(uint8_t), 1235 | &body_index_in_colour_space); 1236 | 1237 | if (result == K4A_RESULT_FAILED) { 1238 | printf("ERROR: Failed to create body index map in colour space\n"); 1239 | return nullptr; 1240 | } 1241 | 1242 | result = k4a_transformation_depth_image_to_color_camera_custom(m_transformation, 1243 | m_image_d_org, 1244 | m_body_index, 1245 | depth_image_in_colour_space, 1246 | body_index_in_colour_space, 1247 | K4A_TRANSFORMATION_INTERPOLATION_TYPE_NEAREST, 1248 | K4ABT_BODY_INDEX_MAP_BACKGROUND); 1249 | if (result == K4A_RESULT_FAILED) { 1250 | printf("ERROR: Failed to transform body index map to colour space\n"); 1251 | return nullptr; 1252 | } 1253 | 1254 | return body_index_in_colour_space; 1255 | } 1256 | #endif 1257 | 1258 | 1259 | -------------------------------------------------------------------------------- /kinect.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file kinect.h 3 | * 4 | * @brief Class of kinect K4 funcionality 5 | * 6 | * @author Juan Terven 7 | * Contact: juan@aifi.io 8 | * 9 | */ 10 | 11 | #ifndef KINECT_H 12 | #define KINECT_H 13 | 14 | #include 15 | #include 16 | #include "utils.h" 17 | #include "calibration.h" 18 | 19 | #ifdef BODY 20 | #include 21 | #endif 22 | 23 | /** 24 | * Implementation of K4a wrapper for python bindings 25 | * 26 | */ 27 | class Kinect { 28 | private: 29 | int res; 30 | // Kinect device 31 | k4a_device_t m_device = NULL; 32 | std::string serial_number; 33 | 34 | // Kinect configuration 35 | k4a_device_configuration_t m_config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL; 36 | 37 | // Maximum timeout 38 | const int32_t TIMEOUT_IN_MS = 1000; 39 | 40 | // capture device 41 | k4a_capture_t m_capture = NULL; 42 | 43 | // color, depth, IR, and body index images 44 | k4a_image_t m_image_c = nullptr; 45 | k4a_image_t m_image_d = nullptr; 46 | k4a_image_t m_image_d_org = nullptr; 47 | k4a_image_t m_image_ir = nullptr; 48 | k4a_image_t m_body_index = nullptr; 49 | bool m_align_depth = false; 50 | 51 | // Sensor data 52 | Imu_sample m_imu_data; 53 | bool m_imu_sensors_available; 54 | 55 | // Body tracking 56 | #ifdef BODY 57 | k4abt_tracker_t m_tracker = NULL; 58 | k4abt_frame_t m_body_frame = NULL; 59 | bool m_body_tracking_available; 60 | uint32_t m_num_bodies; 61 | py::list m_bodies; 62 | #endif 63 | 64 | // calibration and transformation object 65 | k4a_calibration_t m_calibration; 66 | Calibration m_depth_calib; 67 | Calibration m_color_calib; 68 | k4a_transformation_t m_transformation = NULL; 69 | 70 | int initialize(uint8_t deviceIndex, int resolution, bool wfov, 71 | bool binned, uint8_t framerate, bool sensor_color, 72 | bool sensor_depth, bool sensor_ir, bool imu_sensors, 73 | bool body_tracking, bool body_index); 74 | bool align_depth_to_color(int width, int height, 75 | k4a_image_t &transformed_depth_image); 76 | bool align_color_to_depth(k4a_image_t &transformed_color_image); 77 | void update_calibration(Calibration&, bool); 78 | bool depth_image_to_point_cloud(int width, int height, k4a_image_t &xyz_image); 79 | 80 | #ifdef BODY 81 | py::dict get_body_data(k4abt_body_t body); 82 | void change_body_index_to_body_id(uint8_t* image_data, int width, int height); 83 | k4a_image_t convert_body_index_map_to_colour(); 84 | #endif 85 | 86 | public: 87 | Kinect(uint8_t deviceIndex = 0, int resolution = 1080, bool wfov = false, 88 | bool binned = true, uint8_t framerate = 30, bool sensor_color = true, 89 | bool sensor_depth = true, bool sensor_ir = true, 90 | bool imu_sensors = false, bool body_tracking = false, 91 | bool body_index = false); 92 | ~Kinect(); 93 | 94 | void close(); 95 | const int get_frames(bool get_color = true, bool get_depth = true, bool get_ir = true, 96 | bool get_sensors = false, bool get_body = false, 97 | bool get_body_index = false, bool align_depth = false); 98 | Imu_sample get_sensor_data(); 99 | ColorData get_color_data(); 100 | DepthData get_depth_data(); 101 | DepthData get_ir_data(); 102 | BufferPointCloud get_pointcloud(); 103 | BufferColor get_pointcloud_color(); 104 | void save_pointcloud(const char *file_name); 105 | Calibration get_depth_calibration(); 106 | Calibration get_color_calibration(); 107 | std::string get_serial_number(); 108 | void set_exposure(int); 109 | const int get_exposure(); 110 | void set_gain(int); 111 | std::vector > map_coords_color_to_depth(std::vector > &color_coords); 112 | std::vector > map_coords_color_to_3D(std::vector > &color_coords, bool depth_reference); 113 | std::vector > map_coords_depth_to_color(std::vector > &depth_coords); 114 | std::vector > map_coords_depth_to_3D(std::vector > &depth_coords, bool depth_reference); 115 | std::vector > map_coords_3d_to_depth(std::vector > &coords3d, bool depth_reference); 116 | std::vector > map_coords_3d_to_color(std::vector > &coords3d, bool depth_reference); 117 | 118 | // Body tracking functions 119 | #ifdef BODY 120 | int get_num_bodies(); 121 | py::list get_bodies(); 122 | BodyIndexData get_body_index_map(bool returnId=false, bool inColor=false); 123 | #endif 124 | }; 125 | 126 | #endif 127 | -------------------------------------------------------------------------------- /pybinder.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "kinect.h" 4 | 5 | namespace py = pybind11; 6 | 7 | PYBIND11_MODULE(kinz, m) { 8 | 9 | py::class_(m, "BufferDepth", py::buffer_protocol()) 10 | .def_buffer([](BufferDepth &m) -> py::buffer_info { 11 | return py::buffer_info( 12 | m.data(), 13 | sizeof(uint16_t), 14 | py::format_descriptor::format(), 15 | 2, 16 | { m.rows(), m.cols()}, 17 | { m.stride(), sizeof(uint16_t) } 18 | ); 19 | }); 20 | 21 | py::class_(m, "BufferColor", py::buffer_protocol()) 22 | .def_buffer([](BufferColor &m) -> py::buffer_info { 23 | return py::buffer_info( 24 | m.data(), 25 | sizeof(uint8_t), 26 | py::format_descriptor::format(), 27 | 3, 28 | { m.rows(), m.cols(), size_t(4)}, 29 | { sizeof(unsigned char) * 4 * m.cols(), 30 | sizeof(unsigned char) * 4, 31 | sizeof(unsigned char) } 32 | ); 33 | }); 34 | 35 | py::class_(m, "BufferBodyIndex", py::buffer_protocol()) 36 | .def_buffer([](BufferBodyIndex &m) -> py::buffer_info { 37 | return py::buffer_info( 38 | m.data(), 39 | sizeof(uint8_t), 40 | py::format_descriptor::format(), 41 | 2, 42 | { m.rows(), m.cols()}, 43 | { m.stride(), sizeof(uint8_t) } 44 | ); 45 | }); 46 | 47 | py::class_(m, "BufferPointCloud", py::buffer_protocol()) 48 | .def_buffer([](BufferPointCloud &m) -> py::buffer_info { 49 | return py::buffer_info( 50 | m.data(), 51 | sizeof(int16_t), 52 | py::format_descriptor::format(), 53 | 3, 54 | { m.rows(), m.cols(), size_t(3)}, 55 | { m.stride(), 56 | sizeof(int16_t) * 3, 57 | sizeof(int16_t) 58 | } 59 | ); 60 | }); 61 | 62 | 63 | py::class_(m, "Calibration") 64 | .def(py::init()) 65 | .def("get_size", &Calibration::get_size) 66 | .def("get_intrinsics_matrix", &Calibration::get_intrinsics_matrix, 67 | py::arg("extended")) 68 | .def("get_distortion_params", &Calibration::get_distortion_params) 69 | .def("get_rotation_matrix", &Calibration::get_rotation_matrix) 70 | .def("get_translation_vector", &Calibration::get_translation_vector) 71 | .def("get_camera_pose", &Calibration::get_camera_pose) 72 | .def_readonly("width", &Calibration::width); 73 | 74 | 75 | py::class_(m, "Imu_sample") 76 | .def(py::init()) 77 | .def_readonly("temperature", &Imu_sample::temperature) 78 | .def_readonly("acc_x", &Imu_sample::acc_x) 79 | .def_readonly("acc_y", &Imu_sample::acc_y) 80 | .def_readonly("acc_z", &Imu_sample::acc_z) 81 | .def_readonly("acc_timestamp_usec", &Imu_sample::acc_timestamp_usec) 82 | .def_readonly("gyro_x", &Imu_sample::gyro_x) 83 | .def_readonly("gyro_y", &Imu_sample::gyro_y) 84 | .def_readonly("gyro_z", &Imu_sample::gyro_z) 85 | .def_readonly("gyro_timestamp_usec", &Imu_sample::gyro_timestamp_usec); 86 | 87 | 88 | py::class_(m, "ColorData") 89 | .def(py::init()) 90 | .def_readonly("buffer", &ColorData::buffer) 91 | .def_readonly("timestamp_nsec", &ColorData::timestamp_nsec); 92 | 93 | 94 | py::class_(m, "DepthData") 95 | .def(py::init()) 96 | .def_readonly("buffer", &DepthData::buffer) 97 | .def_readonly("timestamp_nsec", &DepthData::timestamp_nsec); 98 | 99 | py::class_(m, "BodyIndexData") 100 | .def(py::init()) 101 | .def_readonly("buffer", &BodyIndexData::buffer) 102 | .def_readonly("timestamp_nsec", &BodyIndexData::timestamp_nsec); 103 | 104 | 105 | py::class_(m, "Kinect") 106 | .def(py::init(), 107 | py::arg("deviceIndex")=0, py::arg("resolution")=1080, py::arg("wfov")=false, 108 | py::arg("binned")=true, py::arg("framerate")=30, py::arg("sensor_color")=true, 109 | py::arg("sensor_depth")=true, py::arg("sensor_ir")=true, 110 | py::arg("imu_sensors")=false, py::arg("body_tracking")=false, 111 | py::arg("body_index")=false) 112 | .def("get_frames", &Kinect::get_frames, "Read frames from Kinect", 113 | py::arg("get_color")=true, py::arg("get_depth")=true, 114 | py::arg("get_ir")=true, py::arg("get_sensors")=false, 115 | py::arg("get_body")=false, py::arg("get_body_index")=false, 116 | py::arg("align_depth")=false) 117 | .def("get_sensor_data", &Kinect::get_sensor_data, "Return sensor struct") 118 | .def("get_color_data", &Kinect::get_color_data, "Return color frame") 119 | .def("get_depth_data", &Kinect::get_depth_data, "Return depth frame") 120 | .def("get_ir_data", &Kinect::get_ir_data, "Return IR frame") 121 | .def("get_pointcloud", &Kinect::get_pointcloud, "Return point cloud") 122 | .def("get_pointcloud_color", &Kinect::get_pointcloud_color, "Return point cloud color values") 123 | .def("save_pointcloud", &Kinect::save_pointcloud, "Save the current pointcloud to a ply file", 124 | py::arg("file_name")) 125 | .def("get_depth_calibration", &Kinect::get_depth_calibration, py::return_value_policy::copy) 126 | .def("get_color_calibration", &Kinect::get_color_calibration, py::return_value_policy::copy) 127 | .def("get_serial_number", &Kinect::get_serial_number, "Return the serial number of the kinect") 128 | .def("close", &Kinect::close) 129 | .def("set_exposure", &Kinect::set_exposure, "Set exposure time", 130 | py::arg("exposure")) 131 | .def("set_gain", &Kinect::set_gain, "Set sensor gain", 132 | py::arg("gain")) 133 | .def("get_exposure", &Kinect::get_exposure, "Get exposure time") 134 | .def("map_coords_color_to_depth", &Kinect::map_coords_color_to_depth, 135 | "Map color pixel coordinates to depth image space") 136 | .def("map_coords_color_to_3D", &Kinect::map_coords_color_to_3D, 137 | "Map color pixel coordinates to 3D space of depth camera", 138 | py::arg("color_coords"), py::arg("depth_reference")=true) 139 | .def("map_coords_depth_to_color", &Kinect::map_coords_depth_to_color, 140 | "Map depth pixel coordinates to color image space") 141 | .def("map_coords_depth_to_3D", &Kinect::map_coords_depth_to_3D, 142 | "Map depth pixel coordinates to 3D space of depth camera", 143 | py::arg("depth_coords"), py::arg("depth_reference")=true) 144 | .def("map_coords_3d_to_depth", &Kinect::map_coords_3d_to_depth, 145 | "Map 3D coordinates to depth space", 146 | py::arg("coords3d"), py::arg("depth_reference")=true) 147 | .def("map_coords_3d_to_color", &Kinect::map_coords_3d_to_color, 148 | "Map 3D coordinates to color space", 149 | py::arg("coords3d"), py::arg("depth_reference")=true) 150 | #ifdef BODY 151 | .def("get_num_bodies", &Kinect::get_num_bodies, "Get number of bodies found") 152 | .def("get_bodies", &Kinect::get_bodies, "Get bodies list") 153 | .def("get_body_index_map", &Kinect::get_body_index_map, "Return body index map frame", 154 | py::arg("returnId")=false, py::arg("inColor")=false) 155 | #endif 156 | ; 157 | } -------------------------------------------------------------------------------- /resources/0002-color_vis.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jrterven/KinZ-Python/5afced8977da538337cefc798d3d181ec284821e/resources/0002-color_vis.jpg -------------------------------------------------------------------------------- /resources/0002-depth_vis.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jrterven/KinZ-Python/5afced8977da538337cefc798d3d181ec284821e/resources/0002-depth_vis.png -------------------------------------------------------------------------------- /resources/0004-color_vis.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jrterven/KinZ-Python/5afced8977da538337cefc798d3d181ec284821e/resources/0004-color_vis.jpg -------------------------------------------------------------------------------- /resources/0004-depth_vis.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jrterven/KinZ-Python/5afced8977da538337cefc798d3d181ec284821e/resources/0004-depth_vis.png -------------------------------------------------------------------------------- /resources/nfov.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jrterven/KinZ-Python/5afced8977da538337cefc798d3d181ec284821e/resources/nfov.png -------------------------------------------------------------------------------- /resources/wfov.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jrterven/KinZ-Python/5afced8977da538337cefc798d3d181ec284821e/resources/wfov.png -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | """ 2 | Installs KinZ library 3 | """ 4 | from setuptools import setup, Extension 5 | from setuptools.command.build_ext import build_ext 6 | import sys 7 | import setuptools 8 | 9 | __version__ = '1.0' 10 | 11 | class get_pybind_include(object): 12 | """Helper class to determine the pybind11 include path 13 | The purpose of this class is to postpone importing pybind11 14 | until it is actually installed, so that the ``get_include()`` 15 | method can be invoked. """ 16 | 17 | def __init__(self, user=False): 18 | self.user = user 19 | 20 | def __str__(self): 21 | import pybind11 22 | return pybind11.get_include(self.user) 23 | 24 | ext_modules = [ 25 | Extension( 26 | 'kinz', 27 | ['pybinder.cpp', 'kinect.cpp', 'calibration.cpp', 'utils.cpp'], 28 | include_dirs=[ 29 | # Path to pybind11 headers 30 | get_pybind_include(), 31 | get_pybind_include(user=True) 32 | ], 33 | language='c++' 34 | ), 35 | ] 36 | 37 | # As of Python 3.6, CCompiler has a `has_flag` method. 38 | # cf http://bugs.python.org/issue26689 39 | def has_flag(compiler, flagname): 40 | """Return a boolean indicating whether a flag name is supported on 41 | the specified compiler. 42 | """ 43 | import tempfile 44 | with tempfile.NamedTemporaryFile('w', suffix='.cpp') as f: 45 | f.write('int main (int argc, char **argv) { return 0; }') 46 | try: 47 | compiler.compile([f.name], extra_postargs=[flagname]) 48 | except setuptools.distutils.errors.CompileError: 49 | return False 50 | return True 51 | 52 | 53 | def cpp_flag(compiler): 54 | """Return the -std=c++[11/14/17] compiler flag. 55 | The newer version is prefered over c++11 (when it is available). 56 | """ 57 | flags = ['-std=c++17', '-std=c++14', '-std=c++11'] 58 | 59 | for flag in flags: 60 | if has_flag(compiler, flag): return flag 61 | 62 | raise RuntimeError('Unsupported compiler -- at least C++11 support ' 63 | 'is needed!') 64 | 65 | class BuildExt(build_ext): 66 | """A custom build extension for adding compiler-specific options.""" 67 | c_opts = { 68 | 'msvc': ['/EHsc'], 69 | 'unix': [], 70 | } 71 | l_opts = { 72 | 'msvc': [], 73 | 'unix': ['-l:libk4a.so'], 74 | } 75 | 76 | if sys.platform == 'darwin': 77 | darwin_opts = ['-stdlib=libc++', '-mmacosx-version-min=10.7'] 78 | c_opts['unix'] += darwin_opts 79 | l_opts['unix'] += darwin_opts 80 | 81 | def build_extensions(self): 82 | ct = self.compiler.compiler_type 83 | opts = self.c_opts.get(ct, []) 84 | link_opts = self.l_opts.get(ct, []) 85 | if '-Wstrict-prototypes' in self.compiler.compiler_so: 86 | self.compiler.compiler_so.remove('-Wstrict-prototypes') 87 | #super().build_extensions() 88 | if ct == 'unix': 89 | opts.append('-DVERSION_INFO="%s"' % self.distribution.get_version()) 90 | opts.append(cpp_flag(self.compiler)) 91 | if has_flag(self.compiler, '-fvisibility=hidden'): 92 | opts.append('-fvisibility=hidden') 93 | elif ct == 'msvc': 94 | opts.append('/DVERSION_INFO=\\"%s\\"' % self.distribution.get_version()) 95 | for ext in self.extensions: 96 | ext.extra_compile_args = opts 97 | ext.extra_link_args = link_opts 98 | build_ext.build_extensions(self) 99 | 100 | setup( 101 | name='kinz', 102 | version=__version__, 103 | author='Juan Terven', 104 | author_email='juan@aifi.io', 105 | description='Python bindings for Kinect for Azure', 106 | ext_modules=ext_modules, 107 | install_requires=['pybind11>=2.3'], 108 | setup_requires=['pybind11>=2.3'], 109 | cmdclass={'build_ext': BuildExt}, 110 | zip_safe=False, 111 | ) 112 | -------------------------------------------------------------------------------- /utils.cpp: -------------------------------------------------------------------------------- 1 | #include "utils.h" 2 | 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define PLY_START_HEADER "ply" 9 | #define PLY_END_HEADER "end_header" 10 | #define PLY_ASCII "format ascii 1.0" 11 | #define PLY_ELEMENT_VERTEX "element vertex" 12 | 13 | BufferDepth::BufferDepth() { 14 | m_data = NULL; 15 | m_rows = 0; 16 | m_cols = 0; 17 | m_stride = 0; 18 | } 19 | 20 | BufferDepth::BufferDepth(uint16_t* data, size_t rows, size_t cols, size_t stride) : 21 | m_data(data), m_rows(rows), m_cols(cols), m_stride(stride) { 22 | } 23 | 24 | BufferDepth::~BufferDepth(){} 25 | uint16_t* BufferDepth::data() { 26 | return m_data.get(); 27 | } 28 | size_t BufferDepth::rows() const { 29 | return m_rows; 30 | } 31 | size_t BufferDepth::cols() const { 32 | return m_cols; 33 | } 34 | size_t BufferDepth::stride() const { 35 | return m_stride; 36 | } 37 | 38 | BufferColor::BufferColor() { 39 | m_data = NULL; 40 | m_rows = 0; 41 | m_cols = 0; 42 | m_stride = 0; 43 | } 44 | BufferColor::BufferColor(uint8_t* data, size_t rows, size_t cols, size_t stride) : 45 | m_data(data), m_rows(rows), m_cols(cols), m_stride(stride) { 46 | } 47 | BufferColor::~BufferColor(){} 48 | uint8_t* BufferColor::data() { 49 | return m_data.get(); 50 | } 51 | size_t BufferColor::rows() const { 52 | return m_rows; 53 | } 54 | size_t BufferColor::cols() const { 55 | return m_cols; 56 | } 57 | size_t BufferColor::stride() const { 58 | return m_stride; 59 | } 60 | 61 | BufferBodyIndex::BufferBodyIndex() { 62 | m_data = NULL; 63 | m_rows = 0; 64 | m_cols = 0; 65 | m_stride = 0; 66 | } 67 | 68 | BufferBodyIndex::BufferBodyIndex(uint8_t* data, size_t rows, size_t cols, size_t stride) : 69 | m_data(data), m_rows(rows), m_cols(cols), m_stride(stride) { 70 | } 71 | 72 | BufferBodyIndex::~BufferBodyIndex(){} 73 | uint8_t* BufferBodyIndex::data() { 74 | return m_data.get(); 75 | } 76 | size_t BufferBodyIndex::rows() const { 77 | return m_rows; 78 | } 79 | size_t BufferBodyIndex::cols() const { 80 | return m_cols; 81 | } 82 | size_t BufferBodyIndex::stride() const { 83 | return m_stride; 84 | } 85 | 86 | BufferPointCloud::BufferPointCloud(int16_t* data, size_t rows, size_t cols, size_t stride) : 87 | m_data(data), m_rows(rows), m_cols(cols), m_stride(stride) { 88 | } 89 | 90 | BufferPointCloud::~BufferPointCloud(){} 91 | int16_t *BufferPointCloud::data() { 92 | return m_data.get(); 93 | } 94 | size_t BufferPointCloud::rows() const { 95 | return m_rows; 96 | } 97 | size_t BufferPointCloud::cols() const { 98 | return m_cols; 99 | } 100 | size_t BufferPointCloud::stride() const { 101 | return m_stride; 102 | } 103 | 104 | 105 | struct color_point_t 106 | { 107 | int16_t xyz[3]; 108 | uint8_t rgb[3]; 109 | }; 110 | 111 | 112 | void write_point_cloud(const k4a_image_t point_cloud_image, 113 | const k4a_image_t color_image, 114 | const char *file_name) { 115 | std::vector points; 116 | 117 | int width = k4a_image_get_width_pixels(point_cloud_image); 118 | int height = k4a_image_get_height_pixels(color_image); 119 | 120 | int16_t *point_cloud_image_data = (int16_t *)(void *)k4a_image_get_buffer(point_cloud_image); 121 | uint8_t *color_image_data = k4a_image_get_buffer(color_image); 122 | 123 | for (int i = 0; i < width * height; i++) 124 | { 125 | color_point_t point; 126 | point.xyz[0] = point_cloud_image_data[3 * i + 0]; 127 | point.xyz[1] = point_cloud_image_data[3 * i + 1]; 128 | point.xyz[2] = point_cloud_image_data[3 * i + 2]; 129 | if (point.xyz[2] == 0) 130 | { 131 | continue; 132 | } 133 | 134 | point.rgb[0] = color_image_data[4 * i + 0]; 135 | point.rgb[1] = color_image_data[4 * i + 1]; 136 | point.rgb[2] = color_image_data[4 * i + 2]; 137 | uint8_t alpha = color_image_data[4 * i + 3]; 138 | 139 | if (point.rgb[0] == 0 && point.rgb[1] == 0 && point.rgb[2] == 0 && alpha == 0) 140 | { 141 | continue; 142 | } 143 | 144 | points.push_back(point); 145 | } 146 | 147 | // save to the ply file 148 | std::ofstream ofs(file_name); // text mode first 149 | ofs << PLY_START_HEADER << std::endl; 150 | ofs << PLY_ASCII << std::endl; 151 | ofs << PLY_ELEMENT_VERTEX << " " << points.size() << std::endl; 152 | ofs << "property float x" << std::endl; 153 | ofs << "property float y" << std::endl; 154 | ofs << "property float z" << std::endl; 155 | ofs << "property uchar red" << std::endl; 156 | ofs << "property uchar green" << std::endl; 157 | ofs << "property uchar blue" << std::endl; 158 | ofs << PLY_END_HEADER << std::endl; 159 | ofs.close(); 160 | 161 | std::stringstream ss; 162 | for (size_t i = 0; i < points.size(); ++i) 163 | { 164 | // image data is BGR 165 | ss << (float)points[i].xyz[0] << " " << (float)points[i].xyz[1] << " " << (float)points[i].xyz[2]; 166 | ss << " " << (float)points[i].rgb[2] << " " << (float)points[i].rgb[1] << " " << (float)points[i].rgb[0]; 167 | ss << std::endl; 168 | } 169 | std::ofstream ofs_text(file_name, std::ios::out | std::ios::app); 170 | ofs_text.write(ss.str().c_str(), (std::streamsize)ss.str().length()); 171 | } 172 | -------------------------------------------------------------------------------- /utils.h: -------------------------------------------------------------------------------- 1 | #ifndef KINUTILS_H 2 | #define KINUTILS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class BufferDepth { 10 | public: 11 | BufferDepth(); 12 | BufferDepth(uint16_t* data, size_t rows, size_t cols, size_t stride); 13 | ~BufferDepth(); 14 | uint16_t *data(); 15 | size_t rows() const; 16 | size_t cols() const; 17 | size_t stride() const; 18 | private: 19 | std::shared_ptr m_data; 20 | size_t m_rows, m_cols, m_stride; 21 | }; 22 | 23 | class BufferColor { 24 | public: 25 | BufferColor(); 26 | BufferColor(uint8_t* data, size_t rows, size_t cols, size_t stride); 27 | ~BufferColor(); 28 | uint8_t *data(); 29 | size_t rows() const; 30 | size_t cols() const; 31 | size_t stride() const; 32 | private: 33 | std::shared_ptr m_data; 34 | size_t m_rows, m_cols, m_stride; 35 | }; 36 | 37 | class BufferBodyIndex { 38 | public: 39 | BufferBodyIndex(); 40 | BufferBodyIndex(uint8_t* data, size_t rows, size_t cols, size_t stride); 41 | ~BufferBodyIndex(); 42 | uint8_t *data(); 43 | size_t rows() const; 44 | size_t cols() const; 45 | size_t stride() const; 46 | private: 47 | std::shared_ptr m_data; 48 | size_t m_rows, m_cols, m_stride; 49 | }; 50 | 51 | class BufferPointCloud { 52 | public: 53 | BufferPointCloud(int16_t* data, size_t rows, size_t cols, size_t stride); 54 | ~BufferPointCloud(); 55 | int16_t *data(); 56 | size_t rows() const; 57 | size_t cols() const; 58 | size_t stride() const; 59 | private: 60 | std::shared_ptr m_data; 61 | size_t m_rows, m_cols, m_stride; 62 | }; 63 | 64 | struct ColorData { 65 | BufferColor buffer; 66 | uint64_t timestamp_nsec; 67 | }; 68 | 69 | struct DepthData { 70 | BufferDepth buffer; 71 | uint64_t timestamp_nsec; 72 | }; 73 | 74 | struct BodyIndexData { 75 | BufferBodyIndex buffer; 76 | uint64_t timestamp_nsec; 77 | }; 78 | 79 | struct Imu_sample { 80 | float temperature; 81 | float acc_x, acc_y, acc_z; 82 | uint64_t acc_timestamp_usec; 83 | float gyro_x, gyro_y, gyro_z; 84 | uint64_t gyro_timestamp_usec; 85 | }; 86 | 87 | void write_point_cloud(const k4a_image_t point_cloud_image, 88 | const k4a_image_t color_image, 89 | const char *file_name); 90 | 91 | #endif 92 | --------------------------------------------------------------------------------