//functions
public void process()
{
while (true)
{
FeatureExtracting.Notifier.WaitOne();
foreach (Bitmap image in queue.GetConsumingEnumerable())
{
AForge.Imaging.BlobCounter bc = new AForge.Imaging.BlobCounter(image);
Rectangle[] rects = bc.GetObjectsRectangles();
Rectangle biggest = new Rectangle(0, 0, 0, 0);
Graphics g = Graphics.FromImage(image);
double ratio = 0;
foreach (Rectangle r in rects)
{
ratio = (r.Height + 1.0) / (r.Width + 1.0);
if (biggest.Width * biggest.Height < r.Width * r.Height)
{
//check ratio
if ((ratio < 1.1) && (ratio > 0.45))
{
if (r.Width * r.Height > 200)
{
biggest = r;
}
}
}
}
int imageCenter = (biggest.Width / 2) + biggest.X;
int screenCenter = image.Width / 2;
g.DrawRectangle(new Pen(Color.FromArgb(255, 0, 0)), biggest);
string drawString = biggest.Height + " <-Height Width-> " + biggest.Width + "\nratio = " + ratio + "\n Image Center = " + imageCenter + "\nScreen Center = " + screenCenter;
System.Drawing.Font drawFont = new System.Drawing.Font("Arial", 11);
System.Drawing.SolidBrush drawBrush = new System.Drawing.SolidBrush(System.Drawing.Color.White);
float x = 10.0F;
float y = 10.0F;
System.Drawing.StringFormat drawFormat = new System.Drawing.StringFormat();
g.DrawString(drawString, drawFont, drawBrush, x, y, drawFormat);
drawFont.Dispose();
drawBrush.Dispose();
Stats a = new Stats();
a.blobCount = 0;
ActionPlanning.queue.Add(a);
ActionPlanning.Notifier.Set();
#if DEBUG
Program.mainForm.VideoViewer.Image = image;
#endif
}
}
}
private void UpdateYellowWall(Stats stats)
{
if (stats.IsNorth)
{
// orientation = N;
}
if (stats.IsSouth)
{
// orientation = S;
}
}
private void UpdateRedBlock(Stats stats)
{
if (stats.RedBlockHeight == 0)
return;
Point p = calculateBlockPosition(25.0, (double)stats.RedBlockHeight, (double)stats.RedBlockWidth, (double)stats.RedBlockCenterLocation.X);
Point newLocation = RotateOnPoint(p);
for (int i = -3; i < 4; i++)
{
for (int j = -3; j < 4; j++)
{
probabilisticMap(newLocation.X + i, newLocation.Y + j, true);
}
}
}
private void UpdateGreenBlock(Stats stats)
{
if (stats.GreenBlockHeight == 0)
return;
Point p = calculateBlockPosition(130.0, (double)stats.GreenBlockHeight, (double)stats.GreenBlockWidth, (double)stats.GreenBlockCenterLocation.X);
Point newLocation = RotateOnPoint(p);
for (int i = -15; i < 15; i++)
{
for (int j = -15; j < 15; j++)
{
probabilisticMap(newLocation.X, newLocation.Y, true);
}
}
}
private void UpdateBlueLine(Stats stats)
{
if (stats.BlueLineDistance == 0)
return;
switch (orientation)
{
case N:
currentLocation.Y = lerp(currentLocation, new Point(currentLocation.X, Convert.ToInt32(stats.BlueLineDistance * 100))).Y;
break;
case E:
currentLocation.X = mapWidth - lerp(currentLocation, new Point(Convert.ToInt32(stats.BlueLineDistance * 100), currentLocation.Y)).X;
break;
case S:
currentLocation.Y = mapHeight - lerp(currentLocation, new Point(currentLocation.X, Convert.ToInt32(stats.BlueLineDistance * 100))).Y;
break;
case W:
currentLocation.X = lerp(currentLocation, new Point(Convert.ToInt32(stats.BlueLineDistance * 100), currentLocation.Y)).X;
break;
default:
break;
}
}
public void runMap()
{
while (true)
{
foreach (Stats stats in queue.GetConsumingEnumerable())
{
lastStats = stats;
if (stats.RedBlockDetected)
{
UpdateRedBlock(stats);
}
if (stats.GreenBlockDetected)
{
UpdateGreenBlock(stats);
}
if (stats.YellowWallDetected)
{
UpdateYellowWall(stats);
}
if (stats.BlueLineDetected)
{
UpdateBlueLine(stats);
}
Draw();
for (int i = 0; i < mapWidth; i++)
{
for (int j = 0; j < mapHeight; j++)
{
if (PointInView(new Point(i,j)))
{
Point ro = new Point(i, j);
ro = RotateOnPoint(ro);
probabilisticMap(ro.X, ro.Y, occupide(i, j));
}
}
}
}
}
}
private Stats ExtractYellowFeatures(Bitmap Filtered)
{
BlobCounter bc = new BlobCounter();
Graphics g = Graphics.FromImage(Filtered);
Pen bluePen = new Pen(Color.Red, 1);
bc.MinWidth = 100;
bc.FilterBlobs = true;
bc.ObjectsOrder = ObjectsOrder.Size;
bc.ProcessImage(Filtered);
List<Blob> blob = new List<Blob>();
Blob[] blobs = bc.GetObjectsInformation();
blob.AddRange(blobs);
//UpdateVideo(Filtered);
for (int i = 0; i < blobs.Length; i++)
{
for (int j = 0; j < blobs.Length; j++)
{
if (i == j)
continue;
if (Math.Abs(blobs[i].Rectangle.Y - blobs[j].Rectangle.Y) < 5)
{
if (!blob.Contains(blobs[i]))
blob.Remove(blob[i]);
}
}
}
switch (blob.Count)
{
case 1:
// Console.WriteLine("North");
Stats a = new Stats();
a.IsNorth = true;
a.YellowWallDetected = true;
return a;
case 2:
// Console.WriteLine("South");
Stats b = new Stats();
b.IsSouth = true;
b.YellowWallDetected = true;
return b;
default:
//Console.WriteLine("erm... houston, we're lost... ");
return new Stats();
}
}
private Stats ExtractRedFeatures(Bitmap Filtered)
{
BlobCounter bc = new BlobCounter();
Stats redStats = new Stats();
bc.MinWidth = 5;
bc.MinHeight = 5;
bc.FilterBlobs = true;
bc.ObjectsOrder = ObjectsOrder.Size;
bc.ProcessImage(Filtered);
Rectangle[] rects = bc.GetObjectsRectangles();
Rectangle biggest = new Rectangle(0, 0, 0, 0);
Graphics g = Graphics.FromImage(Filtered);
if ((rects.Length > 0) && (rects[0].Height > 0))
{
biggest = rects[0];
redStats.RedBlockDetected = true;
redStats.RedBlockCenterLocation = new System.Drawing.Point(((((biggest.Width / 2) + biggest.X))), (biggest.Y + biggest.Height / 2));
redStats.RedBlockHeight = biggest.Height;
redStats.RedBlockWidth = biggest.Width;
redStats.RedBlockDistance = (25.0f / biggest.Height);
Console.WriteLine(redStats.RedBlockDistance);
}
else
{
redStats.RedBlockDetected = false;
}
string objectString = Math.Round((25.0f / biggest.Height), 2).ToString();
string drawString = biggest.Height + " <-- Height Width --> " + biggest.Width + "\n Image Center = " + (redStats.RedBlockCenterLocation.X/* - (Filtered.Width / 2)*/);
g.DrawRectangle(new Pen(Color.Blue), biggest);
g.DrawString(objectString, drawFont, Brushes.White, redStats.RedBlockCenterLocation.X, redStats.RedBlockCenterLocation.Y, drawFormat);
g.DrawString(drawString, drawFont, Brushes.White, x, y, drawFormat);
//return Filtered;
UpdateVideo(Filtered);
return redStats;
}
private Stats ExtractGreenFeatures(Bitmap Filtered)
{
BlobCounter bc = new BlobCounter();
Stats greenStats = new Stats();
bc.MinWidth = 15;
bc.MinHeight = 15;
bc.FilterBlobs = true;
bc.ObjectsOrder = ObjectsOrder.Size;
bc.ProcessImage(Filtered);
Rectangle[] rects = bc.GetObjectsRectangles();
// Graphics g = Graphics.FromImage(Filtered);
if ((rects.Length > 0) && (rects[0].Height > 0))
{
greenStats.GreenBlockDetected = true;
greenStats.GreenBlockCenterLocation = new System.Drawing.Point(((((rects[0].Width / 2) + rects[0].X))), (rects[0].Y + rects[0].Height / 2));
greenStats.GreenBlockHeight = rects[0].Height;
greenStats.GreenBlockWidth = rects[0].Width;
greenStats.GreenBlockDistance = (130.0f / rects[0].Height);
}
else
{
greenStats.GreenBlockDetected = false;
}
//UpdateVideo(Filtered);
return greenStats;
// User Feedback for debug
//string objectString = Math.Round((130.0f / biggest.Height), 2).ToString();
//string drawString = biggest.Height + " <-- Height Width --> " + biggest.Width + "\n Image Center = " + (greenStats.RedBlockCenterLocation.X);
//g.DrawRectangle(new Pen(Color.Blue), biggest);
//g.DrawString(objectString, drawFont, Brushes.White, greenStats.RedBlockCenterLocation.X, greenStats.RedBlockCenterLocation.Y, drawFormat);
//g.DrawString(drawString, drawFont, Brushes.White, x, y, drawFormat);
//return Filtered;
}
private Stats ExtractBlueFeatures(Bitmap Filtered)
{
BlobCounter bc = new BlobCounter();
Stats blueStats = new Stats();
bc.MinWidth = 100;
bc.FilterBlobs = true;
bc.ObjectsOrder = ObjectsOrder.Size;
bc.ProcessImage(Filtered);
Rectangle[] rects = bc.GetObjectsRectangles();
//Graphics g = Graphics.FromImage(Filtered);
//UpdateVideo(Filtered);
if ((rects.Length > 0) && (rects[0].Height > 0))
{
// blueStats.BlueLineDetected = true;
blueStats.BlueLineDistance = (11.0 / rects[0].Height);
}
else
{
blueStats.BlueLineDetected = false;
}
return blueStats;
}
protected void ExtractFeatrures(Bitmap[] filtered)
{
Stats temp = new Stats();
Stats toMap = new Stats();
temp = ExtractRedFeatures(filtered[RED]);
toMap.RedBlockDetected = temp.RedBlockDetected;
toMap.RedBlockCenterLocation = temp.RedBlockCenterLocation;
toMap.RedBlockDistance = temp.RedBlockDistance;
toMap.RedBlockHeight = temp.RedBlockHeight;
toMap.RedBlockWidth = temp.RedBlockWidth;
temp = ExtractGreenFeatures(filtered[GREEN]);
toMap.GreenBlockDetected = temp.GreenBlockDetected;
toMap.GreenBlockCenterLocation = temp.GreenBlockCenterLocation;
toMap.GreenBlockDistance = temp.GreenBlockDistance;
toMap.GreenBlockHeight = temp.GreenBlockHeight;
toMap.GreenBlockWidth = temp.GreenBlockWidth;
temp = ExtractYellowFeatures(filtered[YELLOW]);
toMap.YellowWallDetected = temp.YellowWallDetected;
toMap.IsNorth = temp.IsNorth;
toMap.IsSouth = temp.IsSouth;
temp = ExtractBlueFeatures(filtered[BLUE]);
toMap.BlueLineDetected = temp.BlueLineDetected;
toMap.BlueLineDistance = temp.BlueLineDistance;
Mapping.queue.Add(toMap);
}
//functions
public void process()
{
while (true)
{
FeatureExtracting.Notifier.WaitOne();
foreach (Bitmap image in queue.GetConsumingEnumerable())
{
BlobCounter bc = new BlobCounter();
bc.MinWidth = 5;
bc.MinHeight = 5;
bc.FilterBlobs = true;
bc.ObjectsOrder = ObjectsOrder.Size;
bc.ProcessImage(image);
Rectangle[] rects = bc.GetObjectsRectangles();
Rectangle biggest = new Rectangle(0, 0, 0, 0);
Graphics g = Graphics.FromImage(image);
//double ratio = 0;
foreach (Blob blob in bc.GetObjectsInformation())
{
List<IntPoint> edgePoints = bc.GetBlobsEdgePoints(blob);
List<IntPoint> top;
List<IntPoint> bottom;
bc.GetBlobsTopAndBottomEdges(blob, out top, out bottom);
}
foreach (Rectangle r in rects)
{
biggest = rects[0];
g.DrawRectangle(new Pen(Color.Green, 3), r);
}
int objectCeter = 0;
if (biggest.Width > 70)
{
objectCeter = (((biggest.Width / 2) + biggest.X) - image.Width / 2);
}
g.DrawRectangle(new Pen(Color.Blue), biggest);
if (biggest.Height < 20)
{
}
string drawString = biggest.Height + " <-- Height Width --> " + biggest.Width + "\n Image Center = " + objectCeter;
System.Drawing.Font drawFont = new System.Drawing.Font("Arial", 8);
System.Drawing.SolidBrush drawBrush = new System.Drawing.SolidBrush(System.Drawing.Color.White);
float x = 10.0F;
float y = 10.0F;
System.Drawing.StringFormat drawFormat = new System.Drawing.StringFormat();
g.DrawString(drawString, drawFont, drawBrush, x, y, drawFormat);
drawFont.Dispose();
drawBrush.Dispose();
//Merge a = new Merge(image);
// Program.mainForm.VideoViewer.Image = a.Apply(crop.Apply(this.Camera.Image));
Stats a = new Stats();
a.blobCount = 0;
ActionPlanning.queue.Add(a);
ActionPlanning.Notifier.Set();
#if DEBUG
Program.mainForm.VideoViewer.Image = image;
#endif
}
}
}