/// <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();
}
