/// <summary> /// Get the image not currently being written to /// </summary> /// <returns></returns> public ImageBuffer Image() { lock (m_mutex) { if (!m_updated) { return(null); } m_updated = false; return(m_switch ? m_buf1.Clone() : m_buf2.Clone()); } }
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); } ; }