kinect/codes/Azure-Kinect-Samples/body-tracking-samples/simple_sample/main.c

180 lines
7.3 KiB
C
Raw Permalink Normal View History

2024-03-06 18:05:53 +00:00
// 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;
}