private HeadPostition RegisterPosition(HeadPostition position)
{
positionHistory.Clear();
positionHistory.AddLast(position);
// TODO: smooth with history
return(position);
}
public HeadPostition(HeadPostition source)
{
this.Yaw = source.Yaw;
this.Pitch = source.Pitch;
this.Roll = source.Roll;
this.PosX = source.PosX;
this.PosY = source.PosY;
this.PosZ = source.PosZ;
}
private HeadPostition EstimatePosition()
{
var result = new HeadPostition();
// working
//var objectPoints = new Emgu.CV.Mat(3, 3, Emgu.CV.CvEnum.DepthType.Cv32F, 1);
//var imagePoints = new Emgu.CV.Mat(3, 2, Emgu.CV.CvEnum.DepthType.Cv32F, 1);
// TODO call external pose reconstruction code
/*Tuple<int, int, int> topPoint = model.GetPoint(0);
* Tuple<int, int, int> leftPoint = model.GetPoint(1);
* Tuple<int, int, int> rightPoint = model.GetPoint(2);
* var objectPoints = new Emgu.CV.Matrix<float>(new float[,] {
* { topPoint.Item1, topPoint.Item2, topPoint.Item3 },
* { rightPoint.Item1, rightPoint.Item2, rightPoint.Item3 },
* { leftPoint.Item1, leftPoint.Item2, leftPoint.Item3 }
* });
* var imagePoints = new Emgu.CV.Matrix<float>(new float[,] {
* { inPoints[TopPointIndex].X, inPoints[TopPointIndex].Y },
* { inPoints[RightPointIndex].X, inPoints[RightPointIndex].Y },
* { inPoints[LeftPointIndex].X, inPoints[RightPointIndex].Y }
* });
* float fx = (float)(320/Math.Tan(57.5*Math.PI/360)*0.5);
* float fy = (float)(240/Math.Tan(45*Math.PI/360)*0.5);
* var mIntrinsic = new Emgu.CV.Matrix<float>(new float[,] {
* { fx, 0, 320/2 },
* { 0, fy, 240/2 },
* { 0, 0, 1 }
* });
*
* //mIntrinsic.SetIdentity();
* var distortion = new Emgu.CV.Matrix<float>(new float[] { 0, 0, 0, 0 });
* var rotation = new Emgu.CV.Matrix<float>(new float[] { 0, 0, 0 });
* var translation = new Emgu.CV.Matrix<float>(new float[] { 0, 0, 64 });
* bool success = false;
* try {
* // working
* success = Emgu.CV.CvInvoke.SolvePnP(objectPoints, imagePoints, mIntrinsic, distortion, rotation, translation, true, Emgu.CV.CvEnum.SolvePnpMethod.Iterative);
* //success = Emgu.CV.CvInvoke.SolvePnP(objectPoints, imagePoints, mIntrinsic, distortion, rotation, translation, true, Emgu.CV.CvEnum.SolvePnpMethod.Iterative);
* } catch(Emgu.CV.Util.CvException e) {
* LogManager.Instance.LogError(this, "Failed to detect pose: " + e.StackTrace);
* }
* if (success) {
* result.Yaw = new KeyState(rotation.Data[1, 0], rotation.Data[1, 0]);
* result.Pitch = new KeyState(rotation.Data[0, 0], rotation.Data[0, 0]);
* result.Roll = new KeyState(rotation.Data[2, 0], rotation.Data[2, 0]);
*
* result.PosX = new KeyState(translation.Data[0, 0], translation.Data[0, 0]);
* result.PosY = new KeyState(translation.Data[1, 0], translation.Data[1, 0]);
* result.PosZ = new KeyState(translation.Data[2, 0], translation.Data[2, 0]);
* }*/
return(result);
}
public HeadPostition Estimate(Point2i[] points, int[] labels, int pointNo)
{
if (!Setup(points, labels, pointNo))
{
return(LastPosition);
}
if (!Centered)
{
if (!Center())
{
return(LastPosition);
}
}
HeadPostition position = EstimatePosition();
position = RegisterPosition(position);
return(position);
}
