/// <summary>
/// show the tracked polygons within the given image
/// </summary>
/// <param name="img"></param>
/// <param name="img_width"></param>
/// <param name="img_height"></param>
public void Show(byte[] img, int img_width, int img_height, int minimum_persistence)
{
for (int i = 0; i < tracked.Count; i++)
{
polygon2DTrackerData poly = (polygon2DTrackerData)tracked[i];
if (poly.persistence > minimum_persistence)
{
int x = (int)poly.centre_x;
int y = (int)poly.centre_y;
int perimeter_radius = (int)(poly.av_radius / poly.persistence);
drawing.drawCircle(img, img_width, img_height,
x, y, perimeter_radius,
poly.colour[0], poly.colour[1], poly.colour[2], 1);
for (int k = 0; k < 4; k++)
{
int x2 = x + (int)(perimeter_radius * Math.Sin(poly.orientation[k]));
int y2 = y - (int)(perimeter_radius * Math.Cos(poly.orientation[k]));
drawing.drawLine(img, img_width, img_height,
x, y, x2, y2,
poly.colour[0], poly.colour[1], poly.colour[2],
1, false);
}
}
}
}
/// <summary>
/// update the position and orientation of all tracked polygons
/// </summary>
private void UpdatePositionOrientation()
{
DateTime current_t = DateTime.Now;
for (int j = tracked.Count - 1; j >= 0; j--)
{
polygon2DTrackerData polytrack = (polygon2DTrackerData)tracked[j];
// time since the last sighting
TimeSpan dt = current_t.Subtract(polytrack.last_seen);
float seconds_elapsed = (float)dt.TotalSeconds;
if (dt.TotalMilliseconds > timeout_milliseconds)
{
// remove a polygon if it hasn't been seen for a while
tracked.RemoveAt(j);
}
else
{
// predict the new state
float new_centre_x = polytrack.centre_x + (seconds_elapsed * polytrack.vx);
float new_centre_y = polytrack.centre_y + (seconds_elapsed * polytrack.vy);
//float new_angular = polytrack.orientation + (seconds_elapsed * polytrack.v_angular);
// update the state
polytrack.centre_x = new_centre_x;
polytrack.centre_y = new_centre_y;
//polytrack.orientation = new_angular;
}
}
}
public void Update(ArrayList polygons)
{
DateTime current_t = DateTime.Now;
// forward prediction: update the state of all tracked polygons
UpdatePositionOrientation();
// matching: do any of the tracked polygons match what we can currently see?
for (int i = 0; i < polygons.Count; i++)
{
polygon2D poly = (polygon2D)polygons[i];
float poly_width = poly.right() - poly.left();
float poly_height = poly.bottom() - poly.top();
float av_radius = (poly_width + poly_height) / 4.0f;
float[] orient = poly.getOrientations();
float[] grad = poly.getGradients();
if ((poly_width > minimum_dimension) &&
(poly_height > minimum_dimension))
{
// find the centre of the polygon
float centre_x = 0, centre_y = 0;
poly.getCentreOfGravity(ref centre_x, ref centre_y);
// are any existing centre points close to this?
float min_displacement = 9999;
int index = -1;
for (int j = 0; j < tracked.Count; j++)
{
polygon2DTrackerData polytrack = (polygon2DTrackerData)tracked[j];
// exclusion zone
float perimeter_radius = polytrack.av_radius * 1.4f / polytrack.persistence;
float cx = polytrack.centre_x;
float dx = Math.Abs(cx - centre_x);
if (dx < perimeter_radius)
{
float cy = polytrack.centre_y;
float dy = Math.Abs(cy - centre_y);
if (dy < perimeter_radius)
{
float displacement = (float)Math.Sqrt((dx * dx) + (dy * dy));
if ((displacement < min_displacement) ||
(min_displacement == 9999))
{
min_displacement = displacement;
index = j;
}
}
}
}
if (index > -1)
{
polygon2DTrackerData matched = (polygon2DTrackerData)tracked[index];
// update the position and velocities
float dx = centre_x - matched.centre_x;
float dy = centre_y - matched.centre_y;
//float dorient = orient - matched.orientation;
TimeSpan dt_seconds = current_t.Subtract(matched.last_seen);
if (dt_seconds.TotalSeconds > 0)
{
// use running averages here to smooth out error
float momentum = 0.99f;
matched.vx = (matched.vx * momentum) + ((dx / (float)dt_seconds.TotalSeconds) * (1.0f - momentum));
matched.vy = (matched.vy * momentum) + ((dy / (float)dt_seconds.TotalSeconds) * (1.0f - momentum));
//matched.v_angular = ((matched.v_angular * momentum) + (dorient / (float)dt_seconds.TotalSeconds) * (1.0f - momentum));
}
matched.centre_x = centre_x;
matched.centre_y = centre_y;
for (int k = 0; k < 4; k++)
{
matched.orientation[k] = orient[k];
}
// find the closest orientation
/*
* float min_diff = 9999;
* int closest_index = -1;
* for (int k = 0; k < 4; k++)
* {
* float diff = Math.Abs(orient[k] - matched.orientation);
* if (diff < min_diff)
* {
* min_diff = diff;
* closest_index = k;
* }
* }
*
* // update the orientation
* if (closest_index > -1)
* {
* matched.orientation = (orient[closest_index] * 0.01f) + (matched.orientation * 0.99f);
* }
*/
matched.last_seen = current_t;
matched.persistence++;
matched.av_radius += (long)av_radius;
// avoid overflows
if (matched.persistence > 99999)
{
matched.persistence /= 2;
matched.av_radius /= 2;
}
}
else
{
/*
* float min_orient = 9999;
* for (int k = 0; k < 4; k++)
* {
* if (orient[k] < min_orient)
* {
* min_orient = orient[k];
* }
* }
*/
// add a new tracked polygon to the list
polygon2DTrackerData new_poly = new polygon2DTrackerData();
new_poly.centre_x = centre_x;
new_poly.centre_y = centre_y;
new_poly.orientation = orient;
new_poly.colour[0] = (byte)(100 + rnd.Next(155));
new_poly.colour[1] = (byte)(100 + rnd.Next(155));
new_poly.colour[2] = (byte)(100 + rnd.Next(155));
new_poly.av_radius = (long)av_radius;
tracked.Add(new_poly);
}
}
}
}
public void Update(ArrayList polygons)
{
DateTime current_t = DateTime.Now;
// forward prediction: update the state of all tracked polygons
UpdatePositionOrientation();
// matching: do any of the tracked polygons match what we can currently see?
for (int i = 0; i < polygons.Count; i++)
{
polygon2D poly = (polygon2D)polygons[i];
float poly_width = poly.right() - poly.left();
float poly_height = poly.bottom() - poly.top();
float av_radius = (poly_width + poly_height) / 4.0f;
float[] orient = poly.getOrientations();
float[] grad = poly.getGradients();
if ((poly_width > minimum_dimension) &&
(poly_height > minimum_dimension))
{
// find the centre of the polygon
float centre_x = 0, centre_y = 0;
poly.getCentreOfGravity(ref centre_x, ref centre_y);
// are any existing centre points close to this?
float min_displacement = 9999;
int index = -1;
for (int j = 0; j < tracked.Count; j++)
{
polygon2DTrackerData polytrack = (polygon2DTrackerData)tracked[j];
// exclusion zone
float perimeter_radius = polytrack.av_radius * 1.4f / polytrack.persistence;
float cx = polytrack.centre_x;
float dx = Math.Abs(cx - centre_x);
if (dx < perimeter_radius)
{
float cy = polytrack.centre_y;
float dy = Math.Abs(cy - centre_y);
if (dy < perimeter_radius)
{
float displacement = (float)Math.Sqrt((dx * dx) + (dy * dy));
if ((displacement < min_displacement) ||
(min_displacement == 9999))
{
min_displacement = displacement;
index = j;
}
}
}
}
if (index > -1)
{
polygon2DTrackerData matched = (polygon2DTrackerData)tracked[index];
// update the position and velocities
float dx = centre_x - matched.centre_x;
float dy = centre_y - matched.centre_y;
//float dorient = orient - matched.orientation;
TimeSpan dt_seconds = current_t.Subtract(matched.last_seen);
if (dt_seconds.TotalSeconds > 0)
{
// use running averages here to smooth out error
float momentum = 0.99f;
matched.vx = (matched.vx * momentum) + ((dx / (float)dt_seconds.TotalSeconds) * (1.0f - momentum));
matched.vy = (matched.vy * momentum) + ((dy / (float)dt_seconds.TotalSeconds) * (1.0f - momentum));
//matched.v_angular = ((matched.v_angular * momentum) + (dorient / (float)dt_seconds.TotalSeconds) * (1.0f - momentum));
}
matched.centre_x = centre_x;
matched.centre_y = centre_y;
for (int k = 0; k < 4; k++)
{
matched.orientation[k] = orient[k];
}
// find the closest orientation
/*
float min_diff = 9999;
int closest_index = -1;
for (int k = 0; k < 4; k++)
{
float diff = Math.Abs(orient[k] - matched.orientation);
if (diff < min_diff)
{
min_diff = diff;
closest_index = k;
}
}
// update the orientation
if (closest_index > -1)
{
matched.orientation = (orient[closest_index] * 0.01f) + (matched.orientation * 0.99f);
}
*/
matched.last_seen = current_t;
matched.persistence++;
matched.av_radius += (long)av_radius;
// avoid overflows
if (matched.persistence > 99999)
{
matched.persistence /= 2;
matched.av_radius /= 2;
}
}
else
{
/*
float min_orient = 9999;
for (int k = 0; k < 4; k++)
{
if (orient[k] < min_orient)
{
min_orient = orient[k];
}
}
*/
// add a new tracked polygon to the list
polygon2DTrackerData new_poly = new polygon2DTrackerData();
new_poly.centre_x = centre_x;
new_poly.centre_y = centre_y;
new_poly.orientation = orient;
new_poly.colour[0] = (byte)(100 + rnd.Next(155));
new_poly.colour[1] = (byte)(100 + rnd.Next(155));
new_poly.colour[2] = (byte)(100 + rnd.Next(155));
new_poly.av_radius = (long)av_radius;
tracked.Add(new_poly);
}
}
}
}
