/* 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();
// 画像を初期化
handImage1 = cv::Mat::zeros( DEPTH_HEIGHT, DEPTH_WIDTH, CV_8UC1 );
handImage2 = cv::Mat::zeros( DEPTH_HEIGHT, DEPTH_WIDTH, CV_8UC1 );
// 検出した手の数を取得する
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;
}
// 手のマスク画像を取得する
PXCImage* image = 0;
sts = hand->QuerySegmentationImage( image );
if ( sts < PXC_STATUS_NO_ERROR ) {
continue;
}
// マスク画像を取得する
PXCImage::ImageData data;
sts = image->AcquireAccess(
PXCImage::ACCESS_READ, PXCImage::PIXEL_FORMAT_Y8, &data );
if ( sts < PXC_STATUS_NO_ERROR ){
continue;
}
// マスク画像のサイズはDepthに依存
// 手は2つまで
PXCImage::ImageInfo info = image->QueryInfo();
auto& handImage = (i == 0) ? handImage1 : handImage2;
memcpy( handImage.data, data.planes[0], info.height * info.width );
// データを表示させる(時間がかかります)
//for ( int i = 0; i < info.height * info.width; i++ ){
// std::cout << (int)data.planes[0][i] << " ";
//}
image->ReleaseAccess( &data );
}
}
void ITA_ForcesApp::getRSCursorPos()
{
mHasHand = false;
mMousePos.clear();
auto numHands = mHandData->QueryNumberOfHands();
pxcUID handId;
mNumInputs = 0.0f;
for (int i = 0; i < numHands; ++i)
{
if (mHandData->QueryHandId(PXCHandData::ACCESS_ORDER_BY_ID, i, handId) >= PXC_STATUS_NO_ERROR)
{
PXCHandData::IHand *hand;
if (mHandData->QueryHandDataById(handId, hand) >= PXC_STATUS_NO_ERROR)
{
if (hand->HasCursor())
{
PXCHandData::ICursor *cursor;
if (hand->QueryCursor(cursor) >= PXC_STATUS_NO_ERROR)
{
auto cursorPos = cursor->QueryPointImage();
// Mapping block
PXCPoint3DF32 uvz[1]{ cursorPos };
uvz[0].z *= 1000.0f;
PXCPointF32 ijj[1];
auto stat = mMapper->MapDepthToColor(1, uvz, ijj);
//
if(!mMouseInput)
mMousePos.push_back( vec3(remapPos(vec2(ijj[0].x, ijj[0].y)), 0.0));
mHasHand = true;
mNumInputs += 1.0f;
}
}
}
}
}
if (numHands < 1)
mIdle = true;
else
mIdle = false;
mForceMode = mRepulsing ? FORCE_R : FORCE_A;
}
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);
}
}
}
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 URealSenseTask::GestureProducerTick()
{
PXCHandData* handData = collector->handData;
PXCHandData::GestureData gestureData;
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)
{
//spread fingers
if (!GesturesEnabled)
return;
//FScopeLock lock(&GestureCriticalSection);
//tempHand->setFromRealSenseHand(hand);
//URealSenseHand* copyHand = DuplicateObject(tempHand, nullptr);
if (handData->IsGestureFired(L"spreadfingers", gestureData))
{
runGestureOnGameThread(gestureData.handId, hand->QueryUniqueId(), [&]()
{
tempHand->setFromRealSenseHand(hand);
IRealSenseInterface::Execute_SpreadFingersGestureDetected(collector->interfaceDelegate, tempHand);
});
}
//fist
if (handData->IsGestureFired(L"fist", gestureData))
{
runGestureOnGameThread(gestureData.handId, hand->QueryUniqueId(), [&]()
{
tempHand->setFromRealSenseHand(hand);
IRealSenseInterface::Execute_FistGestureDetected(collector->interfaceDelegate, tempHand);
});
}
//tap
if (handData->IsGestureFired(L"tap", gestureData))
{
runGestureOnGameThread(gestureData.handId, hand->QueryUniqueId(), [&]()
{
tempHand->setFromRealSenseHand(hand);
IRealSenseInterface::Execute_TapGestureDetected(collector->interfaceDelegate, tempHand);
});
}
//thumb_down
if (handData->IsGestureFired(L"thumb_down", gestureData))
{
runGestureOnGameThread(gestureData.handId, hand->QueryUniqueId(), [&]()
{
tempHand->setFromRealSenseHand(hand);
IRealSenseInterface::Execute_ThumbDownGestureDetected(collector->interfaceDelegate, tempHand);
});
}
//thumb_up
if (handData->IsGestureFired(L"thumb_up", gestureData))
{
//If the gesture was fired by the same hand
runGestureOnGameThread(gestureData.handId, hand->QueryUniqueId(), [&]()
{
tempHand->setFromRealSenseHand(hand);
IRealSenseInterface::Execute_ThumbUpGestureDetected(collector->interfaceDelegate, tempHand);
});
}
//two_fingers_pinch_open
if (handData->IsGestureFired(L"two_fingers_pinch_open", gestureData))
{
runGestureOnGameThread(gestureData.handId, hand->QueryUniqueId(), [&]()
{
tempHand->setFromRealSenseHand(hand);
IRealSenseInterface::Execute_TwoFingersPinchOpenGestureDetected(collector->interfaceDelegate, tempHand);
});
}
//v_sign
if (handData->IsGestureFired(L"v_sign", gestureData))
{
runGestureOnGameThread(gestureData.handId, hand->QueryUniqueId(), [&]()
{
tempHand->setFromRealSenseHand(hand);
IRealSenseInterface::Execute_VSignGestureDetected(collector->interfaceDelegate, tempHand);
});
}
//full_pinch
if (handData->IsGestureFired(L"full_pinch", gestureData))
{
runGestureOnGameThread(gestureData.handId, hand->QueryUniqueId(), [&]()
{
tempHand->setFromRealSenseHand(hand);
IRealSenseInterface::Execute_FullPinchGestureDetected(collector->interfaceDelegate, tempHand);
});
}
//swipe
if (handData->IsGestureFired(L"swipe", gestureData))
{
runGestureOnGameThread(gestureData.handId, hand->QueryUniqueId(), [&]()
{
tempHand->setFromRealSenseHand(hand);
IRealSenseInterface::Execute_SwipeGestureDetected(collector->interfaceDelegate, tempHand);
});
}
//wave
if (handData->IsGestureFired(L"wave", gestureData))
{
runGestureOnGameThread(gestureData.handId, hand->QueryUniqueId(), [&]()
{
tempHand->setFromRealSenseHand(hand);
IRealSenseInterface::Execute_WaveGestureDetected(collector->interfaceDelegate, tempHand);
});
}
}
}
}
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 );
}
}
