// Initialize the filter with a set of TransformSmoothParameters. public void Init(KinectWrapper.NuiTransformSmoothParameters smoothingParameters) { this.smoothParameters = smoothingParameters; this.Reset(); this.init = true; }
// Initialize the filter with a set of TransformSmoothParameters. public void Init(KinectWrapper.NuiTransformSmoothParameters smoothingParameters) { this.smoothParameters = smoothingParameters; this.Reset(); this.init = true; }
/// <summary> /// Initialize the filter with a set of manually specified TransformSmoothParameters. /// </summary> /// <param name="smoothingValue">Smoothing = [0..1], lower values is closer to the raw data and more noisy.</param> /// <param name="correctionValue">Correction = [0..1], higher values correct faster and feel more responsive.</param> /// <param name="predictionValue">Prediction = [0..n], how many frames into the future we want to predict.</param> /// <param name="jitterRadiusValue">JitterRadius = The deviation distance in m that defines jitter.</param> /// <param name="maxDeviationRadiusValue">MaxDeviation = The maximum distance in m that filtered positions are allowed to deviate from raw data.</param> public void Init(float smoothingValue, float correctionValue, float predictionValue, float jitterRadiusValue, float maxDeviationRadiusValue) { this.smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); this.smoothParameters.fSmoothing = smoothingValue; // How much soothing will occur. Will lag when too high this.smoothParameters.fCorrection = correctionValue; // How much to correct back from prediction. Can make things springy this.smoothParameters.fPrediction = predictionValue; // Amount of prediction into the future to use. Can over shoot when too high this.smoothParameters.fJitterRadius = jitterRadiusValue; // Size of the radius where jitter is removed. Can do too much smoothing when too high this.smoothParameters.fMaxDeviationRadius = maxDeviationRadiusValue; // Size of the max prediction radius Can snap back to noisy data when too high this.Reset(); this.init = true; }
/// <summary> /// Initialize the filter with a set of manually specified TransformSmoothParameters. /// </summary> /// <param name="smoothingValue">Smoothing = [0..1], lower values is closer to the raw data and more noisy.</param> /// <param name="correctionValue">Correction = [0..1], higher values correct faster and feel more responsive.</param> /// <param name="predictionValue">Prediction = [0..n], how many frames into the future we want to predict.</param> /// <param name="jitterRadiusValue">JitterRadius = The deviation distance in m that defines jitter.</param> /// <param name="maxDeviationRadiusValue">MaxDeviation = The maximum distance in m that filtered positions are allowed to deviate from raw data.</param> public void Init(float smoothingValue, float correctionValue, float predictionValue, float jitterRadiusValue, float maxDeviationRadiusValue) { this.smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); this.smoothParameters.fSmoothing = smoothingValue; // How much soothing will occur. Will lag when too high this.smoothParameters.fCorrection = correctionValue; // How much to correct back from prediction. Can make things springy this.smoothParameters.fPrediction = predictionValue; // Amount of prediction into the future to use. Can over shoot when too high this.smoothParameters.fJitterRadius = jitterRadiusValue; // Size of the radius where jitter is removed. Can do too much smoothing when too high this.smoothParameters.fMaxDeviationRadius = maxDeviationRadiusValue; // Size of the max prediction radius Can snap back to noisy data when too high this.Reset(); this.init = true; }
// Update the filter with a new frame of data and smooth. public void UpdateFilter(ref KinectWrapper.NuiSkeletonData skeleton) { // if (null == skeleton) // { // return; // } if (skeleton.eTrackingState != KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked) { return; } if (this.init == false) { this.Init(); // initialize with default parameters } //Array jointTypeValues = Enum.GetValues(typeof(KinectWrapper.NuiSkeletonPositionIndex)); KinectWrapper.NuiTransformSmoothParameters tempSmoothingParams = new KinectWrapper.NuiTransformSmoothParameters(); // Check for divide by zero. Use an epsilon of a 10th of a millimeter this.smoothParameters.fJitterRadius = Math.Max(0.0001f, this.smoothParameters.fJitterRadius); tempSmoothingParams.fSmoothing = smoothParameters.fSmoothing; tempSmoothingParams.fCorrection = smoothParameters.fCorrection; tempSmoothingParams.fPrediction = smoothParameters.fPrediction; int jointsCount = (int)KinectWrapper.NuiSkeletonPositionIndex.Count; for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++) { //KinectWrapper.NuiSkeletonPositionIndex jt = (KinectWrapper.NuiSkeletonPositionIndex)jointTypeValues.GetValue(jointIndex); // If not tracked, we smooth a bit more by using a bigger jitter radius // Always filter feet highly as they are so noisy if (skeleton.eSkeletonPositionTrackingState[jointIndex] != KinectWrapper.NuiSkeletonPositionTrackingState.Tracked) { tempSmoothingParams.fJitterRadius = smoothParameters.fJitterRadius * 2.0f; tempSmoothingParams.fMaxDeviationRadius = smoothParameters.fMaxDeviationRadius * 2.0f; } else { tempSmoothingParams.fJitterRadius = smoothParameters.fJitterRadius; tempSmoothingParams.fMaxDeviationRadius = smoothParameters.fMaxDeviationRadius; } FilterJoint(ref skeleton, jointIndex, ref tempSmoothingParams); } }
// Update the filter for one joint. protected void FilterJoint(ref KinectWrapper.NuiSkeletonData skeleton, int jointIndex, ref KinectWrapper.NuiTransformSmoothParameters smoothingParameters, ref Matrix4x4[] jointOrientations) { // if (null == skeleton) // { // return; // } // int jointIndex = (int)jt; Quaternion filteredOrientation; Quaternion trend; Vector3 fwdVector = (Vector3)jointOrientations[jointIndex].GetColumn(2); if (fwdVector == Vector3.zero) { return; } Quaternion rawOrientation = Quaternion.LookRotation(fwdVector, jointOrientations[jointIndex].GetColumn(1)); Quaternion prevFilteredOrientation = this.history[jointIndex].FilteredBoneOrientation; Quaternion prevTrend = this.history[jointIndex].Trend; Vector3 rawPosition = (Vector3)skeleton.SkeletonPositions[jointIndex]; bool orientationIsValid = KinectHelper.JointPositionIsValid(rawPosition) && KinectHelper.IsTrackedOrInferred(skeleton, jointIndex) && KinectHelper.BoneOrientationIsValid(rawOrientation); if (!orientationIsValid) { if (this.history[jointIndex].FrameCount > 0) { rawOrientation = history[jointIndex].FilteredBoneOrientation; history[jointIndex].FrameCount = 0; } } // Initial start values or reset values if (this.history[jointIndex].FrameCount == 0) { // Use raw position and zero trend for first value filteredOrientation = rawOrientation; trend = Quaternion.identity; } else if (this.history[jointIndex].FrameCount == 1) { // Use average of two positions and calculate proper trend for end value Quaternion prevRawOrientation = this.history[jointIndex].RawBoneOrientation; filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevRawOrientation, rawOrientation, 0.5f); Quaternion diffStarted = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffStarted, smoothingParameters.fCorrection); } else { // First apply a jitter filter Quaternion diffJitter = KinectHelper.RotationBetweenQuaternions(rawOrientation, prevFilteredOrientation); float diffValJitter = (float)Math.Abs(KinectHelper.QuaternionAngle(diffJitter)); if (diffValJitter <= smoothingParameters.fJitterRadius) { filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevFilteredOrientation, rawOrientation, diffValJitter / smoothingParameters.fJitterRadius); } else { filteredOrientation = rawOrientation; } // Now the double exponential smoothing filter filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, prevFilteredOrientation * prevTrend, smoothingParameters.fSmoothing); diffJitter = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffJitter, smoothingParameters.fCorrection); } // Use the trend and predict into the future to reduce latency Quaternion predictedOrientation = filteredOrientation * KinectHelper.EnhancedQuaternionSlerp(Quaternion.identity, trend, smoothingParameters.fPrediction); // Check that we are not too far away from raw data Quaternion diff = KinectHelper.RotationBetweenQuaternions(predictedOrientation, filteredOrientation); float diffVal = (float)Math.Abs(KinectHelper.QuaternionAngle(diff)); if (diffVal > smoothingParameters.fMaxDeviationRadius) { predictedOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, predictedOrientation, smoothingParameters.fMaxDeviationRadius / diffVal); } // predictedOrientation.Normalize(); // filteredOrientation.Normalize(); // trend.Normalize(); // Save the data from this frame history[jointIndex].RawBoneOrientation = rawOrientation; history[jointIndex].FilteredBoneOrientation = filteredOrientation; history[jointIndex].Trend = trend; history[jointIndex].FrameCount++; // Set the filtered and predicted data back into the bone orientation if (KinectHelper.BoneOrientationIsValid(predictedOrientation)) { jointOrientations[jointIndex].SetTRS(Vector3.zero, predictedOrientation, Vector3.one); } }
void Awake() { int hr = 0; try { hr = KinectWrapper.NuiInitialize(KinectWrapper.NuiInitializeFlags.UsesSkeleton | KinectWrapper.NuiInitializeFlags.UsesDepthAndPlayerIndex | (ComputeColorMap ? KinectWrapper.NuiInitializeFlags.UsesColor : 0)); if (hr != 0) { throw new Exception("NuiInitialize Failed"); } hr = KinectWrapper.NuiSkeletonTrackingEnable(IntPtr.Zero, 8); // 0, 12,8 if (hr != 0) { throw new Exception("Cannot initialize Skeleton Data"); } depthStreamHandle = IntPtr.Zero; if (ComputeUserMap) { hr = KinectWrapper.NuiImageStreamOpen (KinectWrapper.NuiImageType.DepthAndPlayerIndex, KinectWrapper.Constants.DepthImageResolution, 0, 2, IntPtr.Zero, ref depthStreamHandle); if (hr != 0) { throw new Exception("Cannot open depth stream"); } } colorStreamHandle = IntPtr.Zero; if (ComputeColorMap) { hr = KinectWrapper.NuiImageStreamOpen (KinectWrapper.NuiImageType.Color, KinectWrapper.Constants.ColorImageResolution, 0, 2, IntPtr.Zero, ref colorStreamHandle); if (hr != 0) { throw new Exception("Cannot open color stream"); } } // set kinect elevation angle KinectWrapper.NuiCameraElevationSetAngle(SensorAngle); // init skeleton structures skeletonFrame = new KinectWrapper.NuiSkeletonFrame() { SkeletonData = new KinectWrapper.NuiSkeletonData [KinectWrapper.Constants.NuiSkeletonCount] }; // values used to pass to smoothing function smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); switch (smoothing) { case Smoothing.Default: smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.5f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.05f; smoothParameters.fMaxDeviationRadius = 0.04f; break; case Smoothing.Medium: smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.1f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.1f; smoothParameters.fMaxDeviationRadius = 0.1f; break; case Smoothing.Aggressive: smoothParameters.fSmoothing = 0.7f; smoothParameters.fCorrection = 0.3f; smoothParameters.fPrediction = 1.0f; smoothParameters.fJitterRadius = 1.0f; smoothParameters.fMaxDeviationRadius = 1.0f; break; } // create arrays for joint positions and joint orientations int skeletonJointsCount = (int) KinectWrapper.NuiSkeletonPositionIndex.Count; playerJointsTracked = new bool[skeletonJointsCount]; playerPrevTracked = new bool[skeletonJointsCount]; playerJointsPos = new Vector3[skeletonJointsCount]; //create the transform matrix that converts from kinect-space to world-space Quaternion quatTiltAngle = new Quaternion(); quatTiltAngle.eulerAngles = new Vector3(-SensorAngle, 0.0f, 0.0f); //float heightAboveHips = SensorHeight - 1.0f; // transform matrix - kinect to world //kinectToWorld.SetTRS(new Vector3(0.0f, heightAboveHips, 0.0f), quatTiltAngle, Vector3.one); kinectToWorld.SetTRS(new Vector3(0.0f, SensorHeight, 0.0f), quatTiltAngle, Vector3.one); flipMatrix = Matrix4x4.identity; flipMatrix[2, 2] = -1; instance = this; } catch (DllNotFoundException e) { string message = "Please check the Kinect SDK installation."; Debug.LogError(message); Debug.LogError(e.ToString()); return; } catch (Exception e) { string message = e.Message + " - " + KinectWrapper.GetNuiErrorString(hr); Debug.LogError(message); Debug.LogError(e.ToString()); return; } if (ComputeUserMap) { // Initialize depth & label map related stuff usersMapSize = KinectWrapper.GetDepthWidth() * KinectWrapper.GetDepthHeight(); usersLblTex = new Texture2D(KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight()); usersMapColors = new Color32[usersMapSize]; usersPrevState = new ushort[usersMapSize]; usersDepthMap = new ushort[usersMapSize]; usersHistogramMap = new float[8192]; } if (ComputeColorMap) { // Initialize color map related stuff usersClrTex = new Texture2D(KinectWrapper.GetColorWidth(), KinectWrapper.GetColorHeight()); colorImage = new Color32[KinectWrapper.GetColorWidth() * KinectWrapper.GetColorHeight()]; usersColorMap = new byte[colorImage.Length << 2]; } // Initialize user list to contain ALL users. allUsers = new List <uint>(); KinectInitialized = true; }
// Update the filter for one joint. protected void FilterJoint(ref KinectWrapper.NuiSkeletonData skeleton, int jointIndex, ref KinectWrapper.NuiTransformSmoothParameters smoothingParameters) { float filteredState; float trend; float diffVal; float rawState = (float)skeleton.eSkeletonPositionTrackingState[jointIndex]; float prevFilteredState = history[jointIndex].FilteredState; float prevTrend = history[jointIndex].Trend; float prevRawState = history[jointIndex].RawState; // If joint is invalid, reset the filter if (rawState == 0f) { history[jointIndex].FrameCount = 0; } // Initial start values if (history[jointIndex].FrameCount == 0) { filteredState = rawState; trend = 0f; } else if (this.history[jointIndex].FrameCount == 1) { filteredState = (rawState + prevRawState) * 0.5f; diffVal = filteredState - prevFilteredState; trend = (diffVal * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection)); } else { // // First apply jitter filter // diffVal = rawState - prevFilteredState; // // if (diffVal <= smoothingParameters.fJitterRadius) // { // filteredState = (rawState * (diffVal / smoothingParameters.fJitterRadius)) + (prevFilteredState * (1.0f - (diffVal / smoothingParameters.fJitterRadius))); // } // else // { // filteredState = rawState; // } filteredState = rawState; // Now the double exponential smoothing filter filteredState = (filteredState * (1.0f - smoothingParameters.fSmoothing)) + ((prevFilteredState + prevTrend) * smoothingParameters.fSmoothing); diffVal = filteredState - prevFilteredState; trend = (diffVal * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection)); } // Predict into the future to reduce latency float predictedState = filteredState + (trend * smoothingParameters.fPrediction); // Check that we are not too far away from raw data diffVal = predictedState - rawState; if (diffVal > smoothingParameters.fMaxDeviationRadius) { predictedState = (predictedState * (smoothingParameters.fMaxDeviationRadius / diffVal)) + (rawState * (1.0f - (smoothingParameters.fMaxDeviationRadius / diffVal))); } // Save the data from this frame history[jointIndex].RawState = rawState; history[jointIndex].FilteredState = filteredState; history[jointIndex].Trend = trend; history[jointIndex].FrameCount++; // Set the filtered data back into the joint skeleton.eSkeletonPositionTrackingState[jointIndex] = (KinectWrapper.NuiSkeletonPositionTrackingState)(predictedState + 0.5f); }
/// <summary> /// This function usually works up to a second =) /// </summary> void InitializeKinect() { if (KinectText != null) { KinectText.text = "Starting Kinect initialization"; } // Error code. int hr = 0; try { // Initialize kinect for skeleton tracking and video hr = KinectWrapper.NuiInitialize(KinectWrapper.NuiInitializeFlags.UsesColor | KinectWrapper.NuiInitializeFlags.UsesSkeleton); if (hr != 0) { if (KinectText != null) { KinectText.text = "Failed to Initialize Kinect"; } throw new Exception("NuiInitialize Failed"); } // Initialize video colorStreamHandle = IntPtr.Zero; hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.Color, KinectWrapper.Constants.ImageResolution, 0, 2, IntPtr.Zero, ref colorStreamHandle); if (hr != 0) { if (KinectText != null) { KinectText.text = "Failed to Initialize Color Stream"; } throw new Exception("Cannot open color stream"); } // Initialize skeleton hr = KinectWrapper.NuiSkeletonTrackingEnable(IntPtr.Zero, 8); // 0, 12,8 if (hr != 0) { if (KinectText != null) { KinectText.text = "Failed to Initialize Skeleton Stream"; } throw new Exception("Cannot initialize Skeleton Data"); } // init skeleton structures skeletonFrame = new KinectWrapper.NuiSkeletonFrame() { SkeletonData = new KinectWrapper.NuiSkeletonData[KinectWrapper.Constants.NuiSkeletonCount] }; // values used to pass to smoothing function smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.5f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.05f; smoothParameters.fMaxDeviationRadius = 0.04f; // set kinect elevation angle //KinectWrapper.NuiCameraElevationSetAngle(15); } catch (DllNotFoundException e) { if (KinectText != null) { KinectText.text = "Please check the Kinect SDK installation."; } string message = "Please check the Kinect SDK installation."; Debug.LogError(message); Debug.LogError(e.ToString()); return; } catch (Exception e) { string message = e.Message + " - " + KinectWrapper.GetNuiErrorString(hr); if (KinectText != null) { if ((uint)hr == (uint)KinectWrapper.NuiErrorCodes.DeviceNotConnected) { KinectText.text = "O-ops, where is my kinect";//"Упс, де мій Кінект ???"; } else if ((uint)hr == (uint)KinectWrapper.NuiErrorCodes.DeviceNotPowered) { KinectText.text = "O-ops, why my kinect is not powered";//"Упс, чому Кінект не у розетці ???"; } else if ((uint)hr == (uint)KinectWrapper.NuiErrorCodes.DeviceInUse) { KinectText.text = "O-ops, why someone else is using my kinect";//"Упс, хто використовує мій Кінект ???"; } else { KinectText.text = message; } } Debug.LogError(message); return; } // Initialize color map related stuff usersClrTex = new Texture2D(KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight()); if (SmallVideo) { usersClrRect = new Rect(10, 10, 160, 120); } else { usersClrRect = new Rect(10, 10, usersClrTex.width, usersClrTex.height); } colorImage = new Color32[KinectWrapper.GetDepthWidth() * KinectWrapper.GetDepthHeight()]; KinectInitialized = true; if (KinectText != null) { KinectText.text = "Kinect initialization successful"; } }
void Start() { //_adaptiveUi = GameObject.Find("Cube").GetComponent<AdaptiveUi>(); // Waterstrong int hr = 0; try { hr = KinectWrapper.NuiInitialize(KinectWrapper.NuiInitializeFlags.UsesDepthAndPlayerIndex | KinectWrapper.NuiInitializeFlags.UsesSkeleton | KinectWrapper.NuiInitializeFlags.UsesColor); if (hr != 0) { throw new Exception("NuiInitialize Failed."); } hr = KinectWrapper.NuiSkeletonTrackingEnable(IntPtr.Zero, 8); // 0, 12,8 if (hr != 0) { throw new Exception("Cannot initialize Skeleton Data."); } depthStreamHandle = IntPtr.Zero; hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.DepthAndPlayerIndex, KinectWrapper.Constants.ImageResolution, 0, 2, IntPtr.Zero, ref depthStreamHandle); // if (hr != 0) // { // throw new Exception("Cannot open depth stream."); // } colorStreamHandle = IntPtr.Zero; hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.Color, KinectWrapper.Constants.ImageResolution, 0, 2, IntPtr.Zero, ref colorStreamHandle); // if (hr != 0) // { // throw new Exception("Cannot open color stream."); // } // set kinect elevation angle KinectWrapper.NuiCameraSetAngle((long)KinectAngle); // init skeleton structures skeletonFrame = new KinectWrapper.NuiSkeletonFrame() { SkeletonData = new KinectWrapper.NuiSkeletonData[KinectWrapper.Constants.NuiSkeletonCount] }; // values used to pass to smoothing function smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); smoothParameters.fSmoothing = 0.7f; //0.5f; smoothParameters.fCorrection = 0.0f; //0.5f; smoothParameters.fPrediction = 0.0f; //0.5f; smoothParameters.fJitterRadius = 0.05f; smoothParameters.fMaxDeviationRadius = 0.05f; // 0.04f; // Smoothing = 0.3f, // Correction = 0.0f, // Prediction = 0.0f, // JitterRadius = 1.0f, // MaxDeviationRadius = 0.5f //Smoothing = 1.0f, //Correction = 0.1f, //Prediction = 0.1f, //JitterRadius = 0.05f, //MaxDeviationRadius = 0.05f // create arrays for joint positions and joint orientations int skeletonJointsCount = (int)KinectWrapper.NuiSkeletonPositionIndex.Count; player1JointsTracked = new bool[skeletonJointsCount]; player2JointsTracked = new bool[skeletonJointsCount]; player1JointsPos = new Vector3[skeletonJointsCount]; player2JointsPos = new Vector3[skeletonJointsCount]; player1JointsOri = new Matrix4x4[skeletonJointsCount]; player2JointsOri = new Matrix4x4[skeletonJointsCount]; //create the transform matrix that converts from kinect-space to world-space Quaternion quat = new Quaternion(); quat.eulerAngles = new Vector3(-KinectAngle, 0.0f, 0.0f); // transform matrix - kinect to world kinectToWorld.SetTRS(new Vector3(0.0f, SensorHeight, 0.0f), quat, Vector3.one); //quatToWorld = Quaternion.LookRotation(kinectToWorld.GetColumn(2), kinectToWorld.GetColumn(1)); instance = this; DontDestroyOnLoad(gameObject); Debug.Log("success kinect."); // Waterstrong Add _motion = MotionFactory.CreateHandleChain(); Debug.Log("motion chain success."); } catch (Exception e) { Debug.Log(e.Message + ", hr=" + hr.ToString()); return; } // Initialize depth & label map related stuff usersMapSize = KinectWrapper.GetDepthWidth() * KinectWrapper.GetDepthHeight(); usersLblTex = new Texture2D(KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight()); usersMapColors = new Color[usersMapSize]; /* * usersMapRect = new Rect(Screen.width - usersLblTex.width / 2, * Screen.height - usersLblTex.height / 2, * -usersLblTex.width / 2, * usersLblTex.height / 2); */ usersMapRect = new Rect(Screen.width - 10, Screen.height - 240 - 10, -320, 240); if (DisplayColorMap) { usersClrTex = new Texture2D(KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight()); //usersClrColors = new Color[usersMapSize]; usersClrRect = new Rect(Screen.width /**- usersLblTex.width / 2*/ /**- usersClrTex.width / 2*/, Screen.height - usersClrTex.height / 2, -usersClrTex.width / 2, usersClrTex.height / 2); usersMapRect.x -= usersLblTex.width / 2; colorImage = new Color32[usersMapSize]; } usersDepthMap = new short[usersMapSize]; usersHistogramMap = new float[5000]; // Initialize user list to contain ALL users. allUsers = new List <uint>(); // Pull the AvatarController from each of the players Avatars. Player1Controllers = new List <AvatarController>(); Player2Controllers = new List <AvatarController>(); // Add each of the avatars' controllers into a list for each player. foreach (GameObject avatar in Player1Avatars) { Player1Controllers.Add(avatar.GetComponent <AvatarController>()); } foreach (GameObject avatar in Player2Avatars) { Player2Controllers.Add(avatar.GetComponent <AvatarController>()); } // GUI Text. CalibrationText = GameObject.Find("CalibrationText"); if (CalibrationText != null) { CalibrationText.guiText.text = "WAITING FOR USERS"; } Debug.Log("Waiting for users."); KinectInitialized = true; }
//----------------------------------- end of public functions --------------------------------------// void Start() { //CalibrationText = GameObject.Find("CalibrationText"); int hr = 0; try { hr = KinectWrapper.NuiInitialize(KinectWrapper.NuiInitializeFlags.UsesSkeleton | KinectWrapper.NuiInitializeFlags.UsesDepthAndPlayerIndex | (ComputeColorMap ? KinectWrapper.NuiInitializeFlags.UsesColor : 0)); if (hr != 0) { throw new Exception("NuiInitialize Failed"); } hr = KinectWrapper.NuiSkeletonTrackingEnable(IntPtr.Zero, 8); // 0, 12,8 if (hr != 0) { throw new Exception("Cannot initialize Skeleton Data"); } depthStreamHandle = IntPtr.Zero; if(ComputeUserMap) { hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.DepthAndPlayerIndex, KinectWrapper.Constants.ImageResolution, 0, 2, IntPtr.Zero, ref depthStreamHandle); if (hr != 0) { throw new Exception("Cannot open depth stream"); } } colorStreamHandle = IntPtr.Zero; if(ComputeColorMap) { hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.Color, KinectWrapper.Constants.ImageResolution, 0, 2, IntPtr.Zero, ref colorStreamHandle); if (hr != 0) { throw new Exception("Cannot open color stream"); } } // set kinect elevation angle KinectWrapper.NuiCameraElevationSetAngle(SensorAngle); // init skeleton structures skeletonFrame = new KinectWrapper.NuiSkeletonFrame() { SkeletonData = new KinectWrapper.NuiSkeletonData[KinectWrapper.Constants.NuiSkeletonCount] }; // values used to pass to smoothing function smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); switch(smoothing) { case Smoothing.Default: smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.5f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.05f; smoothParameters.fMaxDeviationRadius = 0.04f; break; case Smoothing.Medium: smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.1f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.1f; smoothParameters.fMaxDeviationRadius = 0.1f; break; case Smoothing.Aggressive: smoothParameters.fSmoothing = 0.7f; smoothParameters.fCorrection = 0.3f; smoothParameters.fPrediction = 1.0f; smoothParameters.fJitterRadius = 1.0f; smoothParameters.fMaxDeviationRadius = 1.0f; break; } // init the tracking state filter trackingStateFilter = new TrackingStateFilter[KinectWrapper.Constants.NuiSkeletonMaxTracked]; for(int i = 0; i < trackingStateFilter.Length; i++) { trackingStateFilter[i] = new TrackingStateFilter(); trackingStateFilter[i].Init(); } // init the bone orientation filter boneOrientationFilter = new BoneOrientationsFilter[KinectWrapper.Constants.NuiSkeletonMaxTracked]; for(int i = 0; i < boneOrientationFilter.Length; i++) { boneOrientationFilter[i] = new BoneOrientationsFilter()
void Awake() { WrapperTools.EnsureKinectWrapperPresence(); int hr = 0; try { hr = KinectWrapper.NuiInitialize(KinectWrapper.NuiInitializeFlags.UsesSkeleton | KinectWrapper.NuiInitializeFlags.UsesDepthAndPlayerIndex); if (hr != 0) { throw new Exception("NuiInitialize Failed"); } hr = KinectWrapper.NuiSkeletonTrackingEnable(IntPtr.Zero, 8); // 0, 12,8 if (hr != 0) { throw new Exception("Cannot initialize Skeleton Data"); } depthStreamHandle = IntPtr.Zero; // set kinect elevation angle KinectWrapper.NuiCameraElevationSetAngle(SensorAngle); // init skeleton structures skeletonFrame = new KinectWrapper.NuiSkeletonFrame() { SkeletonData = new KinectWrapper.NuiSkeletonData[KinectWrapper.Constants.NuiSkeletonCount] }; // values used to pass to smoothing function smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); switch (smoothing) { case Smoothing.Default: smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.5f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.05f; smoothParameters.fMaxDeviationRadius = 0.04f; break; case Smoothing.Medium: smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.1f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.1f; smoothParameters.fMaxDeviationRadius = 0.1f; break; case Smoothing.Aggressive: smoothParameters.fSmoothing = 0.7f; smoothParameters.fCorrection = 0.3f; smoothParameters.fPrediction = 1.0f; smoothParameters.fJitterRadius = 1.0f; smoothParameters.fMaxDeviationRadius = 1.0f; break; } // init the tracking state filter trackingStateFilter = new TrackingStateFilter[KinectWrapper.Constants.NuiSkeletonMaxTracked]; for (int i = 0; i < trackingStateFilter.Length; i++) { trackingStateFilter[i] = new TrackingStateFilter(); trackingStateFilter[i].Init(); } // init the bone orientation filter boneOrientationFilter = new BoneOrientationsFilter[KinectWrapper.Constants.NuiSkeletonMaxTracked]; for (int i = 0; i < boneOrientationFilter.Length; i++) { boneOrientationFilter[i] = new BoneOrientationsFilter(); boneOrientationFilter[i].Init(); } // init the clipped legs filter clippedLegsFilter = new ClippedLegsFilter[KinectWrapper.Constants.NuiSkeletonMaxTracked]; for (int i = 0; i < clippedLegsFilter.Length; i++) { clippedLegsFilter[i] = new ClippedLegsFilter(); } // init the bone orientation constraints boneConstraintsFilter = new BoneOrientationsConstraint(); boneConstraintsFilter.AddDefaultConstraints(); // init the self intersection constraints selfIntersectionConstraint = new SelfIntersectionConstraint(); // create arrays for joint positions and joint orientations int skeletonJointsCount = (int)KinectWrapper.NuiSkeletonPositionIndex.Count; player1JointsTracked = new bool[skeletonJointsCount]; player1PrevTracked = new bool[skeletonJointsCount]; player1JointsPos = new Vector3[skeletonJointsCount]; player1JointsOri = new Matrix4x4[skeletonJointsCount]; gestureTrackingAtTime = new float[KinectWrapper.Constants.NuiSkeletonMaxTracked]; //create the transform matrix that converts from kinect-space to world-space Quaternion quatTiltAngle = new Quaternion(); quatTiltAngle.eulerAngles = new Vector3(-SensorAngle, 0.0f, 0.0f); //float heightAboveHips = SensorHeight - 1.0f; // transform matrix - kinect to world kinectToWorld.SetTRS(new Vector3(0.0f, SensorHeight, 0.0f), quatTiltAngle, Vector3.one); flipMatrix = Matrix4x4.identity; flipMatrix[2, 2] = -1; } catch (DllNotFoundException e) { string message = "Please check the Kinect SDK installation."; Debug.LogError(message); Debug.LogError(e.ToString()); return; } catch (Exception e) { string message = e.Message + " - " + KinectWrapper.GetNuiErrorString(hr); Debug.LogError(message); Debug.LogError(e.ToString()); return; } // Initialize user list to contain ALL users. allUsers = new List <uint>(); KinectInitialized = true; }
//----------------------------------- end of public functions --------------------------------------// void Start() { CalibrationText = GameObject.Find("CalibrationText"); int hr = 0; try { hr = KinectWrapper.NuiInitialize(KinectWrapper.NuiInitializeFlags.UsesDepthAndPlayerIndex | KinectWrapper.NuiInitializeFlags.UsesSkeleton | (ComputeColorMap ? KinectWrapper.NuiInitializeFlags.UsesColor : 0)); if (hr != 0) { throw new Exception("NuiInitialize Failed"); } hr = KinectWrapper.NuiSkeletonTrackingEnable(IntPtr.Zero, 8); // 0, 12,8 if (hr != 0) { throw new Exception("Cannot initialize Skeleton Data"); } depthStreamHandle = IntPtr.Zero; if(ComputeUserMap) { hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.DepthAndPlayerIndex, KinectWrapper.Constants.ImageResolution, 0, 2, IntPtr.Zero, ref depthStreamHandle); if (hr != 0) { throw new Exception("Cannot open depth stream"); } } colorStreamHandle = IntPtr.Zero; if(ComputeColorMap) { hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.Color, KinectWrapper.Constants.ImageResolution, 0, 2, IntPtr.Zero, ref colorStreamHandle); if (hr != 0) { throw new Exception("Cannot open color stream"); } } // set kinect elevation angle KinectWrapper.NuiCameraElevationSetAngle(SensorAngle); // init skeleton structures skeletonFrame = new KinectWrapper.NuiSkeletonFrame() { SkeletonData = new KinectWrapper.NuiSkeletonData[KinectWrapper.Constants.NuiSkeletonCount] }; // values used to pass to smoothing function smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); switch(smoothing) { case Smoothing.Default: smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.5f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.05f; smoothParameters.fMaxDeviationRadius = 0.04f; break; case Smoothing.Medium: smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.1f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.1f; smoothParameters.fMaxDeviationRadius = 0.1f; break; case Smoothing.Aggressive: smoothParameters.fSmoothing = 0.7f; smoothParameters.fCorrection = 0.3f; smoothParameters.fPrediction = 1.0f; smoothParameters.fJitterRadius = 1.0f; smoothParameters.fMaxDeviationRadius = 1.0f; break; } // init the tracking state filter trackingStateFilter = new TrackingStateFilter[KinectWrapper.Constants.NuiSkeletonMaxTracked]; for(int i = 0; i < trackingStateFilter.Length; i++) { trackingStateFilter[i] = new TrackingStateFilter(); trackingStateFilter[i].Init(); } // init the bone orientation filter boneOrientationFilter = new BoneOrientationsFilter[KinectWrapper.Constants.NuiSkeletonMaxTracked]; for(int i = 0; i < boneOrientationFilter.Length; i++) { boneOrientationFilter[i] = new BoneOrientationsFilter(); boneOrientationFilter[i].Init(); } // init the clipped legs filter clippedLegsFilter = new ClippedLegsFilter[KinectWrapper.Constants.NuiSkeletonMaxTracked]; for(int i = 0; i < clippedLegsFilter.Length; i++) { clippedLegsFilter[i] = new ClippedLegsFilter(); } // init the bone orientation constraints boneConstraintsFilter = new BoneOrientationsConstraint(); boneConstraintsFilter.AddDefaultConstraints(); // init the self intersection constraints selfIntersectionConstraint = new SelfIntersectionConstraint(); // create arrays for joint positions and joint orientations int skeletonJointsCount = (int)KinectWrapper.NuiSkeletonPositionIndex.Count; player1JointsTracked = new bool[skeletonJointsCount]; player2JointsTracked = new bool[skeletonJointsCount]; player1PrevTracked = new bool[skeletonJointsCount]; player2PrevTracked = new bool[skeletonJointsCount]; player1JointsPos = new Vector3[skeletonJointsCount]; player2JointsPos = new Vector3[skeletonJointsCount]; player1JointsOri = new Matrix4x4[skeletonJointsCount]; player2JointsOri = new Matrix4x4[skeletonJointsCount]; //create the transform matrix that converts from kinect-space to world-space Quaternion quatTiltAngle = new Quaternion(); quatTiltAngle.eulerAngles = new Vector3(-SensorAngle, 0.0f, 0.0f); float heightAboveHips = SensorHeight - 1.0f; // transform matrix - kinect to world kinectToWorld.SetTRS(new Vector3(0.0f, heightAboveHips, 0.0f), quatTiltAngle, Vector3.one); flipMatrix = Matrix4x4.identity; flipMatrix[2, 2] = -1; instance = this; // roger> DontDestroyOnLoad(gameObject); // comment this, we want to reset and reopen kinect every time } catch (Exception e) { string message = e.Message + " - " + KinectWrapper.GetNuiErrorString(hr); Debug.LogError(message); if(CalibrationText != null) CalibrationText.guiText.text = message; return; } if(ComputeUserMap) { // Initialize depth & label map related stuff usersMapSize = KinectWrapper.GetDepthWidth() * KinectWrapper.GetDepthHeight(); usersLblTex = new Texture2D(KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight()); usersMapColors = new Color[usersMapSize]; // roger> user map size usersMapRect = new Rect(Screen.width, Screen.height - usersLblTex.height / 2, -usersLblTex.width / 2, usersLblTex.height / 2); //usersMapRect = new Rect(0, 0, Screen.width, Screen.height); usersDepthMap = new short[usersMapSize]; usersHistogramMap = new float[5000]; } if(ComputeColorMap) { // Initialize color map related stuff usersClrTex = new Texture2D(KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight()); //usersClrColors = new Color[usersMapSize]; usersClrRect = new Rect(Screen.width, Screen.height - usersClrTex.height / 2, -usersClrTex.width / 2, usersClrTex.height / 2); if(ComputeUserMap) usersMapRect.x -= usersClrTex.width / 2; colorImage = new Color32[KinectWrapper.GetDepthWidth() * KinectWrapper.GetDepthHeight()]; } // Initialize user list to contain ALL users. allUsers = new List<uint>(); // Pull the AvatarController from each of the players Avatars. Player1Controllers = new List<TrainingAvatarController>(); Player2Controllers = new List<TrainingAvatarController>(); // Add each of the avatars' controllers into a list for each player. foreach(GameObject avatar in Player1Avatars) { if(avatar.activeInHierarchy) { Player1Controllers.Add(avatar.GetComponent<TrainingAvatarController>()); } } foreach(GameObject avatar in Player2Avatars) { if(avatar.activeInHierarchy) { Player2Controllers.Add(avatar.GetComponent<TrainingAvatarController>()); } } // GUI Text. if(CalibrationText != null) { CalibrationText.guiText.text = "WAITING FOR USERS"; } Debug.Log("Waiting for users."); KinectInitialized = true; }
//----------------------------------- end of public functions --------------------------------------// void Start() { //CalibrationText = GameObject.Find("CalibrationText"); int hr = 0; try { hr = KinectWrapper.NuiInitialize(KinectWrapper.NuiInitializeFlags.UsesSkeleton | KinectWrapper.NuiInitializeFlags.UsesDepthAndPlayerIndex | (ComputeColorMap ? KinectWrapper.NuiInitializeFlags.UsesColor : 0)); if (hr != 0) { throw new Exception("NuiInitialize Failed"); } hr = KinectWrapper.NuiSkeletonTrackingEnable(IntPtr.Zero, 8); // 0, 12,8 if (hr != 0) { throw new Exception("Cannot initialize Skeleton Data"); } _depthStreamHandle = IntPtr.Zero; if (ComputeUserMap) { hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.DepthAndPlayerIndex, KinectWrapper.Constants.DepthImageResolution, 0, 2, IntPtr.Zero, ref _depthStreamHandle); if (hr != 0) { throw new Exception("Cannot open depth stream"); } } _colorStreamHandle = IntPtr.Zero; if (ComputeColorMap) { hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.Color, KinectWrapper.Constants.ColorImageResolution, 0, 2, IntPtr.Zero, ref _colorStreamHandle); if (hr != 0) { throw new Exception("Cannot open color stream"); } } // set kinect elevation angle KinectWrapper.NuiCameraElevationSetAngle(SensorAngle); // init skeleton structures _skeletonFrame = new KinectWrapper.NuiSkeletonFrame() { SkeletonData = new KinectWrapper.NuiSkeletonData[KinectWrapper.Constants.NuiSkeletonCount] }; // values used to pass to smoothing function _smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); switch (smoothing) { case Smoothing.Default: _smoothParameters.fSmoothing = 0.5f; _smoothParameters.fCorrection = 0.5f; _smoothParameters.fPrediction = 0.5f; _smoothParameters.fJitterRadius = 0.05f; _smoothParameters.fMaxDeviationRadius = 0.04f; break; case Smoothing.Medium: _smoothParameters.fSmoothing = 0.5f; _smoothParameters.fCorrection = 0.1f; _smoothParameters.fPrediction = 0.5f; _smoothParameters.fJitterRadius = 0.1f; _smoothParameters.fMaxDeviationRadius = 0.1f; break; case Smoothing.Aggressive: _smoothParameters.fSmoothing = 0.7f; _smoothParameters.fCorrection = 0.3f; _smoothParameters.fPrediction = 1.0f; _smoothParameters.fJitterRadius = 1.0f; _smoothParameters.fMaxDeviationRadius = 1.0f; break; } // create arrays for joint positions and joint orientations int skeletonJointsCount = (int)KinectWrapper.NuiSkeletonPositionIndex.Count; _player1JointsTracked = new bool[skeletonJointsCount]; _player2JointsTracked = new bool[skeletonJointsCount]; _player1PrevTracked = new bool[skeletonJointsCount]; _player2PrevTracked = new bool[skeletonJointsCount]; _player1JointsPos = new Vector3[skeletonJointsCount]; _player2JointsPos = new Vector3[skeletonJointsCount]; _player1JointsOri = new Matrix4x4[skeletonJointsCount]; _player2JointsOri = new Matrix4x4[skeletonJointsCount]; //create the transform matrix that converts from kinect-space to world-space Quaternion quatTiltAngle = new Quaternion(); quatTiltAngle.eulerAngles = new Vector3(-SensorAngle, 0.0f, 0.0f); // transform matrix - kinect to world _kinectToWorld.SetTRS(new Vector3(0.0f, SensorHeight, 0.0f), quatTiltAngle, Vector3.one); _flipMatrix = Matrix4x4.identity; _flipMatrix[2, 2] = -1; Instance = this; DontDestroyOnLoad(gameObject); } catch (DllNotFoundException e) { string message = "Please check the Kinect SDK installation."; Debug.LogError(message); Debug.LogError(e.ToString()); return; } catch (Exception e) { string message = e.Message + " - " + KinectWrapper.GetNuiErrorString(hr); Debug.LogError(message); Debug.LogError(e.ToString()); return; } // get the main camera rectangle Rect cameraRect = Camera.main.pixelRect; if (ComputeUserMap) { var displayMapsWidthPercent = DisplayMapsWidthPercent / 100f; var displayMapsHeightPercent = displayMapsWidthPercent * KinectWrapper.GetDepthHeight() / KinectWrapper.GetDepthWidth(); var displayWidth = cameraRect.width * displayMapsWidthPercent; var displayHeight = cameraRect.width * displayMapsHeightPercent; // Initialize depth & label map related stuff _usersMapSize = KinectWrapper.GetDepthWidth() * KinectWrapper.GetDepthHeight(); _usersLblTex = new Texture2D(KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight()); _usersMapColors = new Color32[_usersMapSize]; _usersPrevState = new ushort[_usersMapSize]; _usersMapRect = new Rect(cameraRect.width - displayWidth, cameraRect.height, displayWidth, -displayHeight); _usersDepthMap = new ushort[_usersMapSize]; _usersHistogramMap = new float[8192]; } if (ComputeColorMap) { var displayMapsWidthPercent = DisplayMapsWidthPercent / 100f; var displayMapsHeightPercent = displayMapsWidthPercent * KinectWrapper.GetColorHeight() / KinectWrapper.GetColorWidth(); var displayWidth = cameraRect.width * displayMapsWidthPercent; var displayHeight = cameraRect.width * displayMapsHeightPercent; // Initialize color map related stuff _usersClrTex = new Texture2D(KinectWrapper.GetColorWidth(), KinectWrapper.GetColorHeight()); _usersClrRect = new Rect(cameraRect.width - displayWidth, cameraRect.height, displayWidth, -displayHeight); _colorImage = new Color32[KinectWrapper.GetColorWidth() * KinectWrapper.GetColorHeight()]; _usersColorMap = new byte[_colorImage.Length << 2]; } // Initialize user list to contain ALL users. _allUsers = new List <uint>(); Debug.Log("Waiting for users."); _kinectInitialized = true; }
// Update the filter for one joint. protected void FilterJoint(ref KinectWrapper.NuiSkeletonData skeleton, int jointIndex, ref KinectWrapper.NuiTransformSmoothParameters smoothingParameters) { // if (null == skeleton) // { // return; // } //int jointIndex = (int)jt; Vector3 filteredPosition; Vector3 diffvec; Vector3 trend; Vector3 predictedPosition; // modify float diffVal; Vector3 rawPosition = (Vector3)skeleton.SkeletonPositions[jointIndex]; Vector3 prevFilteredPosition = this.history[jointIndex].FilteredPosition; Vector3 prevTrend = this.history[jointIndex].Trend; Vector3 prevRawPosition = this.history[jointIndex].RawPosition; Vector3 prevPredictedPosition = this.history[jointIndex].PredictedPosition; // modify bool jointIsValid = KinectHelper.JointPositionIsValid(rawPosition); // If joint is invalid, reset the filter if (!jointIsValid) { history[jointIndex].FrameCount = 0; } // modify if (filterType == 1) { // Initial start values // Single Exponential Smoothing Filter if (this.history[jointIndex].FrameCount == 0) { filteredPosition = rawPosition; } else if (this.history[jointIndex].FrameCount == 1 || this.history[jointIndex].FrameCount == 2) { filteredPosition = (rawPosition + prevRawPosition) * 0.5f; } else { // First apply jitter filter diffvec = rawPosition - prevFilteredPosition; diffVal = Math.Abs(diffvec.magnitude); if (diffVal <= smoothingParameters.fJitterRadius) { filteredPosition = (rawPosition * (diffVal / smoothingParameters.fJitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.fJitterRadius))); } else { filteredPosition = rawPosition; } // Single exponential smoothing filter filteredPosition = (filteredPosition * (1.0f - smoothingParameters.fSmoothing)) + (prevFilteredPosition * smoothingParameters.fSmoothing); } predictedPosition = filteredPosition; // Check if we are not too far away from raw data diffvec = predictedPosition - rawPosition; diffVal = Mathf.Abs(diffvec.magnitude); if (diffVal > smoothingParameters.fMaxDeviationRadius) { predictedPosition = (predictedPosition * (smoothingParameters.fMaxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.fMaxDeviationRadius / diffVal))); } trend = Vector3.zero; } else { // Initial start values // Double Exponential Smoothing Filter if (this.history[jointIndex].FrameCount == 0) { filteredPosition = rawPosition; trend = Vector3.zero; } else if (this.history[jointIndex].FrameCount == 1) { filteredPosition = (rawPosition + prevRawPosition) * 0.5f; diffvec = filteredPosition - prevFilteredPosition; trend = (diffvec * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection)); } else { // First apply jitter filter diffvec = rawPosition - prevFilteredPosition; diffVal = Math.Abs(diffvec.magnitude); if (diffVal <= smoothingParameters.fJitterRadius) { filteredPosition = (rawPosition * (diffVal / smoothingParameters.fJitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.fJitterRadius))); } else { filteredPosition = rawPosition; } // Double exponential smoothing filter filteredPosition = (filteredPosition * (1.0f - smoothingParameters.fSmoothing)) + ((prevFilteredPosition + prevTrend) * smoothingParameters.fSmoothing); diffvec = filteredPosition - prevFilteredPosition; trend = (diffvec * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection)); } // Predict into the future to reduce latency // Vector3 predictedPosition = filteredPosition + (trend * smoothingParameters.fPrediction); predictedPosition = filteredPosition + (trend * smoothingParameters.fPrediction); //modify // Check if we are not too far away from raw data diffvec = predictedPosition - rawPosition; diffVal = Mathf.Abs(diffvec.magnitude); if (diffVal > smoothingParameters.fMaxDeviationRadius) { predictedPosition = (predictedPosition * (smoothingParameters.fMaxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.fMaxDeviationRadius / diffVal))); } } // Save the data from this frame history[jointIndex].RawPosition = rawPosition; history[jointIndex].FilteredPosition = filteredPosition; history[jointIndex].Trend = trend; history[jointIndex].FrameCount++; // Set the filtered data back into the joint // Joint j = skeleton.Joints[jt]; // j.Position = KinectHelper.Vector3ToSkeletonPoint(predictedPosition); // skeleton.Joints[jt] = j; skeleton.SkeletonPositions[jointIndex] = (Vector4)predictedPosition; }
void Start() { int hr = 0; try { hr = KinectWrapper.NuiInitialize(KinectWrapper.NuiInitializeFlags.UsesDepthAndPlayerIndex | KinectWrapper.NuiInitializeFlags.UsesSkeleton | (DisplayColorMap ? KinectWrapper.NuiInitializeFlags.UsesColor : 0)); if (hr != 0) { throw new Exception("NuiInitialize Failed"); } hr = KinectWrapper.NuiSkeletonTrackingEnable(IntPtr.Zero, 8); // 0, 12,8 if (hr != 0) { throw new Exception("Cannot initialize Skeleton Data"); } depthStreamHandle = IntPtr.Zero; hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.DepthAndPlayerIndex, KinectWrapper.Constants.ImageResolution, 0, 2, IntPtr.Zero, ref depthStreamHandle); if (hr != 0) { throw new Exception("Cannot open depth stream"); } colorStreamHandle = IntPtr.Zero; if(DisplayColorMap) { hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.Color, KinectWrapper.Constants.ImageResolution, 0, 2, IntPtr.Zero, ref colorStreamHandle); if (hr != 0) { throw new Exception("Cannot open color stream"); } } // set kinect elevation angle KinectWrapper.NuiCameraSetAngle((long)KinectAngle); // init skeleton structures skeletonFrame = new KinectWrapper.NuiSkeletonFrame() { SkeletonData = new KinectWrapper.NuiSkeletonData[KinectWrapper.Constants.NuiSkeletonCount] }; // values used to pass to smoothing function smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.5f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.05f; smoothParameters.fMaxDeviationRadius = 0.04f; // create arrays for joint positions and joint orientations int skeletonJointsCount = (int)KinectWrapper.NuiSkeletonPositionIndex.Count; player1JointsTracked = new bool[skeletonJointsCount]; player2JointsTracked = new bool[skeletonJointsCount]; player1JointsPos = new Vector3[skeletonJointsCount]; player2JointsPos = new Vector3[skeletonJointsCount]; player1JointsOri = new Matrix4x4[skeletonJointsCount]; player2JointsOri = new Matrix4x4[skeletonJointsCount]; //create the transform matrix that converts from kinect-space to world-space Quaternion quat = new Quaternion(); quat.eulerAngles = new Vector3(-KinectAngle, 0.0f, 0.0f); // transform matrix - kinect to world kinectToWorld.SetTRS(new Vector3(0.0f, SensorHeight, 0.0f), quat, Vector3.one); instance = this; DontDestroyOnLoad(gameObject); } catch (Exception e) { Debug.LogError(e.Message + " - " + KinectWrapper.GetNuiErrorString(hr)); return; } // Initialize depth & label map related stuff usersMapSize = KinectWrapper.GetDepthWidth() * KinectWrapper.GetDepthHeight(); usersLblTex = new Texture2D(KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight()); usersMapColors = new Color[usersMapSize]; usersMapRect = new Rect(Screen.width, Screen.height - usersLblTex.height / 2, -usersLblTex.width / 2, usersLblTex.height / 2); if(DisplayColorMap) { usersClrTex = new Texture2D(KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight()); //usersClrColors = new Color[usersMapSize]; usersClrRect = new Rect(Screen.width, Screen.height - usersClrTex.height / 2, -usersClrTex.width / 2, usersClrTex.height / 2); usersMapRect.x -= usersClrTex.width / 2; colorImage = new Color32[usersMapSize]; } usersDepthMap = new short[usersMapSize]; usersHistogramMap = new float[5000]; // Initialize user list to contain ALL users. allUsers = new List<uint>(); // Pull the AvatarController from each of the players Avatars. Player1Controllers = new List<AvatarController>(); Player2Controllers = new List<AvatarController>(); // Add each of the avatars' controllers into a list for each player. foreach(GameObject avatar in Player1Avatars) { Player1Controllers.Add(avatar.GetComponent<AvatarController>()); } foreach(GameObject avatar in Player2Avatars) { Player2Controllers.Add(avatar.GetComponent<AvatarController>()); } // GUI Text. CalibrationText = GameObject.Find("CalibrationText"); if(CalibrationText != null) { CalibrationText.guiText.text = "WAITING FOR USERS"; } Debug.Log("Waiting for users."); KinectInitialized = true; }
// Update the filter with a new frame of data and smooth. public void UpdateFilter(ref KinectWrapper.NuiSkeletonData skeleton) { // if (null == skeleton) // { // return; // } if (skeleton.eTrackingState != KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked) { return; } if (this.init == false) { this.Init(); // initialize with default parameters } //Array jointTypeValues = Enum.GetValues(typeof(KinectWrapper.NuiSkeletonPositionIndex)); KinectWrapper.NuiTransformSmoothParameters tempSmoothingParams = new KinectWrapper.NuiTransformSmoothParameters(); // Check for divide by zero. Use an epsilon of a 10th of a millimeter this.smoothParameters.fJitterRadius = Math.Max(0.0001f, this.smoothParameters.fJitterRadius); tempSmoothingParams.fSmoothing = smoothParameters.fSmoothing; tempSmoothingParams.fCorrection = smoothParameters.fCorrection; tempSmoothingParams.fPrediction = smoothParameters.fPrediction; int jointsCount = (int)KinectWrapper.NuiSkeletonPositionIndex.Count; for(int jointIndex = 0; jointIndex < jointsCount; jointIndex++) { //KinectWrapper.NuiSkeletonPositionIndex jt = (KinectWrapper.NuiSkeletonPositionIndex)jointTypeValues.GetValue(jointIndex); // If not tracked, we smooth a bit more by using a bigger jitter radius // Always filter feet highly as they are so noisy if (skeleton.eSkeletonPositionTrackingState[jointIndex] != KinectWrapper.NuiSkeletonPositionTrackingState.Tracked) { tempSmoothingParams.fJitterRadius = smoothParameters.fJitterRadius * 2.0f; tempSmoothingParams.fMaxDeviationRadius = smoothParameters.fMaxDeviationRadius * 2.0f; } else { tempSmoothingParams.fJitterRadius = smoothParameters.fJitterRadius; tempSmoothingParams.fMaxDeviationRadius = smoothParameters.fMaxDeviationRadius; } FilterJoint(ref skeleton, jointIndex, ref tempSmoothingParams); } }
//----------------------------------- end of public functions --------------------------------------// void Awake() { if (instance == null) { instance = this; DontDestroyOnLoad(gameObject); } else { if (instance != this) { Destroy(gameObject); } } //CalibrationText = GameObject.Find("CalibrationText"); int hr = 0; try { hr = KinectWrapper.NuiInitialize(KinectWrapper.NuiInitializeFlags.UsesSkeleton | KinectWrapper.NuiInitializeFlags.UsesDepthAndPlayerIndex | (ComputeColorMap ? KinectWrapper.NuiInitializeFlags.UsesColor : 0)); if (hr != 0) { throw new Exception("NuiInitialize Failed"); } hr = KinectWrapper.NuiSkeletonTrackingEnable(IntPtr.Zero, 8); // 0, 12,8 if (hr != 0) { throw new Exception("Cannot initialize Skeleton Data"); } depthStreamHandle = IntPtr.Zero; if(ComputeUserMap) { hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.DepthAndPlayerIndex, KinectWrapper.Constants.DepthImageResolution, 0, 2, IntPtr.Zero, ref depthStreamHandle); if (hr != 0) { throw new Exception("Cannot open depth stream"); } } colorStreamHandle = IntPtr.Zero; if(ComputeColorMap) { hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.Color, KinectWrapper.Constants.ColorImageResolution, 0, 2, IntPtr.Zero, ref colorStreamHandle); if (hr != 0) { throw new Exception("Cannot open color stream"); } } // set kinect elevation angle KinectWrapper.NuiCameraElevationSetAngle(SensorAngle); // init skeleton structures skeletonFrame = new KinectWrapper.NuiSkeletonFrame() { SkeletonData = new KinectWrapper.NuiSkeletonData[KinectWrapper.Constants.NuiSkeletonCount] }; // values used to pass to smoothing function smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); switch(smoothing) { case Smoothing.Default: smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.5f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.05f; smoothParameters.fMaxDeviationRadius = 0.04f; break; case Smoothing.Medium: smoothParameters.fSmoothing = 0.5f; smoothParameters.fCorrection = 0.1f; smoothParameters.fPrediction = 0.5f; smoothParameters.fJitterRadius = 0.1f; smoothParameters.fMaxDeviationRadius = 0.1f; break; case Smoothing.Aggressive: smoothParameters.fSmoothing = 0.7f; smoothParameters.fCorrection = 0.3f; smoothParameters.fPrediction = 1.0f; smoothParameters.fJitterRadius = 1.0f; smoothParameters.fMaxDeviationRadius = 1.0f; break; } // init the tracking state filter trackingStateFilter = new TrackingStateFilter[KinectWrapper.Constants.NuiSkeletonMaxTracked]; for(int i = 0; i < trackingStateFilter.Length; i++) { trackingStateFilter[i] = new TrackingStateFilter(); trackingStateFilter[i].Init(); } // init the bone orientation filter boneOrientationFilter = new BoneOrientationsFilter[KinectWrapper.Constants.NuiSkeletonMaxTracked]; for(int i = 0; i < boneOrientationFilter.Length; i++) { boneOrientationFilter[i] = new BoneOrientationsFilter(); boneOrientationFilter[i].Init(); } // init the clipped legs filter clippedLegsFilter = new ClippedLegsFilter[KinectWrapper.Constants.NuiSkeletonMaxTracked]; for(int i = 0; i < clippedLegsFilter.Length; i++) { clippedLegsFilter[i] = new ClippedLegsFilter(); } // init the bone orientation constraints boneConstraintsFilter = new BoneOrientationsConstraint(); boneConstraintsFilter.AddDefaultConstraints(); // init the self intersection constraints selfIntersectionConstraint = new SelfIntersectionConstraint(); // create arrays for joint positions and joint orientations int skeletonJointsCount = (int)KinectWrapper.NuiSkeletonPositionIndex.Count; player1JointsTracked = new bool[skeletonJointsCount]; player2JointsTracked = new bool[skeletonJointsCount]; player1PrevTracked = new bool[skeletonJointsCount]; player2PrevTracked = new bool[skeletonJointsCount]; player1JointsPos = new Vector3[skeletonJointsCount]; player2JointsPos = new Vector3[skeletonJointsCount]; player1JointsOri = new Matrix4x4[skeletonJointsCount]; player2JointsOri = new Matrix4x4[skeletonJointsCount]; gestureTrackingAtTime = new float[KinectWrapper.Constants.NuiSkeletonMaxTracked]; //create the transform matrix that converts from kinect-space to world-space Quaternion quatTiltAngle = new Quaternion(); quatTiltAngle.eulerAngles = new Vector3(-SensorAngle, 0.0f, 0.0f); //float heightAboveHips = SensorHeight - 1.0f; // transform matrix - kinect to world //kinectToWorld.SetTRS(new Vector3(0.0f, heightAboveHips, 0.0f), quatTiltAngle, Vector3.one); kinectToWorld.SetTRS(new Vector3(0.0f, SensorHeight, 0.0f), quatTiltAngle, Vector3.one); flipMatrix = Matrix4x4.identity; flipMatrix[2, 2] = -1; instance = this; DontDestroyOnLoad(gameObject); } catch(DllNotFoundException e) { string message = "Please check the Kinect SDK installation."; Debug.LogError(message); Debug.LogError(e.ToString()); if(CalibrationText != null) CalibrationText.GetComponent<GUIText>().text = message; return; } catch (Exception e) { string message = e.Message + " - " + KinectWrapper.GetNuiErrorString(hr); Debug.LogError(message); Debug.LogError(e.ToString()); if(CalibrationText != null) CalibrationText.GetComponent<GUIText>().text = message; return; } if(ComputeUserMap) { // Initialize depth & label map related stuff usersMapSize = KinectWrapper.GetDepthWidth() * KinectWrapper.GetDepthHeight(); usersLblTex = new Texture2D(KinectWrapper.GetDepthWidth(), KinectWrapper.GetDepthHeight()); usersMapColors = new Color32[usersMapSize]; usersPrevState = new ushort[usersMapSize]; usersDepthMap = new ushort[usersMapSize]; usersHistogramMap = new float[8192]; } if(ComputeColorMap) { // Initialize color map related stuff usersClrTex = new Texture2D(KinectWrapper.GetColorWidth(), KinectWrapper.GetColorHeight()); colorImage = new Color32[KinectWrapper.GetColorWidth() * KinectWrapper.GetColorHeight()]; usersColorMap = new byte[colorImage.Length << 2]; } // try to automatically find the available avatar controllers in the scene if(Player1Avatars.Count == 0 && Player2Avatars.Count == 0) { AvatarController[] avatars = FindObjectsOfType(typeof(AvatarController)) as AvatarController[]; foreach(AvatarController avatar in avatars) { Player1Avatars.Add(avatar.gameObject); } } // Initialize user list to contain ALL users. allUsers = new List<uint>(); // Pull the AvatarController from each of the players Avatars. Player1Controllers = new List<AvatarController>(); Player2Controllers = new List<AvatarController>(); // Add each of the avatars' controllers into a list for each player. foreach(GameObject avatar in Player1Avatars) { if(avatar != null && avatar.activeInHierarchy) { Player1Controllers.Add(avatar.GetComponent<AvatarController>()); } } foreach(GameObject avatar in Player2Avatars) { if(avatar != null && avatar.activeInHierarchy) { Player2Controllers.Add(avatar.GetComponent<AvatarController>()); } } // create the list of gesture listeners gestureListeners = new List<KinectGestures.GestureListenerInterface>(); foreach(MonoBehaviour script in GestureListeners) { if(script && (script is KinectGestures.GestureListenerInterface)) { KinectGestures.GestureListenerInterface listener = (KinectGestures.GestureListenerInterface)script; gestureListeners.Add(listener); } } // GUI Text. if(CalibrationText != null) { CalibrationText.GetComponent<GUIText>().text = "WAITING FOR USERS"; } Debug.Log("Waiting for users."); KinectInitialized = true; }
void Awake() { //CalibrationText = GameObject.Find("CalibrationText"); int hr = 0; try { hr = KinectWrapper.NuiInitialize(KinectWrapper.NuiInitializeFlags.UsesDepth); if (hr != 0) { throw new Exception("NuiInitialize Failed"); } depthStreamHandle = IntPtr.Zero; hr = KinectWrapper.NuiImageStreamOpen(KinectWrapper.NuiImageType.Depth, KinectWrapper.Constants.DepthImageResolution, 0, 2, IntPtr.Zero, ref depthStreamHandle); if (hr != 0) { throw new Exception("Cannot open depth stream"); } // set kinect elevation angle KinectWrapper.NuiCameraElevationSetAngle(kangle); // values used to pass to smoothing function KinectWrapper.NuiTransformSmoothParameters smoothParameters = new KinectWrapper.NuiTransformSmoothParameters(); //switch (smoothing) //{ // case Smoothing.Default: //smoothParameters.fSmoothing = 0.5f; //smoothParameters.fCorrection = 0.5f; //smoothParameters.fPrediction = 0.5f; //smoothParameters.fJitterRadius = 0.05f; //smoothParameters.fMaxDeviationRadius = 0.04f; //break; // case Smoothing.Medium: // smoothParameters.fSmoothing = 0.5f; // smoothParameters.fCorrection = 0.1f; // smoothParameters.fPrediction = 0.5f; // smoothParameters.fJitterRadius = 0.1f; // smoothParameters.fMaxDeviationRadius = 0.1f; // break; // case Smoothing.Aggressive: smoothParameters.fSmoothing = 0.7f; smoothParameters.fCorrection = 0.3f; smoothParameters.fPrediction = 1.0f; smoothParameters.fJitterRadius = 1.0f; smoothParameters.fMaxDeviationRadius = 1.0f; // break; //} //create the transform matrix that converts from kinect-space to world-space Quaternion quatTiltAngle = new Quaternion(); quatTiltAngle.eulerAngles = new Vector3(-0, 0.0f, 0.0f); //float heightAboveHips = SensorHeight - 1.0f; // transform matrix - kinect to world //kinectToWorld.SetTRS(new Vector3(0.0f, heightAboveHips, 0.0f), quatTiltAngle, Vector3.one); //kinectToWorld.SetTRS(new Vector3(0.0f, SensorHeight, 0.0f), quatTiltAngle, Vector3.one); kinectToWorld.SetTRS(new Vector3(0.0f, 0.0f, 0.0f), quatTiltAngle, Vector3.one); flipMatrix = Matrix4x4.identity; flipMatrix[2, 2] = -1; instance = this; //DontDestroyOnLoad(gameObject); } catch (DllNotFoundException e) { string message = "Please check the Kinect SDK installation."; Debug.LogError(message); Debug.LogError(e.ToString()); return; } catch (Exception e) { string message = e.Message + " - " + KinectWrapper.GetNuiErrorString(hr); Debug.LogError(message); Debug.LogError(e.ToString()); return; } // Initialize depth & label map related stuff usersMapSize = KinectWrapper.GetDepthWidth() * KinectWrapper.GetDepthHeight(); usersDepthMap = new ushort[usersMapSize]; KinectInitialized = true; }