/**
* Match tempalte across entire image
**/
private static double[,] MapGradient(HandFrame handFrame, double[,] xTemplate, double[,] yTemplate)
{
int imageWidth = handFrame.width;
int imageHeight = handFrame.height;
int templateWidth = xTemplate.GetLength(1);
int templateHeight = xTemplate.GetLength(0);
double[,] outMatrix = new double[imageHeight - templateHeight, imageWidth - templateWidth];
// Iterate through each of the image points
for (int Y = 0; Y < imageHeight - templateHeight; Y++)
{
for (int X = 0; X < imageWidth - templateWidth; X++)
{
double sum = 0;
// Iterate through each point in the template
for (int y = 0; y < templateHeight; y++)
{
for (int x = 0; x < templateWidth; x++)
{
sum += Math.Sqrt(Math.Pow(handFrame.dy[Y + y, X + x] - yTemplate[y, x], 2) + Math.Pow(handFrame.dx[Y + y, X + x] - xTemplate[y, x], 2));
}
}
outMatrix[Y, X] = sum;
}
}
return(outMatrix);
}
private static bool ParticleGradient(HandFrame handFrame, HandPoint handObj)
{
int radius = DetectionParameters.radiusFromHandPixel;
int imageWidth = handFrame.width;
int imageHeight = handFrame.height;
int templateWidth = DetectionParameters.dxTemplate.GetLength(1);
int templateHeight = DetectionParameters.dxTemplate.GetLength(0);
short x = 0;
short y = 0;
short dx = 6;
short dy = (short)(Math.Sqrt(3) * dx / 2);
int distanceBetweenParticles = (int)(templateWidth * (0.4)); // Half the width of a "finger"
// TO DO: center the particles
int numParticles = (imageWidth / dx) * (imageHeight / dy);
particle[] particles = new particle[numParticles];
// Initialize particles
for (int i = 0; i < particles.Length; i++)
{
// Set initial x and y
//short x = (short)((i * distanceBetweenParticles) % imageWidth);
//short y = (short)(distanceBetweenParticles * ((i * distanceBetweenParticles) / imageWidth));
// If the particle is within the depth range, initialize it.
if (((handFrame.abs[y, x] > (handObj.Depth - 100)) && (handFrame.abs[y, x] < (handObj.Depth + 100))) && (Math.Sqrt((x - radius) * (x - radius) + (y - radius) * (y - radius)) < (radius - 1)))
{
particles[i] = new particle((short)(x), (short)(y));
}
x += dx;
if (x >= imageWidth)
{
x = 0;
y += dy;
if (y % 2 == 1)
{
x += (short)(dx / 2);
}
}
}
// run for cycles up to the template width.
int iterations = (int)(templateWidth / 2);
for (int t = 0; t < iterations; t++)
{
particle.runTrace(particles, DetectionParameters.dxTemplate, DetectionParameters.dyTemplate, handFrame);
}
DetectFingers.Detect(particles, handObj, handFrame);
return(true);
}
// Runs a convolution of a separable function, using a 1D Kernal that's been separated
public static void seperableConv(double[] kernal, HandFrame handFrame)
{
int imageWidth = handFrame.width;
int imageHeight = handFrame.height;
int kernLength = kernal.Length;
float[,] tempx = new float[imageHeight, imageWidth];
float[,] tempy = new float[imageHeight, imageWidth];
double sumx;
double sumy;
// Run through convolution in the x direction
for (int y = 0; y < imageHeight; y++)
{
for (int x = 0; x < imageWidth - kernLength; x++)
{
sumx = 0;
sumy = 0;
for (int n = 0; n < kernLength; n++)
{
sumx += handFrame.dx[y, x + n] * kernal[n];
sumy += handFrame.dy[y, x + n] * kernal[n];
}
tempx[y, x + Convert.ToInt32(kernLength / 2)] = (float)sumx;
tempy[y, x + Convert.ToInt32(kernLength / 2)] = (float)sumy;
}
}
// Run through the convolution in the y direction
for (int x = 0; x < imageWidth; x++)
{
for (int y = 0; y < imageHeight - kernLength; y++)
{
sumx = 0;
sumy = 0;
for (int n = 0; n < kernLength; n++)
{
sumx += tempx[y + n, x] * kernal[n];
sumy += tempy[y + n, x] * kernal[n];
}
// Scales value back down
handFrame.dx[y + Convert.ToInt32(kernLength / 2), x] = (float)sumx;
handFrame.dy[y + Convert.ToInt32(kernLength / 2), x] = (float)sumy;
}
}
}
public static HandFrame zoomOnHand(short[] depthFrame, DepthFrameParams DFSettings, ObjectPoint handPos)
{
HandFrame HF = new HandFrame(DFSettings.Width, DFSettings.Height, handPos.X, handPos.Y, getRadiusFromHand(handPos.Depth));
// converts the 2d X and Y from output matrix to the 1d array Depth Image: DeptImgWidth*Y + X
// should have xShift and yShift incorporated??????
int startPoint = DFSettings.Width * Math.Max(1, (handPos.Y - HF.radius)) + Math.Max(1, handPos.X - HF.radius);
int endPoint = Math.Min(depthFrame.Length - 1, DFSettings.Width * (handPos.Y + HF.radius) + handPos.X + HF.radius);
int x = 0;
int y = 0;
for (int i = startPoint; i <= endPoint; i++)
{
// wraps the linear array to create the 2D Matrix
if (x >= HF.width)
{
i += DFSettings.Width - HF.width - 1;
x = 0;
y++;
continue;
}
depthFrame[i] = (short)(depthFrame[i] >> DFSettings.Bitmask);
// Kinect reports unknown values as -1 or 0
if (depthFrame[i] <= 0 || depthFrame[i] >= handPos.Depth + HF.radius)
{
depthFrame[i] = (short)(handPos.Depth + HF.radius);
}
HF.abs[y, x] = (short)(depthFrame[i]);
HF.dx[y, x] = threshold(depthFrame[i] - depthFrame[i - 1], FingerDetection.DetectionParameters.dxThreshold);
HF.dy[y, x] = threshold(depthFrame[i] - depthFrame[i - DFSettings.Width], FingerDetection.DetectionParameters.dyThreshold);
x++;
}
return(HF);
}
public static int updateFingersFromDepth(short[] depthFrame, DepthFrameParams DFP, HandPoint[] hands)
{
// Check left hand
if (hands[(int)HandPoint.HANDS.LEFT].tracked)
{
// Zoom on hand and get dx, dy
HandFrame HF = ProcessDepthImage.zoomOnHand(depthFrame, DFP, hands[(int)HandPoint.HANDS.LEFT]);
// Smooth dx, dy;
Convolution.seperableConv(DetectionParameters.seperableKernal, HF);
hands[(int)HandPoint.HANDS.LEFT].tracked = ParticleGradient(HF, hands[(int)HandPoint.HANDS.LEFT]);
}
// Check right hand
if (hands[(int)HandPoint.HANDS.RIGHT].tracked)
{
// Zoom on hand and get dx, dy
HandFrame HF = ProcessDepthImage.zoomOnHand(depthFrame, DFP, hands[(int)HandPoint.HANDS.RIGHT]);
// Smooth dx, dy;
Convolution.seperableConv(DetectionParameters.seperableKernal, HF);
hands[(int)HandPoint.HANDS.RIGHT].tracked = ParticleGradient(HF, hands[(int)HandPoint.HANDS.RIGHT]);
}
return(1);
}
public static void Detect(particle[] particles, HandPoint h, HandFrame hf)
{
int radius = hf.radius;
// Initialize an array of particles matching fingers
particle[] f = new particle[5];
for (int i = 0; i < f.Length; i++)
{
f[i] = new particle((short)(radius - 1), (short)(radius - 1));
}
if (particles != null)
{
// for each finger
for (int i = 0; i < f.Length; i++)
{
//h.fingers[i].position_Pixels += 0.1 * h.fingers[i].elapsedMillis * h.fingers[i].velocity_Pixels;
//double a = .25;
// get the best value
for (int j = 0; j < particles.Length; j++)
{
if (particles[j] != null && particles[j].match_coefficient > f[i].match_coefficient)
{
f[i].x = particles[j].x;
f[i].y = particles[j].y;
f[i].z = hf.abs[particles[j].y, particles[j].x];
f[i].match_coefficient = particles[j].match_coefficient;
//f[i].matchCoefficient /= (hf.abs[f[i].y, f[i].x] - h.Depth + 100);
//f[i].matchCoefficient /= ((1 - a) + a * DetectionParameters.gaussian(Math.Sqrt(
// (particles[j].x - (h.fingers[i].X - h.X + radius))
// * (particles[j].x - (h.fingers[i].X - h.X + radius))
// + (particles[j].y - (h.fingers[i].Y - h.Y + radius))
// * (particles[j].y - (h.fingers[i].Y - h.Y + radius))), 10));
}
}
// remove all particles within the finger area
for (int j = 0; j < particles.Length; j++)
{
// Remove particles within the radius of a finger
if (particles[j] != null &&
Math.Sqrt(
(particles[j].x - f[i].x) * (particles[j].x - f[i].x) +
(particles[j].y - f[i].y) * (particles[j].y - f[i].y)) <= SIZE_OF_FINGER_PIXELS)
//if (particles[j] != null && particles[j].x == f[i].x && particles[j].y == f[i].y)
{
particles[j] = null;
}
}
// Calculate rotations for the finger
SetFingerRotations(f[i], hf.abs[f[i].y, f[i].x], h.angle_from_elbow_degrees, radius);
}
}
Array.Sort(f, Compare); // Sort the 5 fingers clockwise around the hand
for (int i = 0; i < h.fingers.Length; i++)
{
h.fingers[i].updateKalman(new Point3D {
X = f[i].x - radius, Y = f[i].y - radius, Z = hf.abs[f[i].y, f[i].x] - h.position_pixels_absolute.Z
});
h.fingers[i].angle_from_hand_degrees.X = f[i].angle_from_hand_degrees.X;
}
}
// Run one iteration of life-cycle for each particle and update
internal static void runTrace(particle[] p, double[,] xT, double[,] yT, HandFrame image)
{
int templateHeight = xT.GetLength(0);
int templateWidth = xT.GetLength(1);
int halfTemplateHeight = Convert.ToInt32(templateHeight / 2);
int halfTemplateWidth = Convert.ToInt32(templateWidth / 2);
// for each particle
for (int i = 0; i < p.Length; i++)
{
// check if initialized
if ((p[i] != null) && (p[i].alive))
{
if (image.evaluated[p[i].y, p[i].x])
{
p[i] = null;
continue;
}
image.evaluated[p[i].y, p[i].x] = true;
// run match for each direction
int testDirection = p[i].previous_direction; // set the initial direction to test to the previous direction
double testCoefficient = p[i].previous_match_coefficient;
double tempCoefficient;
// test each direction around the point until you reach the starting point
// adds one more iteration if the 'first' bit is set.
int d = (p[i].previous_direction + 1) % 4;
if (p[i].previous_direction < 0)
{
p[i].previous_direction = 0;
}
do
{
// Run match
tempCoefficient = matchFilter(xT, yT, p[i].x + moveX[d] - halfTemplateWidth, p[i].y + moveY[d] - halfTemplateHeight, templateWidth, templateHeight, image);
// Test if value is better than previous direction
if (tempCoefficient > testCoefficient)
{
testCoefficient = tempCoefficient;
testDirection = d;
}
d = ++d % 4;
} while (d != p[i].previous_direction);
// Test if final value is higher than the current center value
if (testCoefficient > p[i].match_coefficient)
{
//if (image.evaluate[p[i].y, p[i].x])
//{
// p[i] = null;
// continue;
//}
// Update particle values
// adjust the previous direction so that it is 180degrees when it is saved.
p[i].previous_direction = (sbyte)((testDirection + 2) % 4);
p[i].previous_match_coefficient = p[i].match_coefficient;
p[i].match_coefficient = testCoefficient;
p[i].x = (short)(p[i].x + moveX[testDirection]);
p[i].y = (short)(p[i].y + moveY[testDirection]);
//image.evaluate[p[i].y, p[i].x] = true;
}
// Otherwise, stop evaluating.
else
{
p[i].alive = false;
}
}
}
}
// Run one match filter
static double matchFilter(double[,] xT, double[,] yT, int startX, int startY, int templateWidth, int templateHeight, HandFrame image)
{
// make sure we're in bounds
if ((startX - 1 < 0) || (startX + templateWidth + 1 > image.width) || (startY - 1 < 0) || (startY + templateHeight + 1 > image.height))
{
return(0);
}
// Reset the sum
double sum = 0;
// iterate through x and y values running a absolute value of the difference
for (int y = 0; y < templateHeight; y += 2)
{
for (int x = 0; x < templateWidth; x += 2)
{
sum += image.dy[startY + y, startX + x] * yT[y, x] + image.dx[startY + y, startX + x] * xT[y, x];
}
}
return(sum);
}
