// Initialize the filter with a set of TransformSmoothParameters.
public void Init(KinectInterop.SmoothParameters smoothParameters)
{
this.smoothParameters = smoothParameters;
this.Reset();
this.init = true;
}
// Initialize the filter with a set of TransformSmoothParameters.
public void Init(KinectInterop.SmoothParameters smoothParameters)
{
this.smoothParameters = smoothParameters;
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 KinectInterop.SmoothParameters();
this.smoothParameters.smoothing = smoothingValue; // How much soothing will occur. Will lag when too high
this.smoothParameters.correction = correctionValue; // How much to correct back from prediction. Can make things springy
this.smoothParameters.prediction = predictionValue; // Amount of prediction into the future to use. Can over shoot when too high
this.smoothParameters.jitterRadius = jitterRadiusValue; // Size of the radius where jitter is removed. Can do too much smoothing when too high
this.smoothParameters.maxDeviationRadius = maxDeviationRadiusValue; // Size of the max prediction radius Can snap back to noisy data when too high
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
this.smoothParameters.jitterRadius = Math.Max(0.0001f, this.smoothParameters.jitterRadius);
this.Reset();
this.init = true;
}
/// <summary>
/// 使用一组手动指定Transform的SmoothParameters初始化过滤器。
/// </summary>
/// <param name="smoothingValue">Smoothing = [0..1], 较低的值更接近于原始数据，并且噪声更大。</param>
/// <param name="correctionValue">Correction = [0..1], 更高的值正确更快，感觉更敏感。</param>
/// <param name="predictionValue">Prediction = [0..n], 将来我们想预测多少帧。</param>
/// <param name="jitterRadiusValue">JitterRadius = 定义抖动的以m为单位的偏差距离。</param>
/// <param name="maxDeviationRadiusValue">MaxDeviation = 过滤位置的最大距离（m）允许偏离原始数据。</param>
public void Init(float smoothingValue, float correctionValue, float predictionValue, float jitterRadiusValue, float maxDeviationRadiusValue)
{
this.smoothParameters = new KinectInterop.SmoothParameters();
this.smoothParameters.smoothing = smoothingValue; // 会发生多少事情 会滞后的时候太高
this.smoothParameters.correction = correctionValue; // 从预测中纠正多少。 可以使事情发生变化
this.smoothParameters.prediction = predictionValue; // 预计未来使用量。 当拍摄太高时可以拍摄
this.smoothParameters.jitterRadius = jitterRadiusValue; // 去除抖动的半径的大小。 太高时可以做太多的平滑
this.smoothParameters.maxDeviationRadius = maxDeviationRadiusValue; // 最大预测半径的大小如果太高，可以回到嘈杂的数据
// 检查除以零。 使用十分之一毫米的ε
this.smoothParameters.jitterRadius = Math.Max(0.0001f, this.smoothParameters.jitterRadius);
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 KinectInterop.SmoothParameters();
this.smoothParameters.smoothing = smoothingValue; // How much soothing will occur. Will lag when too high
this.smoothParameters.correction = correctionValue; // How much to correct back from prediction. Can make things springy
this.smoothParameters.prediction = predictionValue; // Amount of prediction into the future to use. Can over shoot when too high
this.smoothParameters.jitterRadius = jitterRadiusValue; // Size of the radius where jitter is removed. Can do too much smoothing when too high
this.smoothParameters.maxDeviationRadius = maxDeviationRadiusValue; // Size of the max prediction radius Can snap back to noisy data when too high
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
this.smoothParameters.jitterRadius = Math.Max(0.0001f, this.smoothParameters.jitterRadius);
this.Reset();
this.init = true;
}
// Update the filter with a new frame of data and smooth.
public void UpdateFilter(ref KinectInterop.BodyFrameData bodyFrame)
{
if (this.init == false)
{
this.Init(); // initialize with default parameters
}
KinectInterop.SmoothParameters tempSmoothingParams = new KinectInterop.SmoothParameters();
tempSmoothingParams.smoothing = this.smoothParameters.smoothing;
tempSmoothingParams.correction = this.smoothParameters.correction;
tempSmoothingParams.prediction = this.smoothParameters.prediction;
int bodyCount = KinectInterop.Constants.BodyCount;
for (int bodyIndex = 0; bodyIndex < bodyCount; bodyIndex++)
{
if (bodyFrame.bodyData[bodyIndex].bIsTracked != 0)
{
FilterBodyJoints(ref bodyFrame.bodyData[bodyIndex], bodyIndex, ref tempSmoothingParams);
}
}
}
// Update the filter with a new frame of data and smooth.
/// <summary>
/// 使用新的数据帧更新滤镜并平滑。
/// </summary>
/// <param name="bodyFrame">身体帧</param>
public void UpdateFilter(ref KinectInterop.BodyFrameData bodyFrame)
{
if (this.init == false)
{
this.Init(); // 使用默认参数初始化
}
KinectInterop.SmoothParameters tempSmoothingParams = new KinectInterop.SmoothParameters();
tempSmoothingParams.smoothing = this.smoothParameters.smoothing;
tempSmoothingParams.correction = this.smoothParameters.correction;
tempSmoothingParams.prediction = this.smoothParameters.prediction;
KinectManager manager = KinectManager.Instance;
int bodyCount = manager.GetSensorBodyCount();
for (int bodyIndex = 0; bodyIndex < bodyCount; bodyIndex++)
{
if (bodyFrame.bodyData[bodyIndex].bIsTracked != 0)
{
FilterBodyJoints(ref bodyFrame.bodyData[bodyIndex], bodyIndex, ref tempSmoothingParams);
}
}
}
// Update the filter for one joint
private Vector3 FilterJoint(Vector3 rawPosition, int bodyIndex, int jointIndex, ref KinectInterop.SmoothParameters smoothingParameters)
{
Vector3 filteredPosition;
Vector3 diffVec;
Vector3 trend;
float diffVal;
Vector3 prevFilteredPosition = history[bodyIndex, jointIndex].filteredPosition;
Vector3 prevTrend = this.history[bodyIndex, jointIndex].trend;
Vector3 prevRawPosition = this.history[bodyIndex, jointIndex].rawPosition;
bool jointIsValid = (rawPosition != Vector3.zero);
// If joint is invalid, reset the filter
if (!jointIsValid)
{
history[bodyIndex, jointIndex].frameCount = 0;
}
// Initial start values
if (this.history[bodyIndex, jointIndex].frameCount == 0)
{
filteredPosition = rawPosition;
trend = Vector3.zero;
}
else if (this.history[bodyIndex, jointIndex].frameCount == 1)
{
filteredPosition = (rawPosition + prevRawPosition) * 0.5f;
diffVec = filteredPosition - prevFilteredPosition;
trend = (diffVec * smoothingParameters.correction) + (prevTrend * (1.0f - smoothingParameters.correction));
}
else
{
// First apply jitter filter
diffVec = rawPosition - prevFilteredPosition;
diffVal = Math.Abs(diffVec.magnitude);
if (diffVal <= smoothingParameters.jitterRadius)
{
filteredPosition = (rawPosition * (diffVal / smoothingParameters.jitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.jitterRadius)));
}
else
{
filteredPosition = rawPosition;
}
// Now the double exponential smoothing filter
filteredPosition = (filteredPosition * (1.0f - smoothingParameters.smoothing)) + ((prevFilteredPosition + prevTrend) * smoothingParameters.smoothing);
diffVec = filteredPosition - prevFilteredPosition;
trend = (diffVec * smoothingParameters.correction) + (prevTrend * (1.0f - smoothingParameters.correction));
}
// Predict into the future to reduce latency
Vector3 predictedPosition = filteredPosition + (trend * smoothingParameters.prediction);
// Check that we are not too far away from raw data
diffVec = predictedPosition - rawPosition;
diffVal = Mathf.Abs(diffVec.magnitude);
if (diffVal > smoothingParameters.maxDeviationRadius)
{
predictedPosition = (predictedPosition * (smoothingParameters.maxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.maxDeviationRadius / diffVal)));
}
// Save the data from this frame
history[bodyIndex, jointIndex].rawPosition = rawPosition;
history[bodyIndex, jointIndex].filteredPosition = filteredPosition;
history[bodyIndex, jointIndex].trend = trend;
history[bodyIndex, jointIndex].frameCount++;
return(predictedPosition);
}
// Update the filter for all body joints
private void FilterBodyJoints(ref KinectInterop.BodyData bodyData, int bodyIndex, ref KinectInterop.SmoothParameters tempSmoothingParams)
{
KinectManager manager = KinectManager.Instance;
int jointsCount = manager.GetJointCount();
for (int jointIndex = 0; jointIndex < jointsCount; 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 (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.Tracked ||
jointIndex == (int)KinectInterop.JointType.FootLeft || jointIndex == (int)KinectInterop.JointType.FootRight ||
jointIndex == (int)KinectInterop.JointType.HandTipLeft || jointIndex == (int)KinectInterop.JointType.HandTipRight ||
jointIndex == (int)KinectInterop.JointType.ThumbLeft || jointIndex == (int)KinectInterop.JointType.ThumbRight ||
jointIndex == (int)KinectInterop.JointType.Head)
{
tempSmoothingParams.jitterRadius = this.smoothParameters.jitterRadius * 2.0f;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius;
}
bodyData.joint[jointIndex].posVel = FilterJoint(bodyData.joint[jointIndex].posVel, bodyIndex, jointIndex, ref tempSmoothingParams);
}
}
void StartKinect()
{
try
{
// try to initialize the default Kinect2 sensor
KinectInterop.FrameSource dwFlags = KinectInterop.FrameSource.TypeBody;
if(computeUserMap != UserMapType.None)
dwFlags |= KinectInterop.FrameSource.TypeDepth | KinectInterop.FrameSource.TypeBodyIndex;
if(computeColorMap)
dwFlags |= KinectInterop.FrameSource.TypeColor;
if(computeInfraredMap)
dwFlags |= KinectInterop.FrameSource.TypeInfrared;
// if(useAudioSource)
// dwFlags |= KinectInterop.FrameSource.TypeAudio;
// open the default sensor
BackgroundRemovalManager brManager = gameObject.GetComponentInChildren<BackgroundRemovalManager>();
sensorData = KinectInterop.OpenDefaultSensor(sensorInterfaces, dwFlags, sensorAngle, useMultiSourceReader, computeUserMap, brManager);
if (sensorData == null)
{
if(sensorInterfaces == null || sensorInterfaces.Count == 0)
throw new Exception("No sensor found. Make sure you have installed the SDK and the sensor is connected.");
else
throw new Exception("OpenDefaultSensor failed.");
}
// enable or disable getting height and angle info
sensorData.hintHeightAngle = (autoHeightAngle != AutoHeightAngle.DontUse);
//create the transform matrix - kinect to world
Quaternion quatTiltAngle = Quaternion.Euler(-sensorAngle, 0.0f, 0.0f);
kinectToWorld.SetTRS(new Vector3(0.0f, sensorHeight, 0.0f), quatTiltAngle, Vector3.one);
}
catch(DllNotFoundException ex)
{
string message = ex.Message + " cannot be loaded. Please check the Kinect SDK installation.";
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.text = message;
}
return;
}
catch(Exception ex)
{
string message = ex.Message;
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.text = message;
}
return;
}
// set the singleton instance
instance = this;
// init skeleton structures
bodyFrame = new KinectInterop.BodyFrameData(sensorData.bodyCount, KinectInterop.Constants.MaxJointCount); // sensorData.jointCount
bodyFrame.bTurnAnalisys = allowTurnArounds;
KinectInterop.SmoothParameters smoothParameters = new KinectInterop.SmoothParameters();
switch(smoothing)
{
case Smoothing.Default:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.5f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.05f;
smoothParameters.maxDeviationRadius = 0.04f;
break;
case Smoothing.Medium:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.1f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.1f;
smoothParameters.maxDeviationRadius = 0.1f;
break;
case Smoothing.Aggressive:
smoothParameters.smoothing = 0.7f;
smoothParameters.correction = 0.3f;
smoothParameters.prediction = 1.0f;
smoothParameters.jitterRadius = 1.0f;
smoothParameters.maxDeviationRadius = 1.0f;
break;
}
// init data filters
jointPositionFilter = new JointPositionsFilter();
jointPositionFilter.Init(smoothParameters);
// init the bone orientation constraints
if(useBoneOrientationConstraints)
{
boneConstraintsFilter = new BoneOrientationsConstraint();
boneConstraintsFilter.AddDefaultConstraints();
boneConstraintsFilter.SetDebugText(calibrationText);
}
if(computeUserMap != UserMapType.None && computeUserMap != UserMapType.RawUserDepth)
{
// Initialize depth & label map related stuff
usersLblTex = new Texture2D(sensorData.depthImageWidth, sensorData.depthImageHeight, TextureFormat.ARGB32, false);
usersMapSize = sensorData.depthImageWidth * sensorData.depthImageHeight;
usersHistogramImage = new Color32[usersMapSize];
usersPrevState = new ushort[usersMapSize];
usersHistogramMap = new float[5001];
}
if(computeColorMap)
{
// Initialize color map related stuff
//usersClrTex = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGBA32, false);
usersClrSize = sensorData.colorImageWidth * sensorData.colorImageHeight;
}
// try to automatically use the available avatar controllers in the scene
if(avatarControllers.Count == 0)
{
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[];
foreach(MonoBehaviour monoScript in monoScripts)
{
if(typeof(AvatarController).IsAssignableFrom(monoScript.GetType()) && monoScript.enabled)
{
AvatarController avatar = (AvatarController)monoScript;
avatarControllers.Add(avatar);
}
}
}
// set up the gesture manager, if not already set
if(gestureManager == null)
{
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[];
foreach(MonoBehaviour monoScript in monoScripts)
{
if(typeof(KinectGestures).IsAssignableFrom(monoScript.GetType()) && monoScript.enabled)
{
gestureManager = (KinectGestures)monoScript;
break;
}
}
}
// try to automatically use the available gesture listeners in the scene
if(gestureListeners.Count == 0)
{
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[];
foreach(MonoBehaviour monoScript in monoScripts)
{
if(typeof(KinectGestures.GestureListenerInterface).IsAssignableFrom(monoScript.GetType()) &&
monoScript.enabled)
{
//KinectGestures.GestureListenerInterface gl = (KinectGestures.GestureListenerInterface)monoScript;
gestureListeners.Add(monoScript);
}
}
}
// Initialize user list to contain all users.
//alUserIds = new List<Int64>();
//dictUserIdToIndex = new Dictionary<Int64, int>();
// // start the background reader
// kinectReaderThread = new System.Threading.Thread(UpdateKinectStreamsThread);
// kinectReaderThread.Name = "KinectReaderThread";
// kinectReaderThread.IsBackground = true;
// kinectReaderThread.Start();
// kinectReaderRunning = true;
kinectInitialized = true;
#if USE_SINGLE_KM_IN_MULTIPLE_SCENES
DontDestroyOnLoad(gameObject);
#endif
// GUI Text.
if(calibrationText != null)
{
calibrationText.text = "WAITING FOR USERS";
}
Debug.Log("Waiting for users.");
}
// Update the filter for all body joints
private void FilterBodyJoints(ref KinectInterop.BodyData bodyData, int bodyIndex, ref KinectInterop.SmoothParameters tempSmoothingParams)
{
KinectManager manager = KinectManager.Instance;
int jointsCount = manager.GetJointCount();
for (int jointIndex = 0; jointIndex < jointsCount; 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 (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.Tracked)
{
tempSmoothingParams.jitterRadius = this.smoothParameters.jitterRadius * 2.0f;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius;
}
bodyData.joint[jointIndex].position = FilterJoint(bodyData.joint[jointIndex].position, bodyIndex, jointIndex, ref tempSmoothingParams);
}
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
if (jointIndex == 0)
{
bodyData.position = bodyData.joint[jointIndex].position;
bodyData.orientation = bodyData.joint[jointIndex].orientation;
bodyData.joint[jointIndex].direction = Vector3.zero;
}
else
{
int jParent = (int)manager.GetParentJoint((KinectInterop.JointType)jointIndex);
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[jParent].trackingState != KinectInterop.TrackingState.NotTracked)
{
bodyData.joint[jointIndex].direction =
bodyData.joint[jointIndex].position - bodyData.joint[jParent].position;
}
}
}
}
void Start()
{
// int hr = 0;
try
{
// if(startKinectServer)
// {
// // start the Kinect-server app
// hr = StartKinectServer();
// if (hr != 0)
// {
// throw new Exception("Kinect2Server not started");
// }
// }
// try to initialize the default Kinect2 sensor
KinectInterop.FrameSource dwFlags = KinectInterop.FrameSource.TypeBody;
if(computeUserMap)
dwFlags |= KinectInterop.FrameSource.TypeDepth | KinectInterop.FrameSource.TypeBodyIndex;
if(computeColorMap)
dwFlags |= KinectInterop.FrameSource.TypeColor;
if(computeInfraredMap)
dwFlags |= KinectInterop.FrameSource.TypeInfrared;
// hr = KinectInterop.InitDefaultKinectSensor(dwFlags, KinectInterop.Constants.ColorImageWidth, KinectInterop.Constants.ColorImageHeight);
// if (hr != 0)
// {
// throw new Exception("InitDefaultKinectSensor failed");
// }
// open the default kinect sensor
sensorData = KinectInterop.OpenDefaultKinectSensor(dwFlags);
if (sensorData == null)
{
throw new Exception("OpenDefaultKinectSensor failed");
}
// transform matrix - kinect to world
kinectToWorld.SetTRS(new Vector3(0.0f, sensorHeight, 0.0f), Quaternion.identity, Vector3.one);
}
catch(DllNotFoundException ex)
{
string message = ex.Message + " cannot be loaded. Please check the Kinect SDK installation.";
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.guiText.text = message;
}
return;
}
catch(Exception ex)
{
string message = ex.Message;
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.guiText.text = message;
}
return;
}
// init skeleton structures
bodyFrame = new KinectInterop.BodyFrameData(true);
KinectInterop.SmoothParameters smoothParameters = new KinectInterop.SmoothParameters();
switch(smoothing)
{
case Smoothing.Default:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.5f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.05f;
smoothParameters.maxDeviationRadius = 0.04f;
break;
case Smoothing.Medium:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.1f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.1f;
smoothParameters.maxDeviationRadius = 0.1f;
break;
case Smoothing.Aggressive:
smoothParameters.smoothing = 0.7f;
smoothParameters.correction = 0.3f;
smoothParameters.prediction = 1.0f;
smoothParameters.jitterRadius = 1.0f;
smoothParameters.maxDeviationRadius = 1.0f;
break;
}
// init data filters
jointPositionFilter = new JointPositionsFilter();
jointPositionFilter.Init(smoothParameters);
// init the bone orientation constraints
if(useBoneOrientationConstraints)
{
boneConstraintsFilter = new BoneOrientationsConstraint();
boneConstraintsFilter.AddDefaultConstraints();
boneConstraintsFilter.SetDebugText(calibrationText);
}
// get the main camera rectangle
Rect cameraRect = Camera.main.pixelRect;
// calculate map width and height in percent, if needed
if(MapsPercentWidth == 0f)
MapsPercentWidth = (sensorData.depthImageWidth / 2) / cameraRect.width;
if(MapsPercentHeight == 0f)
MapsPercentHeight = (sensorData.depthImageHeight / 2) / cameraRect.height;
if(computeUserMap)
{
// init user-depth structures
//depthImage = new KinectInterop.DepthBuffer(true);
//bodyIndexImage = new KinectInterop.BodyIndexBuffer(true);
// Initialize depth & label map related stuff
usersLblTex = new Texture2D(sensorData.depthImageWidth, sensorData.depthImageHeight);
usersMapRect = new Rect(cameraRect.width - cameraRect.width * MapsPercentWidth, cameraRect.height, cameraRect.width * MapsPercentWidth, -cameraRect.height * MapsPercentHeight);
usersMapSize = sensorData.depthImageWidth * sensorData.depthImageHeight;
usersHistogramImage = new Color32[usersMapSize];
usersPrevState = new ushort[usersMapSize];
usersHistogramMap = new float[5001];
}
if(computeColorMap)
{
// init color image structures
//colorImage = new KinectInterop.ColorBuffer(true);
// Initialize color map related stuff
usersClrTex = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGBA32, false);
usersClrRect = new Rect(cameraRect.width - 320, cameraRect.height, 320, -180);
usersCaliBox = new Rect(cameraRect.width - 160 - 320 / 14, cameraRect.height - 90, 320 / 7, 90);
usersClrSize = sensorData.colorImageWidth * sensorData.colorImageHeight;
if(computeUserMap && displayColorMap)
{
usersMapRect.x -= cameraRect.width * MapsPercentWidth; //usersClrTex.width / 2;
}
}
// try to automatically find the available avatar controllers at the scene
if(avatarControllers.Count == 0)
{
AvatarController[] avatars = FindObjectsOfType(typeof(AvatarController)) as AvatarController[];
foreach(AvatarController avatar in avatars)
{
avatarControllers.Add(avatar);
}
}
// Initialize user list to contain all users.
alUserIds = new List<Int64>();
dictUserIdToIndex = new Dictionary<Int64, int>();
kinectInitialized = true;
instance = this;
//DontDestroyOnLoad(gameObject);
// GUI Text.
if(calibrationText != null)
{
calibrationText.guiText.text = "WAITING FOR USERS";
}
Debug.Log("Waiting for users.");
}
// Update the filter with a new frame of data and smooth.
public void UpdateFilter(ref KinectInterop.BodyFrameData bodyFrame)
{
if (this.init == false)
{
this.Init(); // initialize with default parameters
}
KinectInterop.SmoothParameters tempSmoothingParams = new KinectInterop.SmoothParameters();
tempSmoothingParams.smoothing = this.smoothParameters.smoothing;
tempSmoothingParams.correction = this.smoothParameters.correction;
tempSmoothingParams.prediction = this.smoothParameters.prediction;
KinectManager manager = KinectManager.Instance;
int bodyCount = manager.GetBodyCount();
for(int bodyIndex = 0; bodyIndex < bodyCount; bodyIndex++)
{
if(bodyFrame.bodyData[bodyIndex].bIsTracked != 0)
{
FilterBodyJoints(ref bodyFrame.bodyData[bodyIndex], bodyIndex, ref tempSmoothingParams);
}
}
}
private Vector3 FilterJoint(Vector3 rawPosition, int bodyIndex, int jointIndex, ref KinectInterop.SmoothParameters smoothingParameters)
{
Vector3 filteredPosition;
Vector3 diffVec;
Vector3 trend;
float diffVal;
Vector3 prevFilteredPosition = history[bodyIndex, jointIndex].filteredPosition;
Vector3 prevTrend = this.history[bodyIndex, jointIndex].trend;
Vector3 prevRawPosition = this.history[bodyIndex, jointIndex].rawPosition;
bool jointIsValid = (rawPosition != Vector3.zero);
if (!jointIsValid)
{
history[bodyIndex, jointIndex].frameCount = 0;
}
if (this.history[bodyIndex, jointIndex].frameCount == 0)
{
filteredPosition = rawPosition;
trend = Vector3.zero;
}
else if (this.history[bodyIndex, jointIndex].frameCount == 1)
{
filteredPosition = (rawPosition + prevRawPosition) * 0.5f;
diffVec = filteredPosition - prevFilteredPosition;
trend = (diffVec * smoothingParameters.correction) + (prevTrend * (1.0f - smoothingParameters.correction));
}
else
{
diffVec = rawPosition - prevFilteredPosition;
diffVal = Math.Abs(diffVec.magnitude);
if (diffVal <= smoothingParameters.jitterRadius)
{
filteredPosition = (rawPosition * (diffVal / smoothingParameters.jitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.jitterRadius)));
}
else
{
filteredPosition = rawPosition;
}
filteredPosition = (filteredPosition * (1.0f - smoothingParameters.smoothing)) + ((prevFilteredPosition + prevTrend) * smoothingParameters.smoothing);
diffVec = filteredPosition - prevFilteredPosition;
trend = (diffVec * smoothingParameters.correction) + (prevTrend * (1.0f - smoothingParameters.correction));
}
Vector3 predictedPosition = filteredPosition + (trend * smoothingParameters.prediction);
diffVec = predictedPosition - rawPosition;
diffVal = Mathf.Abs(diffVec.magnitude);
if (diffVal > smoothingParameters.maxDeviationRadius)
{
predictedPosition = (predictedPosition * (smoothingParameters.maxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.maxDeviationRadius / diffVal)));
}
history[bodyIndex, jointIndex].rawPosition = rawPosition;
history[bodyIndex, jointIndex].filteredPosition = filteredPosition;
history[bodyIndex, jointIndex].trend = trend;
history[bodyIndex, jointIndex].frameCount++;
return(predictedPosition);
}
// Update the filter for one joint
//更新一个关节的过滤器
private Vector3 FilterJoint(Vector3 rawPosition, int bodyIndex, int jointIndex, ref KinectInterop.SmoothParameters smoothingParameters)
{
Vector3 filteredPosition;
Vector3 diffVec;
Vector3 trend;
float diffVal;
Vector3 prevFilteredPosition = history[bodyIndex, jointIndex].filteredPosition;
Vector3 prevTrend = this.history[bodyIndex, jointIndex].trend;
Vector3 prevRawPosition = this.history[bodyIndex, jointIndex].rawPosition;
bool jointIsValid = (rawPosition != Vector3.zero);
// 如果关节无效，请重置过滤器
if (!jointIsValid)
{
history[bodyIndex, jointIndex].frameCount = 0;
}
// 初始值
if (this.history[bodyIndex, jointIndex].frameCount == 0)
{
filteredPosition = rawPosition;
trend = Vector3.zero;
}
else if (this.history[bodyIndex, jointIndex].frameCount == 1)
{
filteredPosition = (rawPosition + prevRawPosition) * 0.5f;
diffVec = filteredPosition - prevFilteredPosition;
trend = (diffVec * smoothingParameters.correction) + (prevTrend * (1.0f - smoothingParameters.correction));
}
else
{
// 首先应用抖动筛选
diffVec = rawPosition - prevFilteredPosition;
diffVal = Math.Abs(diffVec.magnitude);
if (diffVal <= smoothingParameters.jitterRadius)
{
filteredPosition = (rawPosition * (diffVal / smoothingParameters.jitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.jitterRadius)));
}
else
{
filteredPosition = rawPosition;
}
// Now the double exponential smoothing filter
//现在是双指数平滑滤波器
filteredPosition = (filteredPosition * (1.0f - smoothingParameters.smoothing)) + ((prevFilteredPosition + prevTrend) * smoothingParameters.smoothing);
diffVec = filteredPosition - prevFilteredPosition;
trend = (diffVec * smoothingParameters.correction) + (prevTrend * (1.0f - smoothingParameters.correction));
}
// Predict into the future to reduce latency
//预测未来将减少延迟
Vector3 predictedPosition = filteredPosition + (trend * smoothingParameters.prediction);
// Check that we are not too far away from raw data
//检查我们离原始数据不太远
diffVec = predictedPosition - rawPosition;
diffVal = Mathf.Abs(diffVec.magnitude);
if (diffVal > smoothingParameters.maxDeviationRadius)
{
predictedPosition = (predictedPosition * (smoothingParameters.maxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.maxDeviationRadius / diffVal)));
}
// Save the data from this frame
//保存此帧中的数据
history[bodyIndex, jointIndex].rawPosition = rawPosition;
history[bodyIndex, jointIndex].filteredPosition = filteredPosition;
history[bodyIndex, jointIndex].trend = trend;
history[bodyIndex, jointIndex].frameCount++;
return(predictedPosition);
}
// Update the filter for all body joints
private void FilterBodyJoints(ref KinectInterop.BodyData bodyData, int bodyIndex, ref KinectInterop.SmoothParameters tempSmoothingParams)
{
int jointsCount = KinectInterop.Constants.JointCount;
for (int jointIndex = 0; jointIndex < jointsCount; 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 ((int)bodyData.joint[jointIndex].trackingState != 2)
{
tempSmoothingParams.jitterRadius = this.smoothParameters.jitterRadius * 2.0f;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius;
}
bodyData.joint[jointIndex].position = FilterJoint(bodyData.joint[jointIndex].position, bodyIndex, jointIndex, ref tempSmoothingParams);
}
}
void StartKinect()
{
try
{
// try to initialize the default Kinect2 sensor
KinectInterop.FrameSource dwFlags = KinectInterop.FrameSource.TypeBody;
if(computeUserMap)
dwFlags |= KinectInterop.FrameSource.TypeDepth | KinectInterop.FrameSource.TypeBodyIndex;
if(computeColorMap)
dwFlags |= KinectInterop.FrameSource.TypeColor;
if(computeInfraredMap)
dwFlags |= KinectInterop.FrameSource.TypeInfrared;
// if(useAudioSource)
// dwFlags |= KinectInterop.FrameSource.TypeAudio;
// open the default sensor
sensorData = KinectInterop.OpenDefaultSensor(sensorInterfaces, dwFlags, sensorAngle, useMultiSourceReader);
if (sensorData == null)
{
throw new Exception("OpenDefaultSensor failed");
}
//create the transform matrix - kinect to world
Quaternion quatTiltAngle = new Quaternion();
quatTiltAngle.eulerAngles = new Vector3(-sensorAngle, 0.0f, 0.0f);
kinectToWorld.SetTRS(new Vector3(0.0f, sensorHeight, 0.0f), quatTiltAngle, Vector3.one);
}
catch(DllNotFoundException ex)
{
string message = ex.Message + " cannot be loaded. Please check the Kinect SDK installation.";
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.guiText.text = message;
}
return;
}
catch(Exception ex)
{
string message = ex.Message;
Debug.LogError(message);
Debug.LogException(ex);
if(calibrationText != null)
{
calibrationText.guiText.text = message;
}
return;
}
// set the singleton instance
instance = this;
// init skeleton structures
bodyFrame = new KinectInterop.BodyFrameData(sensorData.bodyCount, KinectInterop.Constants.JointCount); // sensorData.jointCount
KinectInterop.SmoothParameters smoothParameters = new KinectInterop.SmoothParameters();
switch(smoothing)
{
case Smoothing.Default:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.5f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.05f;
smoothParameters.maxDeviationRadius = 0.04f;
break;
case Smoothing.Medium:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.1f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.1f;
smoothParameters.maxDeviationRadius = 0.1f;
break;
case Smoothing.Aggressive:
smoothParameters.smoothing = 0.7f;
smoothParameters.correction = 0.3f;
smoothParameters.prediction = 1.0f;
smoothParameters.jitterRadius = 1.0f;
smoothParameters.maxDeviationRadius = 1.0f;
break;
}
// init data filters
jointPositionFilter = new JointPositionsFilter();
jointPositionFilter.Init(smoothParameters);
// init the bone orientation constraints
if(useBoneOrientationConstraints)
{
boneConstraintsFilter = new BoneOrientationsConstraint();
boneConstraintsFilter.AddDefaultConstraints();
//boneConstraintsFilter.SetDebugText(calibrationText);
}
// get the main camera rectangle
Rect cameraRect = Camera.main.pixelRect;
// calculate map width and height in percent, if needed
if(DisplayMapsWidthPercent == 0f)
{
DisplayMapsWidthPercent = (sensorData.depthImageWidth / 2) * 100 / cameraRect.width;
}
if(computeUserMap)
{
float displayMapsWidthPercent = DisplayMapsWidthPercent / 100f;
float displayMapsHeightPercent = displayMapsWidthPercent * sensorData.depthImageHeight / sensorData.depthImageWidth;
float displayWidth = cameraRect.width * displayMapsWidthPercent;
float displayHeight = cameraRect.width * displayMapsHeightPercent;
// Initialize depth & label map related stuff
usersLblTex = new Texture2D(sensorData.depthImageWidth, sensorData.depthImageHeight);
usersMapRect = new Rect(cameraRect.width - displayWidth, cameraRect.height, displayWidth, -displayHeight);
usersMapSize = sensorData.depthImageWidth * sensorData.depthImageHeight;
usersHistogramImage = new Color32[usersMapSize];
usersPrevState = new ushort[usersMapSize];
usersHistogramMap = new float[5001];
}
if(computeColorMap)
{
float displayMapsWidthPercent = DisplayMapsWidthPercent / 100f;
float displayMapsHeightPercent = displayMapsWidthPercent * sensorData.colorImageHeight / sensorData.colorImageWidth;
float displayWidth = cameraRect.width * displayMapsWidthPercent;
float displayHeight = cameraRect.width * displayMapsHeightPercent;
// Initialize color map related stuff
usersClrTex = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGBA32, false);
usersClrRect = new Rect(cameraRect.width - displayWidth, cameraRect.height, displayWidth, -displayHeight);
usersClrSize = sensorData.colorImageWidth * sensorData.colorImageHeight;
// if(computeUserMap && displayColorMap)
// {
// usersMapRect.x -= cameraRect.width * displayMapsWidthPercent;
// }
}
// try to automatically use the available avatar controllers in the scene
if(avatarControllers.Count == 0)
{
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[];
foreach(MonoBehaviour monoScript in monoScripts)
{
if(typeof(AvatarController).IsAssignableFrom(monoScript.GetType()))
{
AvatarController avatar = (AvatarController)monoScript;
avatarControllers.Add(avatar);
}
}
}
// try to automatically use the available gesture listeners in the scene
if(gestureListeners.Count == 0)
{
MonoBehaviour[] monoScripts = FindObjectsOfType(typeof(MonoBehaviour)) as MonoBehaviour[];
foreach(MonoBehaviour monoScript in monoScripts)
{
if(typeof(KinectGestures.GestureListenerInterface).IsAssignableFrom(monoScript.GetType()))
{
print ("Found Listener");
//KinectGestures.GestureListenerInterface gl = (KinectGestures.GestureListenerInterface)monoScript;
gestureListeners.Add(monoScript);
}
}
}
// Initialize user list to contain all users.
alUserIds = new List<Int64>();
dictUserIdToIndex = new Dictionary<Int64, int>();
kinectInitialized = true;
DontDestroyOnLoad(gameObject);
// GUI Text.
if(calibrationText != null)
{
calibrationText.guiText.text = "WAITING FOR USERS";
}
Debug.Log("Waiting for users.");
}
private void FilterBodyJoints(ref KinectInterop.BodyData bodyData, int bodyIndex, ref KinectInterop.SmoothParameters tempSmoothingParams)
{
KinectManager manager = KinectManager.Instance;
int jointsCount = manager.GetJointCount();
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.Tracked ||
jointIndex == (int)KinectInterop.JointType.FootLeft || jointIndex == (int)KinectInterop.JointType.FootRight ||
jointIndex == (int)KinectInterop.JointType.HandTipLeft || jointIndex == (int)KinectInterop.JointType.HandTipRight ||
jointIndex == (int)KinectInterop.JointType.ThumbLeft || jointIndex == (int)KinectInterop.JointType.ThumbRight ||
jointIndex == (int)KinectInterop.JointType.Head)
{
tempSmoothingParams.jitterRadius = this.smoothParameters.jitterRadius * 2.0f;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius;
}
bodyData.joint[jointIndex].position = FilterJoint(bodyData.joint[jointIndex].position, bodyIndex, jointIndex, ref tempSmoothingParams);
}
for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++)
{
if (jointIndex == 0)
{
bodyData.position = bodyData.joint[jointIndex].position;
bodyData.orientation = bodyData.joint[jointIndex].orientation;
bodyData.joint[jointIndex].direction = Vector3.zero;
}
else
{
int jParent = (int)manager.GetParentJoint((KinectInterop.JointType)jointIndex);
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[jParent].trackingState != KinectInterop.TrackingState.NotTracked)
{
bodyData.joint[jointIndex].direction =
bodyData.joint[jointIndex].position - bodyData.joint[jParent].position;
}
}
}
}
