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); }