180 lines
7.3 KiB
C
180 lines
7.3 KiB
C
|
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||
|
|
// Licensed under the MIT License.
|
||
|
|
|
||
|
|
#include <assert.h>
|
||
|
|
#include <stdbool.h>
|
||
|
|
#include <stdio.h>
|
||
|
|
#include <stdlib.h>
|
||
|
|
#include <time.h>
|
||
|
|
|
||
|
|
#include <k4a/k4a.h>
|
||
|
|
#include <k4abt.h>
|
||
|
|
|
||
|
|
#define VERIFY(result, error) \
|
||
|
|
if(result != K4A_RESULT_SUCCEEDED) \
|
||
|
|
{ \
|
||
|
|
printf("%s \n - (File: %s, Function: %s, Line: %d)\n", error, __FILE__, __FUNCTION__, __LINE__); \
|
||
|
|
exit(1); \
|
||
|
|
} \
|
||
|
|
|
||
|
|
void print_body_information(k4abt_body_t body)
|
||
|
|
{
|
||
|
|
printf("Body ID: %u\n", body.id);
|
||
|
|
for (int i = 0; i < (int)K4ABT_JOINT_COUNT; i++)
|
||
|
|
{
|
||
|
|
k4a_float3_t position = body.skeleton.joints[i].position;
|
||
|
|
k4a_quaternion_t orientation = body.skeleton.joints[i].orientation;
|
||
|
|
k4abt_joint_confidence_level_t confidence_level = body.skeleton.joints[i].confidence_level;
|
||
|
|
printf("Joint[%d]: Position[mm] ( %f, %f, %f ); Orientation ( %f, %f, %f, %f); Confidence Level (%d) \n",
|
||
|
|
i, position.v[0], position.v[1], position.v[2], orientation.v[0], orientation.v[1], orientation.v[2], orientation.v[3], confidence_level);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void print_body_index_map_middle_line(k4a_image_t body_index_map)
|
||
|
|
{
|
||
|
|
uint8_t* body_index_map_buffer = k4a_image_get_buffer(body_index_map);
|
||
|
|
|
||
|
|
// Given body_index_map pixel type should be uint8, the stride_byte should be the same as width
|
||
|
|
// TODO: Since there is no API to query the byte-per-pixel information, we have to compare the width and stride to
|
||
|
|
// know the information. We should replace this assert with proper byte-per-pixel query once the API is provided by
|
||
|
|
// K4A SDK.
|
||
|
|
assert(k4a_image_get_stride_bytes(body_index_map) == k4a_image_get_width_pixels(body_index_map));
|
||
|
|
|
||
|
|
int middle_line_num = k4a_image_get_height_pixels(body_index_map) / 2;
|
||
|
|
body_index_map_buffer = body_index_map_buffer + middle_line_num * k4a_image_get_width_pixels(body_index_map);
|
||
|
|
|
||
|
|
printf("BodyIndexMap at Line %d:\n", middle_line_num);
|
||
|
|
for (int i = 0; i < k4a_image_get_width_pixels(body_index_map); i++)
|
||
|
|
{
|
||
|
|
printf("%u, ", *body_index_map_buffer);
|
||
|
|
body_index_map_buffer++;
|
||
|
|
}
|
||
|
|
printf("\n");
|
||
|
|
}
|
||
|
|
|
||
|
|
int main()
|
||
|
|
{
|
||
|
|
k4a_device_configuration_t device_config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
|
||
|
|
device_config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
|
||
|
|
|
||
|
|
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!");
|
||
|
|
|
||
|
|
k4a_calibration_t sensor_calibration;
|
||
|
|
VERIFY(k4a_device_get_calibration(device, device_config.depth_mode, K4A_COLOR_RESOLUTION_OFF, &sensor_calibration),
|
||
|
|
"Get depth camera calibration failed!");
|
||
|
|
|
||
|
|
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!");
|
||
|
|
|
||
|
|
double total_success_tracker_time = 0.;
|
||
|
|
uint32_t total_success_body_count = 0;
|
||
|
|
int success_frame_count = 0;
|
||
|
|
|
||
|
|
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);
|
||
|
|
|
||
|
|
clock_t begin_clock = clock();
|
||
|
|
|
||
|
|
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)
|
||
|
|
{
|
||
|
|
clock_t end_clock = clock();
|
||
|
|
double time_spent = (double)(end_clock - begin_clock) / CLOCKS_PER_SEC;
|
||
|
|
total_success_tracker_time += time_spent;
|
||
|
|
success_frame_count++;
|
||
|
|
|
||
|
|
printf("Time spent for this frame: %f s\n", time_spent);
|
||
|
|
|
||
|
|
uint32_t num_bodies = k4abt_frame_get_num_bodies(body_frame);
|
||
|
|
printf("%u bodies are detected!\n", num_bodies);
|
||
|
|
total_success_body_count += num_bodies;
|
||
|
|
|
||
|
|
for (uint32_t i = 0; i < num_bodies; i++)
|
||
|
|
{
|
||
|
|
k4abt_body_t body;
|
||
|
|
VERIFY(k4abt_frame_get_body_skeleton(body_frame, i, &body.skeleton), "Get body from body frame failed!");
|
||
|
|
body.id = k4abt_frame_get_body_id(body_frame, i);
|
||
|
|
|
||
|
|
print_body_information(body);
|
||
|
|
}
|
||
|
|
|
||
|
|
k4a_image_t body_index_map = k4abt_frame_get_body_index_map(body_frame);
|
||
|
|
if (body_index_map != NULL)
|
||
|
|
{
|
||
|
|
print_body_index_map_middle_line(body_index_map);
|
||
|
|
k4a_image_release(body_index_map);
|
||
|
|
}
|
||
|
|
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");
|
||
|
|
|
||
|
|
printf("Total body tracking success frame count: %d\n", success_frame_count);
|
||
|
|
printf("Average number of body detected per frame: %f\n", (double)total_success_body_count / success_frame_count);
|
||
|
|
printf("Average success frame tracker processing time: %f\n", total_success_tracker_time / success_frame_count);
|
||
|
|
|
||
|
|
k4abt_tracker_shutdown(tracker);
|
||
|
|
k4abt_tracker_destroy(tracker);
|
||
|
|
k4a_device_stop_cameras(device);
|
||
|
|
k4a_device_close(device);
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|