void HandsModel::updateskeletonTree()
{
m_rightHandExist = false;
m_leftHandExist = false;
// Iterate over hands
int numOfHands = m_handData->QueryNumberOfHands();
for (int index = 0; index < numOfHands; ++index)
{
// Get hand by access order of entering time
PXCHandData::IHand* handOutput = NULL;
if (m_handData->QueryHandData(PXCHandData::ACCESS_ORDER_BY_TIME, index, handOutput) == PXC_STATUS_NO_ERROR)
{
// Get hand body side (left, right, unknown)
int side = 0;
if (handOutput->QueryBodySide() == PXCHandData::BodySideType::BODY_SIDE_RIGHT)
{
m_rightHandExist = true;
side = 0;
}
else if (handOutput->QueryBodySide() == PXCHandData::BodySideType::BODY_SIDE_LEFT)
{
m_leftHandExist = true;
side = 1;
}
PXCHandData::JointData jointData;
handOutput->QueryTrackedJoint(PXCHandData::JointType::JOINT_WRIST, jointData);
Node<PXCHandData::JointData> rootDataNode(jointData);
// Iterate over hand joints
for (int i = 2; i < MAX_NUMBER_OF_JOINTS - 3; i += 4)
{
handOutput->QueryTrackedJoint((PXCHandData::JointType)(i + 3), jointData);
Node<PXCHandData::JointData> dataNode(jointData);
handOutput->QueryTrackedJoint((PXCHandData::JointType)(i + 2), jointData);
Node<PXCHandData::JointData> dataNode1(jointData);
handOutput->QueryTrackedJoint((PXCHandData::JointType)(i + 1), jointData);
Node<PXCHandData::JointData> dataNode2(jointData);
handOutput->QueryTrackedJoint((PXCHandData::JointType)(i), jointData);
Node<PXCHandData::JointData> dataNode3(jointData);
dataNode1.add(dataNode);
dataNode2.add(dataNode1);
dataNode3.add(dataNode2);
rootDataNode.add(dataNode3);
}
m_skeletonTree[side].setRoot(rootDataNode);
}
}
}
/* Displaying current frames hand joints */
static void ProcessJoints(PXCHandData *handAnalyzer, pxcI64 timeStamp = 0) {
PXCHandData::JointData nodes[2][PXCHandData::NUMBER_OF_JOINTS] = {};
PXCHandData::ExtremityData extremitiesPointsNodes[2][PXCHandData::NUMBER_OF_EXTREMITIES] = {};
uint8_t ethernet_buff[32];
//Iterate hands
for (pxcI32 i = 0; i < handAnalyzer->QueryNumberOfHands(); i++)
{
//Get hand by time of appearence
PXCHandData::IHand* handData;
if (handAnalyzer->QueryHandData(PXCHandData::AccessOrderType::ACCESS_ORDER_BY_TIME, i, handData) == PXC_STATUS_NO_ERROR)
{
PXCHandData::JointData jointData;
//Iterate Joints
for (int j = 0; j < PXCHandData::NUMBER_OF_JOINTS; j++)
{
if (showNormalizedSkeleton == false)
{
handData->QueryTrackedJoint((PXCHandData::JointType)j, jointData);
}
else
{
handData->QueryNormalizedJoint((PXCHandData::JointType)j, jointData);
}
nodes[i][j] = jointData;
}
if (showExtremityPoint == true){
for (int j = 0; j < PXCHandData::NUMBER_OF_EXTREMITIES; j++)
{
handData->QueryExtremityPoint((PXCHandData::ExtremityType)j, extremitiesPointsNodes[i][j]);
}
}
}
cout << "x: " << nodes[0][1].positionWorld.x << "\ty: " << nodes[0][0].positionWorld.y << "\tz: " << nodes[0][0].positionWorld.z << "\topeness: " << handData->QueryOpenness() << "\n";
circle(frame, Point(frame.size().width - nodes[0][1].positionImage.x, nodes[0][1].positionImage.y), 1 / nodes[0][1].positionWorld.z * 15, (0, 0, 255 * handData->QueryOpenness() / 100 ), -1);
if (counter++ >= 0)
{
float pitch = nodes[0][PXCHandData::JointType::JOINT_MIDDLE_BASE].positionWorld.z - nodes[0][PXCHandData::JointType::JOINT_WRIST].positionWorld.z;
//MoveRobotAngels(nodes[0][1].positionWorld.x, nodes[0][1].positionWorld.y, nodes[0][1].positionWorld.z, handData->QueryOpenness(), ethernet_buff);
MoveRobot(nodes[0][1].positionWorld.x, nodes[0][1].positionWorld.y, nodes[0][1].positionWorld.z, handData->QueryOpenness(), pitch, ethernet_buff);
if (isRobotConnected) TCPsendBuff(ethernet_buff);
counter = 0;
}
//DrawJoints(hwndDlg, nodes, extremitiesPointsNodes);
}
}
//Newer version, without the failing
void URealSenseTask::HandProducerTick()
{
if (!HandsEnabled)
return;
//Data is up to date from earlier
if (collector->handData == nullptr)
return;
if (!collector->handTickDataRead)
return;
collector->handTickDataRead = false;
PXCHandData* handData = collector->handData;
collector->numberOfDetectedHands = handData->QueryNumberOfHands();
for (pxcI32 i = 0; i < collector->numberOfDetectedHands; i++)
{
//get hand information
PXCHandData::IHand* hand;
if (handData->QueryHandData(PXCHandData::AccessOrderType::ACCESS_ORDER_BY_TIME, i, hand) == PXC_STATUS_NO_ERROR)
{
//Store hand data
collector->tempHandData[i] = hand;
//Iterate Joints - lambda capture works...
PXCHandData::JointData jointData;
for (int j = 0; j < PXCHandData::NUMBER_OF_JOINTS; j++)
{
hand->QueryTrackedJoint((PXCHandData::JointType)j, jointData);
collector->tempJointData[i][j] = jointData;
collector->jointDataReady[i][j] = true;
//This event should also emit if a frame is missed by caching the latest input
/*RunLambdaOnGameThread([&, jointData]()
{
PXCHandData::JointData tempJointData = jointData;
tempJoint->setFromRealSenseJoint(&tempJointData);
if (collector->interfaceDelegate != nullptr)
IRealSenseInterface::Execute_JointMoved(collector->interfaceDelegate, tempJoint);
//}
});*/
}
}
}
}
void updateHandFrame()
{
// 手のデータを更新する
handData->Update();
auto numOfHands = handData->QueryNumberOfHands();
for ( int i = 0; i < numOfHands; i++ ) {
// 手を取得する
PXCHandData::IHand* hand;
auto sts = handData->QueryHandData(
PXCHandData::AccessOrderType::ACCESS_ORDER_BY_ID, i, hand );
if ( sts < PXC_STATUS_NO_ERROR ) {
continue;
}
// 指の関節を列挙する
for ( int j = 0; j < PXCHandData::NUMBER_OF_JOINTS; j++ ) {
// 指のデータを取得する
PXCHandData::JointData jointData;
sts = hand->QueryTrackedJoint( (PXCHandData::JointType)j, jointData );
if ( sts != PXC_STATUS_NO_ERROR ) {
continue;
}
// Depth座標系をカラー座標系に変換する
PXCPointF32 colorPoint = { 0 };
auto depthPoint = jointData.positionImage;
depthPoint.z = jointData.positionWorld.z * 1000;
projection->MapDepthToColor( 1, &depthPoint, &colorPoint );
// 指の座標を表示する
cv::circle( handImage,
cv::Point( colorPoint.x, colorPoint.y ),
5, cv::Scalar( 255, 255, 0 ), -1 );
}
}
}
void URealSenseTask::HandProducerTickOld()
{
if (!HandsEnabled)
return;
//Data is up to date from earlier
if (collector->handData == nullptr)
return;
PXCHandData* handData = collector->handData;
for (pxcI32 i = 0; i < handData->QueryNumberOfHands(); i++)
{
//get hand information
PXCHandData::IHand* hand;
if (handData->QueryHandData(PXCHandData::AccessOrderType::ACCESS_ORDER_BY_TIME, i, hand) == PXC_STATUS_NO_ERROR)
{
{
FScopeLock lock(&JointCriticalSection);
collector->tempHandData[i] = hand;
}
//Hand data
RunLambdaOnGameThread([&, i]()
{
{
FScopeLock lock(&JointCriticalSection);
if (!collector->handDataRead[i])
{
tempHand->setFromRealSenseHand(hand);//tempHandData[i]);
if (collector->interfaceDelegate != nullptr)
IRealSenseInterface::Execute_HandMoved(collector->interfaceDelegate, tempHand);
//clear it
collector->handDataRead[i] = true;
}
}
});
//Iterate Joints
PXCHandData::JointData jointData;
for (int j = 0; j < PXCHandData::NUMBER_OF_JOINTS; j++)
{
hand->QueryTrackedJoint((PXCHandData::JointType)j, jointData);
//Copy data only if it hasn't been read
{
FScopeLock lock(&JointCriticalSection);
if (collector->jointDataRead[i][j])
{
collector->tempJointData[i][j] = jointData;
FVector vect = FVector(jointData.positionWorld.x, jointData.positionWorld.y, jointData.positionWorld.z);
//UE_LOG(RealSensePluginLog, Log, TEXT("Copied Joint: %s"), *vect.ToString());
collector->jointDataRead[i][j] = false;
}
}
//This event should also emit if a frame is missed by caching the latest input
RunLambdaOnGameThread([&, i, j]()
{
//{
//FScopeLock lock(&JointCriticalSection);
if (!collector->jointDataRead[i][j])
{
//FVector vect = FVector(tempJointData[i][j].positionWorld.x, tempJointData[i][j].positionWorld.y, tempJointData[i][j].positionWorld.z);
//UE_LOG(RealSensePluginLog, Log, TEXT("Reading Joint: %s"), *vect.ToString());
tempJoint->setFromRealSenseJoint(&collector->tempJointData[i][j]);
if (collector->interfaceDelegate != nullptr)
IRealSenseInterface::Execute_JointMoved(collector->interfaceDelegate, tempJoint);
//clear it
collector->jointDataRead[i][j] = true;
}
//}
});
}
}
}
}
void updateHandFrame()
{
// 手のデータを更新する
handData->Update();
// 認識した手の数を取得する
auto numOfHands = handData->QueryNumberOfHands();
for ( int i = 0; i < numOfHands; i++ ) {
// 手を取得する
PXCHandData::IHand* hand;
auto sts = handData->QueryHandData(
PXCHandData::AccessOrderType::ACCESS_ORDER_BY_ID, i, hand );
if ( sts < PXC_STATUS_NO_ERROR ) {
continue;
}
// 指の関節を列挙する
for ( int j = 0; j < PXCHandData::NUMBER_OF_JOINTS; j++ ) {
PXCHandData::JointData jointData;
sts = hand->QueryTrackedJoint( (PXCHandData::JointType)j, jointData );
if ( sts != PXC_STATUS_NO_ERROR ) {
continue;
}
cv::circle( handImage,
cv::Point( jointData.positionImage.x, jointData.positionImage.y ),
5, cv::Scalar( 128, 128, 0 ) );
}
}
// 認識したジェスチャーの数を取得する
auto numOfGestures = handData->QueryFiredGesturesNumber();
for ( int i = 0; i < numOfGestures; i++ ) {
// 認識したジェスチャーを取得する
PXCHandData::GestureData gesture;
auto sts = handData->QueryFiredGestureData( i, gesture );
if ( sts < PXC_STATUS_NO_ERROR ) {
continue;
}
// ジェスチャーをした手を取得する
PXCHandData::IHand* hand;
sts = handData->QueryHandDataById( gesture.handId, hand );
if ( sts < PXC_STATUS_NO_ERROR ) {
continue;
}
// どちらの手でジェスチャーしたのか
auto side = hand->QueryBodySide();
if ( side == PXCHandData::BodySideType::BODY_SIDE_LEFT ){
++leftGestureCount;
}
else {
++rightGestureCount;
}
}
// ジェスチャーの検出数を表示する
{
std::stringstream ss;
ss << "Left gesture : " << leftGestureCount;
cv::putText( handImage, ss.str(), cv::Point( 10, 40 ),
cv::FONT_HERSHEY_SIMPLEX, 1.2, cv::Scalar( 0, 0, 255 ), 2, CV_AA );
}
{
std::stringstream ss;
ss << "Right gesture : " << rightGestureCount;
cv::putText( handImage, ss.str(), cv::Point( 10, 80 ),
cv::FONT_HERSHEY_SIMPLEX, 1.2, cv::Scalar( 0, 0, 255 ), 2, CV_AA );
}
}