/// <summary>
/// Clears the array arround the maximum.
/// </summary>
/// <param name="accumToChange">The accumulator to clear around</param>
/// <param name="rho">the rho value to clear around</param>
/// <param name="theta">the theta value to clear around</param>
/// <returns>the accumulator array that is cleared around</returns>
private short[,] ClearMaxInAccum(short[,] accumToChange, HoughLines line)
{
int rho = (int)(line.rho / this.RhoIncrement);
int theta = (int)(line.theta / this.ThetaIncrement);
int accumDim1 = accumToChange.GetLength(0); // Get the size accum Array
int accumDim2 = accumToChange.GetLength(1);
// Clear the one with the most votes
for (int degree = -ClearArroundMaxDegree / ThetaIncrement; degree < ClearArroundMaxDegree / ThetaIncrement; degree++)
{
for (int phi = -ClearArroundMaxRho / RhoIncrement; phi < ClearArroundMaxRho / RhoIncrement; phi++)
{
int thetaprime = (theta + degree) % accumDim1;
int rhoprime = (rho + phi) % accumDim2;
if (thetaprime > 0 && thetaprime < accumDim1 && rhoprime > 0 && rhoprime < accumDim2)
{
accumToChange[thetaprime, rhoprime] = 0;
}
}
}
return(accumToChange);
}
/// <summary>
/// Called when the class is initialized. Creates many of the arrays and sets many of the values.
/// </summary>
public override void Initialize()
{
this.totalWhiteCnt = 0;
// Create the arrays needed. Building them now will save CPU later.
this.clearPathTrueFalseMap = new bool[this.screenWidth, this.screenHeight];
this.trueFalseMapB = new bool[this.screenWidth, this.screenHeight];
this.findWhiteTrueFalseMap = new bool[this.screenWidth, this.screenHeight];
this.accum2 = new short[360 / ThetaIncrement, AccumLength / RhoIncrement]; // Build the accumlator array. Make is smaller or shorter based on the size of the rho and theta increments
this.accum1 = new short[360 / ThetaIncrement, AccumLengthOld / RhoIncrement];
this.colorArray1D = new Color[this.screenWidth * this.screenHeight]; // Create a 1D array of color
this.colorArrayDirectlyFromRobotCamera = new Color[this.screenWidth, this.screenHeight]; // Create a 2D array of color
//this.houghInfo = new double[(11 * numberOfLinesToFind) + 5]; // Make the array to store hough information. Must be double so that slopes which are fractions can be stored
// Set the color thresholds
this.redGood = ((SimulationMain)Game).config.whiteParam;
this.blueGood = ((SimulationMain)Game).config.whiteParam;
this.greenGood = ((SimulationMain)Game).config.whiteParam;
//this.houghLineStartandStopVectors = new Vector3[numberOfLinesToFind * 4];
// for (int i = 0; i < numberOfLinesToFind * 4; i++)
// {
// this.houghLineStartandStopVectors[i] = Vector3.Zero;
//}
this.middleValues = new int[this.screenHeight]; // Steering desisions are based off the average middle clear value for each row
// Creates the image drawing analysis object.
this.drawAnalysis = new DrawImageAnalysis(Game);
this.Game.Components.Add(this.drawAnalysis);
// Great a some Hough lines (the number that of lines we are trying to find)
for (int i = 0; i < numberOfLinesToFind; i++)
{
HoughLines houghLine = new HoughLines(Game);
this.Game.Components.Add(houghLine);
this.houghLineList.Add(houghLine);
}
this.debugText = new DebugText(Game);
this.debugText.imageAnalysisLinked = this;
this.Game.Components.Add(this.debugText);
this.lineErrorAnalysis = new LineErrorAnalysis(Game);
// base.Initialize();
}
/// <summary>
/// Finds Max value in Hough. Store information about that max.
/// </summary>
/// <param name="accumToAnalze">The accumlator of bins we want to search</param>
/// <param name="thetaIncrement">How large is the quantitization of the theta values. </param>
/// <param name="startIndexOfStoringHoughInfoList">What value in the Array 'HoughInfo' should we start storing information.</param>
private void FindMaxInAccumArrayOfHough(short[,] accumToAnalze, HoughLines houghLine)
{
int maxTheta = 1;
int maxRho = 1;
int maxAccum = 1;
int accumDim1 = accumToAnalze.GetLength(0);
int accumDim2 = accumToAnalze.GetLength(1);
// Run through to find cell with most votes
for (int s = 0; s < accumDim1; s++)
{
for (int t = 0; t < accumDim2; t++)
{
if (accumToAnalze[s, t] > maxAccum)
{
maxTheta = s;
maxRho = t;
maxAccum = accumToAnalze[s, t];
}
}
}
double slope1 = 1;
int yintercept1 = 0;
double x1 = 0;
double y1 = 0;
if (((SimulationMain)Game).config.currentHoughMode == New_HOUGH_MODE)
{
maxTheta *= this.ThetaIncrement ; // Scale the Theta back to real size.
maxRho = maxRho * RhoIncrement; // Scale the Rho back to real size.
x1 = (int)(maxRho * Math.Cos(MathHelper.ToRadians(maxTheta))); // Find the x Point corresponding the theta, rho
y1 = (int)(maxRho * Math.Sin(MathHelper.ToRadians(maxTheta))); // Find the y Point corresponding the theta, rho
if (x1 == 0)
{
x1 = 0.00001; // don't divide by zero
}
slope1 = Math.Round(Math.Tan(MathHelper.ToRadians(maxTheta)), 2); // Calculating the slope and round to 2 digits.
yintercept1 = (int)(y1 - (slope1 * x1));
// Store the information found about the line in the 'this.houghInfo' array starting at the value 'StartIndexOfStoringHoughInfoList'
houghLine.slope = slope1;
houghLine.yIntercept = yintercept1;
houghLine.rho = maxRho;
houghLine.theta = maxTheta;
houghLine.xValue = x1;
houghLine.yValue = y1;
houghLine.sizeOfBin = maxAccum;
}
if (this.currentMode == OLD_HOUGH_MODE)
{
maxTheta = (this.ThetaIncrement * maxTheta) - 180; // Scale the Theta back to real size.
maxRho = maxRho * RhoIncrement; // Scale the Rho back to real size.
x1 = (int)(maxRho * Math.Cos(MathHelper.ToRadians(maxTheta))); // Find the x Point corresponding the theta, rho
y1 = (int)(maxRho * Math.Sin(MathHelper.ToRadians(maxTheta))); // Find the y Point corresponding the theta, rho
if (x1 == 0)
{
x1 = 0.00001; // don't divide by zero
}
slope1 = Math.Round(Math.Tan(MathHelper.ToRadians(maxTheta)), 2); // Calculating the slope and round to 2 digits.
yintercept1 = (int)(y1 - (slope1 * x1));
///{(x^3 - b x y + a y^2 + x y^2)/(x^2 + y^2), x^2/y + y - (x (x^3 - b x y + a y^2 + x y^2))/(y (x^2 + y^2))}
int distance = 0;
int angle = 0;
int a = this.screenWidth / 2;
int b = this.screenHeight;
//// double xTran = (int)((x1 * x1 * x1 - b * x1 * y1 + a * y1 * y1 + x1 * y1 * y1) / (x1 * x1 + y1 * y1));
double xTransformed = (int)((Math.Pow(x1, 3) - b * x1 * y1 + a * Math.Pow(y1, 2) + x1 * Math.Pow(y1, 2)) / (Math.Pow(x1, 2) + Math.Pow(y1, 2)));
//// xTran = xTran - a;
////double yTran = (int)(x1 * x1 / y1 + y1 - (x1 * (x1 * x1 * x1 - b * x1 * y1 + a * y1 * y1 + x1 * y1 * y1)) / (y1 * (x1 * x1 + y1 * y1)));
double yTransformed = (int)(Math.Pow(x1, 2) / y1 + y1 - (x1 * (Math.Pow(x1, 3) - b * x1 * y1 + a * Math.Pow(y1, 2) + x1 * Math.Pow(y1, 2))) / (y1 * (Math.Pow(x1, 2) + Math.Pow(y1, 2))));
distance = (int)Math.Sqrt(Math.Pow(xTransformed - a, 2) + Math.Pow(b - yTransformed, 2));
angle = (int)MathHelper.ToDegrees((float)Math.Atan((b - yTransformed) / (xTransformed - a)));
if (angle < 0)
{
angle += 180;
}
// Store the information found about the line in the 'this.houghInfo' array starting at the value 'StartIndexOfStoringHoughInfoList'
houghLine.slope = slope1;
houghLine.yIntercept = yintercept1;
houghLine.rho = maxRho;
houghLine.theta = maxTheta + 180; // Stay in the range of the array.
houghLine.xValue = x1;
houghLine.yValue = y1;
houghLine.sizeOfBin = maxAccum;
houghLine.xTransformedValue = xTransformed;
houghLine.yTransformedValue = yTransformed;
houghLine.distanceToLine = distance; // distance to line.
houghLine.angleToLine = angle;// angle to line.
}
}
/// <summary>
/// Clears the array arround the maximum.
/// </summary>
/// <param name="accumToChange">The accumulator to clear around</param>
/// <param name="rho">the rho value to clear around</param>
/// <param name="theta">the theta value to clear around</param>
/// <returns>the accumulator array that is cleared around</returns>
private short[,] ClearMaxInAccum(short[,] accumToChange, HoughLines line)
{
int rho =(int)( line.rho / this.RhoIncrement);
int theta =(int)( line.theta / this.ThetaIncrement);
int accumDim1 = accumToChange.GetLength(0); // Get the size accum Array
int accumDim2 = accumToChange.GetLength(1);
// Clear the one with the most votes
for (int degree = -ClearArroundMaxDegree / ThetaIncrement; degree < ClearArroundMaxDegree / ThetaIncrement; degree++)
{
for (int phi = -ClearArroundMaxRho / RhoIncrement; phi < ClearArroundMaxRho / RhoIncrement; phi++)
{
int thetaprime = (theta + degree) % accumDim1;
int rhoprime = (rho + phi) % accumDim2;
if (thetaprime > 0 && thetaprime < accumDim1 && rhoprime > 0 && rhoprime < accumDim2)
{
accumToChange[thetaprime, rhoprime] = 0;
}
}
}
return accumToChange;
}
/// <summary>
/// Part of the old Hough system. Finds the edge values on the screen of the lines based on the slope and yInt.
/// </summary>
/// <param name="line">The line we are dealing with</param>
private void CalculateStartandStopofLine( HoughLines line)
{
int startX = 0;
int startY = 0;
int endX = 0;
int endY = 0;
int yIntReal = 0;
double slopeReal = 0;
double x1 = line.xValue;
double y1 = line.yValue;
double slope1 = line.slope;
if (this.currentMode == New_HOUGH_MODE)
{
slopeReal = -1 / slope1; // Slope of the actual line-- not the slope of the line perpendicular (which is what the hough found)
yIntReal = (int)(y1 - (slopeReal * (x1 + this.screenWidth / 2))); // y-intercept of the actual line-- not the slope of the line perpendicular (which is what the hough found)
// So far all the calculations assume origin is in bottom center. Now based off that information find the cordinates of where to start and stop
// the hough lines in screen cordinates. Down is positive y in screen cordinates.
// Left Side
if (yIntReal >= 0 && yIntReal < this.screenHeight)
{
startX = 0;
startY = this.screenHeight - yIntReal;
}
else if (yIntReal <= 0)
{
startX = -(int)(yIntReal / slopeReal);
startY = this.screenHeight;
}
else if (yIntReal > this.screenHeight)
{
startX = (int)((this.screenHeight - yIntReal) / slopeReal);
startY = 0;
}
// Find the end cordinates of the line.
// Right Side
int yright = (int)((slopeReal * this.screenWidth) + yIntReal);
if (yright > 0 && yright < this.screenHeight)
{
endX = this.screenWidth;
endY = this.screenHeight - yright;
}
else if (yright < 0)
{
endX = (int)(-yIntReal / slopeReal);
endY = this.screenHeight;
}
else
if (yright > this.screenHeight)
{
endX = (int)((this.screenHeight - yIntReal) / slopeReal);
endY = 0;
}
}
if (this.currentMode == OLD_HOUGH_MODE)
{
// remember that origin is top left corner and down is positive y
slopeReal = -1 / -slope1;
yIntReal = (int)(-y1 - (slopeReal * x1));
if (yIntReal >= 0)
{
startX = (int)(-yIntReal / slopeReal);
startY = 0;
}
if (yIntReal <= 0 && yIntReal > -this.screenHeight)
{
startX = 0;
startY = -(int)yIntReal;
}
if (yIntReal < -this.screenHeight)
{
startX = (int)((-this.screenHeight - yIntReal) / slopeReal);
startY = this.screenHeight;
}
// Find the end cordinates of the line.
// Right Side
int yright = (int)((slopeReal * this.screenWidth) + yIntReal);
if (yright > 0)
{
endX = (int)(-yIntReal / slopeReal);
endY = 0;
}
if (yright < 0 && yright > -this.screenHeight)
{
endX = this.screenWidth;
endY = -yright;
}
if (yright < -this.screenHeight)
{
endX = (int)((-this.screenHeight - yIntReal) / slopeReal);
endY = this.screenHeight;
}
}
// Store the Line information in the array
line.houghStartVector = new Vector3(startX, startY, 0);
line.houghEndVector = new Vector3(endX, endY, 0);
}
/// <summary>
/// Called when the class is initialized. Creates many of the arrays and sets many of the values.
/// </summary>
public override void Initialize()
{
this.totalWhiteCnt = 0;
// Create the arrays needed. Building them now will save CPU later.
this.clearPathTrueFalseMap = new bool[this.screenWidth, this.screenHeight];
this.trueFalseMapB = new bool[this.screenWidth, this.screenHeight];
this.findWhiteTrueFalseMap = new bool[this.screenWidth, this.screenHeight];
this.accum2 = new short[360 / ThetaIncrement, AccumLength / RhoIncrement]; // Build the accumlator array. Make is smaller or shorter based on the size of the rho and theta increments
this.accum1 = new short[360 / ThetaIncrement, AccumLengthOld / RhoIncrement];
this.colorArray1D = new Color[this.screenWidth * this.screenHeight]; // Create a 1D array of color
this.colorArrayDirectlyFromRobotCamera = new Color[this.screenWidth, this.screenHeight]; // Create a 2D array of color
//this.houghInfo = new double[(11 * numberOfLinesToFind) + 5]; // Make the array to store hough information. Must be double so that slopes which are fractions can be stored
// Set the color thresholds
this.redGood = ((SimulationMain)Game).config.whiteParam;
this.blueGood = ((SimulationMain)Game).config.whiteParam;
this.greenGood = ((SimulationMain)Game).config.whiteParam;
//this.houghLineStartandStopVectors = new Vector3[numberOfLinesToFind * 4];
// for (int i = 0; i < numberOfLinesToFind * 4; i++)
// {
// this.houghLineStartandStopVectors[i] = Vector3.Zero;
//}
this.middleValues = new int[this.screenHeight]; // Steering desisions are based off the average middle clear value for each row
// Creates the image drawing analysis object.
this.drawAnalysis = new DrawImageAnalysis(Game);
this.Game.Components.Add(this.drawAnalysis);
// Great a some Hough lines (the number that of lines we are trying to find)
for (int i = 0; i < numberOfLinesToFind; i++)
{
HoughLines houghLine = new HoughLines(Game);
this.Game.Components.Add(houghLine);
this.houghLineList.Add(houghLine);
}
this.debugText = new DebugText(Game);
this.debugText.imageAnalysisLinked = this;
this.Game.Components.Add(this.debugText);
this.lineErrorAnalysis = new LineErrorAnalysis(Game);
// base.Initialize();
}
/// <summary>
/// Part of the old Hough system. Finds the edge values on the screen of the lines based on the slope and yInt.
/// </summary>
/// <param name="line">The line we are dealing with</param>
private void CalculateStartandStopofLine(HoughLines line)
{
int startX = 0;
int startY = 0;
int endX = 0;
int endY = 0;
int yIntReal = 0;
double slopeReal = 0;
double x1 = line.xValue;
double y1 = line.yValue;
double slope1 = line.slope;
if (this.currentMode == New_HOUGH_MODE)
{
slopeReal = -1 / slope1; // Slope of the actual line-- not the slope of the line perpendicular (which is what the hough found)
yIntReal = (int)(y1 - (slopeReal * (x1 + this.screenWidth / 2))); // y-intercept of the actual line-- not the slope of the line perpendicular (which is what the hough found)
// So far all the calculations assume origin is in bottom center. Now based off that information find the cordinates of where to start and stop
// the hough lines in screen cordinates. Down is positive y in screen cordinates.
// Left Side
if (yIntReal >= 0 && yIntReal < this.screenHeight)
{
startX = 0;
startY = this.screenHeight - yIntReal;
}
else if (yIntReal <= 0)
{
startX = -(int)(yIntReal / slopeReal);
startY = this.screenHeight;
}
else if (yIntReal > this.screenHeight)
{
startX = (int)((this.screenHeight - yIntReal) / slopeReal);
startY = 0;
}
// Find the end cordinates of the line.
// Right Side
int yright = (int)((slopeReal * this.screenWidth) + yIntReal);
if (yright > 0 && yright < this.screenHeight)
{
endX = this.screenWidth;
endY = this.screenHeight - yright;
}
else if (yright < 0)
{
endX = (int)(-yIntReal / slopeReal);
endY = this.screenHeight;
}
else
if (yright > this.screenHeight)
{
endX = (int)((this.screenHeight - yIntReal) / slopeReal);
endY = 0;
}
}
if (this.currentMode == OLD_HOUGH_MODE)
{
// remember that origin is top left corner and down is positive y
slopeReal = -1 / -slope1;
yIntReal = (int)(-y1 - (slopeReal * x1));
if (yIntReal >= 0)
{
startX = (int)(-yIntReal / slopeReal);
startY = 0;
}
if (yIntReal <= 0 && yIntReal > -this.screenHeight)
{
startX = 0;
startY = -(int)yIntReal;
}
if (yIntReal < -this.screenHeight)
{
startX = (int)((-this.screenHeight - yIntReal) / slopeReal);
startY = this.screenHeight;
}
// Find the end cordinates of the line.
// Right Side
int yright = (int)((slopeReal * this.screenWidth) + yIntReal);
if (yright > 0)
{
endX = (int)(-yIntReal / slopeReal);
endY = 0;
}
if (yright < 0 && yright > -this.screenHeight)
{
endX = this.screenWidth;
endY = -yright;
}
if (yright < -this.screenHeight)
{
endX = (int)((-this.screenHeight - yIntReal) / slopeReal);
endY = this.screenHeight;
}
}
// Store the Line information in the array
line.houghStartVector = new Vector3(startX, startY, 0);
line.houghEndVector = new Vector3(endX, endY, 0);
}
/// <summary>
/// Finds Max value in Hough. Store information about that max.
/// </summary>
/// <param name="accumToAnalze">The accumlator of bins we want to search</param>
/// <param name="thetaIncrement">How large is the quantitization of the theta values. </param>
/// <param name="startIndexOfStoringHoughInfoList">What value in the Array 'HoughInfo' should we start storing information.</param>
private void FindMaxInAccumArrayOfHough(short[,] accumToAnalze, HoughLines houghLine)
{
int maxTheta = 1;
int maxRho = 1;
int maxAccum = 1;
int accumDim1 = accumToAnalze.GetLength(0);
int accumDim2 = accumToAnalze.GetLength(1);
// Run through to find cell with most votes
for (int s = 0; s < accumDim1; s++)
{
for (int t = 0; t < accumDim2; t++)
{
if (accumToAnalze[s, t] > maxAccum)
{
maxTheta = s;
maxRho = t;
maxAccum = accumToAnalze[s, t];
}
}
}
double slope1 = 1;
int yintercept1 = 0;
double x1 = 0;
double y1 = 0;
if (((SimulationMain)Game).config.currentHoughMode == New_HOUGH_MODE)
{
maxTheta *= this.ThetaIncrement; // Scale the Theta back to real size.
maxRho = maxRho * RhoIncrement; // Scale the Rho back to real size.
x1 = (int)(maxRho * Math.Cos(MathHelper.ToRadians(maxTheta))); // Find the x Point corresponding the theta, rho
y1 = (int)(maxRho * Math.Sin(MathHelper.ToRadians(maxTheta))); // Find the y Point corresponding the theta, rho
if (x1 == 0)
{
x1 = 0.00001; // don't divide by zero
}
slope1 = Math.Round(Math.Tan(MathHelper.ToRadians(maxTheta)), 2); // Calculating the slope and round to 2 digits.
yintercept1 = (int)(y1 - (slope1 * x1));
// Store the information found about the line in the 'this.houghInfo' array starting at the value 'StartIndexOfStoringHoughInfoList'
houghLine.slope = slope1;
houghLine.yIntercept = yintercept1;
houghLine.rho = maxRho;
houghLine.theta = maxTheta;
houghLine.xValue = x1;
houghLine.yValue = y1;
houghLine.sizeOfBin = maxAccum;
}
if (this.currentMode == OLD_HOUGH_MODE)
{
maxTheta = (this.ThetaIncrement * maxTheta) - 180; // Scale the Theta back to real size.
maxRho = maxRho * RhoIncrement; // Scale the Rho back to real size.
x1 = (int)(maxRho * Math.Cos(MathHelper.ToRadians(maxTheta))); // Find the x Point corresponding the theta, rho
y1 = (int)(maxRho * Math.Sin(MathHelper.ToRadians(maxTheta))); // Find the y Point corresponding the theta, rho
if (x1 == 0)
{
x1 = 0.00001; // don't divide by zero
}
slope1 = Math.Round(Math.Tan(MathHelper.ToRadians(maxTheta)), 2); // Calculating the slope and round to 2 digits.
yintercept1 = (int)(y1 - (slope1 * x1));
///{(x^3 - b x y + a y^2 + x y^2)/(x^2 + y^2), x^2/y + y - (x (x^3 - b x y + a y^2 + x y^2))/(y (x^2 + y^2))}
int distance = 0;
int angle = 0;
int a = this.screenWidth / 2;
int b = this.screenHeight;
//// double xTran = (int)((x1 * x1 * x1 - b * x1 * y1 + a * y1 * y1 + x1 * y1 * y1) / (x1 * x1 + y1 * y1));
double xTransformed = (int)((Math.Pow(x1, 3) - b * x1 * y1 + a * Math.Pow(y1, 2) + x1 * Math.Pow(y1, 2)) / (Math.Pow(x1, 2) + Math.Pow(y1, 2)));
//// xTran = xTran - a;
////double yTran = (int)(x1 * x1 / y1 + y1 - (x1 * (x1 * x1 * x1 - b * x1 * y1 + a * y1 * y1 + x1 * y1 * y1)) / (y1 * (x1 * x1 + y1 * y1)));
double yTransformed = (int)(Math.Pow(x1, 2) / y1 + y1 - (x1 * (Math.Pow(x1, 3) - b * x1 * y1 + a * Math.Pow(y1, 2) + x1 * Math.Pow(y1, 2))) / (y1 * (Math.Pow(x1, 2) + Math.Pow(y1, 2))));
distance = (int)Math.Sqrt(Math.Pow(xTransformed - a, 2) + Math.Pow(b - yTransformed, 2));
angle = (int)MathHelper.ToDegrees((float)Math.Atan((b - yTransformed) / (xTransformed - a)));
if (angle < 0)
{
angle += 180;
}
// Store the information found about the line in the 'this.houghInfo' array starting at the value 'StartIndexOfStoringHoughInfoList'
houghLine.slope = slope1;
houghLine.yIntercept = yintercept1;
houghLine.rho = maxRho;
houghLine.theta = maxTheta + 180; // Stay in the range of the array.
houghLine.xValue = x1;
houghLine.yValue = y1;
houghLine.sizeOfBin = maxAccum;
houghLine.xTransformedValue = xTransformed;
houghLine.yTransformedValue = yTransformed;
houghLine.distanceToLine = distance; // distance to line.
houghLine.angleToLine = angle; // angle to line.
}
}
