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