Commit 55fc6bc4 authored by Alexander Krause's avatar Alexander Krause

Refactoring 2

parent c2c3a593
......@@ -64,7 +64,7 @@
<Compile Include="Gestures\ContinuousGestureDetector.cs" />
<Compile Include="Gestures\DiscreteGestureDetector.cs" />
<Compile Include="Gestures\GestureDatabase.cs" />
<Compile Include="Gestures\KinectJointFilter.cs" />
<Compile Include="General\KinectJointFilter.cs" />
<Compile Include="MainWindow.xaml.cs">
<DependentUpon>MainWindow.xaml</DependentUpon>
</Compile>
......
//--------------------------------------------------------------------------------------
// KinectJointFilter.cs
//
// This file contains Holt Double Exponential Smoothing filter for filtering Joints
//
// Copyright (C) Microsoft Corporation. All rights reserved.
//--------------------------------------------------------------------------------------
using System.Collections;
using Microsoft.Kinect;
using System;
namespace ExplorVizGestureControl.General
{
public class KinectJointFilter
{
public struct TRANSFORM_SMOOTH_PARAMETERS
{
public float fSmoothing; // [0..1], lower values closer to raw data
public float fCorrection; // [0..1], lower values slower to correct towards the raw data
public float fPrediction; // [0..n], the number of frames to predict into the future
public float fJitterRadius; // The radius in meters for jitter reduction
public float fMaxDeviationRadius; // The maximum radius in meters that filtered positions are allowed to deviate from raw data
}
public class FilterDoubleExponentialData
{
public CameraSpacePoint m_vRawPosition;
public CameraSpacePoint m_vFilteredPosition;
public CameraSpacePoint m_vTrend;
public int m_dwFrameCount;
}
// Holt Double Exponential Smoothing filter
CameraSpacePoint[] m_pFilteredJoints;
FilterDoubleExponentialData[] m_pHistory;
float m_fSmoothing;
float m_fCorrection;
float m_fPrediction;
float m_fJitterRadius;
float m_fMaxDeviationRadius;
public KinectJointFilter(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
{
m_pFilteredJoints = new CameraSpacePoint[Body.JointCount];
m_pHistory = new FilterDoubleExponentialData[Body.JointCount];
for (int i = 0; i < Body.JointCount; i++)
{
m_pHistory[i] = new FilterDoubleExponentialData();
}
Init(fSmoothing, fCorrection, fPrediction, fJitterRadius, fMaxDeviationRadius);
}
~KinectJointFilter()
{
Shutdown();
}
public void Init(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
{
Reset(fSmoothing, fCorrection, fPrediction, fJitterRadius, fMaxDeviationRadius);
}
public void Shutdown()
{
}
public void Reset(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
{
if (m_pFilteredJoints == null || m_pHistory == null)
{
return;
}
m_fMaxDeviationRadius = fMaxDeviationRadius; // Size of the max prediction radius Can snap back to noisy data when too high
m_fSmoothing = fSmoothing; // How much smothing will occur. Will lag when too high
m_fCorrection = fCorrection; // How much to correct back from prediction. Can make things springy
m_fPrediction = fPrediction; // Amount of prediction into the future to use. Can over shoot when too high
m_fJitterRadius = fJitterRadius; // Size of the radius where jitter is removed. Can do too much smoothing when too high
for (int i = 0; i < Body.JointCount; i++)
{
m_pFilteredJoints[i].X = 0.0f;
m_pFilteredJoints[i].Y = 0.0f;
m_pFilteredJoints[i].Z = 0.0f;
m_pHistory[i].m_vFilteredPosition.X = 0.0f;
m_pHistory[i].m_vFilteredPosition.Y = 0.0f;
m_pHistory[i].m_vFilteredPosition.Z = 0.0f;
m_pHistory[i].m_vRawPosition.X = 0.0f;
m_pHistory[i].m_vRawPosition.Y = 0.0f;
m_pHistory[i].m_vRawPosition.Z = 0.0f;
m_pHistory[i].m_vTrend.X = 0.0f;
m_pHistory[i].m_vTrend.Y = 0.0f;
m_pHistory[i].m_vTrend.Z = 0.0f;
m_pHistory[i].m_dwFrameCount = 0;
}
}
//--------------------------------------------------------------------------------------
// Implementation of a Holt Double Exponential Smoothing filter. The double exponential
// smooths the curve and predicts. There is also noise jitter removal. And maximum
// prediction bounds. The paramaters are commented in the init function.
//--------------------------------------------------------------------------------------
public void UpdateFilter(Body pBody)
{
if (pBody == null)
{
return;
}
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
m_fJitterRadius = Math.Max(0.0001f, m_fJitterRadius);
TRANSFORM_SMOOTH_PARAMETERS SmoothingParams;
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
SmoothingParams.fSmoothing = m_fSmoothing;
SmoothingParams.fCorrection = m_fCorrection;
SmoothingParams.fPrediction = m_fPrediction;
SmoothingParams.fJitterRadius = m_fJitterRadius;
SmoothingParams.fMaxDeviationRadius = m_fMaxDeviationRadius;
// If inferred, we smooth a bit more by using a bigger jitter radius
Microsoft.Kinect.Joint joint = pBody.Joints[jt];
if (joint.TrackingState == TrackingState.Inferred)
{
SmoothingParams.fJitterRadius *= 2.0f;
SmoothingParams.fMaxDeviationRadius *= 2.0f;
}
UpdateJoint(pBody, jt, SmoothingParams);
}
}
//--------------------------------------------------------------------------------------
// if joint is 0 it is not valid.
//--------------------------------------------------------------------------------------
bool JointPositionIsValid(CameraSpacePoint vJointPosition)
{
return (vJointPosition.X != 0.0f ||
vJointPosition.Y != 0.0f ||
vJointPosition.Z != 0.0f);
}
public CameraSpacePoint[] GetFilteredJoints()
{
return m_pFilteredJoints;
}
CameraSpacePoint CSVectorSet(float x, float y, float z)
{
CameraSpacePoint point = new CameraSpacePoint();
point.X = x;
point.Y = y;
point.Z = z;
return point;
}
CameraSpacePoint CSVectorZero()
{
CameraSpacePoint point = new CameraSpacePoint();
point.X = 0.0f;
point.Y = 0.0f;
point.Z = 0.0f;
return point;
}
CameraSpacePoint CSVectorAdd(CameraSpacePoint p1, CameraSpacePoint p2)
{
CameraSpacePoint sum = new CameraSpacePoint();
sum.X = p1.X + p2.X;
sum.Y = p1.Y + p2.Y;
sum.Z = p1.Z + p2.Z;
return sum;
}
CameraSpacePoint CSVectorScale(CameraSpacePoint p, float scale)
{
CameraSpacePoint point = new CameraSpacePoint();
point.X = p.X * scale;
point.Y = p.Y * scale;
point.Z = p.Z * scale;
return point;
}
CameraSpacePoint CSVectorSubtract(CameraSpacePoint p1, CameraSpacePoint p2)
{
CameraSpacePoint diff = new CameraSpacePoint();
diff.X = p1.X - p2.X;
diff.Y = p1.Y - p2.Y;
diff.Z = p1.Z - p2.Z;
return diff;
}
float CSVectorLength(CameraSpacePoint p)
{
return Convert.ToSingle(Math.Sqrt(p.X * p.X + p.Y * p.Y + p.Z * p.Z));
}
void UpdateJoint(Body body, JointType jt, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
{
CameraSpacePoint vPrevRawPosition;
CameraSpacePoint vPrevFilteredPosition;
CameraSpacePoint vPrevTrend;
CameraSpacePoint vRawPosition;
CameraSpacePoint vFilteredPosition;
CameraSpacePoint vPredictedPosition;
CameraSpacePoint vDiff;
CameraSpacePoint vTrend;
float fDiff;
bool bJointIsValid;
Microsoft.Kinect.Joint joint = body.Joints[jt];
vRawPosition = joint.Position;
vPrevFilteredPosition = m_pHistory[(int)jt].m_vFilteredPosition;
vPrevTrend = m_pHistory[(int)jt].m_vTrend;
vPrevRawPosition = m_pHistory[(int)jt].m_vRawPosition;
bJointIsValid = JointPositionIsValid(vRawPosition);
// If joint is invalid, reset the filter
if (!bJointIsValid)
{
m_pHistory[(int)jt].m_dwFrameCount = 0;
}
// Initial start values
if (m_pHistory[(int)jt].m_dwFrameCount == 0)
{
vFilteredPosition = vRawPosition;
vTrend = CSVectorZero();
m_pHistory[(int)jt].m_dwFrameCount++;
}
else if (m_pHistory[(int)jt].m_dwFrameCount == 1)
{
vFilteredPosition = CSVectorScale(CSVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
m_pHistory[(int)jt].m_dwFrameCount++;
}
else
{
// First apply jitter filter
vDiff = CSVectorSubtract(vRawPosition, vPrevFilteredPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff <= smoothingParams.fJitterRadius)
{
vFilteredPosition = CSVectorAdd(CSVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius),
CSVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius));
}
else
{
vFilteredPosition = vRawPosition;
}
// Now the double exponential smoothing filter
vFilteredPosition = CSVectorAdd(CSVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing),
CSVectorScale(CSVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing));
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
vPredictedPosition = CSVectorAdd(vFilteredPosition, CSVectorScale(vTrend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
vDiff = CSVectorSubtract(vPredictedPosition, vRawPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
vPredictedPosition = CSVectorAdd(CSVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff),
CSVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
}
// Save the data from this frame
m_pHistory[(int)jt].m_vRawPosition = vRawPosition;
m_pHistory[(int)jt].m_vFilteredPosition = vFilteredPosition;
m_pHistory[(int)jt].m_vTrend = vTrend;
// Output the data
m_pFilteredJoints[(int)jt] = vPredictedPosition;
//Uncomment for raw data usage as provided by Kinect runtime
//m_pFilteredJoints[(int)jt] = vRawPosition;
}
}
}
\ No newline at end of file
......@@ -17,8 +17,7 @@ namespace ExplorVizGestureControl.Gestures
{
public class ContinuousGestureDetector
{
#region Variables
private CoordinateMapper coordinateMapper = null;
#region Variables
[DllImport("user32.dll", CharSet = CharSet.Auto, CallingConvention = CallingConvention.StdCall)]
private static extern void mouse_event(int dwFlags, int dx, int dy, int cButtons, int dwExtraInfo);
......@@ -26,21 +25,16 @@ namespace ExplorVizGestureControl.Gestures
private const int MOUSEEVENTF_LEFTUP = 0x04;
private const int MOUSEEVENTF_RIGHTDOWN = 0x08;
private const int MOUSEEVENTF_RIGHTUP = 0x10;
private const int MOUSEEVENTF_WHEEL = 0x0800;
private Vector3D handRight;
private Vector3D handLeft;
private GestureDatabase.PhIZ phiz;
private const int MOUSEEVENTF_WHEEL = 0x0800;
private GestureTimer scrollTimer;
private GestureTimer openObjectTimer;
public static GestureTimer delayTimer;
internal static GestureTimer delayTimer;
private Boolean rotationActive = false;
private double oldZ = 0;
private double oldY = 0;
private CameraSpacePoint oldHandRight;
private Boolean gripActive = false;
private Dictionary<string, double> dynamicValues = new Dictionary<string, double>();
......@@ -51,9 +45,8 @@ namespace ExplorVizGestureControl.Gestures
public ContinuousGestureDetector()
{
KinectSensor kinect = KinectSensor.GetDefault();
coordinateMapper = kinect.CoordinateMapper;
// dynamic values initialization
// dynamic values initialization, based on gui
dynamicValues.Add("scrollAccelerationFactor", 2000);
dynamicValues.Add("scrollForwardFactor", 0.05);
dynamicValues.Add("scrollBackwardFactor", 0.05);
......@@ -69,14 +62,7 @@ namespace ExplorVizGestureControl.Gestures
public void DetectContinousGestures()
{
if (delayTimer.busy) return;
handRight = GestureDatabase.handRight;
handLeft = GestureDatabase.handLeft;
phiz = GestureDatabase.phiz;
// Detect and handle continuous gestures
if (delayTimer.busy) return;
if (IntersectWithPhIZ() || gripActive) HandleGripAndSelection();
......@@ -87,13 +73,13 @@ namespace ExplorVizGestureControl.Gestures
private bool IntersectWithPhIZ()
{
double maxX = phiz.botRightCorner.X;
double minX = phiz.botLeftCorner.X;
double maxY = phiz.topRightCorner.Y;
double minY = phiz.botRightCorner.Y;
double maxX = GestureDatabase.Phiz.botRightCorner.X;
double minX = GestureDatabase.Phiz.botLeftCorner.X;
double maxY = GestureDatabase.Phiz.topRightCorner.Y;
double minY = GestureDatabase.Phiz.botRightCorner.Y;
return (handRight.X <= maxX && handRight.X >= minX) &&
(handRight.Y <= maxY && handRight.Y >= minY) ? true : false;
return (GestureDatabase.Joints[(int)JointType.HandRight].X <= maxX && GestureDatabase.Joints[(int)JointType.HandRight].X >= minX) &&
(GestureDatabase.Joints[(int)JointType.HandRight].Y <= maxY && GestureDatabase.Joints[(int)JointType.HandRight].Y >= minY) ? true : false;
}
public void updateDynamicValues(string key, double value)
......@@ -117,18 +103,18 @@ namespace ExplorVizGestureControl.Gestures
{
mouse_event(MOUSEEVENTF_RIGHTDOWN, 0, 0, 0, 0);
rotationActive = true;
oldZ = handLeft.Z - handRight.Z;
oldY = handLeft.Y - handRight.Y;
oldZ = GestureDatabase.Joints[(int)JointType.HandLeft].Z - GestureDatabase.Joints[(int)JointType.HandRight].Z;
oldY = GestureDatabase.Joints[(int)JointType.HandLeft].Y - GestureDatabase.Joints[(int)JointType.HandRight].Y;
}
double distanceZ = 0;
double distanceY = 0;
double distanceZHands = handLeft.Z - handRight.Z;
double distanceYHands = handLeft.Y - handRight.Y;
double distanceZHands = GestureDatabase.Joints[(int)JointType.HandLeft].Z - GestureDatabase.Joints[(int)JointType.HandRight].Z;
double distanceYHands = GestureDatabase.Joints[(int)JointType.HandLeft].Y - GestureDatabase.Joints[(int)JointType.HandRight].Y;
int yDetection = 0;
double handsAbsDistanceY = Math.Abs(Math.Abs(handLeft.Y) - Math.Abs(handRight.Y));
double handsAbsDistanceY = Math.Abs(Math.Abs(GestureDatabase.Joints[(int)JointType.HandLeft].Y) - Math.Abs(GestureDatabase.Joints[(int)JointType.HandRight].Y));
//Console.WriteLine("{0}", handsAbsDistanceY);
......@@ -161,7 +147,7 @@ namespace ExplorVizGestureControl.Gestures
if (gripActive) return;
int scrollValue = 0;
double distance = GestureDatabase.spineBase.Z - GestureDatabase.oldSpineBase.Z;
double distance = GestureDatabase.Joints[(int)JointType.SpineBase].Z - GestureDatabase.PreviousFrameJoints[(int)JointType.SpineBase].Z;
//if (distance > dynamicValues["scrollForwardFactor"] ||
// distance < dynamicValues["scrollBackwardFactor"])
......@@ -197,22 +183,15 @@ namespace ExplorVizGestureControl.Gestures
}
}
if (!gripActive)
{
oldHandRight = GestureDatabase.Joints[(int)JointType.HandRight];
gripActive = true;
}
if (!gripActive) gripActive = true;
double distanceX = GestureDatabase.Joints[(int)JointType.HandRight].X - oldHandRight.X;
double distanceY = GestureDatabase.Joints[(int)JointType.HandRight].Y - oldHandRight.Y;
double distanceX = GestureDatabase.Joints[(int)JointType.HandRight].X - GestureDatabase.PreviousFrameJoints[(int)JointType.HandRight].X;
double distanceY = GestureDatabase.Joints[(int)JointType.HandRight].Y - GestureDatabase.PreviousFrameJoints[(int)JointType.HandRight].Y;
int moveX = (int)(distanceX * 1000);
int moveY = (int)(distanceY * 1000);
Cursor.Position = new Point(Cursor.Position.X, Cursor.Position.Y) + new Size(moveX, -moveY);
oldHandRight.X = GestureDatabase.Joints[(int)JointType.HandRight].X;
oldHandRight.Y = GestureDatabase.Joints[(int)JointType.HandRight].Y;
Cursor.Position = new Point(Cursor.Position.X, Cursor.Position.Y) + new Size(moveX, -moveY);
}
else if (gripActive)
{
......
......@@ -26,12 +26,12 @@ namespace ExplorVizGestureControl.Gestures
#region Variables
private static IReadOnlyDictionary<string, Func<bool>> gestureDictionary;
public static PhIZ phiz;
private static PhIZ phiz;
private static CameraSpacePoint[] joints;
private static CameraSpacePoint[] previousFrameJoints;
public static HandState handRightState;
public static HandState handLeftState;
private static HandState handRightState;
private static HandState handLeftState;
private static int maxCapacity;
private static int newestOldHead = 0;
......@@ -261,6 +261,9 @@ namespace ExplorVizGestureControl.Gestures
public static CameraSpacePoint[] Joints { get { return joints; } }
public static CameraSpacePoint[] PreviousFrameJoints { get { return previousFrameJoints; } }
public static PhIZ Phiz { get { return phiz; } }
#endregion
......
//--------------------------------------------------------------------------------------
// KinectJointFilter.cs
//
// This file contains Holt Double Exponential Smoothing filter for filtering Joints
//
// Copyright (C) Microsoft Corporation. All rights reserved.
//--------------------------------------------------------------------------------------
using System.Collections;
using Microsoft.Kinect;
using System;
public class KinectJointFilter
{
public struct TRANSFORM_SMOOTH_PARAMETERS
{
public float fSmoothing; // [0..1], lower values closer to raw data
public float fCorrection; // [0..1], lower values slower to correct towards the raw data
public float fPrediction; // [0..n], the number of frames to predict into the future
public float fJitterRadius; // The radius in meters for jitter reduction
public float fMaxDeviationRadius; // The maximum radius in meters that filtered positions are allowed to deviate from raw data
}
public class FilterDoubleExponentialData
{
public CameraSpacePoint m_vRawPosition;
public CameraSpacePoint m_vFilteredPosition;
public CameraSpacePoint m_vTrend;
public int m_dwFrameCount;
}
// Holt Double Exponential Smoothing filter
CameraSpacePoint[] m_pFilteredJoints;
FilterDoubleExponentialData[] m_pHistory;
float m_fSmoothing;
float m_fCorrection;
float m_fPrediction;
float m_fJitterRadius;
float m_fMaxDeviationRadius;
public KinectJointFilter(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
{
m_pFilteredJoints = new CameraSpacePoint[Body.JointCount];
m_pHistory = new FilterDoubleExponentialData[Body.JointCount];
for (int i = 0; i < Body.JointCount; i++)
{
m_pHistory[i] = new FilterDoubleExponentialData();
}
Init(fSmoothing, fCorrection, fPrediction, fJitterRadius, fMaxDeviationRadius);
}
~KinectJointFilter()
{
Shutdown();
}
public void Init(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
{
Reset(fSmoothing, fCorrection, fPrediction, fJitterRadius, fMaxDeviationRadius);
}
public void Shutdown()
{
}
public void Reset(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
{
if (m_pFilteredJoints == null || m_pHistory == null)
{
return;
}
m_fMaxDeviationRadius = fMaxDeviationRadius; // Size of the max prediction radius Can snap back to noisy data when too high
m_fSmoothing = fSmoothing; // How much smothing will occur. Will lag when too high
m_fCorrection = fCorrection; // How much to correct back from prediction. Can make things springy
m_fPrediction = fPrediction; // Amount of prediction into the future to use. Can over shoot when too high
m_fJitterRadius = fJitterRadius; // Size of the radius where jitter is removed. Can do too much smoothing when too high
for (int i = 0; i < Body.JointCount; i++)
{
m_pFilteredJoints[i].X = 0.0f;
m_pFilteredJoints[i].Y = 0.0f;
m_pFilteredJoints[i].Z = 0.0f;
m_pHistory[i].m_vFilteredPosition.X = 0.0f;
m_pHistory[i].m_vFilteredPosition.Y = 0.0f;
m_pHistory[i].m_vFilteredPosition.Z = 0.0f;
m_pHistory[i].m_vRawPosition.X = 0.0f;
m_pHistory[i].m_vRawPosition.Y = 0.0f;
m_pHistory[i].m_vRawPosition.Z = 0.0f;
m_pHistory[i].m_vTrend.X = 0.0f;
m_pHistory[i].m_vTrend.Y = 0.0f;
m_pHistory[i].m_vTrend.Z = 0.0f;
m_pHistory[i].m_dwFrameCount = 0;
}
}
//--------------------------------------------------------------------------------------
// Implementation of a Holt Double Exponential Smoothing filter. The double exponential
// smooths the curve and predicts. There is also noise jitter removal. And maximum
// prediction bounds. The paramaters are commented in the init function.
//--------------------------------------------------------------------------------------
public void UpdateFilter(Body pBody)
{
if (pBody == null)
{
return;
}
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
m_fJitterRadius = Math.Max(0.0001f, m_fJitterRadius);