/// <summary> /// /// </summary> /// <param name="src"> /// Image where the image where tablets are /// </param> /// <param name="tablets"> /// location of tabletsin the src image /// </param> private void DetectGoodPickupTablets(Image<Bgr, Byte> src, CircleF[] tablets) { //Tablet tab = new Tablet(); //List<Tablet> TabletLists = new List<Tablet>(); List<CircleF> TabletsInList = new List<CircleF>(); Image<Bgr, Byte> drawTab = src.Clone(); var tabletHSV = new List<Tuple<int[][], int [][], int[][]>>(tablets.Length); for (int i = 0; i < tablets.Length; i++) { var tablet = tablets[i]; Tablet tab = new Tablet(); var histo = ImagesToHisto(GetTablet(src, tablet)); int[][] hue = getHighLowHSV(histo, HSVDetectionThreshold, HSVdata.Hue); int[][] sat = getHighLowHSV(histo, HSVDetectionThreshold, HSVdata.Sat); int[][] val = getHighLowHSV(histo, HSVDetectionThreshold, HSVdata.Val); tabletHSV.Add(new Tuple<int[][], int[][], int[][]>(hue, sat, val)); if (FirstPass(hue, sat, val, tablet, tablets, HSVTabletcolorsRanges) == true) { drawTab.Draw(tablet, new Bgr(Color.Green), 6); TabletsInList.Add(tablet); } else { drawTab.Draw(tablet, new Bgr(Color.White), 2); #if true //Debug.WriteLine("Hue: " + hue[0][0] + " - " + hue[0][1] + ", Sat: " + sat[0][0] + " - " + sat[0][1] + ", Val: " + val[0][0] + " - " + val[0][1]); Debug.WriteLine("Na: \t" + hue[0][0] + "\t" + hue[0][1] + "\t" + sat[0][0] + "\t" + sat[0][1] + "\t" + val[0][0] + "\t" + val[0][1]); int hueM = hue.GetLength(0) - 1; int satM = sat.GetLength(0) - 1; int valM = val.GetLength(0) - 1; //Debug.WriteLine("Hue: " + hue[hueM][0] + " - " + hue[hueM][1] + ", Sat: " + sat[satM][0] + " - " + sat[satM][1] + ", Val: " + val[valM][0] + " - " + val[valM][1]); Debug.WriteLine("Na: \t" + hue[hueM][0] + "\t" + hue[hueM][1] + "\t" + sat[satM][0] + "\t" + sat[satM][1] + "\t" + val[valM][0] + "\t" + val[valM][1]); Debug.Flush(); #endif } } for (int i = 0; i < tablets.Length; i++) { var tablet = tablets[i]; int[][] hue = tabletHSV[i].Item1; int[][] sat = tabletHSV[i].Item2; int[][] val = tabletHSV[i].Item3; int a = 0; if (IsVisableTablet(src, tablet, 5) == true) { foreach (CircleF tabl in TabletsInList) { if (checkCircles(tabl, tablet)) { a++; } } if (a > 0) { continue; } if (FirstPass(hue, sat, val, tablet, tablets, HSVTabletcolorsRanges) == true) { drawTab.Draw(tablet, new Bgr(Color.Blue), 6); TabletsInList.Add(tablet); } else { drawTab.Draw(tablet, new Bgr(Color.Red), 4); TabletsInList.Add(tablet); } } //CvInvoke.cvWaitKey(0); } foreach (CircleF tablet in TabletsInList) { Tablet tab = new Tablet(); if ((IsVisableTablet(src, tablet, 5) == true) || (OtherTabletsNear(TabletsInList.ToArray(), tablet) == true)) { float[][] abca = ImagesToHisto(GetTablet(src, tablet)); //b++; int[][] hue = getHighLowHSV(abca, HSVDetectionThreshold, HSVdata.Hue); int[][] sat = getHighLowHSV(abca, HSVDetectionThreshold, HSVdata.Sat); int[][] val = getHighLowHSV(abca, HSVDetectionThreshold, HSVdata.Val); if (FirstPass(hue, sat, val, tablet, tablets, HSVTabletcolorsRanges) == true) { tab.LocationPoint = CalculateTrueCordXYmm(ChessboardPoints, new PointF(tablet.Center.X, tablet.Center.Y)); tab.Color = detectcolor(new Hsv((hue[0][0] + hue[0][1]) / 2, (sat[0][0] + sat[0][1]) / 2, (val[0][0] + val[0][1]) / 2), HSVTabletcolorsRanges); if ((IsVisableTablet(src, tablet, 8) == true)) {//if it's right color and we can detect it's on top pop to start of the list TabletList.Insert(0, tab); } else {//otherwise add to the end TabletList.Add(tab); } drawTab.Draw(tablet, new Bgr(Color.Red), 2); #if DEBUG //Debug.WriteLine("Hue: " + hue[0][0] + " - " + hue[0][1] + ", Sat: " + sat[0][0] + " - " + sat[0][1] + ", Val: " + val[0][0] + " - " + val[0][1]); Debug.WriteLine("Good: \t" + hue[0][0] + "\t" + hue[0][1] + "\t" + sat[0][0] + "\t" + sat[0][1] + "\t" + val[0][0] + "\t" + val[0][1]); int hueM = hue.GetLength(0) - 1; int satM = sat.GetLength(0) - 1; int valM = val.GetLength(0) - 1; //Debug.WriteLine("Hue: " + hue[hueM][0] + " - " + hue[hueM][1] + ", Sat: " + sat[satM][0] + " - " + sat[satM][1] + ", Val: " + val[valM][0] + " - " + val[valM][1]); Debug.WriteLine("Good: \t" + hue[hueM][0] + "\t" + hue[hueM][1] + "\t" + sat[satM][0] + "\t" + sat[satM][1] + "\t" + val[valM][0] + "\t" + val[valM][1]); Debug.Flush(); #endif } else {//unknown tablet tab.LocationPoint = CalculateTrueCordXYmm(ChessboardPoints, new PointF(tablet.Center.X, tablet.Center.Y)); tab.Color = TabletColors.Unknown; TabletList.Add(tab); drawTab.Draw(tablet, new Bgr(Color.Red), 5); drawTab.Draw(tablet, new Bgr(Color.Blue), 2); #if DEBUG //Debug.WriteLine("Hue: " + hue[0][0] + " - " + hue[0][1] + ", Sat: " + sat[0][0] + " - " + sat[0][1] + ", Val: " + val[0][0] + " - " + val[0][1]); Debug.WriteLine("Bad: \t" + hue[0][0] + "\t" + hue[0][1] + "\t" + sat[0][0] + "\t" + sat[0][1] + "\t" + val[0][0] + "\t" + val[0][1]); int hueM = hue.GetLength(0) - 1; int satM = sat.GetLength(0) - 1; int valM = val.GetLength(0) - 1; //Debug.WriteLine("Hue: " + hue[hueM][0] + " - " + hue[hueM][1] + ", Sat: " + sat[satM][0] + " - " + sat[satM][1] + ", Val: " + val[valM][0] + " - " + val[valM][1]); Debug.WriteLine("Bad: \t" + hue[hueM][0] + "\t" + hue[hueM][1] + "\t" + sat[satM][0] + "\t" + sat[satM][1] + "\t" + val[valM][0] + "\t" + val[valM][1]); Debug.Flush(); #endif } } } saveImage(drawTab, "sorter tablets.jpg"); }
private void PlaceTablet(Tablet tablet, TabletMagazine mag) { var p = TransformToRobotSpace(tablet.LocationPoint); Logger.Instance.Write(String.Format("[Sorter] Picking ({0}) tablet from ({1},{2}) in robot space", tablet.Color, p.X, p.Y)); _robot.GetTablet(p.X, p.Y, mag.GetSlotIndex(tablet.Color)); mag.AddTablet(tablet.Color); }
public string colToString(Tablet tablet) { if (tablet == null) return "none"; else return tablet.Color.ToString(); }