/// <summary>
/// At first we try to search near the last found point (in a rectangle of 100x100px)
/// This means, that a point could have moved 50px in every direction. TODO: analyze moving direction and velocity
/// </summary>
/// <param name="lastFoundPoint"></param>
/// <returns></returns>
private Rectangle getEstimatedSearchArea(PointFrame lastFoundPoint)
{
int areaX = lastFoundPoint.Point.X - 50; // TODO Magic Number
int areaY = lastFoundPoint.Point.Y - 50;
return(new Rectangle(areaX, areaY, 100, 100));
}
/// <summary>
/// Estimates the pen position on a certain time.
/// </summary>
/// <param name="timestamp"></param>
/// <returns></returns>
public Point GetPenPoint(long timestamp)
{
PointFrame previousFrame = null;
List <PointFrame> .Enumerator enumerator;
lock (this._penPoints)
{
enumerator = this._penPoints.ToList().GetEnumerator();
// Search a predecessor and a successor
while (enumerator.MoveNext() && timestamp < enumerator.Current.Timestamp)
{
if (timestamp == enumerator.Current.Timestamp)
{
return(enumerator.Current.Point);
}
previousFrame = enumerator.Current;
}
// Calculate interpolation
if (previousFrame != null && enumerator.Current != null)
{
long ratio = previousFrame.Timestamp / enumerator.Current.Timestamp;
return(PointTools.CalculateIntermediatePoint(previousFrame.Point, enumerator.Current.Point, ratio));
}
}
return(Point.Empty);
}
/// <summary>
/// At first we try to search near the last found point (in a rectangle of 100x100px)
/// This means, that a point could have moved 50px in every direction. TODO: analyze moving direction and velocity
/// </summary>
/// <param name="lastFoundPoint"></param>
/// <returns></returns>
private Rectangle GetEstimatedSearchArea(PointFrame lastFoundPoint)
{
var areaX = lastFoundPoint.Point.X - SearchAreaRadius;
var areaY = lastFoundPoint.Point.Y - SearchAreaRadius;
return(new Rectangle(areaX, areaY, 2 * SearchAreaRadius, 2 * SearchAreaRadius));
}
//private int lastRandX = 100;
//private int lastRandY = 100;
//Random tempRandom = new Random();
private async void Found(object sender, PenFoundEventArgs penPositionEventArgs)
{
//Bitmap redaction = (Bitmap)e.Frame.Bitmap.Clone();
PointFrame pointFrame = penPositionEventArgs.Frame;
if (pointFrame != null)
{
penDrawingBuffer.Enqueue(pointFrame);
}
// draw points in buffer to image
using (Graphics g = Graphics.FromImage(_bitmap))
{
using (SolidBrush brush = new SolidBrush(Color.Black))
{
Point previousPoint = Point.Empty;
foreach (PointFrame f in penDrawingBuffer)
{
g.DrawEllipse(Pens.Green, f.Point.X - 3, f.Point.Y - 3, 3, 3);
if (!previousPoint.IsEmpty && PointTools.CalculateDistance(previousPoint, f.Point) < 50)
{
g.DrawLine(Pens.Red, previousPoint, f.Point);
}
previousPoint = f.Point;
}
}
g.Save();
this.calibrationPictureBox.Image = _bitmap;
}
}
/// <summary>
/// Mapping the point
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void PenFound(object sender, PenFoundEventArgs e)
{
Point point = _mapper.FromPresentation(e.Frame.Point.X, e.Frame.Point.Y);
PointFrame frame = e.Frame.ApplyRebase(point);
if (PenPositionChanged != null)
{
PenPositionChanged(this, new VirtualPenPositionEventArgs(frame, CalibratorGrid.Contains(point)));
}
}
public override void Copy(Frame frame)
{
base.Copy(frame);
PointFrame pointFrame = frame as PointFrame;
if (pointFrame == null)
{
return;
}
this.X = pointFrame.X;
this.Y = pointFrame.Y;
}
/*********************************************
* LINES
*********************************************/
private void createFrameLines(PointFrame from, PointFrame to)
{
LineFrame lineFrame = new LineFrame();
for (int i = 0; i < from.Points.Length; i++)
{
lineFrame.Lines.Add(createLine(from.Points[i], to.Points[i]));
}
lineFrame.Lines.Add(createLine(to.Points[0], to.Points[numOfHorizontalPointsInRoad - 1]));
_FrameLineList.Add(lineFrame);
}
/***********************************************************
** PUBLIC
***********************************************************/
/// <summary>
/// Clear this instance.
/// </summary>
public void Clear()
{
_FrameLineList = new List <LineFrame>();
_FramePointList = new List <PointFrame>();
//create new frame for starting point
Vector3[] v = new Vector3[numOfHorizontalPointsInRoad];
PointFrame pf = new PointFrame(this, 0);
_FramePointList.Add(pf);
_numOfLiveLines = 0;
}
/// <summary>
/// Mapping the point
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void PenFound(object sender, PenFoundEventArgs e)
{
Debug.WriteLine("Pen Nr\t{0} at {1},{2}", e.Frame.Number, e.Frame.Point.X, e.Frame.Point.Y);
Point point = e.Frame.Point;
double distortionX = (double)CalibratorGrid.ScreenSize.Width / (double)TransformWidth;
double distortionY = (double)CalibratorGrid.ScreenSize.Height / (double)TransformHeight;
//point.X *= (int)Math.Round((double)CalibratorGrid.ScreenSize.Width / TransformWidth);
//point.Y *= (int)Math.Round((double)CalibratorGrid.ScreenSize.Height / TransformHeight);
point.X = (int)Math.Round(distortionX * (double)e.Frame.Point.X);
point.Y = (int)Math.Round(distortionY * (double)e.Frame.Point.Y);
PointFrame frame = e.Frame.ApplyRebase(point);
if (PenPositionChanged != null)
{
PenPositionChanged(this, new VirtualPenPositionEventArgs(frame, true));
}
}
/*********************************************
* POINTS
*********************************************/
//Adding the lines
public void addPointFrame(float t)
{
PointFrame pf = new PointFrame(this, t);
_FramePointList.Add(pf);
if (_FramePointList.Count < 2)
{
return;
}
createFrameLines(_FramePointList[_FramePointList.Count - 2], _FramePointList[_FramePointList.Count - 1]);
if (_FrameLineList.Count > _historySize)
{
_FrameLineList[0].Delete();
_FrameLineList.RemoveAt(0);
_FramePointList[0].Delete();
_FramePointList.RemoveAt(0);
}
}
public static void TrackPenOnLibrary()
{
Console.WriteLine("Starting...");
TimedFilesystemCamera camera = new TimedFilesystemCamera(new DirectoryInfo(@"c:\temp\live\cap-127"), 1);
AForgePenTracker tracker = new AForgePenTracker(new WhiteLedStrategy(), camera);
camera.FrameReady += delegate(object o, FrameReadyEventArgs e) {
Console.WriteLine("Cam Frame: {0}", e.Frame.Number);
};
tracker.PenFound += delegate(object o, PenFoundEventArgs e) {
PointFrame p = e.Frame;
Console.WriteLine("-> Found {0} at {1},{2}", p.Number, p.Point.X, p.Point.Y);
};
tracker.Start();
camera.Start();
Console.WriteLine("Tracker and Camera Started.");
Console.ReadLine();
}
private void CheckPointReader()
{
// Assumes AstraUnityContext.Instance.IsUpdateAsyncComplete is already true
ReaderFrame frame;
if (_readerPoint.TryOpenFrame(0, out frame))
{
using (frame)
{
PointFrame pointFrame = frame.GetFrame <PointFrame>();
if (pointFrame != null)
{
if (_lastPointFrameIndex != pointFrame.FrameIndex)
{
_lastPointFrameIndex = pointFrame.FrameIndex;
NewPointFrameEvent.Invoke(pointFrame);
}
}
}
}
}
private void processFrame(VideoFrame currentFrame)
{
// We can only do our work if there is at least one frame in the buffer already
if (this._frameBuffer.Count < 1)
{
// But we keep the frame for the next incoming processing request
this._frameBuffer.Enqueue(currentFrame);
return;
}
// We can do our work, since there is a previously taken frame.
VideoFrame previousFrame = this._frameBuffer.Last();
this._frameBuffer.Enqueue(currentFrame);
// Status:
// - We now have references to a previous and a current video frame in the correct order.
// - Additionally we have some previous points location (not necessarily matching to the previous video frame).
try
{
// Reference to the last found point is needed to search more efficiently.
// Efficiently means: Arround the last found point
PointFrame previousPoint = this._penPoints.LastOrDefault();
// At first we search for the pen near the old one (if there was one)
IEnumerable <PenCandidate> candidates = null;
if (previousPoint != null)
{
candidates = this.findPenCandidatesInArea(
(Bitmap)previousFrame.Bitmap.Clone(),
(Bitmap)currentFrame.Bitmap.Clone(),
this.getEstimatedSearchArea(previousPoint)
);
}
// If there is no previous point (previousPoint == null) or we couldn't use the information
// of a previously tracked point (candidates == 0), we search the whole image.
// WEAKNESS: if we detect one in our search area, but two are present on the whole picture,
// we give FALSE feedback. Therefore, the searcharea must be choosen carefully
if (previousPoint == null || candidates.Count() == 0)
{
candidates = this.findPenCandidatesInArea(
(Bitmap)previousFrame.Bitmap.Clone(),
(Bitmap)currentFrame.Bitmap.Clone(),
Rectangle.Empty
);
}
// Evaluate all the pen candidates for valid pen positions
Point foundPoint = this.findPen(candidates);
// TODO empty may be a valid point (0,0)
if (foundPoint.IsEmpty)
{
if (this.NoPenFound != null)
{
this.NoPenFound(this, null);
}
return;
}
// Put the qualified result in a new point frame and queue it to the result list
PointFrame resultFrame = new PointFrame(currentFrame.Number, foundPoint, currentFrame.Timestamp);
this._penPoints.Enqueue(resultFrame);
if (this.PenFound != null)
{
this.PenFound(this, new PenFoundEventArgs(resultFrame));
}
}
catch (Exception e)
{
Debug.WriteLine("Error in Frame Processing: \"" + e.ToString() + "\"");
// TODO Error Handling: Maybe we should catch everything for stability.
}
}
public PenFoundEventArgs(PointFrame frame)
{
Frame = frame;
}
/// <summary>
/// Probability of accuracy
/// </summary>
/// <value>Has to be between 0 and 1</value>
//public double Confidance
//{
// get
// {
// return this._confidence;
// }
// set
// {
// if (value < 0 || value > 1)
// {
// throw new ArgumentOutOfRangeException("Value range is [0..1].");
// }
// this._confidence = value;
// }
//}
public VirtualPenPositionEventArgs(PointFrame frame, bool inside)
: base(frame)
{
IsInside = inside;
}
public PenPositionEventArgs(PointFrame frame, bool isOutOfOrder = false)
{
IsOutOfOrder = isOutOfOrder;
Frame = frame;
}
