in body-tracking-samples/camera_space_transform_sample/main.cpp [83:250]
int main()
{
k4a_device_configuration_t device_config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
device_config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
device_config.color_resolution = K4A_COLOR_RESOLUTION_720P;
k4a_device_t device;
VERIFY(k4a_device_open(0, &device), "Open K4A Device failed!");
VERIFY(k4a_device_start_cameras(device, &device_config), "Start K4A cameras failed!");
// Make sure to pass in the correct device config for both depth camera and color camera to get the correct sensor calibration
k4a_calibration_t sensor_calibration;
VERIFY(k4a_device_get_calibration(device, device_config.depth_mode, device_config.color_resolution, &sensor_calibration),
"Get depth camera calibration failed!");
// Create transformation handle to perform the body index map space transform
k4a_transformation_t transformation = NULL;
transformation = k4a_transformation_create(&sensor_calibration);
if (transformation == NULL)
{
printf("Failed to create transformation from sensor calibration!");
exit(1);
}
k4abt_tracker_t tracker = NULL;
k4abt_tracker_configuration_t tracker_config = K4ABT_TRACKER_CONFIG_DEFAULT;
VERIFY(k4abt_tracker_create(&sensor_calibration, tracker_config, &tracker), "Body tracker initialization failed!");
// Preallocated the buffers to hold the depth image in color space and the body index map in color space
int color_image_width_pixels = sensor_calibration.color_camera_calibration.resolution_width;
int color_image_height_pixels = sensor_calibration.color_camera_calibration.resolution_height;
k4a_image_t depth_image_in_color_space = NULL;
VERIFY(k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16,
color_image_width_pixels,
color_image_height_pixels,
color_image_width_pixels * (int)sizeof(uint16_t),
&depth_image_in_color_space), "Failed to create empty image for the depth image in color space");
k4a_image_t body_index_map_in_color_space = NULL;
VERIFY(k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM8,
color_image_width_pixels,
color_image_height_pixels,
color_image_width_pixels * (int)sizeof(uint8_t),
&body_index_map_in_color_space), "Failed to create empty image for the body index map in color space");
int frame_count = 0;
do
{
k4a_capture_t sensor_capture;
k4a_wait_result_t get_capture_result = k4a_device_get_capture(device, &sensor_capture, K4A_WAIT_INFINITE);
if (get_capture_result == K4A_WAIT_RESULT_SUCCEEDED)
{
frame_count++;
printf("Start processing frame %d\n", frame_count);
k4a_wait_result_t queue_capture_result = k4abt_tracker_enqueue_capture(tracker, sensor_capture, K4A_WAIT_INFINITE);
k4a_capture_release(sensor_capture);
if (queue_capture_result == K4A_WAIT_RESULT_TIMEOUT)
{
// It should never hit timeout when K4A_WAIT_INFINITE is set.
printf("Error! Add capture to tracker process queue timeout!\n");
break;
}
else if (queue_capture_result == K4A_WAIT_RESULT_FAILED)
{
printf("Error! Add capture to tracker process queue failed!\n");
break;
}
k4abt_frame_t body_frame = NULL;
k4a_wait_result_t pop_frame_result = k4abt_tracker_pop_result(tracker, &body_frame, K4A_WAIT_INFINITE);
if (pop_frame_result == K4A_WAIT_RESULT_SUCCEEDED)
{
uint32_t num_bodies = k4abt_frame_get_num_bodies(body_frame);
printf("%u bodies are detected!\n", num_bodies);
// Transform each 3d joints from 3d depth space to 2d color image space
for (uint32_t i = 0; i < num_bodies; i++)
{
printf("Person[%u]:\n", i);
k4abt_skeleton_t skeleton;
VERIFY(k4abt_frame_get_body_skeleton(body_frame, i, &skeleton), "Get body from body frame failed!");
for (int joint_id = 0; joint_id < (int)K4ABT_JOINT_COUNT; joint_id++)
{
k4a_float2_t joint_in_color_2d;
bool valid = transform_joint_from_depth_3d_to_color_2d(
&sensor_calibration,
skeleton.joints[joint_id].position,
joint_in_color_2d);
if (valid)
{
printf("Joint[%d]: Pixel Location at Color Image ( %f, %f) \n",
joint_id, joint_in_color_2d.v[0], joint_in_color_2d.v[1]);
}
else
{
printf("Joint[%d]: Invalid Pixel Location \n", joint_id);
}
}
}
// Transform the body index map from the depth space to color space
k4a_image_t body_index_map_in_depth_space = k4abt_frame_get_body_index_map(body_frame);
if (body_index_map_in_depth_space != NULL)
{
// Depth image is needed in order to perform the body index map space transform
k4a_image_t depth_image = k4a_capture_get_depth_image(sensor_capture);
transform_body_index_map_from_depth_to_color(
transformation,
depth_image,
body_index_map_in_depth_space,
depth_image_in_color_space,
body_index_map_in_color_space);
print_body_index_map_middle_line(body_index_map_in_color_space);
k4a_image_release(body_index_map_in_depth_space);
k4a_image_release(depth_image);
}
else
{
printf("Error: Fail to generate bodyindex map!\n");
}
k4abt_frame_release(body_frame);
}
else if (pop_frame_result == K4A_WAIT_RESULT_TIMEOUT)
{
// It should never hit timeout when K4A_WAIT_INFINITE is set.
printf("Error! Pop body frame result timeout!\n");
break;
}
else
{
printf("Pop body frame result failed!\n");
break;
}
}
else if (get_capture_result == K4A_WAIT_RESULT_TIMEOUT)
{
// It should never hit time out when K4A_WAIT_INFINITE is set.
printf("Error! Get depth frame time out!\n");
break;
}
else
{
printf("Get depth capture returned error: %d\n", get_capture_result);
break;
}
} while (frame_count < 100);
printf("Finished body tracking processing!\n");
k4a_image_release(depth_image_in_color_space);
k4a_image_release(body_index_map_in_color_space);
k4a_transformation_destroy(transformation);
k4abt_tracker_shutdown(tracker);
k4abt_tracker_destroy(tracker);
k4a_device_stop_cameras(device);
k4a_device_close(device);
return 0;
}