void Device::update() { if ( mFrameReader == 0 ) { return; } IAudioBeamFrame* audioFrame = 0; IBodyFrame* bodyFrame = 0; IBodyIndexFrame* bodyIndexFrame = 0; IColorFrame* colorFrame = 0; IDepthFrame* depthFrame = 0; IMultiSourceFrame* frame = 0; IInfraredFrame* infraredFrame = 0; ILongExposureInfraredFrame* infraredLongExposureFrame = 0; HRESULT hr = mFrameReader->AcquireLatestFrame( &frame ); if ( SUCCEEDED( hr ) && mDeviceOptions.isAudioEnabled() ) { // TODO audio } if ( SUCCEEDED( hr ) && mDeviceOptions.isBodyEnabled() ) { IBodyFrameReference* frameRef = 0; hr = frame->get_BodyFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &bodyFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isBodyIndexEnabled() ) { IBodyIndexFrameReference* frameRef = 0; hr = frame->get_BodyIndexFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &bodyIndexFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isColorEnabled() ) { IColorFrameReference* frameRef = 0; hr = frame->get_ColorFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &colorFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isDepthEnabled() ) { IDepthFrameReference* frameRef = 0; hr = frame->get_DepthFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &depthFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isInfraredEnabled() ) { IInfraredFrameReference* frameRef = 0; hr = frame->get_InfraredFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &infraredFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) && mDeviceOptions.isInfraredLongExposureEnabled() ) { ILongExposureInfraredFrameReference* frameRef = 0; hr = frame->get_LongExposureInfraredFrameReference( &frameRef ); if ( SUCCEEDED( hr ) ) { hr = frameRef->AcquireFrame( &infraredLongExposureFrame ); } if ( frameRef != 0 ) { frameRef->Release(); frameRef = 0; } } if ( SUCCEEDED( hr ) ) { long long timeStamp = 0L; // TODO audio std::vector<Body> bodies; int64_t bodyTime = 0L; IBody* kinectBodies[ BODY_COUNT ] = { 0 }; Vec4f floorClipPlane = Vec4f::zero(); Channel8u bodyIndexChannel; IFrameDescription* bodyIndexFrameDescription = 0; int32_t bodyIndexWidth = 0; int32_t bodyIndexHeight = 0; uint32_t bodyIndexBufferSize = 0; uint8_t* bodyIndexBuffer = 0; int64_t bodyIndexTime = 0L; Surface8u colorSurface; IFrameDescription* colorFrameDescription = 0; int32_t colorWidth = 0; int32_t colorHeight = 0; ColorImageFormat colorImageFormat = ColorImageFormat_None; uint32_t colorBufferSize = 0; uint8_t* colorBuffer = 0; Channel16u depthChannel; IFrameDescription* depthFrameDescription = 0; int32_t depthWidth = 0; int32_t depthHeight = 0; uint16_t depthMinReliableDistance = 0; uint16_t depthMaxReliableDistance = 0; uint32_t depthBufferSize = 0; uint16_t* depthBuffer = 0; Channel16u infraredChannel; IFrameDescription* infraredFrameDescription = 0; int32_t infraredWidth = 0; int32_t infraredHeight = 0; uint32_t infraredBufferSize = 0; uint16_t* infraredBuffer = 0; Channel16u infraredLongExposureChannel; IFrameDescription* infraredLongExposureFrameDescription = 0; int32_t infraredLongExposureWidth = 0; int32_t infraredLongExposureHeight = 0; uint32_t infraredLongExposureBufferSize = 0; uint16_t* infraredLongExposureBuffer = 0; hr = depthFrame->get_RelativeTime( &timeStamp ); // TODO audio if ( mDeviceOptions.isAudioEnabled() ) { } if ( mDeviceOptions.isBodyEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = bodyFrame->get_RelativeTime( &bodyTime ); } if ( SUCCEEDED( hr ) ) { hr = bodyFrame->GetAndRefreshBodyData( BODY_COUNT, kinectBodies ); } if ( SUCCEEDED( hr ) ) { Vector4 v; hr = bodyFrame->get_FloorClipPlane( &v ); floorClipPlane = toVec4f( v ); } if ( SUCCEEDED( hr ) ) { for ( uint8_t i = 0; i < BODY_COUNT; ++i ) { IBody* kinectBody = kinectBodies[ i ]; if ( kinectBody != 0 ) { uint8_t isTracked = false; hr = kinectBody->get_IsTracked( &isTracked ); if ( SUCCEEDED( hr ) && isTracked ) { Joint joints[ JointType_Count ]; kinectBody->GetJoints( JointType_Count, joints ); JointOrientation jointOrientations[ JointType_Count ]; kinectBody->GetJointOrientations( JointType_Count, jointOrientations ); uint64_t id = 0; kinectBody->get_TrackingId( &id ); std::map<JointType, Body::Joint> jointMap; for ( int32_t j = 0; j < JointType_Count; ++j ) { Body::Joint joint( toVec3f( joints[ j ].Position ), toQuatf( jointOrientations[ j ].Orientation ), joints[ j ].TrackingState ); jointMap.insert( pair<JointType, Body::Joint>( static_cast<JointType>( j ), joint ) ); } Body body( id, i, jointMap ); bodies.push_back( body ); } } } } } if ( mDeviceOptions.isBodyIndexEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrame->get_RelativeTime( &bodyIndexTime ); } if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrame->get_FrameDescription( &bodyIndexFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrameDescription->get_Width( &bodyIndexWidth ); } if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrameDescription->get_Height( &bodyIndexHeight ); } if ( SUCCEEDED( hr ) ) { hr = bodyIndexFrame->AccessUnderlyingBuffer( &bodyIndexBufferSize, &bodyIndexBuffer ); } if ( SUCCEEDED( hr ) ) { bodyIndexChannel = Channel8u( bodyIndexWidth, bodyIndexHeight ); memcpy( bodyIndexChannel.getData(), bodyIndexBuffer, bodyIndexWidth * bodyIndexHeight * sizeof( uint8_t ) ); } } if ( mDeviceOptions.isColorEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = colorFrame->get_FrameDescription( &colorFrameDescription ); if ( SUCCEEDED( hr ) ) { float vFov = 0.0f; float hFov = 0.0f; float dFov = 0.0f; colorFrameDescription->get_VerticalFieldOfView( &vFov ); colorFrameDescription->get_HorizontalFieldOfView( &hFov ); colorFrameDescription->get_DiagonalFieldOfView( &dFov ); } } if ( SUCCEEDED( hr ) ) { hr = colorFrameDescription->get_Width( &colorWidth ); } if ( SUCCEEDED( hr ) ) { hr = colorFrameDescription->get_Height( &colorHeight ); } if ( SUCCEEDED( hr ) ) { hr = colorFrame->get_RawColorImageFormat( &colorImageFormat ); } if ( SUCCEEDED( hr ) ) { colorBufferSize = colorWidth * colorHeight * sizeof( uint8_t ) * 4; colorBuffer = new uint8_t[ colorBufferSize ]; hr = colorFrame->CopyConvertedFrameDataToArray( colorBufferSize, reinterpret_cast<uint8_t*>( colorBuffer ), ColorImageFormat_Rgba ); if ( SUCCEEDED( hr ) ) { colorSurface = Surface8u( colorWidth, colorHeight, false, SurfaceChannelOrder::RGBA ); memcpy( colorSurface.getData(), colorBuffer, colorWidth * colorHeight * sizeof( uint8_t ) * 4 ); } delete [] colorBuffer; colorBuffer = 0; } } if ( mDeviceOptions.isDepthEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = depthFrame->get_FrameDescription( &depthFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = depthFrameDescription->get_Width( &depthWidth ); } if ( SUCCEEDED( hr ) ) { hr = depthFrameDescription->get_Height( &depthHeight ); } if ( SUCCEEDED( hr ) ) { hr = depthFrame->get_DepthMinReliableDistance( &depthMinReliableDistance ); } if ( SUCCEEDED( hr ) ) { hr = depthFrame->get_DepthMaxReliableDistance( &depthMaxReliableDistance ); } if ( SUCCEEDED( hr ) ) { hr = depthFrame->AccessUnderlyingBuffer( &depthBufferSize, &depthBuffer ); } if ( SUCCEEDED( hr ) ) { depthChannel = Channel16u( depthWidth, depthHeight ); memcpy( depthChannel.getData(), depthBuffer, depthWidth * depthHeight * sizeof( uint16_t ) ); } } if ( mDeviceOptions.isInfraredEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = infraredFrame->get_FrameDescription( &infraredFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = infraredFrameDescription->get_Width( &infraredWidth ); } if ( SUCCEEDED( hr ) ) { hr = infraredFrameDescription->get_Height( &infraredHeight ); } if ( SUCCEEDED( hr ) ) { hr = infraredFrame->AccessUnderlyingBuffer( &infraredBufferSize, &infraredBuffer ); } if ( SUCCEEDED( hr ) ) { infraredChannel = Channel16u( infraredWidth, infraredHeight ); memcpy( infraredChannel.getData(), infraredBuffer, infraredWidth * infraredHeight * sizeof( uint16_t ) ); } } if ( mDeviceOptions.isInfraredLongExposureEnabled() ) { if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrame->get_FrameDescription( &infraredLongExposureFrameDescription ); } if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrameDescription->get_Width( &infraredLongExposureWidth ); } if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrameDescription->get_Height( &infraredLongExposureHeight ); } if ( SUCCEEDED( hr ) ) { hr = infraredLongExposureFrame->AccessUnderlyingBuffer( &infraredLongExposureBufferSize, &infraredLongExposureBuffer ); } if ( SUCCEEDED( hr ) ) { infraredLongExposureChannel = Channel16u( infraredLongExposureWidth, infraredLongExposureHeight ); memcpy( infraredLongExposureChannel.getData(), infraredLongExposureBuffer, infraredLongExposureWidth * infraredLongExposureHeight * sizeof( uint16_t ) ); } } if ( SUCCEEDED( hr ) ) { mFrame.mBodies = bodies; mFrame.mChannelBodyIndex = bodyIndexChannel; mFrame.mChannelDepth = depthChannel; mFrame.mChannelInfrared = infraredChannel; mFrame.mChannelInfraredLongExposure = infraredLongExposureChannel; mFrame.mDeviceId = mDeviceOptions.getDeviceId(); mFrame.mSurfaceColor = colorSurface; mFrame.mTimeStamp = timeStamp; mFrame.mFloorClipPlane = floorClipPlane; } if ( bodyIndexFrameDescription != 0 ) { bodyIndexFrameDescription->Release(); bodyIndexFrameDescription = 0; } if ( colorFrameDescription != 0 ) { colorFrameDescription->Release(); colorFrameDescription = 0; } if ( depthFrameDescription != 0 ) { depthFrameDescription->Release(); depthFrameDescription = 0; } if ( infraredFrameDescription != 0 ) { infraredFrameDescription->Release(); infraredFrameDescription = 0; } if ( infraredLongExposureFrameDescription != 0 ) { infraredLongExposureFrameDescription->Release(); infraredLongExposureFrameDescription = 0; } } if ( audioFrame != 0 ) { audioFrame->Release(); audioFrame = 0; } if ( bodyFrame != 0 ) { bodyFrame->Release(); bodyFrame = 0; } if ( bodyIndexFrame != 0 ) { bodyIndexFrame->Release(); bodyIndexFrame = 0; } if ( colorFrame != 0 ) { colorFrame->Release(); colorFrame = 0; } if ( depthFrame != 0 ) { depthFrame->Release(); depthFrame = 0; } if ( frame != 0 ) { frame->Release(); frame = 0; } if ( infraredFrame != 0 ) { infraredFrame->Release(); infraredFrame = 0; } if ( infraredLongExposureFrame != 0 ) { infraredLongExposureFrame->Release(); infraredLongExposureFrame = 0; } }
void KinectPlugin::ProcessBody(INT64 time, int bodyCount, IBody** bodies) { bool foundOneBody = false; if (_coordinateMapper) { for (int i = 0; i < bodyCount; ++i) { if (foundOneBody) { break; } IBody* body = bodies[i]; if (body) { BOOLEAN tracked = false; HRESULT hr = body->get_IsTracked(&tracked); if (SUCCEEDED(hr) && tracked) { foundOneBody = true; if (_joints.size() != JointType_Count) { _joints.resize(JointType_Count, { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f, 0.0f } }); } Joint joints[JointType_Count]; JointOrientation jointOrientations[JointType_Count]; HandState leftHandState = HandState_Unknown; HandState rightHandState = HandState_Unknown; body->get_HandLeftState(&leftHandState); body->get_HandRightState(&rightHandState); hr = body->GetJoints(_countof(joints), joints); hr = body->GetJointOrientations(_countof(jointOrientations), jointOrientations); if (SUCCEEDED(hr)) { auto jointCount = _countof(joints); //qDebug() << __FUNCTION__ << "nBodyCount:" << nBodyCount << "body:" << i << "jointCount:" << jointCount; for (int j = 0; j < jointCount; ++j) { //QString jointName = kinectJointNames[joints[j].JointType]; glm::vec3 jointPosition { joints[j].Position.X, joints[j].Position.Y, joints[j].Position.Z }; // Kinect Documentation is unclear on what these orientations are, are they absolute? // or are the relative to the parent bones. It appears as if it has changed between the // older 1.x SDK and the 2.0 sdk // // https://social.msdn.microsoft.com/Forums/en-US/31c9aff6-7dab-433d-9af9-59942dfd3d69/kinect-v20-preview-sdk-jointorientation-vs-boneorientation?forum=kinectv2sdk // seems to suggest these are absolute... // "These quaternions are absolute, so you can take a mesh in local space, transform it by the quaternion, // and it will match the exact orientation of the bone. If you want relative orientation quaternion, you // can multiply the absolute quaternion by the inverse of the parent joint's quaternion." // // - Bone direction(Y green) - always matches the skeleton. // - Normal(Z blue) - joint roll, perpendicular to the bone // - Binormal(X orange) - perpendicular to the bone and normal glm::quat jointOrientation { jointOrientations[j].Orientation.x, jointOrientations[j].Orientation.y, jointOrientations[j].Orientation.z, jointOrientations[j].Orientation.w }; // filling in the _joints data... if (joints[j].TrackingState != TrackingState_NotTracked) { _joints[j].position = jointPosition; //_joints[j].orientation = jointOrientation; } } } } } } } }
//---------- void Body::update() { CHECK_OPEN IBodyFrame * frame = NULL; IFrameDescription * frameDescription = NULL; try { //acquire frame if (FAILED(this->reader->AcquireLatestFrame(&frame))) { return; // we often throw here when no new frame is available } INT64 nTime = 0; if (FAILED(frame->get_RelativeTime(&nTime))) { throw Exception("Failed to get relative time"); } if (FAILED(frame->get_FloorClipPlane(&floorClipPlane))) { throw(Exception("Failed to get floor clip plane")); } IBody* ppBodies[BODY_COUNT] = {0}; if (FAILED(frame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies))) { throw Exception("Failed to refresh body data"); } for (int i = 0; i < BODY_COUNT; ++i) { auto & body = bodies[i]; body.clear(); IBody* pBody = ppBodies[i]; if (pBody) { BOOLEAN bTracked = false; if (FAILED(pBody->get_IsTracked(&bTracked))) { throw Exception("Failed to get tracking status"); } body.tracked = bTracked; body.bodyId = i; if (bTracked) { // retrieve tracking id UINT64 trackingId = -1; if (FAILED(pBody->get_TrackingId(&trackingId))) { throw Exception("Failed to get tracking id"); } body.trackingId = trackingId; // retrieve joint position & orientation _Joint joints[JointType_Count]; _JointOrientation jointsOrient[JointType_Count]; if (FAILED(pBody->GetJoints(JointType_Count, joints))) { throw Exception("Failed to get joints"); } if (FAILED(pBody->GetJointOrientations(JointType_Count, jointsOrient))) { throw Exception("Failed to get joints orientation"); } for (int j = 0; j < JointType_Count; ++j) { body.joints[joints[j].JointType] = Data::Joint(joints[j], jointsOrient[j]); } // retrieve hand states HandState leftHandState = HandState_Unknown; HandState rightHandState = HandState_Unknown; if (FAILED(pBody->get_HandLeftState(&leftHandState))) { throw Exception("Failed to get left hand state"); } if (FAILED(pBody->get_HandRightState(&rightHandState))) { throw Exception("Failed to get right hand state"); } body.leftHandState = leftHandState; body.rightHandState = rightHandState; } } } for (int i = 0; i < _countof(ppBodies); ++i) { SafeRelease(ppBodies[i]); } } catch (std::exception & e) { OFXKINECTFORWINDOWS2_ERROR << e.what(); } SafeRelease(frameDescription); SafeRelease(frame); }