XnStatus PoseToggleUserSelector::DetectPose(XnUserID nUserId)
{
UserSelectionState* pState;
XnStatus nRetVal = m_hUsersState.Get(nUserId, pState);
if(nRetVal!=XN_STATUS_OK)
{
return nRetVal;
}
XnUInt64 lastOutOfPoseTime;
nRetVal=m_usersOutOfPoseTime.Get(nUserId, lastOutOfPoseTime);
if(nRetVal!=XN_STATUS_OK)
{
return nRetVal; // we don't have a user time at all. This should not happen!!!
}
XnUInt64 curOutOfPoseTime=m_pUserGenerator->GetTimestamp();
if(pState->m_eState == XN_SELECTION_UNSELECTED || pState->m_eState == XN_SELECTION_FAILED)
{
// we want to select the user
if(lastOutOfPoseTime > 0 && (curOutOfPoseTime-lastOutOfPoseTime < m_timeBeforeToggle))
{
return XN_STATUS_OK; // not enough time has passed since out of pose
}
if(GetNumberOfSelectedUsers() >= m_nMaxUsersToTrack)
{
return XN_STATUS_OK; // we don't select because we already have enough selected users
}
XnUInt64 tmpTime = 0;
m_usersOutOfPoseTime.Set(nUserId,tmpTime); // we start so we don't care about previous out of pose any more
return PoseUserSelector::DetectPose(nUserId); // we select as the parent does.
}
else
{
// we might want to unselect the user.
if(lastOutOfPoseTime == 0 || (curOutOfPoseTime - lastOutOfPoseTime < m_timeBeforeToggle))
{
return XN_STATUS_OK; // not enough time has passed
}
// we need to unselect!
UpdateUserSelectionState(nUserId,XN_SELECTION_UNSELECTED,0);
m_pTrackingInitializer->AbortTracking(nUserId); // stop tracking
m_usersOutOfPoseTime.Set(nUserId,curOutOfPoseTime); // we are not tracking so no time is relevant
}
return XN_STATUS_ERROR;
}
XnStatus PoseUserSelector::UpdatePoseProgress(XnUserID nUserId, XnPoseDetectionStatus ePoseError)
{
UserSelectionState* pState;
XnStatus nRetVal=m_hUsersState.Get(nUserId, pState);
if(nRetVal != XN_STATUS_OK)
{
return nRetVal;
}
if(pState->m_eState != XN_SELECTION_UNSELECTED)
{
return XN_STATUS_ERROR; // the user is in the wrong state.
}
return UpdateUserSelectionState(nUserId,pState->m_eState, ePoseError);
}
XnStatus UserSelector::UpdateUserTrackingProgress(XnUserID nUserId, XnInt64 newSubState)
{
// update by getting the current state, making sure it is ok and then change the relevant
// sub state
UserSelectionState* pState;
XnStatus nRetVal = m_hUsersState.Get(nUserId , pState);
if(nRetVal != XN_STATUS_OK)
{
return nRetVal;
}
if(pState == NULL || pState->m_eState != XN_SELECTION_SELECTED)
{
xnLogError("SAMPLE","User %d should have been in an unselected state but was instead in %d state.\n", nUserId, pState->m_eState);
return XN_STATUS_ERROR; // this should be only called when a user is selected!
}
return UpdateUserSelectionState(nUserId, pState->m_eState, newSubState);
}
XnStatus UserSelector::UpdateUserTracking(XnUserID nUserId, XnBool bTracked, XnInt64 newSubState)
{
// update by checking the current state is ok and then update it based on the success and the
// sub state.
UserSelectionState* pState;
XnStatus nRetVal = m_hUsersState.Get(nUserId,pState);
if(nRetVal!=XN_STATUS_OK)
{
return nRetVal;
}
if(pState == NULL || pState->m_eState != XN_SELECTION_SELECTED)
{
xnLogError("SAMPLE","User %d should have been in an unselected state but was instead in %d state.\n", nUserId, pState->m_eState);
return XN_STATUS_ERROR; // this should be only called when a user is selected!
}
XnSelectionState newState = bTracked ? XN_SELECTION_TRACKING : XN_SELECTION_FAILED;
return UpdateUserSelectionState(nUserId, newState, newSubState);
}
XnStatus PoseUserSelector::StartTracking(XnUserID nUserId)
{
UserSelectionState* pState;
XnStatus nRetVal=m_hUsersState.Get(nUserId, pState);
if(nRetVal != XN_STATUS_OK)
{
return nRetVal; // we don't have such user ready
}
if(pState->m_eState != XN_SELECTION_UNSELECTED)
{
return XN_STATUS_ERROR; // the user is in the wrong state.
}
nRetVal=UpdateUserSelectionState(nUserId, XN_SELECTION_SELECTED, 0);
if(nRetVal!=XN_STATUS_OK)
{
return nRetVal; // we can't continue because we can't update the state!
}
return m_pTrackingInitializer->StartTracking(nUserId,FALSE);
}
void ClosestUserSelector::UpdateFrame()
{
ClearUserList();
// find out who are the closest ones.
for(UserStateHash::Iterator iter=m_hUsersState.begin(); iter!=m_hUsersState.end(); ++iter)
{
//qDebug("Entro en el bucle");
UserSelectionState* val=iter.Value();
XnUserID newUser=iter.Key();
XnPoint3D newCom;
if(m_pUserGenerator->GetCoM(newUser,newCom)!=XN_STATUS_OK)
continue; // irrelevant user.
if(val->m_eState == XN_SELECTION_FAILED)
{
UserStatusWithCom* valWithCom=(UserStatusWithCom*)val;
XnFloat distSqr=0.0f;
XnFloat diff;
diff = newCom.X - valWithCom->m_COM.X;
distSqr += diff * diff;
diff = newCom.Y - valWithCom->m_COM.Y;
distSqr += diff * diff;
diff = newCom.Z - valWithCom->m_COM.Z;
distSqr += diff * diff;
if(distSqr<MIN_SQR_DIST_TO_RETRY)
{
newCom.Z+=DIST_MOD_FOR_FAILURE;
}
}
if(val->m_eState == XN_SELECTION_SELECTED || val->m_eState == XN_SELECTION_TRACKING)
{
newCom.Z-=100.0f; // hysteresis to make the current user closer to avoid back and forth.
if(newCom.Z<=0)
{
newCom.Z=1.0f; // minimum dist!
}
}
InsertNewUser(newUser,newCom.Z);
}
for(UserStateHash::Iterator iter=m_hUsersState.begin(); iter!=m_hUsersState.end(); ++iter)
{
UserSelectionState* pState=iter.Value();
XnUInt32 curUser=iter.Key();
if(TestIfShouldTrack(curUser))
{
if(pState->m_eState == XN_SELECTION_SELECTED || pState->m_eState == XN_SELECTION_TRACKING)
{
continue; // we are already tracking so nothing to do...
}
UpdateUserSelectionState(curUser, XN_SELECTION_SELECTED, 0);
m_pTrackingInitializer->StartTracking(curUser);
}
else
{
if(pState->m_eState == XN_SELECTION_SELECTED || pState->m_eState == XN_SELECTION_TRACKING)
{
// we need to unselect it...
m_pTrackingInitializer->AbortTracking(curUser);
UpdateUserSelectionState(curUser,XN_SELECTION_UNSELECTED,0);
}
}
}
}
