/// <summary>
/// Create a 3X3 grid of paper colors using a source image and the
/// contours for each paper color.
/// </summary>
/// <param name="contourImage">Source image.</param>
/// <param name="yellowContours">Yellow paper contours.</param>
/// <param name="greenContours">Green paper contours.</param>
/// <returns></returns>
public Paper[,] detectColumnPaperColors(Image<Bgr, byte> contourImage, List<Contour<Point>> yellowContours, List<Contour<Point>> greenContours)
{
Paper[,] papers = new Paper[3,3];
for (int i = 0; i < papers.GetLength(0); i++)
{
for(int j = 0; j < papers.GetLength(1); j++)
{
papers[i, j] = new Paper();
papers[i, j].Color = PaperColor.UNKNOWN;
}
}
detectColumns(contourImage, papers, yellowContours, PaperColor.YELLOW);
detectColumns(contourImage, papers, greenContours, PaperColor.GREEN);
return papers;
}
/// <summary>
/// Check what side of the robot the columns are on and
/// sets the direction used throughout the state machine.
/// See <see cref="State.CHECK_DIRECTION"/>.
/// </summary>
/// <param name="papers">Current Paper Grid</param>
/// <returns><see cref="State.COLUMN_ANGLE_CORRECTION"/></returns>
private State checkDirection(Paper[,] papers)
{
int leftPaperCount = 0;
int rightPaperCount = 0;
for(int i = 0; i < papers.GetLength(0); i++)
{
for(int j = 0; j < papers.GetLength(1); j++)
{
if(papers[i,j].Color != PaperColor.UNKNOWN)
{
if(j == 0)
{
leftPaperCount++;
} else if(j == 2)
{
rightPaperCount++;
}
}
}
if (leftPaperCount > rightPaperCount)
{
this.direction = Direction.LEFT;
}
else
{
this.direction = Direction.RIGHT;
}
}
return State.COLUMN_ANGLE_CORRECTION;
}
/// <summary>
/// Calculates the digit represented by all of the
/// paper color columns. The detected digit is sent
/// to the UI using
/// <see cref="IDigitDetectionCallback.DigitDetected(int, PaperColor[])"/>.
/// </summary>
/// <param name="papers">Current Paper Grid</param>
/// <returns><see cref="State.STEER_720_DEGREES"/></returns>
private State calculateDigit(Paper[,] papers)
{
// The columns are read top to bottom; however, the digit needs to
// be bottom to top, which translate into left to right when the
// digit is compared.
for (int i = 0; i < this.paperColumns.Count; i++)
{
PaperColor temp = this.paperColumns[i][0];
this.paperColumns[i][0] = this.paperColumns[i][2];
this.paperColumns[i][2] = temp;
}
if (this.direction == Direction.LEFT)
{
this.paperColumns.Reverse();
}
// Convert paper color columns to an array
PaperColor[] paperColors = new PaperColor[this.paperColumns.Count * 3];
for (int i = 0; i < paperColumns.Count; i++)
{
for(int j = 0; j < 3; j++)
{
paperColors[i * papers.GetLength(0) + j] = this.paperColumns[i][j];
}
}
int matchNumber = 0;
int matchCount = 0;
// Find the digit that is the closest match. The idea behind
// this algorithm is that the closest match is the digit with
// the most papers in common with the digit represented by
// the paper viewed by the robot. In the event that the robot
// is unable to read a sheet of paper, this algorithm assumes
// that it would have been a match.
for(int i = 0; i < this.digitsToMatch.GetLength(0); i++)
{
int currentMatchCount = 0;
for(int j = 0; j < this.digitsToMatch.GetLength(1); j++)
{
if(paperColors[j] == PaperColor.UNKNOWN)
{
currentMatchCount++;
} else if(paperColors[j] == this.digitsToMatch[i, j])
{
currentMatchCount++;
}
}
if(currentMatchCount > matchCount)
{
matchNumber = i;
matchCount = currentMatchCount;
}
}
this.callback.DigitDetected(matchNumber, paperColors);
return State.STEER_720_DEGREES;
}
