public static GaussianHolder GenerateSumOfGaussians(GaussianHolder first, GaussianHolder second)
{
return(new GaussianHolder(first.mean + second.mean, first.variance + second.variance));
}
public static GaussianHolder GenerateScalarProductOfGaussian(GaussianHolder gaussian, double scalar)
{
return(new GaussianHolder(gaussian.mean * scalar, gaussian.variance * Math.Pow(scalar, 2)));
}
public bool IsValidPoint(DepthPoint point)
{
long now = System.DateTime.UtcNow.Ticks;
if (previousPoints.Count < MIN_POINTS)
{
previousPoints.Enqueue(new Tuple <DepthPoint, long>(point, now));
return(true);
}
else
{
// Apply the 6-dimensional gaussian
// If it's 2+ std dev's away in any dimension, reject it
ICollection <double> xPos = new LinkedList <double>();
ICollection <double> yPos = new LinkedList <double>();
ICollection <double> depthPos = new LinkedList <double>();
ICollection <long> times = new LinkedList <long>();
ICollection <double> xVel = new LinkedList <double>();
ICollection <double> yVel = new LinkedList <double>();
ICollection <double> depthVel = new LinkedList <double>();
foreach (Tuple <DepthPoint, long> pointTuple in previousPoints)
{
xPos.Add(pointTuple.Item1.x);
yPos.Add(pointTuple.Item1.y);
depthPos.Add(pointTuple.Item1.depth);
times.Add(pointTuple.Item2);
if (xPos.Count > 1 && yPos.Count > 1 && depthPos.Count > 1 && times.Count > 1)
{
int prev2Index = times.Count - 2;
int prev1Index = times.Count - 1;
long timeInterval = times.ElementAt(prev1Index) - times.ElementAt(prev2Index);
double deltaX = xPos.ElementAt(prev1Index) - xPos.ElementAt(prev2Index);
double deltaY = yPos.ElementAt(prev1Index) - yPos.ElementAt(prev2Index);
double deltaDepth = depthPos.ElementAt(prev1Index) - depthPos.ElementAt(prev2Index);
xVel.Add(deltaX / timeInterval);
yVel.Add(deltaY / timeInterval);
depthVel.Add(deltaDepth / timeInterval);
}
}
GaussianHolder xPosGaussian = GaussianHolder.GenerateGaussianFromValueCollection(xPos);
GaussianHolder yPosGaussian = GaussianHolder.GenerateGaussianFromValueCollection(yPos);
GaussianHolder depthPosGaussian = GaussianHolder.GenerateGaussianFromValueCollection(depthPos);
GaussianHolder xVelGaussian = GaussianHolder.GenerateGaussianFromValueCollection(xVel);
GaussianHolder yVelGaussian = GaussianHolder.GenerateGaussianFromValueCollection(yVel);
GaussianHolder depthVelGaussian = GaussianHolder.GenerateGaussianFromValueCollection(depthVel);
double newPointTimeDelta = now - times.ElementAt(times.Count - 1);
double newXVel = (point.x - xPos.ElementAt(xPos.Count - 1)) / newPointTimeDelta;
double newYVel = (point.y - yPos.ElementAt(yPos.Count - 1)) / newPointTimeDelta;
double newDepthVel = (point.depth - depthPos.ElementAt(depthPos.Count - 1)) / newPointTimeDelta;
// approximate the new expected position
GaussianHolder deltaXPosGaussian = GaussianHolder.GenerateScalarProductOfGaussian(xVelGaussian, newPointTimeDelta);
GaussianHolder deltaYPosGaussian = GaussianHolder.GenerateScalarProductOfGaussian(yVelGaussian, newPointTimeDelta);
GaussianHolder deltaDepthPosGaussian = GaussianHolder.GenerateScalarProductOfGaussian(depthVelGaussian, newPointTimeDelta);
GaussianHolder newXPosGaussian = GaussianHolder.GenerateSumOfGaussians(xPosGaussian, deltaXPosGaussian);
GaussianHolder newYPosGaussian = GaussianHolder.GenerateSumOfGaussians(yPosGaussian, deltaYPosGaussian);
GaussianHolder newDepthPosGaussian = GaussianHolder.GenerateSumOfGaussians(depthPosGaussian, deltaDepthPosGaussian);
double xVelPhi = xVelGaussian.GetPhiValue(newXVel);
double yVelPhi = yVelGaussian.GetPhiValue(newYVel);
double depthVelPhi = depthVelGaussian.GetPhiValue(newDepthVel);
double xPosPhi = newXPosGaussian.GetPhiValue(point.x);
double yPosPhi = newYPosGaussian.GetPhiValue(point.y);
double depthPosPhi = newDepthPosGaussian.GetPhiValue(point.depth);
//Console.WriteLine("phi: " + xPosPhi + " | " + yPosPhi + " | " + depthPosPhi + " | " +
// xVelPhi + " | " + yVelPhi + " | " + depthVelPhi);
if ((xVelPhi > PHI_CUTOFF && xVelPhi < (1 - PHI_CUTOFF)) &&
(yVelPhi > PHI_CUTOFF && yVelPhi < (1 - PHI_CUTOFF)) &&
(depthVelPhi > PHI_CUTOFF && depthVelPhi < (1 - PHI_CUTOFF)) &&
(xPosPhi > PHI_CUTOFF && xPosPhi < (1 - PHI_CUTOFF)) &&
(yPosPhi > PHI_CUTOFF && yPosPhi < (1 - PHI_CUTOFF)) &&
(depthPosPhi > PHI_CUTOFF && depthPosPhi < (1 - PHI_CUTOFF)))
{
previousPoints.Enqueue(new Tuple <DepthPoint, long>(point, now));
if (previousPoints.Count > HISTORY_COUNT)
{
previousPoints.Dequeue();
}
return(true);
}
else
{
//Console.WriteLine("Rejected a point!!!!!!!!!!!!!!!!!!!!!!");
rejectedPoints.Enqueue(new Tuple <DepthPoint, long>(point, now));
while (now - rejectedPoints.First().Item2 > ERROR_TIME_CUTOFF_TICKS)
{
//Console.WriteLine("dumping rejected points");
rejectedPoints.Dequeue();
}
if (rejectedPoints.Count > ERROR_POINT_LIMIT)
{
// Received so many error points recently we should probably start over
rejectedPoints.Clear();
previousPoints.Clear();
previousPoints.Enqueue(new Tuple <DepthPoint, long>(point, now));
//Console.WriteLine("***************** STARTING OVER ************************");
return(true);
}
else
{
// Haven't received that many error points ecently and we rejected the most recent one
return(false);
}
}
}
}
public static GaussianHolder GenerateScalarSumOfGaussian(GaussianHolder gaussian, double scalar)
{
return(new GaussianHolder(gaussian.mean + scalar, gaussian.variance));
}
public static GaussianHolder GenerateSumOfGaussians(GaussianHolder first, GaussianHolder second)
{
return new GaussianHolder(first.mean + second.mean, first.variance + second.variance);
}
public static GaussianHolder GenerateScalarSumOfGaussian(GaussianHolder gaussian, double scalar)
{
return new GaussianHolder(gaussian.mean + scalar, gaussian.variance);
}
public static GaussianHolder GenerateScalarProductOfGaussian(GaussianHolder gaussian, double scalar)
{
return new GaussianHolder(gaussian.mean * scalar, gaussian.variance * Math.Pow(scalar, 2));
}
