Beispiel #1
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);

		}
	}
}
Beispiel #2
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);
	}
}
//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);
					//}
				});*/
			}
		}
	}
}
Beispiel #4
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 );
            }
        }
    }
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;
						}
					//}
				});
			}
		}

	}
}
Beispiel #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;
                }

                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 );
        }
    }