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;
        }
Exemple #2
0
        //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;
        }