kinect/codes/Azure-Kinect-Samples/body-tracking-samples/simple_cpp_sample/main.cpp

124 lines
4.6 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 <iostream>
#include <k4a/k4a.hpp>
#include <k4abt.hpp>
void print_body_information(k4abt_body_t body)
{
std::cout << "Body ID: " << body.id << std::endl;
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 body_index_map)
{
uint8_t* body_index_map_buffer = body_index_map.get_buffer();
// 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(body_index_map.get_stride_bytes() == body_index_map.get_width_pixels());
int middle_line_num = body_index_map.get_height_pixels() / 2;
body_index_map_buffer = body_index_map_buffer + middle_line_num * body_index_map.get_width_pixels();
std::cout << "BodyIndexMap at Line " << middle_line_num << ":" << std::endl;
for (int i = 0; i < body_index_map.get_width_pixels(); i++)
{
std::cout << (int)*body_index_map_buffer << ", ";
body_index_map_buffer++;
}
std::cout << std::endl;
}
int main()
{
try
{
k4a_device_configuration_t device_config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
device_config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
k4a::device device = k4a::device::open(0);
device.start_cameras(&device_config);
k4a::calibration sensor_calibration = device.get_calibration(device_config.depth_mode, device_config.color_resolution);
k4abt::tracker tracker = k4abt::tracker::create(sensor_calibration);
int frame_count = 0;
do
{
k4a::capture sensor_capture;
if (device.get_capture(&sensor_capture, std::chrono::milliseconds(K4A_WAIT_INFINITE)))
{
frame_count++;
std::cout << "Start processing frame " << frame_count << std::endl;
if (!tracker.enqueue_capture(sensor_capture))
{
// It should never hit timeout when K4A_WAIT_INFINITE is set.
std::cout << "Error! Add capture to tracker process queue timeout!" << std::endl;
break;
}
k4abt::frame body_frame = tracker.pop_result();
if (body_frame != nullptr)
{
uint32_t num_bodies = body_frame.get_num_bodies();
std::cout << num_bodies << " bodies are detected!" << std::endl;
for (uint32_t i = 0; i < num_bodies; i++)
{
k4abt_body_t body = body_frame.get_body(i);
print_body_information(body);
}
k4a::image body_index_map = body_frame.get_body_index_map();
if (body_index_map != nullptr)
{
print_body_index_map_middle_line(body_index_map);
}
else
{
std::cout << "Error: Failed to generate bodyindex map!" << std::endl;
}
}
else
{
// It should never hit timeout when K4A_WAIT_INFINITE is set.
std::cout << "Error! Pop body frame result time out!" << std::endl;
break;
}
}
else
{
// It should never hit time out when K4A_WAIT_INFINITE is set.
std::cout << "Error! Get depth frame time out!" << std::endl;
break;
}
} while (frame_count < 100);
std::cout << "Finished body tracking processing!" << std::endl;
}
catch (const std::exception& e)
{
std::cerr << "Failed with exception:" << std::endl
<< " " << e.what() << std::endl;
return 1;
}
return 0;
}