Esempio n. 1
0
/* 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);
	}
}
Esempio n. 2
0
//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);
					//}
				});*/
			}
		}
	}
}
Esempio n. 3
0
    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 );
        }
    }
Esempio n. 4
0
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;
}
Esempio n. 5
0
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);

		}
	}
}
Esempio n. 6
0
    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 );
            }
        }
    }
Esempio n. 7
0
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;
						}
					//}
				});
			}
		}

	}
}
Esempio n. 8
0
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);
				});
			}
		}
	}
}
Esempio n. 9
0
    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 );
        }
    }