// calculates joint velocities in a body frame private static void CalcBodyFrameJointVels(SensorData sensorData, ref BodyFrameData bodyFrame) { // calculate the inter-frame time float frameTime = (float)(bodyFrame.liRelativeTime - bodyFrame.liPreviousTime) / 100000000000f; for (int i = 0; i < sensorData.bodyCount; i++) { if (bodyFrame.bodyData [i].bIsTracked != 0) { for (int j = 0; j < sensorData.jointCount; j++) { KinectInterop.JointData jointData = bodyFrame.bodyData [i].joint [j]; int p = (int)sensorData.sensorInterface.GetParentJoint((KinectInterop.JointType)j); Vector3 parentPos = bodyFrame.bodyData [i].joint [p].position; jointData.posRel = jointData.position - parentPos; jointData.posVel = frameTime > 0f ? (jointData.position - jointData.posPrev) / frameTime : Vector3.zero; jointData.posPrev = jointData.position; bodyFrame.bodyData [i].joint [j] = jointData; } } } }
// processes some special cases in body joints private void ProcessBodyFrameSpecialCases(int i, ref KinectInterop.BodyFrameData bodyFrame) { // special case - shoulder center int r = (int)KinectInterop.JointType.ShoulderRight; int l = (int)KinectInterop.JointType.ShoulderLeft; int c = (int)KinectInterop.JointType.SpineShoulder; if (bodyFrame.bodyData[i].joint[r].trackingState == KinectInterop.TrackingState.Tracked && bodyFrame.bodyData[i].joint[l].trackingState == KinectInterop.TrackingState.Tracked) { KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[c]; jointData.trackingState = bodyFrame.bodyData[i].joint[r].trackingState; jointData.orientation = Quaternion.identity; Vector3 posRight = bodyFrame.bodyData[i].joint[r].kinectPos; Vector3 posLeft = bodyFrame.bodyData[i].joint[l].kinectPos; jointData.kinectPos = (posRight + posLeft) * 0.5f; posRight = bodyFrame.bodyData[i].joint[r].position; posLeft = bodyFrame.bodyData[i].joint[l].position; jointData.position = (posRight + posLeft) * 0.5f; bodyFrame.bodyData[i].joint[c] = jointData; } else { bodyFrame.bodyData[i].joint[c].trackingState = KinectInterop.TrackingState.NotTracked; } // special case - hip center r = (int)KinectInterop.JointType.HipRight; l = (int)KinectInterop.JointType.HipLeft; c = (int)KinectInterop.JointType.SpineBase; if (bodyFrame.bodyData[i].joint[r].trackingState == KinectInterop.TrackingState.Tracked && bodyFrame.bodyData[i].joint[l].trackingState == KinectInterop.TrackingState.Tracked) { KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[c]; jointData.trackingState = bodyFrame.bodyData[i].joint[r].trackingState; jointData.orientation = Quaternion.identity; Vector3 posRight = bodyFrame.bodyData[i].joint[r].kinectPos; Vector3 posLeft = bodyFrame.bodyData[i].joint[l].kinectPos; jointData.kinectPos = (posRight + posLeft) * 0.5f; posRight = bodyFrame.bodyData[i].joint[r].position; posLeft = bodyFrame.bodyData[i].joint[l].position; jointData.position = (posRight + posLeft) * 0.5f; bodyFrame.bodyData[i].joint[c] = jointData; // modify the body position, too bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } }
public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld) { bool bNewFrame = false; if ((multiSourceFrameReader != null && multiSourceFrame != null) || bodyFrameReader != null) { var frame = multiSourceFrame != null?multiSourceFrame.BodyFrameReference.AcquireFrame() : bodyFrameReader.AcquireLatestFrame(); if (frame != null) { frame.GetAndRefreshBodyData(bodyData); bodyFrame.liRelativeTime = frame.RelativeTime.Ticks; frame.Dispose(); frame = null; for (int i = 0; i < sensorData.bodyCount; i++) { Body body = bodyData[i]; if (body == null) { bodyFrame.bodyData[i].bIsTracked = 0; continue; } bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0); if (body.IsTracked) { // transfer body and joints data //传送身体和关节数据 bodyFrame.bodyData[i].liTrackingID = (long)body.TrackingId; for (int j = 0; j < sensorData.jointCount; j++) { Windows.Kinect.Joint joint = body.Joints[(Windows.Kinect.JointType)j]; KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j]; jointData.jointType = (KinectInterop.JointType)j; jointData.trackingState = (KinectInterop.TrackingState)joint.TrackingState; if ((int)joint.TrackingState != (int)TrackingState.NotTracked) { jointData.kinectPos = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z); jointData.position = kinectToWorld.MultiplyPoint3x4(jointData.kinectPos); } jointData.orientation = Quaternion.identity; // Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation; // jointData.orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W); if (j == 0) { bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } bodyFrame.bodyData[i].joint[j] = jointData; } // tranfer hand states //转变手的状态 bodyFrame.bodyData[i].leftHandState = (KinectInterop.HandState)body.HandLeftState; bodyFrame.bodyData[i].leftHandConfidence = (KinectInterop.TrackingConfidence)body.HandLeftConfidence; bodyFrame.bodyData[i].rightHandState = (KinectInterop.HandState)body.HandRightState; bodyFrame.bodyData[i].rightHandConfidence = (KinectInterop.TrackingConfidence)body.HandRightConfidence; } } bNewFrame = true; } } return(bNewFrame); }
public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld) { bool bNewFrame = false; if ((multiSourceFrameReader != null && multiSourceFrame != null) || bodyFrameReader != null) { var frame = multiSourceFrame != null?multiSourceFrame.BodyFrameReference.AcquireFrame() : bodyFrameReader.AcquireLatestFrame(); if (frame != null) { frame.GetAndRefreshBodyData(bodyData); bodyFrame.liRelativeTime = frame.RelativeTime.Ticks; if (sensorData.hintHeightAngle) { // get the floor plane Windows.Kinect.Vector4 vFloorPlane = frame.FloorClipPlane; Vector3 floorPlane = new Vector3(vFloorPlane.X, vFloorPlane.Y, vFloorPlane.Z); sensorData.sensorRotDetected = Quaternion.FromToRotation(floorPlane, Vector3.up); sensorData.sensorHgtDetected = vFloorPlane.W; } frame.Dispose(); frame = null; for (int i = 0; i < sensorData.bodyCount; i++) { Body body = bodyData[i]; if (body == null) { bodyFrame.bodyData[i].bIsTracked = 0; continue; } bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0); if (body.IsTracked) { // transfer body and joints data bodyFrame.bodyData[i].liTrackingID = (long)body.TrackingId; // cache the body joints (following the advice of Brian Chasalow) Dictionary <Windows.Kinect.JointType, Windows.Kinect.Joint> bodyJoints = body.Joints; for (int j = 0; j < sensorData.jointCount; j++) { Windows.Kinect.Joint joint = bodyJoints[(Windows.Kinect.JointType)j]; KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j]; jointData.jointType = (KinectInterop.JointType)j; jointData.trackingState = (KinectInterop.TrackingState)joint.TrackingState; if ((int)joint.TrackingState != (int)TrackingState.NotTracked) { jointData.kinectPos = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z); jointData.position = kinectToWorld.MultiplyPoint3x4(jointData.kinectPos); } jointData.orientation = Quaternion.identity; // Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation; // jointData.orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W); if (j == 0) { bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } bodyFrame.bodyData[i].joint[j] = jointData; } // tranfer hand states bodyFrame.bodyData[i].leftHandState = (KinectInterop.HandState)body.HandLeftState; bodyFrame.bodyData[i].leftHandConfidence = (KinectInterop.TrackingConfidence)body.HandLeftConfidence; bodyFrame.bodyData[i].rightHandState = (KinectInterop.HandState)body.HandRightState; bodyFrame.bodyData[i].rightHandConfidence = (KinectInterop.TrackingConfidence)body.HandRightConfidence; } } bNewFrame = true; } } return(bNewFrame); }
public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld, bool bIgnoreJointZ) { bool bNewFrame = _bodyFrameReady; if (_bodyFrameReady) { lock (_bodyFrameLock) { bodyFrame.liPreviousTime = bodyFrame.liRelativeTime; bodyFrame.liRelativeTime = _bodyFrameTime; if (sensorData.hintHeightAngle) { //// get the floor plane //Windows.Kinect.Vector4 vFloorPlane = _bodyFrame.FloorClipPlane; //Vector3 floorPlane = new Vector3(vFloorPlane.X, vFloorPlane.Y, vFloorPlane.Z); //sensorData.sensorRotDetected = Quaternion.FromToRotation(floorPlane, Vector3.up); //sensorData.sensorHgtDetected = vFloorPlane.W; } for (int i = 0; i < sensorData.bodyCount; i++) { Body body = i < _bodyFrame.Bodies.Length ? _bodyFrame.Bodies[i] : null; if (body == null) { bodyFrame.bodyData[i].bIsTracked = 0; continue; } bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0); if (body.IsTracked) { // transfer body and joints data byte[] entityBytes = body.EntityId.ToByteArray(); bodyFrame.bodyData[i].liTrackingID = BitConverter.ToInt64(entityBytes, 8); // cache the body joints (following the advice of Brian Chasalow) //Dictionary<Windows.Kinect.JointType, Windows.Kinect.Joint> bodyJoints = body.Joints; // calculate the inter-frame time float frameTime = 0f; if (bodyFrame.bTurnAnalisys && bodyFrame.liPreviousTime > 0) { frameTime = (float)(bodyFrame.liRelativeTime - bodyFrame.liPreviousTime) / 100000000000; } for (int j = 0; j < sensorData.jointCount; j++) { if (j >= body.Joints.Count) { continue; } MultiK2.Tracking.Joint joint = body.Joints[(MultiK2.Tracking.JointType)j]; KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j]; //jointData.jointType = (KinectInterop.JointType)j; jointData.trackingState = (KinectInterop.TrackingState)joint.PositionTrackingState; if ((int)joint.PositionTrackingState != (int)TrackingState.NotTracked) { float jPosZ = (bIgnoreJointZ && j > 0) ? bodyFrame.bodyData[i].joint[0].kinectPos.z : joint.Position.Z; jointData.kinectPos = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z); jointData.position = kinectToWorld.MultiplyPoint3x4(new Vector3(joint.Position.X, joint.Position.Y, jPosZ)); } jointData.orientation = Quaternion.identity; if (j == 0) { bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } bodyFrame.bodyData[i].joint[j] = jointData; } //if (bodyFrame.bTurnAnalisys && bodyFrame.liPreviousTime > 0) //{ // for (int j = 0; j < sensorData.jointCount; j++) // { // KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j]; // int p = (int)GetParentJoint((KinectInterop.JointType)j); // Vector3 parentPos = bodyFrame.bodyData[i].joint[p].position; // jointData.posRel = jointData.position - parentPos; // jointData.posDrv = frameTime > 0f ? (jointData.position - jointData.posPrev) / frameTime : Vector3.zero; // jointData.posPrev = jointData.position; // bodyFrame.bodyData[i].joint[j] = jointData; // } //} // tranfer hand states bodyFrame.bodyData[i].leftHandState = (KinectInterop.HandState)body.HandStateLeft; bodyFrame.bodyData[i].leftHandConfidence = (KinectInterop.TrackingConfidence)body.ConfidenceLeft; bodyFrame.bodyData[i].rightHandState = (KinectInterop.HandState)body.HandStateRight; bodyFrame.bodyData[i].rightHandConfidence = (KinectInterop.TrackingConfidence)body.ConfidenceRight; } } _bodyFrameReady = false; } } return(bNewFrame); }
public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld) { for (int i = 0; i < oniUsersCount; i++) { uint userId = (uint)oniUsers[i]; short userState = oniStates[i]; switch (userState) { case 1: // new user Debug.Log(String.Format("New user: {0}", userId)); break; case 2: // calibration started //Debug.Log(String.Format("Calibration started for user: {0}", userId)); break; case 3: // calibration succeeded Debug.Log(String.Format("Calibration succeeded for user: {0}", userId)); if (!allUsers.Contains(userId)) { allUsers.Add(userId); } break; case 4: // calibration failed Debug.Log(String.Format("Calibration failed for user: {0}", userId)); break; case 5: // user lost Debug.Log(String.Format("User lost: {0}", userId)); if (allUsers.Contains(userId)) { allUsers.Remove(userId); } break; } } bool newSkeleton = (allUsers.Count > 0); // set initially all the skeletons as not tracked for (int i = 0; i < sensorData.bodyCount; i++) { bodyFrame.bodyData[i].liTrackingID = 0; bodyFrame.bodyData[i].bIsTracked = 0; } // fill in joint info for the tracked skeletons for (int i = 0; i < allUsers.Count; i++) { uint userId = allUsers[i]; int userIndex = (int)userId % 6; bodyFrame.bodyData[userIndex].liTrackingID = (long)userId; bodyFrame.bodyData[userIndex].bIsTracked = 1; for (int j = 0; j < sensorData.jointCount; j++) { KinectInterop.JointData jointData = bodyFrame.bodyData[userIndex].joint[j]; int oniJ = Constants.BodyJoint2OniJoint[j]; if (oniJ >= 0) { //jointData.jointType = GetJointAtIndex(j); float fConfidence = GetJointPositionConfidence(userId, oniJ); jointData.trackingState = (fConfidence > 0.7f ? KinectInterop.TrackingState.Tracked : (fConfidence > 0.3f ? KinectInterop.TrackingState.Inferred : KinectInterop.TrackingState.NotTracked)); if (jointData.trackingState != KinectInterop.TrackingState.NotTracked) { if (GetJointPosition(userId, oniJ, ref jointPosition)) { jointData.kinectPos = new Vector3(jointPosition.x * 0.001f, jointPosition.y * 0.001f, jointPosition.z * 0.001f);; jointData.position = kinectToWorld.MultiplyPoint3x4(jointData.kinectPos); } } jointData.orientation = Quaternion.identity; if (j == 0) { bodyFrame.bodyData[userIndex].position = jointData.position; bodyFrame.bodyData[userIndex].orientation = jointData.orientation; } bodyFrame.bodyData[userIndex].joint[j] = jointData; } } } return(newSkeleton); }
public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld, bool bIgnoreJointZ) { bool bNewFrame = false; // look for skeleton frame if (skeletonData != null && skeletonDataTimestamp != lastSkeletonFrameTimestamp) { lastSkeletonFrameTimestamp = skeletonDataTimestamp; //long timeNowTicks = DateTime.Now.Ticks; bodyFrame.liPreviousTime = bodyFrame.liRelativeTime; bodyFrame.liRelativeTime = skeletonDataTimestamp; int bodyCount = skeletonData.Skeletons != null ? skeletonData.Skeletons.Length : 0; if (lastBodyCount != bodyCount) { sbDebugBodies.Append(bodyCount).Append(" bodies - "); } // clear id2index //bodyIdToIndex.Clear(); // create bodyIndexUsed-array on the 1st use if (bodyIndexUsed == null) { bodyIndexUsed = new bool[sensorData.bodyCount]; } // clear the tracked flags and find empty index int eIndex = -1; for (int i = 0; i < sensorData.bodyCount; i++) { bodyFrame.bodyData[i].bIsTracked = 0; if (eIndex < 0 && !bodyIndexUsed[i]) { eIndex = i; } } for (int i = 0; i < sensorData.bodyCount; i++) { // compare to real body count if (i >= bodyCount) { //bodyFrame.bodyData[i].bIsTracked = 0; continue; } // get body and joints data nuitrack.Skeleton nuiBody = skeletonData.Skeletons[i]; if (lastBodyCount != bodyCount) { sbDebugBodies.Append(nuiBody.ID).Append(":"); nuitrack.Joint jUser = nuiBody.Joints[(int)nuitrack.JointType.Waist]; Vector3 vUserPos = new Vector3(dontHFlipBodyFrame ? jUser.Real.X : -jUser.Real.X, jUser.Real.Y, jUser.Real.Z); sbDebugBodies.Append(vUserPos); sbDebugBodies.Append(" "); } // create the body index if needed ushort uBodyId = (ushort)nuiBody.ID; if (!bodyIdToIndex.ContainsKey(uBodyId)) { Debug.Log(" New body ID:" + uBodyId + ", index: " + eIndex); bodyIdToIndex[uBodyId] = (byte)eIndex; bodyIndexUsed[eIndex] = true; } // get existing body index int bi = bodyIdToIndex[uBodyId]; bodyIdToTime[uBodyId] = Time.time; // set body tracking state bodyFrame.bodyData[bi].bIsTracked = 1; // transfer body and joints data bodyFrame.bodyData[bi].liTrackingID = (long)nuiBody.ID; //bodyIdToIndex[(ushort)nuiBody.ID] = (byte)i; // z-position of the waist float waistPosZ = nuiBody.Joints[(int)nuitrack.JointType.Waist].Real.Z / 1000f; for (int j = 0; j < sensorData.jointCount; j++) { KinectInterop.JointData jointData = bodyFrame.bodyData[bi].joint[j]; int nuiJI = !dontHFlipBodyFrame ? BodyJoint2NormalNuitrackJoint[j] : BodyJoint2MirroredNuitrackJoint[j]; if (nuiJI >= 0) { nuitrack.Joint nuiJoint = nuiBody.Joints[nuiJI]; if (nuiJoint.Confidence >= 0.5f) { jointData.trackingState = KinectInterop.TrackingState.Tracked; } else if (nuiJoint.Confidence >= 0.1f) { jointData.trackingState = KinectInterop.TrackingState.Inferred; } else { jointData.trackingState = KinectInterop.TrackingState.NotTracked; } Vector3 jointPos = new Vector3((dontHFlipBodyFrame ? nuiJoint.Real.X : -nuiJoint.Real.X) / 1000f, nuiJoint.Real.Y / 1000f, nuiJoint.Real.Z / 1000f); float jPosZ = (bIgnoreJointZ && j > 0) ? waistPosZ : jointPos.z; jointData.kinectPos = jointPos; jointData.position = kinectToWorld.MultiplyPoint3x4(new Vector3(jointPos.x, jointPos.y, jPosZ)); jointData.orientation = Quaternion.identity; } else { jointData.trackingState = KinectInterop.TrackingState.NotTracked; } if (j == 0) { bodyFrame.bodyData[bi].position = jointData.position; bodyFrame.bodyData[bi].orientation = jointData.orientation; } bodyFrame.bodyData[bi].joint[j] = jointData; } if (handTrackerData != null) { nuitrack.UserHands hands = handTrackerData.GetUserHandsByID(nuiBody.ID); if (hands != null) { nuitrack.HandContent?leftHand = !dontHFlipBodyFrame ? hands.LeftHand : hands.RightHand; bodyFrame.bodyData[bi].leftHandState = leftHand.HasValue && leftHand.Value.Click ? KinectInterop.HandState.Closed : KinectInterop.HandState.Open; bodyFrame.bodyData[bi].leftHandConfidence = leftHand.HasValue ? KinectInterop.TrackingConfidence.High : KinectInterop.TrackingConfidence.Low; nuitrack.HandContent?rightHand = !dontHFlipBodyFrame ? hands.RightHand : hands.LeftHand; bodyFrame.bodyData[bi].rightHandState = rightHand.HasValue && rightHand.Value.Click ? KinectInterop.HandState.Closed : KinectInterop.HandState.Open; bodyFrame.bodyData[bi].rightHandConfidence = rightHand.HasValue ? KinectInterop.TrackingConfidence.High : KinectInterop.TrackingConfidence.Low; } } // processes some special joint cases ProcessBodyFrameSpecialCases(bi, ref bodyFrame); } // check for lost users List <ushort> lostUsers = new List <ushort>(); foreach (ushort uBodyId in bodyIdToTime.Keys) { // prevent user removal upon sporadical tracking failures if ((Time.time - bodyIdToTime[uBodyId]) > waitTimeBeforeRemove) { lostUsers.Add(uBodyId); } } // remove the lost users if (lostUsers.Count > 0) { foreach (ushort uBodyId in lostUsers) { Debug.Log(" Lost body ID:" + uBodyId + ", index: " + bodyIdToIndex[uBodyId]); int bi = bodyIdToIndex[uBodyId]; bodyIndexUsed[bi] = false; bodyIdToIndex.Remove(uBodyId); bodyIdToTime.Remove(uBodyId); } // clean up lostUsers.Clear(); } // write bodies-debug info, if needed if (sbDebugBodies.Length > 0) { sbDebugBodies.Append("Time: ").Append(Time.realtimeSinceStartup); Debug.Log(sbDebugBodies.ToString()); sbDebugBodies.Remove(0, sbDebugBodies.Length); } lastBodyCount = bodyCount; bNewFrame = true; } return(bNewFrame); }
// processes some special cases in body joints private void ProcessBodyFrameSpecialCases(int i, ref KinectInterop.BodyFrameData bodyFrame) { // special case - wrist left int h = (int)KinectInterop.JointType.HandLeft; //if(bodyFrame.bodyData[i].joint[h].trackingState == KinectInterop.TrackingState.Tracked) { int w = h - 1; // wrist int e = h - 2; // elbow KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[w]; jointData.trackingState = bodyFrame.bodyData[i].joint[h].trackingState; jointData.orientation = Quaternion.identity; Vector3 posHand = bodyFrame.bodyData[i].joint[h].kinectPos; Vector3 posElbow = bodyFrame.bodyData[i].joint[e].kinectPos; jointData.kinectPos = posElbow + (posHand - posElbow) * 0.9f; posHand = bodyFrame.bodyData[i].joint[h].position; posElbow = bodyFrame.bodyData[i].joint[e].position; jointData.position = posElbow + (posHand - posElbow) * 0.9f; bodyFrame.bodyData[i].joint[w] = jointData; } // special case - wrist right h = (int)KinectInterop.JointType.HandRight; //if(bodyFrame.bodyData[i].joint[h].trackingState == KinectInterop.TrackingState.Tracked) { int w = h - 1; // wrist int e = h - 2; // elbow KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[w]; jointData.trackingState = bodyFrame.bodyData[i].joint[h].trackingState; jointData.orientation = Quaternion.identity; Vector3 posHand = bodyFrame.bodyData[i].joint[h].kinectPos; Vector3 posElbow = bodyFrame.bodyData[i].joint[e].kinectPos; jointData.kinectPos = posElbow + (posHand - posElbow) * 0.9f; posHand = bodyFrame.bodyData[i].joint[h].position; posElbow = bodyFrame.bodyData[i].joint[e].position; jointData.position = posElbow + (posHand - posElbow) * 0.9f; bodyFrame.bodyData[i].joint[w] = jointData; } // special case - shoulder center int r = (int)KinectInterop.JointType.ShoulderRight; int l = (int)KinectInterop.JointType.ShoulderLeft; if (bodyFrame.bodyData[i].joint[r].trackingState == KinectInterop.TrackingState.Tracked && bodyFrame.bodyData[i].joint[l].trackingState == KinectInterop.TrackingState.Tracked) { int c = (int)KinectInterop.JointType.SpineShoulder; KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[c]; jointData.trackingState = bodyFrame.bodyData[i].joint[r].trackingState; jointData.orientation = Quaternion.identity; Vector3 posRight = bodyFrame.bodyData[i].joint[r].kinectPos; Vector3 posLeft = bodyFrame.bodyData[i].joint[l].kinectPos; jointData.kinectPos = (posRight + posLeft) * 0.5f; posRight = bodyFrame.bodyData[i].joint[r].position; posLeft = bodyFrame.bodyData[i].joint[l].position; jointData.position = (posRight + posLeft) * 0.5f; bodyFrame.bodyData[i].joint[c] = jointData; } // special case - hip center r = (int)KinectInterop.JointType.HipRight; l = (int)KinectInterop.JointType.HipLeft; if (bodyFrame.bodyData[i].joint[r].trackingState == KinectInterop.TrackingState.Tracked && bodyFrame.bodyData[i].joint[l].trackingState == KinectInterop.TrackingState.Tracked) { int c = (int)KinectInterop.JointType.SpineBase; KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[c]; jointData.trackingState = bodyFrame.bodyData[i].joint[r].trackingState; jointData.orientation = Quaternion.identity; Vector3 posRight = bodyFrame.bodyData[i].joint[r].kinectPos; Vector3 posLeft = bodyFrame.bodyData[i].joint[l].kinectPos; jointData.kinectPos = (posRight + posLeft) * 0.5f; posRight = bodyFrame.bodyData[i].joint[r].position; posLeft = bodyFrame.bodyData[i].joint[l].position; jointData.position = (posRight + posLeft) * 0.5f; bodyFrame.bodyData[i].joint[c] = jointData; // modify the body position, too bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } }
public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld, bool bIgnoreJointZ) { bool bNewFrame = false; // look for frame int hr = PollBodyFrame(); if (hr != 0) { Debug.Log("PollBodyFrame error: " + hr); } if (hr == 0) { int bodyFrameIndex = GetBodyFrameIndex(); if (bodyFrameIndex != lastBodyFrameIndex) { lastBodyFrameIndex = bodyFrameIndex; long timeNowTicks = DateTime.Now.Ticks; // get body index frame var pBodyIndexData = GCHandle.Alloc(sensorData.bodyIndexImage, GCHandleType.Pinned); hr = GetBodyIndexData(pBodyIndexData.AddrOfPinnedObject()); pBodyIndexData.Free(); if (hr != 0) { Debug.Log("GetBodyIndexData() error: " + hr); } sensorData.lastBodyIndexFrameTime = (hr == 0) ? timeNowTicks : sensorData.lastBodyIndexFrameTime; bodyFrame.liPreviousTime = bodyFrame.liRelativeTime; bodyFrame.liRelativeTime = timeNowTicks; if (sensorData.hintHeightAngle) { Vector4 floorInfo = Vector4.zero; if (GetFloorInfo(ref floorInfo) != 0) { Vector3 floorPlane = (Vector3)floorInfo; sensorData.sensorRotDetected = Quaternion.FromToRotation(floorPlane, Vector3.up); sensorData.sensorHgtDetected = Mathf.Abs(floorInfo.w / 1000f); } } // int bodySize1 = Marshal.SizeOf(typeof(ObtBody)); // int bodySize2 = GetBodyDataSize(); int bodyCount = GetBodyCount(); if (lastBodyCount != bodyCount) { sbDebugBodies.Append(bodyCount).Append(" bodies - "); } for (int i = 0; i < sensorData.bodyCount; i++) { // compare to real body count if (i >= bodyCount || !obtBodyInited) { bodyFrame.bodyData[i].bIsTracked = 0; continue; } // get body and joints data hr = GetBodyData(i, ref obtBody); if (lastBodyCount != bodyCount) { sbDebugBodies.Append(obtBody.id).Append(":").Append(obtBody.status).Append(":").Append(hr).Append(" "); } // if there is error, consider body as not-tracked if (hr != 0) { bodyFrame.bodyData[i].bIsTracked = 0; continue; } // set body tracking state bodyFrame.bodyData[i].bIsTracked = (short)(obtBody.status != (byte)ObtBodyStatus.BODY_STATUS_NOT_TRACKING ? 1 : 0); if (obtBody.status != (byte)ObtBodyStatus.BODY_STATUS_NOT_TRACKING) { // transfer body and joints data bodyFrame.bodyData[i].liTrackingID = (long)obtBody.id; for (int j = 0; j < sensorData.jointCount; j++) { KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j]; int obtJI = BodyJoint2AstraJoint[j]; if (obtJI >= 0) { jointData.trackingState = (KinectInterop.TrackingState)obtBody.joints[obtJI].status; } else { jointData.trackingState = KinectInterop.TrackingState.NotTracked; } if (jointData.trackingState != KinectInterop.TrackingState.NotTracked) { Vector3 jointPos = obtBody.joints[obtJI].worldPosition / 1000f; float jPosZ = (bIgnoreJointZ && j > 0) ? bodyFrame.bodyData[i].joint[0].kinectPos.z : jointPos.z; jointData.kinectPos = jointPos; jointData.position = kinectToWorld.MultiplyPoint3x4(new Vector3(jointPos.x, jointPos.y, jPosZ)); } jointData.orientation = Quaternion.identity; if (j == 0) { bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } bodyFrame.bodyData[i].joint[j] = jointData; } // processes some special joint cases ProcessBodyFrameSpecialCases(i, ref bodyFrame); // // hand states - is this available on orbbec? // bodyFrame.bodyData[i].leftHandState = (KinectInterop.HandState)body.HandLeftState; // bodyFrame.bodyData[i].leftHandConfidence = (KinectInterop.TrackingConfidence)body.HandLeftConfidence; // // bodyFrame.bodyData[i].rightHandState = (KinectInterop.HandState)body.HandRightState; // bodyFrame.bodyData[i].rightHandConfidence = (KinectInterop.TrackingConfidence)body.HandRightConfidence; } } if (sbDebugBodies.Length > 0) { Debug.Log(sbDebugBodies.ToString()); sbDebugBodies.Remove(0, sbDebugBodies.Length); } lastBodyCount = bodyCount; bNewFrame = true; } // release the frame ReleaseBodyFrame(); } return(bNewFrame); }
public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld, bool bIgnoreJointZ) { bool bNewFrame = false; // look for frame int hr = PollBodyFrame(); if (hr != 0) { Debug.Log("PollBodyFrame error: " + hr); } if (hr == 0) { int bodyFrameIndex = GetBodyFrameIndex(); if (bodyFrameIndex != lastBodyFrameIndex) { lastBodyFrameIndex = bodyFrameIndex; long timeNowTicks = DateTime.Now.Ticks; // get body index frame var pBodyIndexData = GCHandle.Alloc(sensorData.bodyIndexImage, GCHandleType.Pinned); hr = GetBodyIndexData(pBodyIndexData.AddrOfPinnedObject()); pBodyIndexData.Free(); if (hr != 0) { Debug.Log("GetBodyIndexData() error: " + hr); } sensorData.lastBodyIndexFrameTime = (hr == 0) ? timeNowTicks : sensorData.lastBodyIndexFrameTime; bodyFrame.liPreviousTime = bodyFrame.liRelativeTime; bodyFrame.liRelativeTime = timeNowTicks; if (sensorData.hintHeightAngle) { Vector4 floorInfo = Vector4.zero; if (GetFloorInfo(ref floorInfo) != 0) { Vector3 floorPlane = (Vector3)floorInfo; sensorData.sensorRotDetected = Quaternion.FromToRotation(floorPlane, Vector3.up); sensorData.sensorHgtDetected = Mathf.Abs(floorInfo.w / 1000f); } } // int bodySize1 = Marshal.SizeOf(typeof(ObtBody)); // int bodySize2 = GetBodyDataSize(); int bodyCount = GetBodyCount(); if (lastBodyCount != bodyCount) { sbDebugBodies.Append(bodyCount).Append(" bodies - "); } // create bodyIndexUsed-array on the 1st use if (bodyIndexUsed == null) { bodyIndexUsed = new bool[sensorData.bodyCount]; } // clear the tracked flags and find empty index int eIndex = -1; for (int i = 0; i < sensorData.bodyCount; i++) { bodyFrame.bodyData[i].bIsTracked = 0; if (eIndex < 0 && !bodyIndexUsed[i]) { eIndex = i; } } for (int i = 0; i < sensorData.bodyCount; i++) { // compare to real body count if (i >= bodyCount || !obtBodyInited) { //bodyFrame.bodyData[i].bIsTracked = 0; continue; } // get body and joints data hr = GetBodyData(i, ref obtBody); if (lastBodyCount != bodyCount) { sbDebugBodies.Append(obtBody.id).Append(":").Append(obtBody.status).Append(":"); Vector3 vUserPos = obtBody.joints[(int)AstraJoint.JOINT_BASE_SPINE].worldPosition; sbDebugBodies.Append(vUserPos); sbDebugBodies.Append(" "); } // if there is error, consider body as not-tracked if (hr != 0) { //bodyFrame.bodyData[i].bIsTracked = 0; continue; } // create the body index if needed ushort uBodyId = (ushort)obtBody.id; if (!bodyIdToIndex.ContainsKey(uBodyId)) { Debug.Log(" New body ID:" + uBodyId + ", index: " + eIndex); bodyIdToIndex[uBodyId] = (byte)eIndex; bodyIndexUsed[eIndex] = true; } // get existing body index int bi = bodyIdToIndex[uBodyId]; bodyIdToTime[uBodyId] = Time.time; // set body tracking state bodyFrame.bodyData[bi].bIsTracked = (short)(obtBody.status != (byte)ObtBodyStatus.BODY_STATUS_NOT_TRACKING ? 1 : 0); if (obtBody.status != (byte)ObtBodyStatus.BODY_STATUS_NOT_TRACKING) { // transfer body and joints data bodyFrame.bodyData[bi].liTrackingID = (long)obtBody.id; // z-position of the waist float waistPosZ = obtBody.joints[(int)AstraJoint.JOINT_BASE_SPINE].worldPosition.z / 1000f; for (int j = 0; j < sensorData.jointCount; j++) { KinectInterop.JointData jointData = bodyFrame.bodyData[bi].joint[j]; int obtJI = BodyJoint2AstraJoint[j]; if (obtJI >= 0) { jointData.trackingState = (KinectInterop.TrackingState)obtBody.joints[obtJI].status; } else { jointData.trackingState = KinectInterop.TrackingState.NotTracked; } if (jointData.trackingState != KinectInterop.TrackingState.NotTracked) { Vector3 jointPos = obtBody.joints[obtJI].worldPosition / 1000f; float jPosZ = (bIgnoreJointZ && j > 0) ? waistPosZ : jointPos.z; jointData.kinectPos = jointPos; jointData.position = kinectToWorld.MultiplyPoint3x4(new Vector3(jointPos.x, jointPos.y, jPosZ)); } jointData.orientation = Quaternion.identity; if (j == 0) { bodyFrame.bodyData[bi].position = jointData.position; bodyFrame.bodyData[bi].orientation = jointData.orientation; } bodyFrame.bodyData[bi].joint[j] = jointData; } // processes some special joint cases ProcessBodyFrameSpecialCases(bi, ref bodyFrame); // // hand states - is this available on orbbec? // bodyFrame.bodyData[bi].leftHandState = (KinectInterop.HandState)body.HandLeftState; // bodyFrame.bodyData[bi].leftHandConfidence = (KinectInterop.TrackingConfidence)body.HandLeftConfidence; // // bodyFrame.bodyData[bi].rightHandState = (KinectInterop.HandState)body.HandRightState; // bodyFrame.bodyData[bi].rightHandConfidence = (KinectInterop.TrackingConfidence)body.HandRightConfidence; } } // check for lost users List <ushort> lostUsers = new List <ushort>(); foreach (ushort uBodyId in bodyIdToTime.Keys) { // prevent user removal upon sporadical tracking failures if ((Time.time - bodyIdToTime[uBodyId]) > waitTimeBeforeRemove) { lostUsers.Add(uBodyId); } } // remove the lost users if (lostUsers.Count > 0) { foreach (ushort uBodyId in lostUsers) { Debug.Log(" Lost body ID:" + uBodyId + ", index: " + bodyIdToIndex[uBodyId]); int bi = bodyIdToIndex[uBodyId]; bodyIndexUsed[bi] = false; bodyIdToIndex.Remove(uBodyId); bodyIdToTime.Remove(uBodyId); } // clean up lostUsers.Clear(); } if (sbDebugBodies.Length > 0) { sbDebugBodies.Append("Time: ").Append(Time.realtimeSinceStartup); Debug.Log(sbDebugBodies.ToString()); sbDebugBodies.Remove(0, sbDebugBodies.Length); } lastBodyCount = bodyCount; bNewFrame = true; if (bMultiSource) { bMultiFrameBody = true; } } // release the frame ReleaseBodyFrame(); } return(bNewFrame); }
public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld) { bool newSkeleton = false; int hr = NuiSkeletonGetNextFrame(0, ref skeletonFrame); if (hr == 0) { newSkeleton = true; } if (newSkeleton) { hr = NuiTransformSmooth(ref skeletonFrame, ref smoothParameters); if (hr != 0) { Debug.LogError("Skeleton Data Smoothing failed"); } for (uint i = 0; i < sensorData.bodyCount; i++) { NuiSkeletonData body = skeletonFrame.SkeletonData[i]; bodyFrame.bodyData[i].bIsTracked = (short)(body.eTrackingState == NuiSkeletonTrackingState.SkeletonTracked ? 1 : 0); if (body.eTrackingState == NuiSkeletonTrackingState.SkeletonTracked) { // transfer body and joints data bodyFrame.bodyData[i].liTrackingID = (long)body.dwTrackingID; for (int j = 0; j < sensorData.jointCount; j++) { KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j]; jointData.jointType = GetJointAtIndex(j); jointData.trackingState = (KinectInterop.TrackingState)body.eSkeletonPositionTrackingState[j]; if (jointData.trackingState != KinectInterop.TrackingState.NotTracked) { jointData.kinectPos = body.SkeletonPositions[j]; jointData.position = kinectToWorld.MultiplyPoint3x4(jointData.kinectPos); } jointData.orientation = Quaternion.identity; // Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation; // jointData.orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W); if (j == 0) { bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } bodyFrame.bodyData[i].joint[j] = jointData; } // tranfer hand states uint intCount = GetInteractorsCount(); for (uint intIndex = 0; intIndex < intCount; intIndex++) { uint skeletonId = GetSkeletonTrackingID(intIndex); if (skeletonId == body.dwTrackingID) { uint leftHandState = GetLeftHandState(intIndex); InteractionHandEventType leftHandEvent = GetLeftHandEvent(intIndex); uint rightHandState = GetRightHandState(intIndex); InteractionHandEventType rightHandEvent = GetRightHandEvent(intIndex); GetHandStateAndConf(leftHandState, leftHandEvent, ref bodyFrame.bodyData[i].leftHandState, ref bodyFrame.bodyData[i].leftHandConfidence); GetHandStateAndConf(rightHandState, rightHandEvent, ref bodyFrame.bodyData[i].rightHandState, ref bodyFrame.bodyData[i].rightHandConfidence); } } } } } return(newSkeleton); }
public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld, bool bIgnoreJointZ) { bool bNewFrame = false; if ((multiSourceFrameReader != null && multiSourceFrame != null) || bodyFrameReader != null) { BodyFrame frame = multiSourceFrame != null ? msBodyFrame : bodyFrameReader.AcquireLatestFrame(); if (frame != null) { frame.GetAndRefreshBodyData(bodyData); bodyFrame.liPreviousTime = bodyFrame.liRelativeTime; bodyFrame.liRelativeTime = frame.RelativeTime.Ticks; frame.Dispose(); frame = null; for (int i = 0; i < sensorData.bodyCount; i++) { Body body = bodyData [i]; if (body == null) { bodyFrame.bodyData [i].bIsTracked = 0; continue; } bodyFrame.bodyData [i].bIsTracked = (short)(body.IsTracked ? 1 : 0); if (body.IsTracked) { // transfer body and joints data bodyFrame.bodyData [i].liTrackingID = (long)body.TrackingId; // cache the body joints (following the advice of Brian Chasalow) Dictionary <Windows.Kinect.JointType, Windows.Kinect.Joint> bodyJoints = body.Joints; for (int j = 0; j < sensorData.jointCount; j++) { Windows.Kinect.Joint joint = bodyJoints [(Windows.Kinect.JointType)j]; KinectInterop.JointData jointData = bodyFrame.bodyData [i].joint [j]; //jointData.jointType = (KinectInterop.JointType)j; jointData.trackingState = (KinectInterop.TrackingState)joint.TrackingState; if ((int)joint.TrackingState != (int)TrackingState.NotTracked) { float jPosZ = (bIgnoreJointZ && j > 0) ? bodyFrame.bodyData [i].joint [0].kinectPos.z : joint.Position.Z; jointData.kinectPos = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z); jointData.position = kinectToWorld.MultiplyPoint3x4(new Vector3(joint.Position.X, joint.Position.Y, jPosZ)); } jointData.orientation = Quaternion.identity; // Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation; // jointData.orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W); if (j == 0) { bodyFrame.bodyData [i].position = jointData.position; bodyFrame.bodyData [i].orientation = jointData.orientation; } bodyFrame.bodyData [i].joint [j] = jointData; } // tranfer hand states bodyFrame.bodyData [i].leftHandState = (KinectInterop.HandState)body.HandLeftState; bodyFrame.bodyData [i].leftHandConfidence = (KinectInterop.TrackingConfidence)body.HandLeftConfidence; bodyFrame.bodyData [i].rightHandState = (KinectInterop.HandState)body.HandRightState; bodyFrame.bodyData [i].rightHandConfidence = (KinectInterop.TrackingConfidence)body.HandRightConfidence; } } bNewFrame = true; } } return(bNewFrame); }