def transform_color_xy_to_pcd_xyz(self, input_color_image_handle,
input_depth_image_handle, color_xy):
"""
:param color_xy np.array([x,y])
author:weiwei
date: 20210712
"""
# image_format = _k4a_types.K4A_IMAGE_FORMAT_COLOR_BGRA32
# image_width = self.image_get_width_pixels(input_color_image_handle)
# image_height = self.image_get_height_pixels(input_color_image_handle)
# image_stride = 0
# tmp_color_image_handle = _k4a.k4a_image_t()
# self.image_create(image_format, image_width, image_height, image_stride, tmp_color_image_handle)
calibration = self.get_calibration()
position_2d = _k4a.k4a_float2_t()
position_2d.xy.x = color_xy[0]
position_2d.xy.y = color_xy[1]
depth_on_color = self.transform_depth_to_color(
input_depth_image_handle, input_color_image_handle)
source_depth_mm = depth_on_color[color_xy[1], color_xy[0]]
target_point3d_mm = _k4a.k4a_float3_t()
valid = ctypes.c_int()
_k4a.VERIFY(
self.k4a.k4a_calibration_2d_to_3d(calibration, position_2d,
source_depth_mm,
_k4a.K4A_CALIBRATION_TYPE_COLOR,
_k4a.K4A_CALIBRATION_TYPE_DEPTH,
target_point3d_mm, valid),
"Transformation from color to depth failed!")
xyz = target_point3d_mm.xyz
return np.array([xyz.x, xyz.y, xyz.z]) * 1e-3
def transform_depth_image_to_point_cloud(
self, depth_image_handle: _k4a.k4a_image_t):
"""
Transforms the depth map to point clouds
:param depth_image_handle (k4a_image_t): Handle to the Image
:return point_cloud numpy array
author: weiwei
date: 20210708
"""
point_cloud = _k4a_types.k4a_image_t()
self.image_create(_k4a_types.K4A_IMAGE_FORMAT_CUSTOM,
self.image_get_width_pixels(depth_image_handle),
self.image_get_height_pixels(depth_image_handle),
self.image_get_width_pixels(depth_image_handle) * 6,
point_cloud)
_k4a.VERIFY(
self.k4a.k4a_transformation_depth_image_to_point_cloud(
self.transformation_handle, depth_image_handle,
_k4a_types.K4A_CALIBRATION_TYPE_DEPTH, point_cloud),
"Error Occur When Make Point Cloud")
# convert to point cloud
buffer_pointer = self.image_get_buffer(point_cloud)
image_size = self.image_get_size(point_cloud)
image_width = self.image_get_width_pixels(point_cloud)
image_height = self.image_get_height_pixels(point_cloud)
buffer_array = np.ctypeslib.as_array(buffer_pointer,
shape=(image_size, ))
return np.frombuffer(buffer_array, dtype=np.int16).reshape(
image_height * image_width, 3) / 1000
def device_get_serialnum(self):
"""
Get the Azure Kinect device serial number.
Queries the device for its serial number. If the caller needs to know the size of the serial number to allocate
memory, the function should be called once with a NULL serial_number to get the needed size in the
serial_number_size output, and then again with the allocated buffer.
Only a complete serial number will be returned. If the caller's buffer is too small, the function will return
::K4A_BUFFER_RESULT_TOO_SMALL without returning any data in serial_number.
:return: A return of ::K4A_BUFFER_RESULT_SUCCEEDED means that the serial_number has been filled in. If the buffer is too
small the function returns ::K4A_BUFFER_RESULT_TOO_SMALL and the size of the serial number is
returned in the serial_number_size parameter. All other failures return ::K4A_BUFFER_RESULT_FAILED.
"""
# First call to get the size of the buffer
serial_number_size = ctypes.c_size_t()
result = self.k4a.k4a_device_get_serialnum(self.device_handle, None,
serial_number_size)
if result == _k4a.K4A_BUFFER_RESULT_TOO_SMALL:
serial_number = ctypes.create_string_buffer(
serial_number_size.value)
_k4a.VERIFY(
self.k4a.k4a_device_get_serialnum(self.device_handle,
serial_number,
serial_number_size),
"Read serial number failed!")
return serial_number.value.decode("utf-8")
def device_open(self, index=0):
"""
Open an Azure Kinect device.
If successful, k4a_device_open() will return a device handle in the device_handle parameter.
This handle grants exclusive access to the device and may be used in the other Azure Kinect API calls.
When done with the device, close the handle with k4a_device_close()
:param index: int, the index of the device to open, starting with
:return:
"""
_k4a.VERIFY(self.k4a.k4a_device_open(index, self.device_handle),
"Open K4A Device failed!")
def device_start_imu(self):
"""
Starts the IMU sample stream.
Call this API to start streaming IMU data. It is not valid to call this function a second time on the same
k4a_device_t until k4a_device_stop_imu() has been called.
This function is dependent on the state of the cameras. The color or depth camera must be started before the IMU.
K4A_RESULT_FAILED will be returned if one of the cameras is not running.
"""
if self.cameras_running:
if not self.imu_running:
_k4a.VERIFY(self.k4a.k4a_device_start_imu(self.device_handle),
"Start K4A IMU failed!")
self.imu_running = True
else:
print("\nTurn on cameras before running IMU.\n")
def device_get_calibration(self, depth_mode, color_resolution,
calibration):
"""
Get the camera calibration for the entire Azure Kinect device.
The calibration represents the data needed to transform between the camera views and may be
different for each operating depth_mode and color_resolution the device is configured to operate in.
The calibration output is used as input to all calibration and transformation functions.
:param depth_mode(k4a_depth_mode_t): Mode in which depth camera is operated.
:param color_resolution(k4a_color_resolution_t): Resolution in which color camera is operated.
:param calibration(k4a_calibration_t): Location to write the calibration
:return K4A_RESULT_SUCCEEDED if calibration was successfully written. ::K4A_RESULT_FAILED otherwise.
"""
_k4a.VERIFY(
self.k4a.k4a_device_get_calibration(self.device_handle, depth_mode,
color_resolution, calibration),
"Get calibration failed!")
def bt_project_skeleton(self, skeleton, dest_camera=None):
if dest_camera is None:
dest_camera = _k4a.K4A_CALIBRATION_TYPE_DEPTH
# Project using the calibration of the camera for the image
position_2d = _k4a.k4a_float2_t()
valid = ctypes.c_int()
skeleton2D = _k4abt.k4abt_skeleton2D_t()
for jointID, joint in enumerate(skeleton.joints):
_k4a.VERIFY(
self.k4a.k4a_calibration_3d_to_2d(
self.body_tracker.sensor_calibration, joint.position,
_k4a.K4A_CALIBRATION_TYPE_DEPTH, dest_camera, position_2d,
valid), "Project skeleton failed")
skeleton2D.joints2D[jointID].position = position_2d
skeleton2D.joints2D[
jointID].confidence_level = joint.confidence_level
return skeleton2D
def device_start_cameras(self, device_config=None):
"""
Starts color and depth camera capture.
Individual sensors configured to run will now start to stream captured data..
It is not valid to call k4a_device_start_cameras() a second time on the same k4a_device_t until
k4a_device_stop_cameras() has been called.
:param device_config (k4a_device_configuration_t): The configuration we want to run the device in. This can be initialized with ::K4A_DEVICE_CONFIG_INIT_DEFAULT.
:return None
"""
if device_config is not None:
self.config = device_config
if not self.cameras_running:
_k4a.VERIFY(
self.k4a.k4a_device_start_cameras(self.device_handle,
self.config.current_config),
"Start K4A cameras failed!")
self.cameras_running = True
def transformation_color_image_to_depth_camera(
self, transformation_handle, input_depth_image_handle,
input_color_image_handle, transformed_color_image_handle):
"""
Transforms the color image into the geometry of the depth camera.
transformed_color_image must have a width and height matching the width and height of the depth camera in the mode
specified by the k4a_calibration_t used to create the transformation_handle with k4a_transformation_create().
This produces a depth image for which each pixel matches the corresponding pixel coordinates of the depth camera.
:param transformation_handle (k4a_transformation_t): Transformation handle.
input_color_image_handle (k4a_image_t): Handle to input color image.
transformed_color_image_handle (k4a_image_t): Handle to output transformed color image.
:return K4A_RESULT_SUCCEEDED if transformed_depth_image was successfully written and ::K4A_RESULT_FAILED otherwise.
"""
_k4a.VERIFY(
self.k4a.k4a_transformation_color_image_to_depth_camera(
transformation_handle, input_depth_image_handle,
input_color_image_handle, transformed_color_image_handle),
"Transformation from color to depth failed!")
def device_get_imu_sample(self, timeout_in_ms=_k4a.K4A_WAIT_INFINITE):
"""
Reads an IMU sample.
Gets the next sample in the streamed sequence of IMU samples from the device. If a new sample is not currently
available, this function will block until the timeout is reached. The API will buffer at least two camera capture
intervals worth of samples before dropping the oldest sample. Callers needing to capture all data need to ensure they
read the data as fast as the data is being produced on average.
Upon successfully reading a sample this function will return success and populate imu_sample.
If a sample is not available in the configured timeout_in_ms, then the API will return ::K4A_WAIT_RESULT_TIMEOUT.
:param timeout_in_ms (int):Specifies the time in milliseconds the function should block waiting for the capture. If set to 0, the function will
return without blocking. Passing a value of #K4A_WAIT_INFINITE will block indefinitely until data is available, the
device is disconnected, or another error occurs.
:return
"""
if self.imu_running:
_k4a.VERIFY(
self.k4a.k4a_device_get_imu_sample(self.device_handle,
self.imu_sample,
timeout_in_ms),
"Get IMU failed!")
def image_create(self, image_format, width_pixels, height_pixels,
stride_bytes, image_handle):
"""
Create an image.
This function is used to create images of formats that have consistent stride. The function is not suitable for
compressed formats that may not be represented by the same number of bytes per line.
For most image formats, the function will allocate an image buffer of size height_pixels * stride_bytes.
Buffers #K4A_IMAGE_FORMAT_COLOR_NV12 format will allocate an additional height_pixels / 2 set of lines (each of
stride_bytes). This function cannot be used to allocate #K4A_IMAGE_FORMAT_COLOR_MJPG buffers.
:param image_format: k4a_image_format_t, the format of the image that will be stored in this image container.
:param width_pixels: int, width in pixels
:param height_pixels: int, height in pixels
:param stride_bytes: int, the number of bytes per horizontal line of the image.
If set to 0, the stride will be set to the minimum size given the format and width_pixels.
:param image_handle: k4a_image_t, pointer to store image handle in.
:return: #K4A_RESULT_SUCCEEDED on success. Errors are indicated with #K4A_RESULT_FAILED.
"""
_k4a.VERIFY(
self.k4a.k4a_image_create(image_format, width_pixels,
height_pixels, stride_bytes,
image_handle), "Create image failed!")