bool MKinect::track(){
HRESULT hResult = S_OK;
// Body Frame
IBodyFrame* pBodyFrame = nullptr;
hResult = pBodyReader->AcquireLatestFrame(&pBodyFrame);
if (SUCCEEDED(hResult)){
IBody* pBody[BODY_COUNT] = { 0 };
hResult = pBodyFrame->GetAndRefreshBodyData(BODY_COUNT, pBody);
if (SUCCEEDED(hResult)){
for (int count = 0; count < BODY_COUNT; count++){
BOOLEAN bTracked = false;
hResult = pBody[count]->get_IsTracked(&bTracked);
if (SUCCEEDED(hResult) && bTracked){
/*// Joint
Joint joint[JointType::JointType_Count];
hResult = pBody[count]->GetJoints( JointType::JointType_Count, joint );
if( SUCCEEDED( hResult ) ){
for( int type = 0; type < JointType::JointType_Count; type++ ){
ColorSpacePoint colorSpacePoint = { 0 };
pCoordinateMapper->MapCameraPointToColorSpace( joint[type].Position, &colorSpacePoint );
int x = static_cast<int>( colorSpacePoint.X );
int y = static_cast<int>( colorSpacePoint.Y );
if( ( x >= 0 ) && ( x < width ) && ( y >= 0 ) && ( y < height ) ){
cv::circle( bufferMat, cv::Point( x, y ), 5, static_cast<cv::Scalar>( color[count] ), -1, CV_AA );
}
}
}*/
// Set TrackingID to Detect Face
UINT64 trackingId = _UI64_MAX;
hResult = pBody[count]->get_TrackingId(&trackingId);
if (SUCCEEDED(hResult)){
pFaceSource[count]->put_TrackingId(trackingId);
}
}
}
}
for (int count = 0; count < BODY_COUNT; count++){
if (pBody[count]) pBody[count]->Release();
}
}
if (pBodyFrame) pBodyFrame->Release();
// Face Frame
for (int count = 0; count < BODY_COUNT; count++){
IFaceFrame* pFaceFrame = nullptr;
hResult = pFaceReader[count]->AcquireLatestFrame(&pFaceFrame);
if (SUCCEEDED(hResult) && pFaceFrame != nullptr){
BOOLEAN bFaceTracked = false;
hResult = pFaceFrame->get_IsTrackingIdValid(&bFaceTracked);
if (SUCCEEDED(hResult) && bFaceTracked){
IFaceFrameResult* pFaceResult = nullptr;
hResult = pFaceFrame->get_FaceFrameResult(&pFaceResult);
if (SUCCEEDED(hResult) && pFaceResult != nullptr){
std::vector<std::string> result;
// Face Rotation
Vector4 faceRotation;
hResult = pFaceResult->get_FaceRotationQuaternion(&faceRotation);
if (SUCCEEDED(hResult)){
int pitch, yaw, roll;
ExtractFaceRotationInDegrees(&faceRotation, &pitch, &yaw, &roll);
_yaw = yaw;
_pitch = pitch;
_roll = roll;
}
}
if (pFaceResult) pFaceResult->Release();
}
}
if (pFaceFrame) pFaceFrame->Release();
}
return true;
}
void Kin2::getFaces(std::vector<k2::FaceData>& facesData)
{
if (!(m_flags & k2::FACE))
{
mexPrintf("ERROR: NO FACE FUNCTIONALITY SELECTED!\n");
return;
}
HRESULT hr;
facesData.clear();
// iterate through each face reader
for (int iFace = 0; iFace < BODY_COUNT; ++iFace)
{
// retrieve the latest face frame from this reader
IFaceFrame* pFaceFrame = nullptr;
hr = m_pFaceFrameReaders[iFace]->AcquireLatestFrame(&pFaceFrame);
BOOLEAN bFaceTracked = false;
if (SUCCEEDED(hr) && nullptr != pFaceFrame)
{
// check if a valid face is tracked in this face frame
hr = pFaceFrame->get_IsTrackingIdValid(&bFaceTracked);
}
if (SUCCEEDED(hr))
{
// If face tracked, save its data on the facesData structure array
if (bFaceTracked)
{
IFaceFrameResult* pFaceFrameResult = nullptr;
hr = pFaceFrame->get_FaceFrameResult(&pFaceFrameResult);
k2::FaceData faceData;
// need to verify if pFaceFrameResult contains data before trying to access it
if (SUCCEEDED(hr) && pFaceFrameResult != nullptr)
{
hr = pFaceFrameResult->get_FaceBoundingBoxInColorSpace(&faceData.faceBox);
if (SUCCEEDED(hr))
{
hr = pFaceFrameResult->GetFacePointsInColorSpace(FacePointType::FacePointType_Count, faceData.facePoints);
}
if (SUCCEEDED(hr))
{
hr = pFaceFrameResult->get_FaceRotationQuaternion(&faceData.faceRotation);
}
if (SUCCEEDED(hr))
{
hr = pFaceFrameResult->GetFaceProperties(FaceProperty::FaceProperty_Count, faceData.faceProperties);
}
facesData.push_back(faceData);
}
SafeRelease(pFaceFrameResult);
}
else
{
// face tracking is not valid - attempt to fix the issue
// a valid body is required to perform this step
if (m_bHaveBodyData)
{
// check if the corresponding body is tracked
// if this is true then update the face frame source to track this body
IBody* pBody = m_ppBodies[iFace];
if (pBody != nullptr)
{
BOOLEAN bTracked = false;
hr = pBody->get_IsTracked(&bTracked);
UINT64 bodyTId;
if (SUCCEEDED(hr) && bTracked)
{
// get the tracking ID of this body
hr = pBody->get_TrackingId(&bodyTId);
if (SUCCEEDED(hr))
{
// update the face frame source with the tracking ID
m_pFaceFrameSources[iFace]->put_TrackingId(bodyTId);
}
}
}
}
}
}
SafeRelease(pFaceFrame);
}
}
/// <summary>
/// Processes new face frames
/// </summary>
void CFaceBasics::ProcessFaces()
{
HRESULT hr;
IBody* ppBodies[BODY_COUNT] = {0};
bool bHaveBodyData = SUCCEEDED( UpdateBodyData(ppBodies) );
UINT32 vertexCount = 0;
hr = GetFaceModelVertexCount(&vertexCount);
UINT colorSpaceCount = vertexCount;
ColorSpacePoint * pFaceColors = new ColorSpacePoint[colorSpaceCount];
// iterate through each face reader
for (int iFace = 0; iFace < BODY_COUNT; ++iFace)
{
// retrieve the latest face frame from this reader
IFaceFrame* pFaceFrame = nullptr;
hr = m_pFaceFrameReaders[iFace]->AcquireLatestFrame(&pFaceFrame);
BOOLEAN bFaceTracked = false;
if (SUCCEEDED(hr) && nullptr != pFaceFrame)
{
// check if a valid face is tracked in this face frame
hr = pFaceFrame->get_IsTrackingIdValid(&bFaceTracked);
}
if (SUCCEEDED(hr))
{
if (bFaceTracked)
{
IFaceFrameResult* pFaceFrameResult = nullptr;
RectI faceBox = {0};
PointF facePoints[FacePointType::FacePointType_Count];
Vector4 faceRotation;
DetectionResult faceProperties[FaceProperty::FaceProperty_Count];
D2D1_POINT_2F faceTextLayout;
hr = pFaceFrame->get_FaceFrameResult(&pFaceFrameResult);
// need to verify if pFaceFrameResult contains data before trying to access it
if (SUCCEEDED(hr) && pFaceFrameResult != nullptr)
{
hr = pFaceFrameResult->get_FaceBoundingBoxInColorSpace(&faceBox);
if (SUCCEEDED(hr))
{
hr = pFaceFrameResult->GetFacePointsInColorSpace(FacePointType::FacePointType_Count, facePoints);
}
if (SUCCEEDED(hr))
{
hr = pFaceFrameResult->get_FaceRotationQuaternion(&faceRotation);
}
if (SUCCEEDED(hr))
{
hr = pFaceFrameResult->GetFaceProperties(FaceProperty::FaceProperty_Count, faceProperties);
}
if (SUCCEEDED(hr))
{
hr = GetFaceTextPositionInColorSpace(ppBodies[iFace], &faceTextLayout);
}
if (SUCCEEDED(hr))
{
//let's see if we can get hd face frame here
// retrieve the latest face frame from this reader
IHighDefinitionFaceFrame* phdFaceFrame = nullptr;
hr = m_phdFaceFrameReaders[iFace]->AcquireLatestFrame(&phdFaceFrame);
if (SUCCEEDED(hr) && nullptr != phdFaceFrame)
{
//we have a hd face frame so get the vertices
hr = phdFaceFrame->GetAndRefreshFaceAlignmentResult(m_phdFaceAlignments[iFace]);
IFaceModel * pFaceModel = nullptr;
if (SUCCEEDED(hr))
{
//we have updated the faceAlignment results
hr = phdFaceFrame->get_FaceModel(&pFaceModel);
if (SUCCEEDED(hr) && nullptr != pFaceModel)
{
CameraSpacePoint * pFacePoints = new CameraSpacePoint[vertexCount];
hr = pFaceModel->CalculateVerticesForAlignment(m_phdFaceAlignments[iFace], vertexCount, pFacePoints);
const CameraSpacePoint * pConstFacePoints = pFacePoints; //now convert cameraspace points to colorspacepoints
hr = m_pCoordinateMapper->MapCameraPointsToColorSpace(vertexCount, pConstFacePoints, colorSpaceCount, pFaceColors);
if (FAILED(hr))
{
pFaceColors = nullptr;
}
delete pFacePoints;
}
SafeRelease(pFaceModel);
}
SafeRelease(phdFaceFrame);
}
if (nullptr != pFaceColors)
{
m_pDrawDataStreams->DrawFaceFrameResults(iFace, &faceBox, facePoints, &faceRotation, faceProperties, &faceTextLayout, pFaceColors);
}
else
{
// draw face frame results
m_pDrawDataStreams->DrawFaceFrameResults(iFace, &faceBox, facePoints, &faceRotation, faceProperties, &faceTextLayout);
}
}
}
SafeRelease(pFaceFrameResult);
}
else
{
// face tracking is not valid - attempt to fix the issue
// a valid body is required to perform this step
if (bHaveBodyData)
{
// check if the corresponding body is tracked
// if this is true then update the face frame source to track this body
IBody* pBody = ppBodies[iFace];
if (pBody != nullptr)
{
BOOLEAN bTracked = false;
hr = pBody->get_IsTracked(&bTracked);
UINT64 bodyTId;
if (SUCCEEDED(hr) && bTracked)
{
// get the tracking ID of this body
hr = pBody->get_TrackingId(&bodyTId);
if (SUCCEEDED(hr))
{
// update the face frame source with the tracking ID
m_pFaceFrameSources[iFace]->put_TrackingId(bodyTId);
m_phdFaceFrameSources[iFace]->put_TrackingId(bodyTId);
}
}
}
}
}
}
SafeRelease(pFaceFrame);
}
delete pFaceColors;
if (bHaveBodyData)
{
for (int i = 0; i < _countof(ppBodies); ++i)
{
SafeRelease(ppBodies[i]);
}
}
}
