static void Main(string[] args)
{
NetworkTable.SetClientMode();
NetworkTable.SetTeam(4488);
NetworkTable.SetIPAddress("10.44.88.2");
#if KANGAROO
NetworkTable.SetNetworkIdentity("Kangaroo");
#else
NetworkTable.SetNetworkIdentity("CameraTracking");
#endif
//Switch between Kangaroo and Desktop.
//On kangaroo, use different table and don't display image
visionTable = NetworkTable.GetTable("SmartDashboard");
//ImageGrabber imageGrabber = new ImageGrabber(visionTable);
Mat HsvIn = new Mat(), HsvOut = new Mat(), output = new Mat(), Temp = new Mat();
VectorOfVectorOfPoint contours = new VectorOfVectorOfPoint();
//VectorOfVectorOfPoint filteredContours = new VectorOfVectorOfPoint();
//MCvScalar low = new MCvScalar(63, 44, 193);
//MCvScalar high = new MCvScalar(97, 255, 255);
double[] defaultLow = new double[] { 50, 44, 193 };
double[] defaultHigh = new double[] { 90, 255, 255 };
VectorOfDouble arrayLow = new VectorOfDouble(3);
VectorOfDouble arrayHigh = new VectorOfDouble(3);
Point TopMidPoint = new Point((int)(ImageWidth / 2), 0);
Point BottomMidPoint = new Point((int)(ImageWidth / 2), (int)ImageHeight);
Point LeftMidPoint = new Point(0, (int)(ImageHeight / 2));
Point RightMidPoint = new Point((int)ImageWidth, (int)(ImageHeight / 2));
Stopwatch sw = new Stopwatch();
CameraWatcher cameraChecker = new CameraWatcher();
//UsbManager2 cameraChecker = new UsbManager2();
//cameraChecker.startWatcher();
int count = 0;
//visionTable.PutNumberArray("HSVLow", defaultLow);
//visionTable.PutNumberArray("HSVHigh", defaultHigh);
//visionTable.PutNumber("ShooterOffsetDegreesX", ShooterOffsetDegreesX);
//visionTable.PutNumber("ShooterOffsetDegreesY", ShooterOffsetDegreesY);
Thread timer = new Thread(() =>
{
while (true)
{
// update kangaroo battery info
visionTable.PutNumber("KangarooBattery",
System.Windows.Forms.SystemInformation.PowerStatus.BatteryLifePercent);
// check camera status
int cameraState = cameraChecker.CheckState;
// camera states:
// 0 = Camera is found and working
// 1 = Camera is not found, waiting for reconnect to reinitialize
// 2 = Camera was found again, re-init was kicked off
visionTable.PutNumber("CameraState", cameraState);
if (cameraState == 0)
{
// Camera is connected and fine
//Console.WriteLine("Camera alive");
}
else if (cameraState == 1)
{
// Camera is disconnected or having problems
//Console.WriteLine("Camera dead, waiting for reconnect");
}
else if (cameraState == 2)
{
// Camera reconnected
//Console.WriteLine("Camera found again, reinitializing");
Process.Start("C:/Users/Shockwave/Desktop/NewKangaroo/cameraRestart.exe"); // Launch external exe to kill process, set up camera, and restart
}
Thread.Sleep(5000);
}
});
timer.Start();
GC.KeepAlive(timer);
int imageCount = 0;
ImageBuffer im = new ImageBuffer();
Capture cap = new Capture(0); //Change me to 1 to use external camera
cap.FlipVertical = true;
cap.SetCaptureProperty(Emgu.CV.CvEnum.CapProp.FrameWidth, 1280);
cap.SetCaptureProperty(Emgu.CV.CvEnum.CapProp.FrameHeight, 720);
ImageSaver saver = new ImageSaver();
//int saveCount = 0;
int rdi = 1;
int kernalSize = 6 * rdi + 1;
Size ksize = new Size(kernalSize, kernalSize);
while (true)
{
count++;
sw.Restart();
//ImageBuffer image = imageGrabber.Image();
cap.Grab();
im.GyroAngle = visionTable.GetNumber("Gyro", 0.0);
cap.Retrieve(im.Image);
ImageBuffer image = im.Clone();
#if KANGAROO
visionTable.PutNumber("KangarooHeartBeat", count);
#endif
if (image == null || image.IsEmpty)
{
image?.Dispose();
Thread.Yield();
continue;
}
/*
* // Image saver for debugging
* if (visionTable.GetBoolean("LightsOn", false))
* {
* saveCount++;
* if (saveCount >= 6)
* {
* saver.AddToQueue(image.Image);
* saveCount = 0;
* }
* }*/
double[] ntLow = visionTable.GetNumberArray("HSVLow", defaultLow);
double[] ntHigh = visionTable.GetNumberArray("HSVHigh", defaultHigh);
if (ntLow.Length != 3)
{
ntLow = defaultLow;
}
if (ntHigh.Length != 3)
{
ntHigh = defaultHigh;
}
arrayLow.Clear();
arrayLow.Push(ntLow);
arrayHigh.Clear();
arrayHigh.Push(ntHigh);
Mat BlurTemp = new Mat();
CvInvoke.GaussianBlur(image.Image, BlurTemp, ksize, rdi);
Mat oldImage = image.Image;
image.Image = BlurTemp;
oldImage.Dispose();
//HSV Filter
CvInvoke.CvtColor(image.Image, HsvIn, Emgu.CV.CvEnum.ColorConversion.Bgr2Hsv);
CvInvoke.InRange(HsvIn, arrayLow, arrayHigh, HsvOut);
HsvOut.ConvertTo(Temp, DepthType.Cv8U);
//Contours
CvInvoke.FindContours(Temp, contours, null, RetrType.List, ChainApproxMethod.ChainApproxTc89Kcos);
//CvInvoke.DrawContours(output, contours, -1, new MCvScalar(0, 0, 0));
VectorOfVectorOfPoint convexHulls = new VectorOfVectorOfPoint(contours.Size);
for (int i = 0; i < contours.Size; i++)
{
CvInvoke.ConvexHull(contours[i], convexHulls[i]);
}
Rectangle?largestRectangle = null;
double currentLargestArea = 0.0;
//Filter contours
for (int i = 0; i < convexHulls.Size; i++)
{
VectorOfPoint contour = convexHulls[i];
VectorOfPoint polygon = new VectorOfPoint(convexHulls.Size);
CvInvoke.ApproxPolyDP(contour, polygon, 10, true);
//VectorOfVectorOfPoint cont = new VectorOfVectorOfPoint(1);
//cont.Push(polygon);
//CvInvoke.DrawContours(image.Image, cont,-1, Green, 2);
// Filter if shape has more than 4 corners after contour is applied
if (polygon.Size != 4)
{
polygon.Dispose();
continue;
}
// Filter if not convex
if (!CvInvoke.IsContourConvex(polygon))
{
polygon.Dispose();
continue;
}
///////////////////////////////////////////////////////////////////////
// Filter if there isn't a nearly horizontal line
///////////////////////////////////////////////////////////////////////
//int numVertical = 0;
int numHorizontal = 0;
for (int j = 0; j < 4; j++)
{
double dx = polygon[j].X - polygon[(j + 1) % 4].X;
double dy = polygon[j].Y - polygon[(j + 1) % 4].Y;
double slope = double.MaxValue;
if (dx != 0)
{
slope = Math.Abs(dy / dx);
}
double nearlyHorizontalSlope = Math.Tan(ToRadians(20));
//double rad = ToRadians(60);
//double nearlyVerticalSlope = Math.Tan(rad);
//if (slope > nearlyVerticalSlope) numVertical++;
if (slope < nearlyHorizontalSlope)
{
numHorizontal++;
}
}
if (numHorizontal < 1)
{
polygon.Dispose();
continue;
}
///////////////////////////////////////////////////////////////////////
//CvInvoke.PutText(image.Image, "Number of horizontal (>=1): " + (numHorizontal).ToString(), TextPoint4, FontFace.HersheyPlain, 2, Green);
///////////////////////////////////////////////////////////////////////
// Filter if polygon is above a set limit. This should remove overhead lights and windows
///////////////////////////////////////////////////////////////////////
Rectangle bounds = CvInvoke.BoundingRectangle(polygon);
CvInvoke.PutText(image.Image, "Vertical (>=300): " + (bounds.Location.Y).ToString(), TextPoint, FontFace.HersheyPlain, 2, Green);
int topY = 300;
if (bounds.Location.Y < topY)
{
polygon.Dispose();
continue;
}
///////////////////////////////////////////////////////////////////////
CvInvoke.PutText(image.Image, "Image Height (45-115) and Width (65-225): " + bounds.Height.ToString() + " , " + bounds.Width, TextPoint2, FontFace.HersheyPlain, 2, Green);
///////////////////////////////////////////////////////////////////////
// Filter by minimum and maximum height
///////////////////////////////////////////////////////////////////////
if (bounds.Height < 45 || bounds.Height > 115)
{
polygon.Dispose();
continue;
}
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
// Filter by minimum and maximum width
///////////////////////////////////////////////////////////////////////
if (bounds.Width < 65 || bounds.Width > 225)
{
polygon.Dispose();
continue;
}
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
// Filter by height to width ratio
///////////////////////////////////////////////////////////////////////
double ratio = (double)bounds.Height / bounds.Width;
CvInvoke.PutText(image.Image, "Ratio: " + ratio.ToString(), TextPoint3, FontFace.HersheyPlain, 2, Green);
if (ratio > 1.0 || ratio < .3)
{
polygon.Dispose();
continue;
}
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
// Filter by area to vertical position ratio
///////////////////////////////////////////////////////////////////////
double area = CvInvoke.ContourArea(contour);
double areaVertRatio = area / (1280 - bounds.Location.Y);
CvInvoke.PutText(image.Image, "Area/Vert Ratio (8-19): " + areaVertRatio.ToString(), TextPoint4, FontFace.HersheyPlain, 2, Green);
if (areaVertRatio < 8 || areaVertRatio > 19)
{
polygon.Dispose();
continue;
}
///////////////////////////////////////////////////////////////////////
//CvInvoke.PutText(image.Image, "Area: " + area.ToString(), TextPoint2, FontFace.HersheyPlain, 2, Green);
CvInvoke.Rectangle(image.Image, bounds, Blue, 2);
if (area > currentLargestArea)
{
largestRectangle = bounds;
}
//filteredContours.Push(contour);
polygon.Dispose();
}
visionTable.PutBoolean("TargetFound", largestRectangle != null);
//CvInvoke.PutText(image.Image, "Target found: " + (largestRectangle != null).ToString(), TextPoint5, FontFace.HersheyPlain, 2, Green);
if (largestRectangle != null)
{
ProcessData(largestRectangle.Value, image);
CvInvoke.Rectangle(image.Image, largestRectangle.Value, Red, 5);
}
//ToDo, Draw Crosshairs
//CvInvoke.Line(image.Image, TopMidPoint, BottomMidPoint, Blue, 3);
//CvInvoke.Line(image.Image, LeftMidPoint, RightMidPoint, Blue, 3);
//int fps = (int)(1.0 / sw.Elapsed.TotalSeconds);
//CvInvoke.PutText(image.Image, fps.ToString(), TextPoint, FontFace.HersheyPlain, 2, Green);
imageCount++;
// Uncomment below to see the HSV window
//CvInvoke.Imshow("HSV", HsvOut);
// Uncomment below to see the main image window
CvInvoke.Imshow("MainWindow", image.Image);
image.Dispose();
//report to NetworkTables
//Cleanup
for (int i = 0; i < contours.Size; i++)
{
contours[i].Dispose();
}
contours.Clear();
for (int i = 0; i < convexHulls.Size; i++)
{
convexHulls[i].Dispose();
}
convexHulls.Dispose();
/*
* for (int i = 0; i < filteredContours.Size; i++)
* {
* filteredContours[i].Dispose();
* }
* filteredContours.Clear();
*/
CvInvoke.WaitKey(1);
}
;
}
