void Start()
{
saverPlayer = KinectRecorderPlayer.Instance;
sensorData = KinectManager.Instance.GetSensorData();
kinectToWorld = KinectManager.Instance.GetKinectToWorldMatrix();
bodyFrame = new KinectInterop.BodyFrameData(sensorData.bodyCount, KinectInterop.Constants.MaxJointCount);
}
// 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;
}
}
// Update the filter with a new frame of data and smooth.
public void UpdateFilter(ref KinectInterop.BodyFrameData bodyFrame)
{
if (this.init == false)
{
this.Init(); // initialize with default parameters
}
KinectInterop.SmoothParameters tempSmoothingParams = new KinectInterop.SmoothParameters();
tempSmoothingParams.smoothing = this.smoothParameters.smoothing;
tempSmoothingParams.correction = this.smoothParameters.correction;
tempSmoothingParams.prediction = this.smoothParameters.prediction;
int bodyCount = KinectInterop.Constants.BodyCount;
for (int bodyIndex = 0; bodyIndex < bodyCount; bodyIndex++)
{
if (bodyFrame.bodyData[bodyIndex].bIsTracked != 0)
{
FilterBodyJoints(ref bodyFrame.bodyData[bodyIndex], bodyIndex, ref tempSmoothingParams);
}
}
}
// Update the filter with a new frame of data and smooth.
/// <summary>
/// 使用新的数据帧更新滤镜并平滑。
/// </summary>
/// <param name="bodyFrame">身体帧</param>
public void UpdateFilter(ref KinectInterop.BodyFrameData bodyFrame)
{
if (this.init == false)
{
this.Init(); // 使用默认参数初始化
}
KinectInterop.SmoothParameters tempSmoothingParams = new KinectInterop.SmoothParameters();
tempSmoothingParams.smoothing = this.smoothParameters.smoothing;
tempSmoothingParams.correction = this.smoothParameters.correction;
tempSmoothingParams.prediction = this.smoothParameters.prediction;
KinectManager manager = KinectManager.Instance;
int bodyCount = manager.GetSensorBodyCount();
for (int bodyIndex = 0; bodyIndex < bodyCount; bodyIndex++)
{
if (bodyFrame.bodyData[bodyIndex].bIsTracked != 0)
{
FilterBodyJoints(ref bodyFrame.bodyData[bodyIndex], bodyIndex, ref tempSmoothingParams);
}
}
}
void StartKinect()
{
try
{
// try to initialize the default Kinect2 sensor
KinectInterop.FrameSource dwFlags = KinectInterop.FrameSource.TypeBody;
if(computeUserMap != UserMapType.None)
dwFlags |= KinectInterop.FrameSource.TypeDepth | KinectInterop.FrameSource.TypeBodyIndex;
if(computeColorMap)
dwFlags |= KinectInterop.FrameSource.TypeColor;
if(computeInfraredMap)
dwFlags |= KinectInterop.FrameSource.TypeInfrared;
// if(useAudioSource)
// dwFlags |= KinectInterop.FrameSource.TypeAudio;
// open the default sensor
BackgroundRemovalManager brManager = gameObject.GetComponentInChildren<BackgroundRemovalManager>();
sensorData = KinectInterop.OpenDefaultSensor(sensorInterfaces, dwFlags, sensorAngle, useMultiSourceReader, computeUserMap, brManager);
if (sensorData == null)
{
if(sensorInterfaces == null || sensorInterfaces.Count == 0)
throw new Exception("No sensor found. Make sure you have installed the SDK and the sensor is connected.");
else
throw new Exception("OpenDefaultSensor failed.");
}
// enable or disable getting height and angle info
sensorData.hintHeightAngle = (autoHeightAngle != AutoHeightAngle.DontUse);
//create the transform matrix - kinect to world
Quaternion quatTiltAngle = Quaternion.Euler(-sensorAngle, 0.0f, 0.0f);
kinectToWorld.SetTRS(new Vector3(0.0f, sensorHeight, 0.0f), quatTiltAngle, Vector3.one);
}
catch(DllNotFoundException ex)
{
string message = ex.Message + " cannot be loaded. Please check the Kinect SDK installation.";
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.text = message;
}
return;
}
catch(Exception ex)
{
string message = ex.Message;
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.text = message;
}
return;
}
// set the singleton instance
instance = this;
// init skeleton structures
bodyFrame = new KinectInterop.BodyFrameData(sensorData.bodyCount, KinectInterop.Constants.MaxJointCount); // sensorData.jointCount
bodyFrame.bTurnAnalisys = allowTurnArounds;
KinectInterop.SmoothParameters smoothParameters = new KinectInterop.SmoothParameters();
switch(smoothing)
{
case Smoothing.Default:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.5f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.05f;
smoothParameters.maxDeviationRadius = 0.04f;
break;
case Smoothing.Medium:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.1f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.1f;
smoothParameters.maxDeviationRadius = 0.1f;
break;
case Smoothing.Aggressive:
smoothParameters.smoothing = 0.7f;
smoothParameters.correction = 0.3f;
smoothParameters.prediction = 1.0f;
smoothParameters.jitterRadius = 1.0f;
smoothParameters.maxDeviationRadius = 1.0f;
break;
}
// init data filters
jointPositionFilter = new JointPositionsFilter();
jointPositionFilter.Init(smoothParameters);
// init the bone orientation constraints
if(useBoneOrientationConstraints)
{
boneConstraintsFilter = new BoneOrientationsConstraint();
boneConstraintsFilter.AddDefaultConstraints();
boneConstraintsFilter.SetDebugText(calibrationText);
}
if(computeUserMap != UserMapType.None && computeUserMap != UserMapType.RawUserDepth)
{
// Initialize depth & label map related stuff
usersLblTex = new Texture2D(sensorData.depthImageWidth, sensorData.depthImageHeight, TextureFormat.ARGB32, false);
usersMapSize = sensorData.depthImageWidth * sensorData.depthImageHeight;
usersHistogramImage = new Color32[usersMapSize];
usersPrevState = new ushort[usersMapSize];
usersHistogramMap = new float[5001];
}
if(computeColorMap)
{
// Initialize color map related stuff
//usersClrTex = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGBA32, false);
usersClrSize = sensorData.colorImageWidth * sensorData.colorImageHeight;
}
// try to automatically use the available avatar controllers in the scene
if(avatarControllers.Count == 0)
{
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[];
foreach(MonoBehaviour monoScript in monoScripts)
{
if(typeof(AvatarController).IsAssignableFrom(monoScript.GetType()) && monoScript.enabled)
{
AvatarController avatar = (AvatarController)monoScript;
avatarControllers.Add(avatar);
}
}
}
// set up the gesture manager, if not already set
if(gestureManager == null)
{
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[];
foreach(MonoBehaviour monoScript in monoScripts)
{
if(typeof(KinectGestures).IsAssignableFrom(monoScript.GetType()) && monoScript.enabled)
{
gestureManager = (KinectGestures)monoScript;
break;
}
}
}
// try to automatically use the available gesture listeners in the scene
if(gestureListeners.Count == 0)
{
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[];
foreach(MonoBehaviour monoScript in monoScripts)
{
if(typeof(KinectGestures.GestureListenerInterface).IsAssignableFrom(monoScript.GetType()) &&
monoScript.enabled)
{
//KinectGestures.GestureListenerInterface gl = (KinectGestures.GestureListenerInterface)monoScript;
gestureListeners.Add(monoScript);
}
}
}
// Initialize user list to contain all users.
//alUserIds = new List<Int64>();
//dictUserIdToIndex = new Dictionary<Int64, int>();
// // start the background reader
// kinectReaderThread = new System.Threading.Thread(UpdateKinectStreamsThread);
// kinectReaderThread.Name = "KinectReaderThread";
// kinectReaderThread.IsBackground = true;
// kinectReaderThread.Start();
// kinectReaderRunning = true;
kinectInitialized = true;
#if USE_SINGLE_KM_IN_MULTIPLE_SCENES
DontDestroyOnLoad(gameObject);
#endif
// GUI Text.
if(calibrationText != null)
{
calibrationText.text = "WAITING FOR USERS";
}
Debug.Log("Waiting for users.");
}
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);
}
void StartKinect()
{
try
{
// try to initialize the default Kinect2 sensor
KinectInterop.FrameSource dwFlags = KinectInterop.FrameSource.TypeBody;
if(computeUserMap)
dwFlags |= KinectInterop.FrameSource.TypeDepth | KinectInterop.FrameSource.TypeBodyIndex;
if(computeColorMap)
dwFlags |= KinectInterop.FrameSource.TypeColor;
if(computeInfraredMap)
dwFlags |= KinectInterop.FrameSource.TypeInfrared;
// if(useAudioSource)
// dwFlags |= KinectInterop.FrameSource.TypeAudio;
// open the default sensor
sensorData = KinectInterop.OpenDefaultSensor(sensorInterfaces, dwFlags, sensorAngle, useMultiSourceReader);
if (sensorData == null)
{
throw new Exception("OpenDefaultSensor failed");
}
//create the transform matrix - kinect to world
Quaternion quatTiltAngle = new Quaternion();
quatTiltAngle.eulerAngles = new Vector3(-sensorAngle, 0.0f, 0.0f);
kinectToWorld.SetTRS(new Vector3(0.0f, sensorHeight, 0.0f), quatTiltAngle, Vector3.one);
}
catch(DllNotFoundException ex)
{
string message = ex.Message + " cannot be loaded. Please check the Kinect SDK installation.";
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.guiText.text = message;
}
return;
}
catch(Exception ex)
{
string message = ex.Message;
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.guiText.text = message;
}
return;
}
// set the singleton instance
instance = this;
// init skeleton structures
bodyFrame = new KinectInterop.BodyFrameData(sensorData.bodyCount, KinectInterop.Constants.JointCount); // sensorData.jointCount
KinectInterop.SmoothParameters smoothParameters = new KinectInterop.SmoothParameters();
switch(smoothing)
{
case Smoothing.Default:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.5f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.05f;
smoothParameters.maxDeviationRadius = 0.04f;
break;
case Smoothing.Medium:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.1f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.1f;
smoothParameters.maxDeviationRadius = 0.1f;
break;
case Smoothing.Aggressive:
smoothParameters.smoothing = 0.7f;
smoothParameters.correction = 0.3f;
smoothParameters.prediction = 1.0f;
smoothParameters.jitterRadius = 1.0f;
smoothParameters.maxDeviationRadius = 1.0f;
break;
}
// init data filters
jointPositionFilter = new JointPositionsFilter();
jointPositionFilter.Init(smoothParameters);
// init the bone orientation constraints
if(useBoneOrientationConstraints)
{
boneConstraintsFilter = new BoneOrientationsConstraint();
boneConstraintsFilter.AddDefaultConstraints();
//boneConstraintsFilter.SetDebugText(calibrationText);
}
// get the main camera rectangle
Rect cameraRect = Camera.main.pixelRect;
// calculate map width and height in percent, if needed
if(DisplayMapsWidthPercent == 0f)
{
DisplayMapsWidthPercent = (sensorData.depthImageWidth / 2) * 100 / cameraRect.width;
}
if(computeUserMap)
{
float displayMapsWidthPercent = DisplayMapsWidthPercent / 100f;
float displayMapsHeightPercent = displayMapsWidthPercent * sensorData.depthImageHeight / sensorData.depthImageWidth;
float displayWidth = cameraRect.width * displayMapsWidthPercent;
float displayHeight = cameraRect.width * displayMapsHeightPercent;
// Initialize depth & label map related stuff
usersLblTex = new Texture2D(sensorData.depthImageWidth, sensorData.depthImageHeight);
usersMapRect = new Rect(cameraRect.width - displayWidth, cameraRect.height, displayWidth, -displayHeight);
usersMapSize = sensorData.depthImageWidth * sensorData.depthImageHeight;
usersHistogramImage = new Color32[usersMapSize];
usersPrevState = new ushort[usersMapSize];
usersHistogramMap = new float[5001];
}
if(computeColorMap)
{
float displayMapsWidthPercent = DisplayMapsWidthPercent / 100f;
float displayMapsHeightPercent = displayMapsWidthPercent * sensorData.colorImageHeight / sensorData.colorImageWidth;
float displayWidth = cameraRect.width * displayMapsWidthPercent;
float displayHeight = cameraRect.width * displayMapsHeightPercent;
// Initialize color map related stuff
usersClrTex = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGBA32, false);
usersClrRect = new Rect(cameraRect.width - displayWidth, cameraRect.height, displayWidth, -displayHeight);
usersClrSize = sensorData.colorImageWidth * sensorData.colorImageHeight;
// if(computeUserMap && displayColorMap)
// {
// usersMapRect.x -= cameraRect.width * displayMapsWidthPercent;
// }
}
// try to automatically use the available avatar controllers in the scene
if(avatarControllers.Count == 0)
{
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[];
foreach(MonoBehaviour monoScript in monoScripts)
{
if(typeof(AvatarController).IsAssignableFrom(monoScript.GetType()))
{
AvatarController avatar = (AvatarController)monoScript;
avatarControllers.Add(avatar);
}
}
}
// try to automatically use the available gesture listeners in the scene
if(gestureListeners.Count == 0)
{
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[];
foreach(MonoBehaviour monoScript in monoScripts)
{
if(typeof(KinectGestures.GestureListenerInterface).IsAssignableFrom(monoScript.GetType()))
{
print ("Found Listener");
//KinectGestures.GestureListenerInterface gl = (KinectGestures.GestureListenerInterface)monoScript;
gestureListeners.Add(monoScript);
}
}
}
// Initialize user list to contain all users.
alUserIds = new List<Int64>();
dictUserIdToIndex = new Dictionary<Int64, int>();
kinectInitialized = true;
DontDestroyOnLoad(gameObject);
// GUI Text.
if(calibrationText != null)
{
calibrationText.guiText.text = "WAITING FOR USERS";
}
Debug.Log("Waiting for users.");
}
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);
}
void Start()
{
// int hr = 0;
try
{
// if(startKinectServer)
// {
// // start the Kinect-server app
// hr = StartKinectServer();
// if (hr != 0)
// {
// throw new Exception("Kinect2Server not started");
// }
// }
// try to initialize the default Kinect2 sensor
KinectInterop.FrameSource dwFlags = KinectInterop.FrameSource.TypeBody;
if(computeUserMap)
dwFlags |= KinectInterop.FrameSource.TypeDepth | KinectInterop.FrameSource.TypeBodyIndex;
if(computeColorMap)
dwFlags |= KinectInterop.FrameSource.TypeColor;
if(computeInfraredMap)
dwFlags |= KinectInterop.FrameSource.TypeInfrared;
// hr = KinectInterop.InitDefaultKinectSensor(dwFlags, KinectInterop.Constants.ColorImageWidth, KinectInterop.Constants.ColorImageHeight);
// if (hr != 0)
// {
// throw new Exception("InitDefaultKinectSensor failed");
// }
// open the default kinect sensor
sensorData = KinectInterop.OpenDefaultKinectSensor(dwFlags);
if (sensorData == null)
{
throw new Exception("OpenDefaultKinectSensor failed");
}
// transform matrix - kinect to world
kinectToWorld.SetTRS(new Vector3(0.0f, sensorHeight, 0.0f), Quaternion.identity, Vector3.one);
}
catch(DllNotFoundException ex)
{
string message = ex.Message + " cannot be loaded. Please check the Kinect SDK installation.";
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.guiText.text = message;
}
return;
}
catch(Exception ex)
{
string message = ex.Message;
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.guiText.text = message;
}
return;
}
// init skeleton structures
bodyFrame = new KinectInterop.BodyFrameData(true);
KinectInterop.SmoothParameters smoothParameters = new KinectInterop.SmoothParameters();
switch(smoothing)
{
case Smoothing.Default:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.5f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.05f;
smoothParameters.maxDeviationRadius = 0.04f;
break;
case Smoothing.Medium:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.1f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.1f;
smoothParameters.maxDeviationRadius = 0.1f;
break;
case Smoothing.Aggressive:
smoothParameters.smoothing = 0.7f;
smoothParameters.correction = 0.3f;
smoothParameters.prediction = 1.0f;
smoothParameters.jitterRadius = 1.0f;
smoothParameters.maxDeviationRadius = 1.0f;
break;
}
// init data filters
jointPositionFilter = new JointPositionsFilter();
jointPositionFilter.Init(smoothParameters);
// init the bone orientation constraints
if(useBoneOrientationConstraints)
{
boneConstraintsFilter = new BoneOrientationsConstraint();
boneConstraintsFilter.AddDefaultConstraints();
boneConstraintsFilter.SetDebugText(calibrationText);
}
// get the main camera rectangle
Rect cameraRect = Camera.main.pixelRect;
// calculate map width and height in percent, if needed
if(MapsPercentWidth == 0f)
MapsPercentWidth = (sensorData.depthImageWidth / 2) / cameraRect.width;
if(MapsPercentHeight == 0f)
MapsPercentHeight = (sensorData.depthImageHeight / 2) / cameraRect.height;
if(computeUserMap)
{
// init user-depth structures
//depthImage = new KinectInterop.DepthBuffer(true);
//bodyIndexImage = new KinectInterop.BodyIndexBuffer(true);
// Initialize depth & label map related stuff
usersLblTex = new Texture2D(sensorData.depthImageWidth, sensorData.depthImageHeight);
usersMapRect = new Rect(cameraRect.width - cameraRect.width * MapsPercentWidth, cameraRect.height, cameraRect.width * MapsPercentWidth, -cameraRect.height * MapsPercentHeight);
usersMapSize = sensorData.depthImageWidth * sensorData.depthImageHeight;
usersHistogramImage = new Color32[usersMapSize];
usersPrevState = new ushort[usersMapSize];
usersHistogramMap = new float[5001];
}
if(computeColorMap)
{
// init color image structures
//colorImage = new KinectInterop.ColorBuffer(true);
// Initialize color map related stuff
usersClrTex = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGBA32, false);
usersClrRect = new Rect(cameraRect.width - 320, cameraRect.height, 320, -180);
usersCaliBox = new Rect(cameraRect.width - 160 - 320 / 14, cameraRect.height - 90, 320 / 7, 90);
usersClrSize = sensorData.colorImageWidth * sensorData.colorImageHeight;
if(computeUserMap && displayColorMap)
{
usersMapRect.x -= cameraRect.width * MapsPercentWidth; //usersClrTex.width / 2;
}
}
// try to automatically find the available avatar controllers at the scene
if(avatarControllers.Count == 0)
{
AvatarController[] avatars = FindObjectsOfType(typeof(AvatarController)) as AvatarController[];
foreach(AvatarController avatar in avatars)
{
avatarControllers.Add(avatar);
}
}
// Initialize user list to contain all users.
alUserIds = new List<Int64>();
dictUserIdToIndex = new Dictionary<Int64, int>();
kinectInitialized = true;
instance = this;
//DontDestroyOnLoad(gameObject);
// GUI Text.
if(calibrationText != null)
{
calibrationText.guiText.text = "WAITING FOR USERS";
}
Debug.Log("Waiting for users.");
}
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 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 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 bIgnoreJointZ)
{
return(false);
}
