public List<Sphere> FindSpheres(RangeImage rangeData)
{
int maxLp = 0;
List<Sphere> sphereList = new List<Sphere>();
do {
dataProcess(rangeData);
subsTol = subsTol + .05;
maxLp++;
} while (groupNum <= 6 && maxLp < 9);
for (int i = 1; i < groupNum; i++)
{
if (spBl[i] == 1)
{
Sphere updtSphere = new Sphere(spCr[i],(float).11777);
sphereList.Add(updtSphere);
}
}
return sphereList;
}
//PAREI AQUIIIIIIIIII
/**
* @brief Prepare the spheres to be drawn on the MapCanvas
* @param actual A Sphere
* @param add A bool
* @returns spherePosition A PointF
*/
private PointF prepareSphere(Sphere actual, bool add)
{
PointF spherePosition;
double globalAngle, realAngle, radius, hypo;
spherePosition = new PointF();
hypo = Math.Sqrt(Math.Pow(actual.absolutePosition.X * 100, 2) + Math.Pow(actual.absolutePosition.Z * 100, 2)) / 100;
//Angle of the spheres relative to the MotherBot
realAngle = Math.Asin(actual.absolutePosition.X / actual.absolutePosition.Z);
//Angle of the robot relative to map
if (m_Robot != null)
globalAngle = (m_Robot.Telemetry.Heading - 90) * deg2rad;
else
globalAngle = (angleee - 90) * deg2rad;
spherePosition.X = (float)(Math.Cos(globalAngle + realAngle) * hypo) - actual.realRadius / 2;
spherePosition.Y = (float)(Math.Sin(globalAngle + realAngle) * hypo) - actual.realRadius / 2;
if (add)
{
spherePositions.Add(spherePosition);
sphereRadii.Add(actual.realRadius);
}
return spherePosition;
}
//Returns a list of Sphere objects
//Parameters:
// RangeImage object
// Array containing the edges of the image with an edge being a "1" and no-edge "0"
// THIS CAN BE NULL
// Maximum Radius
// Minimum Radius
// Edge threshold (see detectEdges method)
// Percentile, which percentaje of the possible spheres will be discarded (based on their score)
/**
* @brief Returns a list of Sphere objects
* @param rangeData A RangeImage Object
* @param edges A float[,] - Array containing the edges of the image with an edge being a "1" and no-edge "0". THIS CAN BE NULL.
* @param maxRad An integer - Maximum Radius
* @param minRad An integer - Minimum Radius
* @param edgeThreshold A float - Edge threshold (see @sa detectEdges method)
* @param percentile A float - Which percentage of the possible spheres will be discarded (based on their score)
* @returns The found spheres
*/
public List<Sphere> FindSpheres(RangeImage rangeData, float[,] edges, int maxRad, int minRad, float edgeThreshold, float percentile)
{
DebugTimer dt = new DebugTimer();
dt.debug("Find spheres!");
//Determine the range of radii that we will search for
int diffRadii = maxRad - minRad;
//Create the array that will hold the edge data
//Normal bi-dimensional array x,y
if (edges == null)
{
edges = new float[rangeData.Width + 1, rangeData.Height + 1];
edges = detectEdges(rangeData, edgeThreshold);
//edges = detectEdgesRestricted(spheres, float.Parse(edgeTop.Text), float.Parse(edgeBottom.Text));
}
//Initialize the array that will hold the accumulation data
//First two dimensions are the are a x,y plane for the accumulation
//Third dimension is the radius for which that accumulation is done
//Forh dimension is only 2 items long:
// -The first one is the accumulation value
// -The second is an average of all the distances (z) to the point
// in the edge array that projects a circle over this particular
// x,y,k(radius) point
accumulation = new float[rangeData.Width + 1, rangeData.Height + 1, diffRadii, 4];
//Initialize an array for recording the local MAX and MIN from each accumulation for
//all the different radii. The first dimension of the array is the computed radius,
//and on the second dimension the item 0 is the minimum value, and the item 1 is the max.
float[,] localMaxMin = new float[maxRad - minRad, 2];
float tmpHeight = 0f;
dt.debug("Starting Hough transform algorithm...");
for (int i = 0; i < edges.GetLength(0); i++)
{
for (int j = 0; j < edges.GetLength(1); j++)
{
//If we have an edge in this point on the edges array, we proceed
if (edges[i, j] == 1)
{
//For every radius
for (int k = minRad; k < maxRad; k++)
{
//statusText.Text = "Generating circles (x,y,r)("+i+","+j+","+k+")";
for (int a = 0; a < 360; a += 5)
{
//Circle with 'k' radius to be traced on layer 'k'
//Points on the circumference
int x = (int)(i + Math.Sin(a * (Math.PI / 180)) * k);
int y = (int)(j + Math.Cos(a * (Math.PI / 180)) * k);
//If we are withing the bounds of the image
if (x > 0 && x < edges.GetLength(0) && y > 0 && y < edges.GetLength(1))
{
tmpHeight = accumulation[x, y, maxRad - k - 1, 0] + 1F;
//ACCUMULATE!
accumulation[x, y, maxRad - k - 1, 0] = tmpHeight;
//Update the average of distances of the circumference
accumulation[x, y, maxRad - k - 1, 1] += rangeData.GetCustom(i, j);
//Update the closest point in the circumference
if (rangeData.GetCustom(i, j) < accumulation[x, y, maxRad - k - 1, 2] || accumulation[x, y, maxRad - k - 1, 2] == 0)
accumulation[x, y, maxRad - k - 1, 2] = rangeData.GetCustom(i, j);
if (rangeData.GetCustom(i, j) > accumulation[x, y, maxRad - k - 1, 2])
accumulation[x, y, maxRad - k - 1, 3] = rangeData.GetCustom(i, j);
//accumulation[x, y, maxRad - k - 1, 1] = (accumulation[x, y, maxRad - k - 1, 1] + rangeData.GetCustom(i, j)) / 2;
//If we have a local (in the "layer", for a specific radius) MINIMUM we record it
if (tmpHeight < localMaxMin[maxRad - k - 1, 0] || localMaxMin[maxRad - k - 1, 0] == 0)
localMaxMin[maxRad - k - 1, 0] = tmpHeight;
//If we have a local (in the "layer", for a specific radius) MAXIMUM we record it
if (tmpHeight > localMaxMin[maxRad - k - 1, 1])
localMaxMin[maxRad - k - 1, 1] = tmpHeight;
}
}
}
}
}
}
dt.debugEnlapsed("Done accumulating...");
dt.debug("Starting tests...");
//Global list of positions, radius, accumulatedValues (SCORE), the average distance from center to midpoints, and average to edge points of spheres
List<Point> centers = new List<Point>();
List<int> radii = new List<int>();
List<float> accumulatedValues = new List<float>();
List<double> midPoints = new List<double>();
List<double> avgEdges = new List<double>();
List<double> avgDistances = new List<double>();
List<double> diffCircumferences = new List<double>();
//Initialize variables
float tmpPerc = 0;
Point tmpCenter = new Point(0, 0);
double avgDistance, distance, formula1, formula2, valN, valE, valS, valW, avgPoints, avgEdge, pointDifference, offset, diffCircumference;
float pointDifferenteThreshold = 0.03f;//0.02
float midPointDifferenteThreshold = 0.02f;//0.01
//DEBUG
List<String> trash = new List<String>();
for (int k = 0; k < accumulation.GetLength(2); k++)
{
//Calculate half the radius for sphere double cheching in line 163-179
offset = (maxRad - (float)k) / 2F;
for (int i = 0; i < rangeData.Width - 1; i++)
{
for (int j = 0; j < rangeData.Height - 1; j++)
{
//Calculate if the point is in the predefined percentile
tmpPerc = Math.Abs(accumulation[i, j, k, 0] - localMaxMin[k, 0]) / localMaxMin[k, 1];
//Average distance to the center of the sphere
avgDistance = (rangeData.GetCustom((i + 1 > rangeData.Width - 2) ? i : i + 1, j) + rangeData.GetCustom((i < 1) ? i + 1 : i - 1, j) + rangeData.GetCustom(i, (j + 1 > rangeData.Height - 2) ? j : j + 1) + rangeData.GetCustom(i, (j < 1) ? j + 1 : j - 1)) / 4f;
//Average edge distance
avgEdge = accumulation[i, j, k, 1] / accumulation[i, j, k, 0];
//Distance from center to edges
//(difference between average distance at the circumference, minus the center)
distance = avgEdge - avgDistance;
//Formula that approximates de excpected distance difference for a sphere of radius k
//formula1 = (7f / 1800f) * (maxRad - k) + (122f / 900f);
//Threshold for difference between expected and optimal
//float thresholdDistance = 0.2f;
//IF the point is in the predefined percentile and
//the distance from edges of the circumference and the center is possitive and
//the difference between the average distance at circumference minus the formula for k is
// bigger tan threshold
//if (tmpPerc > percentile / 100 && distance > 0 && (distance - formula1) < thresholdDistance)
//-Math.Min(0,(maxRad-k-15))
if (tmpPerc > (percentile / 100) || (tmpPerc >= (percentile / 100) - ((float)(maxRad - k) / 90)))
{
//Get distances of four points in the sphere surface
valE = rangeData.GetCustom((i - offset < 0) ? i : (int)(i - offset), j);
valS = rangeData.GetCustom((i + offset > rangeData.Width - 1) ? i : (int)(i + offset), j);
valW = rangeData.GetCustom(i, (j - offset < 0) ? j : (int)(j - offset));
valN = rangeData.GetCustom(i, (j + offset > rangeData.Height - 1) ? j : (int)(j + offset));
//Calculate the difference between the closest point, and the farther away
pointDifference = Math.Max(Math.Max(valE, valN), Math.Max(valS, valW)) - Math.Min(Math.Min(valE, valN), Math.Min(valS, valW));
//Average of all four points minus the distance to center
avgPoints = ((valE + valS + valW + valN) / 4);
//Formula for expected difference between center distance and distance of four points at k/2 for sphere with radius k
formula2 = ((17f / 9 * (float)(maxRad - k)) - 94f / 9f) / 1000f;
//Console.WriteLine("----------------\nSPHERE ID="+centers.Count+"\nradius="+(maxRad-k)+"\ndistance edges=" + accumulation[i, j, k, 1] + "\ndistanceAvg=" + avgDistance + "\n(distance edges)-distanceAvg=" + distance + "\n(Midpoint points avg distance)-avgDistance(center)=" + avgPoints + "\nNEWNUMBER***=" + formula2 + "\npointDifference=" + pointDifference+"\n--------------");
diffCircumference = accumulation[i, j, k, 3] - accumulation[i, j, k, 2];
//if (avgEdges > 1.6 && avgEdges < 1.9)
float diffToOptimal = Math.Abs(((float)(maxRad - k) / 800f) - (float)(avgPoints - avgDistance));
//if (pointDifference < 0.02d && avgPoints - formula2 > 0 && avgPoints - formula2 < 0.002d)
// && Math.Abs((avgPoints-avgDistance)-formula2)<0.1f
float tmptmp = (float)(avgEdge - (avgPoints + (float)(((float)(maxRad - k) * 2) / 100f)));
float tmptmptmp = (float)(maxRad - k) / 100;
double radiusMeters = (float)(maxRad - k) * (Math.Cos(89.75 * (Math.PI / 180)) * avgDistance);
float circumferenceThreshold = 0.1f;
float distanceCenterPointsThreshold = 0.005f;
//&& tmptmp < tmptmptmp//&& tmptmp < tmptmptmp
if (diffCircumference < circumferenceThreshold &&
avgPoints - avgDistance > distanceCenterPointsThreshold &&
pointDifference < pointDifferenteThreshold &&
diffToOptimal < midPointDifferenteThreshold &&
avgEdge > avgPoints && avgPoints > avgDistance &&
avgEdge - avgDistance > radiusMeters &&
avgEdge < avgDistance * 3 //&&
// Magic Number!
//avgEdge - avgDistance < radiusMeters * 3
)
{
//Console.WriteLine("----------------\nSPHERE ID=" + centers.Count + "\nradius=" + (maxRad - k) + "\ndistance edges=" + avgEdge + "\ndistanceAvg=" + avgDistance + "\n(distance edges)-distanceAvg=" + distance + "\n(Midpoint points avg distance)-avgDistance(center)=" + avgPoints + "\nNEWNUMBER***=" + formula2 + "\npointDifference=" + pointDifference + "\nSCORE=" + accumulation[i, j, k, 0] + "\n--------------");
//trash.Add("distance edges=" + accumulation[i, j, k, 1] + "\ndistanceAvg=" + avgDistance + "\ndiference=" + distance + "\ndiff2 - avgDistance=" + avgPoints + "\nNEWNUMBER***="+formula2+"\npointDifference=" + pointDifference+ "\nRESULT=" + (avgPoints - distance * Math.Sin(45 * (Math.PI / 180))));
//Add the values of this sphere to the global lists
centers.Add(new Point(i, j));
radii.Add(maxRad - k);
accumulatedValues.Add(accumulation[i, j, k, 0]);
midPoints.Add(avgPoints);
avgEdges.Add(avgEdge);
avgDistances.Add(avgDistance);
diffCircumferences.Add(diffCircumference);
}
}
}
}
}
dt.debugEnlapsed("Done with tests...");
List<Sphere> repeatedList = new List<Sphere>();
double deg2rad = Math.PI / 180;
double realRadius, theta, phi;
for (int i = 0; i < centers.Count; i++)
{
Vector3 absolutePosition = new Vector3();
//double theta = Math.Asin(((centers[i].X - rangeData.Width / 2) * Math.Cos(89.75 * deg2rad)) / avgDistances[i]);
//double phi = Math.Asin(((centers[i].Y - rangeData.Height / 2) * Math.Cos(89.75 * deg2rad)) / avgDistances[i]);
theta = Math.Asin((centers[i].X - rangeData.Width / 2) * Math.Cos(89.75 * deg2rad));
phi = Math.Asin((centers[i].Y - rangeData.Height / 2) * Math.Cos(89.75 * deg2rad));
absolutePosition.X = (float)(avgDistances[i] * Math.Cos(phi) * Math.Sin(theta));
absolutePosition.Y = (float)(avgDistances[i] * Math.Sin(phi) * Math.Sin(theta));
absolutePosition.Z = (float)(avgDistances[i] * Math.Cos(phi));
realRadius = avgDistances[i] * Math.Cos(89.75 * deg2rad) * radii[i];
Sphere a = new Sphere(absolutePosition, centers[i], radii[i], (float)realRadius, accumulatedValues[i], (float)avgEdges[i], (float)avgDistances[i], (float)midPoints[i], (float)diffCircumferences[i]);
repeatedList.Add(a);
}
List<Sphere> finalList = new List<Sphere>();
dt.debug("Start sphere merging algorithm...");
//Minimum dstance to consider that two centers are the same sphere
int minDistanceThreshold = 16;
//finalList = repeatedList;
/**/
List<Sphere> toMerge = new List<Sphere>();
IEnumerator<Sphere> enu = repeatedList.GetEnumerator();
IEnumerator<Sphere> enu2;
List<Sphere> tmpSpheres = new List<Sphere>(repeatedList);
int limit = repeatedList.Count - 1;
Sphere actual, test;
while (enu.MoveNext())
{
actual = enu.Current;
toMerge.Clear();
toMerge.Add(actual);
if (tmpSpheres.Contains(actual))
{
enu2 = tmpSpheres.GetEnumerator();
while (enu2.MoveNext())
{
test = enu2.Current;
if (actual != test && pointDistance(actual.center, test.center) < minDistanceThreshold)
toMerge.Add(test);
}
foreach (Sphere sph in toMerge)
tmpSpheres.Remove(sph);
if (toMerge.Count > 1)
finalList.Add(mergeSpheres(toMerge));
else
finalList.Add(actual);
}
}
dt.debugEnlapsed("Done with merging...");
/**/
List<Sphere> balancedFinalList = new List<Sphere>(finalList);
/**/
dt.debug("Finally filtering by score...");
foreach (Sphere sph in finalList)
{
if (sph.score < sph.radius * Math.PI)
balancedFinalList.Remove(sph);
}
/**/
dt.debugEnlapsed("Final list prepared.");
dt.debug("Returning list of spheres (" + balancedFinalList.Count + "). Done.");
//return repeatedList;
return balancedFinalList;
}