Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Body Tracking with Kinect v2 C++
#include <Kinect.h>
#include <iostream>
void processBodies(const unsigned int &bodyCount, IBody **bodies);
template<class Interface>
static inline void safeRelease(Interface *&interfaceToRelease)
{
if (interfaceToRelease != nullptr) {
interfaceToRelease->Release();
interfaceToRelease = nullptr;
}
}
int main(int argc, char *argv[])
{
IKinectSensor *sensor = nullptr;
IBodyFrameReader *bodyFrameReader = nullptr;
//Get the default Kinect sensor
HRESULT hr = GetDefaultKinectSensor(&sensor);
//If the function succeeds, open the sensor
if (SUCCEEDED(hr)) {
hr = sensor->Open();
if (SUCCEEDED(hr)) {
//Get a body frame source from which we can get our body frame reader
IBodyFrameSource *bodyFrameSource = nullptr;
hr = sensor->get_BodyFrameSource(&bodyFrameSource);
if (SUCCEEDED(hr)) {
hr = bodyFrameSource->OpenReader(&bodyFrameReader);
}
//We're done with bodyFrameSource, so we'll release it
safeRelease(bodyFrameSource);
}
}
if (sensor == nullptr || FAILED(hr)) {
std::cerr << "Cannot find any sensors.\n";
return E_FAIL;
}
while (bodyFrameReader != nullptr) {
IBodyFrame *bodyFrame = nullptr;
hr = bodyFrameReader->AcquireLatestFrame(&bodyFrame);
if (SUCCEEDED(hr)) {
IBody *bodies[BODY_COUNT] = {0};
hr = bodyFrame->GetAndRefreshBodyData(_countof(bodies), bodies);
if (SUCCEEDED(hr)) {
processBodies(BODY_COUNT, bodies);
//After body processing is done, we're done with our bodies so release them.
for (unsigned int bodyIndex = 0; bodyIndex < _countof(bodies); bodyIndex++) {
safeRelease(bodies[bodyIndex]);
}
safeRelease(bodyFrame);
}
}
else if (sensor) {
BOOLEAN isSensorAvailable = false;
hr = sensor->get_IsAvailable(&isSensorAvailable);
if (SUCCEEDED(hr) && isSensorAvailable == false) {
std::cerr << "No available sensor is found.\n";
}
}
else {
std::cerr << "Trouble reading the body frame.\n";
}
}
return 0;
}
void processBodies(const unsigned int &bodyCount, IBody **bodies)
{
for (unsigned int bodyIndex = 0; bodyIndex < bodyCount; bodyIndex++) {
IBody *body = bodies[bodyIndex];
//Get the tracking status for the body, if it's not tracked we'll skip it
BOOLEAN isTracked = false;
HRESULT hr = body->get_IsTracked(&isTracked);
if (FAILED(hr) || isTracked == false) {
continue;
}
//If we're here the body is tracked so lets get the joint properties for this skeleton
Joint joints[JointType_Count];
hr = body->GetJoints(_countof(joints), joints);
if (SUCCEEDED(hr)) {
//Let's print the head's position
const CameraSpacePoint &headPos = joints[JointType_Head].Position;
const CameraSpacePoint &leftHandPos = joints[JointType_HandLeft].Position;
//Let's check if the use has his hand up
if (leftHandPos.Y >= headPos.Y) {
std::cout << "LEFT HAND UP!!\n";
}
HandState leftHandState;
hr = body->get_HandLeftState(&leftHandState);
if (SUCCEEDED(hr)) {
if (leftHandState == HandState_Closed) {
std::cout << "CLOSED HAND\n";
}
else if (leftHandState == HandState_Open) {
std::cout << "OPEN HAND\n";
}
else if (leftHandState == HandState_Lasso) {
std::cout << "PEW PEW HANDS\n";
}
else if (leftHandState == HandState_NotTracked) {
std::cout << "HAND IS NOT TRACKED\n";
}
else if (leftHandState == HandState_Unknown) {
std::cout << "HANDS STATE IS UNKNOWN\n";
}
}
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.
You signed in with another tab or window. Reload to refresh your session. You signed out in another tab or window. Reload to refresh your session.