bool SkeletonTrackerHelper::SubmitFaceTrackingResult(IFTResult* pResult)
{
if (pResult != NULL && SUCCEEDED(pResult->GetStatus()))
{
FLOAT* pSU = NULL;
UINT numSU;
BOOL suConverged;
m_pFaceTracker->GetShapeUnits(NULL, &pSU, &numSU, &suConverged);
POINT viewOffset = {0, 0};
FT_CAMERA_CONFIG cameraConfig;
if (m_KinectSensorPresent)
{
m_KinectSensor.GetVideoConfiguration(&cameraConfig);
}
else
{
cameraConfig.Width = 640;
cameraConfig.Height = 480;
cameraConfig.FocalLength = 500.0f;
}
IFTModel* ftModel;
HRESULT hr = m_pFaceTracker->GetFaceModel(&ftModel);
if (SUCCEEDED(hr))
{
UINT vertexCount = ftModel->GetVertexCount();
FT_VECTOR3D* points3D = reinterpret_cast<FT_VECTOR3D*> (_malloca(sizeof(FT_VECTOR3D) * vertexCount));
FLOAT* pAU = NULL;
UINT numAU;
pResult->GetAUCoefficients(&pAU, &numAU);
//pApp->m_eggavatar.SetCandideAU(pAU, numAU);
FLOAT scale;
FLOAT rotationXYZ[3];
FLOAT translationXYZ[3];
pResult->Get3DPose(&scale, rotationXYZ, translationXYZ);
hr = ftModel->Get3DShape(pSU,ftModel->GetSUCount(), pAU, numAU, 1.0f, rotationXYZ, translationXYZ, points3D, vertexCount);
m_leftEyePosition.x = (points3D[LEFT_UP_EYE_INDEX].x + points3D[LEFT_DOWN_EYE_INDEX].x) * 0.5;
m_leftEyePosition.y = (points3D[LEFT_UP_EYE_INDEX].y + points3D[LEFT_DOWN_EYE_INDEX].y) * 0.5;
m_leftEyePosition.z = (points3D[LEFT_UP_EYE_INDEX].z + points3D[LEFT_DOWN_EYE_INDEX].z) * 0.5;
m_rightEyePosition.x = (points3D[RIGHT_UP_EYE_INDEX].x + points3D[RIGHT_DOWN_EYE_INDEX].x) * 0.5;
m_rightEyePosition.y = (points3D[RIGHT_UP_EYE_INDEX].y + points3D[RIGHT_DOWN_EYE_INDEX].y) * 0.5;
m_rightEyePosition.z = (points3D[RIGHT_UP_EYE_INDEX].z + points3D[RIGHT_DOWN_EYE_INDEX].z) * 0.5;
ftModel->Release();
_freea(points3D);
}
if (m_CallBack && SUCCEEDED(hr))
{
(*m_CallBack)(m_CallBackParam);
}
}
return true;
}
void FubiKinectSDKSensor::update()
{
HRESULT hrFT = S_OK;
// Get new stream data
if (m_videoBuffer && m_depthBuffer && m_imageDataNew)
{
m_imageDataNew = false;
FT_SENSOR_DATA sensorData(m_videoBuffer, m_depthBuffer, m_zoomFactor, &m_viewOffset);
selectUsersToTrack(KINECT_SDK_MAX_NUM_FACES_TRACKED, m_userContext);
for (UINT i=0; i<KINECT_SDK_MAX_NUM_FACES_TRACKED; i++)
{
if(m_userContext[i].m_CountUntilFailure > 0) // user has been selected
{
m_headTracked[m_userContext[i].m_SkeletonId] = false;
m_faceTracked[m_userContext[i].m_SkeletonId] = false;
m_face2DTracked[m_userContext[i].m_SkeletonId] = false;
FT_VECTOR3D hint[2];
hint[0] = m_skelPos[m_userContext[i].m_SkeletonId][NUI_SKELETON_POSITION_SHOULDER_CENTER];
hint[1] = m_skelPos[m_userContext[i].m_SkeletonId][NUI_SKELETON_POSITION_HEAD];
if (m_userContext[i].m_LastTrackSucceeded)
{
hrFT = m_userContext[i].m_pFaceTracker->ContinueTracking(&sensorData, hint, m_userContext[i].m_pFTResult);
}
else
{
hrFT = m_userContext[i].m_pFaceTracker->StartTracking(&sensorData, NULL, hint, m_userContext[i].m_pFTResult);
}
m_userContext[i].m_LastTrackSucceeded = SUCCEEDED(hrFT) && SUCCEEDED(m_userContext[i].m_pFTResult->GetStatus());
if (m_userContext[i].m_LastTrackSucceeded)
{
// Store head orientation
static FLOAT scale;
hrFT = m_userContext[i].m_pFTResult->Get3DPose(&scale, m_headOrient[m_userContext[i].m_SkeletonId], m_headPos[m_userContext[i].m_SkeletonId]);
if (SUCCEEDED(hrFT))
{
m_headTracked[m_userContext[i].m_SkeletonId] = true;
IFTModel* ftModel;
HRESULT hr = m_userContext[i].m_pFaceTracker->GetFaceModel(&ftModel);
if (SUCCEEDED(hr))
{
FLOAT* pAUCOeffs;
UINT pAUCOunt;
m_userContext[i].m_pFTResult->GetAUCoefficients(&pAUCOeffs, &pAUCOunt);
FLOAT* pSU = NULL;
UINT numSU;
BOOL suConverged;
m_userContext[i].m_pFaceTracker->GetShapeUnits(NULL, &pSU, &numSU, &suConverged);
FT_VECTOR2D* pPts2D;
UINT pts2DCount;
m_userContext[i].m_pFTResult->Get2DShapePoints(&pPts2D, &pts2DCount);
if (pts2DCount <= 121)
{
m_face2DTracked[m_userContext[i].m_SkeletonId] = true;
for(UINT j = 0; j < pts2DCount; j++)
{
m_face2DPos[m_userContext[i].m_SkeletonId][j] = pPts2D[j];
}
}
else
{
static double lastWarning = -99;
if (Fubi::currentTime() - lastWarning > 10)
{
Fubi_logErr("Error in face tracking - face point count does not match!\n");
lastWarning = Fubi::currentTime();
}
}
UINT vertexCount = ftModel->GetVertexCount();
if (vertexCount <= 121)
{
if (SUCCEEDED(ftModel->Get3DShape(pSU, numSU, pAUCOeffs, pAUCOunt, scale, m_headOrient[m_userContext[i].m_SkeletonId], m_headPos[m_userContext[i].m_SkeletonId], m_facePos[m_userContext[i].m_SkeletonId], vertexCount)))
{
m_faceTracked[m_userContext[i].m_SkeletonId] = true;
FT_TRIANGLE* pTriangles;
UINT triangleCount;
if (SUCCEEDED(ftModel->GetTriangles(&pTriangles, &triangleCount)))
{
for (UINT j = 0; j < triangleCount; ++j)
{
m_faceTriangleIndices[m_userContext[i].m_SkeletonId][j] = pTriangles[j];
}
}
}
}
else
{
static double lastWarning = -99;
if (Fubi::currentTime() - lastWarning > 10)
{
Fubi_logErr("Error in face tracking - vertex count does not match!\n");
lastWarning = Fubi::currentTime();
}
}
}
}
}
else
{
m_userContext[i].m_pFTResult->Reset();
}
}
}
}
}
