public void ColorizePoint(BeamerPoint point)
{
byte[] ColorData = { 255, 0, 0, 0 }; // B G R
Int32Rect rect = new Int32Rect(
point.X,
point.Y,
1,
1);
BeamerImage.WritePixels(rect, ColorData, 4, 0);
}
public Vector2D ToVector2D(BeamerPoint beamerPoint)
{
return new Vector2D { X = X, Y = Y , BeamerPoint = beamerPoint};
}
public static KinectPoint CalculateKinectPoint(BeamerPoint beamerPoint)
{
var kinectVectorA = new Vector2D { X = 639 - pointA.KinectPoint.X, Y = pointA.KinectPoint.Y };
var kinectVectorB = new Vector2D { X = 639 - pointB.KinectPoint.X, Y = pointB.KinectPoint.Y };
var kinectVectorC = new Vector2D { X = 639 - pointC.KinectPoint.X, Y = pointC.KinectPoint.Y };
var kinectVectorD = new Vector2D { X = 639 - pointD.KinectPoint.X, Y = pointD.KinectPoint.Y };
var beamerVectorA = pointA.BeamerPoint.ToVector2D();
var beamerVectorB = pointB.BeamerPoint.ToVector2D();
var beamerVectorC = pointC.BeamerPoint.ToVector2D();
var beamerVectorD = pointD.BeamerPoint.ToVector2D();
// ReSharper disable InconsistentNaming
var detDA = beamerVectorD.Determinant(beamerVectorA);
var detDB = beamerVectorD.Determinant(beamerVectorB);
var detCA = beamerVectorC.Determinant(beamerVectorA);
var detCB = beamerVectorC.Determinant(beamerVectorB);
var detDC = beamerVectorD.Determinant(beamerVectorC);
var detAB = beamerVectorA.Determinant(beamerVectorB);
var detAC = beamerVectorA.Determinant(beamerVectorC);
var beamerVectorP = beamerPoint.ToVector2D();
var detDP = beamerVectorD.Determinant(beamerVectorP);
var detCP = beamerVectorC.Determinant(beamerVectorP);
var detPA = beamerVectorP.Determinant(beamerVectorA);
var detPB = beamerVectorP.Determinant(beamerVectorB);
var detAP = beamerVectorA.Determinant(beamerVectorP);
var detPC = beamerVectorP.Determinant(beamerVectorC);
// ReSharper restore InconsistentNaming
var alphaMy = detDA - detDB - detCA + detCB;
var betaMy = -2 * detDA + detDB + detDP + detCA - detCP + detPA - detPB;
var gammaMy = detDA - detDP - detPA;
var detMy = Math.Sqrt(betaMy * betaMy - 4 * alphaMy * gammaMy);
var my1 = (-(betaMy / 2) + detMy) / (2 * alphaMy);
var my2 = (-(betaMy / 2) - detMy) / (2 * alphaMy);
var tmy = -gammaMy / betaMy;
var my = tmy;
var alphaLambda = detDC - detDB - detAC + detAB;
var betaLambda = -2 * detDC + detDB + detDP + detAC - detAP + detPC - detPB;
var gammaLambda = detDC - detDP - detPC;
var detLambda = Math.Sqrt(betaLambda * betaLambda - 4 * alphaLambda * gammaLambda);
var lambda1 = (-(betaLambda / 2) + detLambda) / (2 * alphaLambda);
var lambda2 = (-(betaLambda / 2) - detLambda) / (2 * alphaLambda);
var tlambda = -gammaLambda / betaLambda;
var lambda = tlambda;
var kinectVectorP =
kinectVectorD.Multiply((1 - lambda) * (1 - my))
.Add(kinectVectorC.Multiply(((1 - lambda) * my)))
.Add(kinectVectorA.Multiply(lambda * (1 - my)))
.Add(kinectVectorB.Multiply(my * lambda));
return kinectVectorP.ToKinectPoint();
}
private List<Vector2D> CalculateNewPoints()
{
var divisor = (int) Math.Pow(2, CALIBRATION_ROUNDS);
var numberOfAreaHorizontal = divisor;
var numberOfAreaVertical = numberOfAreaHorizontal;
var beamerWidth = (int) Math.Sqrt(Math.Pow(pointA.BeamerPoint.X, 2) + Math.Pow(pointB.BeamerPoint.X, 2))/2;
var beamerHeight = (int) Math.Sqrt(Math.Pow(pointA.BeamerPoint.Y, 2) + Math.Pow(pointD.BeamerPoint.Y, 2))/2;
var beameAreaWidth = beamerWidth/divisor;
var beamerAreaHeight = beamerHeight/divisor;
var kinectVectorA = pointA.KinectPoint.ToVector2D();
var kinectVectorB = pointA.KinectPoint.ToVector2D();
var kinectVectorC = pointA.KinectPoint.ToVector2D();
var kinectVectorD = pointA.KinectPoint.ToVector2D();
var beamerVectorA = pointA.BeamerPoint.ToVector2D();
var beamerVectorB = pointB.BeamerPoint.ToVector2D();
var beamerVectorC = pointC.BeamerPoint.ToVector2D();
var beamerVectorD = pointD.BeamerPoint.ToVector2D();
var detDA = beamerVectorD.Determinant(beamerVectorA);
var detDB = beamerVectorD.Determinant(beamerVectorB);
var detCA = beamerVectorC.Determinant(beamerVectorA);
var detCB = beamerVectorC.Determinant(beamerVectorB);
var detDC = beamerVectorD.Determinant(beamerVectorC);
var detAB = beamerVectorA.Determinant(beamerVectorB);
var detAC = beamerVectorA.Determinant(beamerVectorC);
for (var i = 0; i <= numberOfAreaHorizontal; i++)
{
for (var j = 0; j <= numberOfAreaVertical; j++)
{
if (i == 0 && j == 0 || i == numberOfAreaHorizontal && j == 0 || i == 0 && j == numberOfAreaVertical ||
i == numberOfAreaHorizontal && j == numberOfAreaVertical)
{
continue;
}
var beamerPoint = new BeamerPoint
{
X = pointA.BeamerPoint.X + i*beameAreaWidth,
Y = pointA.BeamerPoint.Y + j*beamerAreaHeight
};
var beamerVectorP = beamerPoint.ToVector2D();
var detDP = beamerVectorD.Determinant(beamerVectorP);
var detCP = beamerVectorC.Determinant(beamerVectorP);
var detPA = beamerVectorP.Determinant(beamerVectorA);
var detPB = beamerVectorP.Determinant(beamerVectorB);
var detPP = beamerVectorP.Determinant(beamerVectorP);
var detAP = beamerVectorA.Determinant(beamerVectorP);
var detPC = beamerVectorP.Determinant(beamerVectorC);
var alphaMy = detDA - detDB - detCA + detCB;
var betaMy = -2*detDA + detDB + detDP + detCA - detCP + detPA - detPB;
var gammaMy = detDA - detDP - detPA + detPP;
var my1 = (-(betaMy/2) + Math.Sqrt(betaMy*betaMy - 4*alphaMy*gammaMy))/(2*alphaMy);
var my2 = (-(betaMy/2) - Math.Sqrt(betaMy*betaMy - 4*alphaMy*gammaMy))/(2*alphaMy);
var my = my1 >= 0 && my1 <= 1 ? my1 : my2;
var alphaLambda = detDC - detDB - detAC + detAB;
var betaLambda = -2*detDC + detDB + detDP + detAC - detAP + detPB;
var gammaLambda = detDC - detDP - detPC + detPP;
var lambda1 = (-(betaLambda/2) + Math.Sqrt(betaLambda*betaLambda - 4*alphaLambda*gammaLambda))/(2*
alphaLambda);
var lambda2 = (-(betaLambda/2) - Math.Sqrt(betaLambda*betaLambda - 4*alphaLambda*gammaLambda))/(2*
alphaLambda);
var lambda = lambda1 >= 0 && lambda1 <= 1 ? lambda1 : lambda2;
var kinectVectorP =
kinectVectorD.Multiply((1 - lambda)*(1 - my))
.Add(kinectVectorC.Multiply(((1 - lambda)*my)))
.Add(kinectVectorA.Multiply(lambda*(1 - my)))
.Add(kinectVectorB.Multiply(my*lambda));
var kinectPointPredicted = kinectVectorP.ToKinectPoint();
var newPoint = new Point
{
BeamerPoint = beamerPoint,
KinectPoint = kinectPointPredicted
};
Calibration.Points.Add(newPoint);
}
}
return new List<Vector2D>();
}
private static BeamerPoint CalculateBeamerPoint(int beamerWidth, int beamerHeight, int position)
{
var beamerPoint = new BeamerPoint();
switch (position)
{
case 0:
beamerPoint.X = TILE_WIDTH;
beamerPoint.Y = TILE_HEIGHT;
break;
case 1:
beamerPoint.X = beamerWidth - TILE_WIDTH;
beamerPoint.Y = TILE_HEIGHT;
break;
case 2:
beamerPoint.X = beamerWidth - TILE_WIDTH;
beamerPoint.Y = beamerHeight - TILE_HEIGHT;
break;
case 3:
beamerPoint.X = TILE_WIDTH;
beamerPoint.Y = beamerHeight - TILE_HEIGHT;
break;
}
return beamerPoint;
}
