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;
        }
Beispiel #2
0
 public HeightMap(RangeImage RangeImage)
 {
     m_RangeImage = RangeImage;
        occ_count = -1;
        GRID_WIDTH_Z = 0;
        GRID_WIDTH_Z = 0;
 }
        //Detect the Edges!
        //Returns a bi-dimensional array sized as RangeImage width and height + 1
        //with an "1" for every pixel (x,y) where an edge is detected
        //Parameters
        // RangeImage object, double with threshold to determine "layers" in the picture
        //                   (How separated the object has to be from the background
        //                    to be considered an object and not part of the background)
        /**
         * @brief Detect edges:
         * @param threshold A float
         * @param rangeData A RangeImage object - @brief Double with threshold to determine "layers" in the picture
         *                                        @brief (How separated the object has to be from the background
         *                                        @brief to be considered an object and not part of the background)
         * @returns edgesGen - A float[,]
         * @brief A bi-dimensional array sized as RangeImage width and height + 1
         */
        public float[,] detectEdges(RangeImage rangeData, float threshold)
        {
            int cont = 0;
            float[,] edgesGen = new float[rangeData.Width + 1, rangeData.Height + 1];
            for (int i = 2; i < rangeData.Width - 3; i++)
            {
                for (int j = 2; j < rangeData.Height - 3; j++)
                {
                    //Calculate neighboring pixels
                    float wZ = rangeData.GetCustom(i, j - 1);
                    float eZ = rangeData.GetCustom(i, j + 1);
                    float nZ = rangeData.GetCustom(i - 1, j);
                    float sZ = rangeData.GetCustom(i + 1, j);

                    double diffX = Math.Abs(wZ - eZ);
                    double diffY = Math.Abs(nZ - sZ);

                    //If we have a match and its not neighboring other edges, we mark it as edge
                    if ((diffX > threshold * rangeData.GetCustom(i, j) && edgesGen[i, j - 1] == 0 && edgesGen[i, j - 2] == 0) || (diffY > threshold * rangeData.GetCustom(i, j) && edgesGen[i - 1, j] == 0 && edgesGen[i - 2, j] == 0))
                    //if ((diffX > threshold && edgesGen[i, j - 1] == 0 && edgesGen[i, j - 2] == 0) || (diffY > threshold && edgesGen[i - 1, j] == 0 && edgesGen[i - 2, j] == 0))
                    //if ((diffX > threshold && edgesGen[i, j - 1] == 0) || (diffY > threshold && edgesGen[i - 1, j] == 0))
                    //if (diffX > threshold || diffY > threshold)
                    {
                        //Console.WriteLine("Edge at: ("+i+","+j+")");
                        cont++;
                        edgesGen[i, j] = 1;
                    }
                }

            }
            return edgesGen;
        }
        public void StartRecognition(RangeImage rangeImage)
        {
            rangeData = rangeImage;
            height = rangeImage.Height;
            width = rangeImage.Width;

            bm = new Bitmap(width, height);
            g = Graphics.FromImage(bm);
            image.BackgroundImage = bm;
            image.BackgroundImageLayout = ImageLayout.None;

            image.Size = new Size(width, height);

            g.Clear(Color.White);

            SphereRecognition sr = new SphereRecognition();

            //System.Windows.Forms.MessageBox.Show("W/H = " + width + "/" + height);

            image.Bounds = new Rectangle(0, 0, width, height);

            paintImage();

            edges = sr.detectEdges(rangeData, float.Parse(conf1.Text));

            //doFix();

            /**/
            paintEdges();

            List<Sphere> spheres = sr.FindSpheres(rangeData, edges, int.Parse(maxRadius.Text), int.Parse(minRadius.Text), float.Parse(conf1.Text), float.Parse(conf2.Text));

            Sphere actual;
            for (int i = 0; i < spheres.Count; i++)
            {
                actual = spheres[i];
                //int tmp = actual.center.X;
                actual.center.Y = height - actual.center.Y;
                //actual.center.Y = tmp;
                //actual.center.X += (int)(actual.radius*1.35f);
                g.DrawEllipse(Pens.Blue, (int)actual.center.X - actual.radius, ((int)actual.center.Y - actual.radius), (int)actual.radius * 2, (int)actual.radius * 2);
                g.DrawLine(Pens.Blue, (actual.center.X - 1), (actual.center.Y - 1) - 5, (actual.center.X - 1), (actual.center.Y - 1) + 5);
                g.DrawLine(Pens.Blue, (actual.center.X - 1) - 5, (actual.center.Y - 1), (actual.center.X - 1) + 5, (actual.center.Y - 1));
                image.Refresh();
                //g.DrawEllipse(Pens.Blue, (int)actual.center.Y - actual.radius, ((int)actual.center.X - actual.radius), (int)actual.radius * 2, (int)actual.radius * 2);
                //g.DrawLine(Pens.Blue, (actual.center.Y - 1), (actual.center.X - 1) - 5, (actual.center.Y - 1), (actual.center.X - 1) + 5);
                //g.DrawLine(Pens.Blue, (actual.center.Y - 1) - 5, (actual.center.X - 1), (actual.center.Y - 1) + 5, (actual.center.X - 1));
                float formula2 = ((17f / 9 * (float)actual.radius) - 94f / 9f) / 1000f;
                String algo = "haha " + actual.radius * (Math.Cos(89.9 * (Math.PI / 180)) * actual.avgDistance) + "\n" + (actual.midPoints - actual.avgDistance) + "\n" + Math.Abs(actual.midPoints - actual.avgDistance) + "\n" + formula2 + "\n" + (Math.Abs(actual.midPoints - actual.avgDistance) - formula2);
                //System.Windows.Forms.MessageBox.Show("ABSOLUTE=\nX="+actual.absolutePosition.X+"\nY="+actual.absolutePosition.Y+"\nZ="+actual.absolutePosition.Z+"\n\nx=" + actual.center.X + "\ny=" + actual.center.Y + "\n\nR=" + actual.radius + "\nDiffC=" + actual.diffCircumference + "\nS=" + actual.score + "\nAVGD=" + actual.avgDistance + "\nAVGM=" + actual.midPoints + "\nAVGE=" + actual.avgEdges + "\n\n\n" + algo);
                //paintImage();
                //paintEdges();
            }
            /**/
        }
        private void StartRecognition(String filename)
        {
            rangeData = new RangeImage();
            String extension = filename.Substring(filename.LastIndexOf(".")+1).ToLower();

            switch (extension)
            {
                case "rangeimage":
                    rangeData = RangeImage.Read(filename);
                    width = rangeData.Width;
                    height = rangeData.Height;
                    button2.Enabled = true;
                break;

                case "scan":
                    FileStream f = File.Open(filename, FileMode.Open);
                    BinaryReader br = new BinaryReader(f);
                    width = br.ReadInt32();
                    height = br.ReadInt32();
                    rangeData.setWidth(width);
                    rangeData.setHeight(height);

                    if (rangeData.initializeRangeValues(width, height))
                    {

                        for (int i = 0; i < height * width; i++)
                        {
                            try
                            {
                                rangeData.addValue(i, br.ReadSingle());
                            }
                            catch (Exception E) { }
                        }
                        //System.Windows.Forms.MessageBox.Show("W/H = " + width+"/"+height);
                        filename = filename.Replace(".scan", ".rangeimage");
                        rangeData.Write(filename);

                        /*if (value > max)
                                max = value;
                            if (value < min || min == 0)
                                min = value;

                            int val = (int)Math.Min(255, Math.Abs((value / 75f) * 255f));
                            SolidBrush sb = new SolidBrush(Color.FromArgb(255, val, val, val));
                            g.FillRectangle(sb, i/width, i%width, 1, 1);
                             */
                        //System.Windows.Forms.MessageBox.Show("avg" + avg/(height*width));
                        //System.Windows.Forms.MessageBox.Show("min" + min);
                        //System.Windows.Forms.MessageBox.Show("max" + max);
                        f.Close();
                    }
                    button2.Enabled = true;
                break;

                default:
                    System.Windows.Forms.MessageBox.Show("The format of the data is not recognized");
                    return;
                break;
            }

            StartRecognition(rangeData);

            /*
            TextWriter tw = new StreamWriter("c:\\GROUP.txt");

            for(int i=0;i<width*height;i++)
            {
                tw.WriteLine(i/width);
                tw.WriteLine(i % width);
                tw.WriteLine(rangeData.Get(i));
            }
            */
        }
Beispiel #6
0
        private void LoadCommand_Click(object sender, EventArgs e)
        {
            if (OpenFileDialog.ShowDialog() == DialogResult.OK)
            {
                m_RangeImage = RangeImage.Read(OpenFileDialog.FileName);
                InitRangeImage();
                GenerateFlatImage();
                Canvas.Invalidate();

            }
        }
        private void dataProcess(RangeImage rangeData)
        {
            int inc = 0;
            numP = 0;
            groupNum = 0;

            TextWriter nw = new StreamWriter("spTesting.txt");
            //nw.WriteLine(subsTol);

            //Load data into a matrixv and initialize variables

            Vector3[] pointLine = rangeData.ConvertToPoints(float.MaxValue);

            for (int i = 0; i<VERTICAL_RESOLUTION; i++) {
                for (int j = 0; j<HORIZONTAL_RESOLUTION; j++) {
                    points[i,j] = pointLine[inc];
                    inc++;

                    //nw.WriteLine(pointLine[inc].X);
                    //nw.WriteLine(pointLine[inc].Y);
                    //nw.WriteLine(pointLine[inc].Z);

                    gpNm[i,j] = 0; //Initializes the group number for clustering
                    if (Math.Sqrt(Math.Pow(points[i,j].X,2)+Math.Pow(points[i,j].Y,2)+Math.Pow(points[i,j].Z,2)) < 30.0) {
                        spPt[i,j] = 0; //Assign the value 0 to all the points (for sphere detection)
                    } else {
                        spPt[i,j] = 5;
                    }

                }
            }

            //preProcess data to filter points that can be potential spheres

            int iPr = 0;
            int jPr = 0;

            for (int i=0;i<VERTICAL_RESOLUTION;i++) {
                for (int j=1;j<HORIZONTAL_RESOLUTION;j++) {
                    if (spPt[i,j] != 5) {
                        if (iPr != i) {
                            iPr = i;
                            jPr = 0;
                        }
                        if (Math.Sqrt(Math.Pow(points[i,j].X-points[iPr,jPr].X,2)+Math.Pow(points[i,j].Y-points[iPr,jPr].Y,2)+Math.Pow(points[i,j].Z-points[iPr,jPr].Z,2)) > 2.0*.117775 || jPr == 0) {
                            spPt[i,j] = 1;
                            spPt[iPr,jPr] = 1;

                            //nw.WriteLine(points[i,j].X);
                            //nw.WriteLine(points[i,j].Y);
                            //nw.WriteLine(points[i,j].Z);
                        }
                        if (Math.Sqrt(Math.Pow(points[i,j].X-points[i,j-1].X,2)+Math.Pow(points[i,j].Y-points[i,j-1].Y,2)+Math.Pow(points[i,j].Z-points[i,j-1].Z,2)) > 2.0*.117775) {
                            iPr = i;
                            jPr = j;
                        }
                        if (j==400 || j==401 || j==0 || j== 1) {
                            spPt[i,j] = 1;
                        }
                    }
                } // End of "j loop"
            } //End of "i loop"

            //Process data and find points that belong to spheres. Use the sphere equation to process each point and its neighbors

            double rad = .11777;
            double mag = 0.0;
            double[] nrm = {0.0,0.0,1.0};
            int sum = 0;
            int cond = 0;
            int maxSum = 0;
            int iMax = 0;
            int jMax = 0;
            double[] center=new double[3];
            double tol = .001;
            double z = 0.0;
            double ptAng; //Angle used to obtain resolution
            int ptRes; //Point resolution
            double newPtRes;
            int newim;
            int newjn;

            for (int i=0;i<VERTICAL_RESOLUTION;i++) {
                for (int j=10;j<HORIZONTAL_RESOLUTION;j++) {
                    sum = 0;
                    cond = 0;

                    mag = Math.Sqrt(Math.Pow(points[i,j].X,2)+Math.Pow(points[i,j].Y,2)+Math.Pow(points[i,j].Z,2));
                    nrm[0] = points[i,j].X/mag;
                    nrm[1] = points[i,j].Y/mag;
                    nrm[2] = points[i,j].Z/mag;

                    //nw.WriteLine(nrm[0]);
                    //nw.WriteLine(nrm[1]);
                    //nw.WriteLine(nrm[2]);

                    if (spPt[i,j] == 0) {
                        center[0] = points[i,j].X + rad*nrm[0];
                        center[1] = points[i,j].Y + rad*nrm[1];
                        center[2] = points[i,j].Z + rad*nrm[2];

                        //nw.WriteLine(center[0]);
                        //nw.WriteLine(center[1]);
                        //nw.WriteLine(center[2]);

                        //Obtaining point resolution based on distance to point
                        ptAng = Math.Asin(.09/Math.Sqrt(Math.Pow(points[i,j].X,2)+Math.Pow(points[i,j].Y,2)+Math.Pow(points[i,j].Z,2)));
                        ptRes = (int)Math.Floor(ptAng/.0043633);

                        //nw.WriteLine(ptAng);
                        //nw.WriteLine(ptRes);

                        for (int m=-ptRes;m<ptRes;m++) {
                            for (int n=-ptRes;n<ptRes;n++) {
                                newim = i + m;
                                newjn = j + n;
                                if (newim < 0) { newim = 0; }
                                if (newjn < 0) { newjn = 0; }
                                if (newim > 359) { newim = 359; }
                                if (newjn > 399) { newjn = 399; }

                                if (Math.Sqrt(Math.Pow(points[newim,newjn].X-points[i,j].X,2)+Math.Pow(points[newim,newjn].Y-points[i,j].Y,2)+Math.Pow(points[newim,newjn].Z-points[i,j].Z,2)) < rad) {
                                    if (Math.Pow(center[2]-points[newim,newjn].Z,2)+Math.Pow(points[newim,newjn].X-center[0],2)+Math.Pow(points[newim,newjn].Y-center[1],2)-Math.Pow(rad,2) < tol) {
                                        sum++;
                                    }
                                }

                            }//End of 'n loop'
                        }//End of 'm loop'

                        newPtRes = Math.Pow(2.0*(double)ptRes,2) - subsTol*Math.Pow(2.0*(double)ptRes,2);

                        if (newPtRes > 2.0) {

                            //nw.WriteLine(sum);

                            if ((double)sum > newPtRes && Math.Sqrt(Math.Pow(points[i,j].X,2)+Math.Pow(points[i,j].Y,2)+Math.Pow(points[i,j].Z,2)) < 7) {
                                spPt[i,j] = 3;

                                //nw.WriteLine(points[i,j].X);
                                //nw.WriteLine(points[i,j].Y);
                                //nw.WriteLine(points[i,j].Z);

                            }
                        }

                    }
                } //End of 'i loop'
            } //End of 'j loop'

            //Code to postprocess data

            groupNum = 1;
            int mMin = 0;
            int nMin = 0;
            double dis;
            double minDis = 10000.0;

            for (int i = 0; i < VERTICAL_RESOLUTION; i++)
            {
                for (int j = 0; j < HORIZONTAL_RESOLUTION; j++)
                {
                    if (spPt[i,j] == 3)
                    {
                        for (int m = 0; m < VERTICAL_RESOLUTION; m++)
                        {
                            for (int n = 0; n < HORIZONTAL_RESOLUTION; n++)
                            {
                                if (spPt[m,n] == 3)
                                {
                                    dis = Math.Sqrt(Math.Pow(points[i,j].X - points[m,n].X, 2) + Math.Pow(points[i,j].Y - points[m,n].Y, 2) + Math.Pow(points[i,j].Z - points[m,n].Z, 2));
                                    //Look at distance between points

                                    if (dis < .117775 && dis > 0)
                                    {
                                        if (gpNm[i,j] > 0)
                                        {
                                            gpNm[m,n] = gpNm[i,j];
                                        }
                                        if (gpNm[m,n] > 0)
                                        {
                                            gpNm[i,j] = gpNm[m,n];
                                        }
                                        if (gpNm[i,j] == 0 && gpNm[m,n] == 0)
                                        {
                                            gpNm[i,j] = groupNum;
                                            gpNm[m,n] = groupNum;
                                            groupNum++;
                                        }

                                    }

                                }
                            }//End of "m loop"
                        } //End of "n loop"

                    }
                } //End of "j loop"
            }//End of "i loop"

            //Assign group number to single points that are not part of a cluster

            for (int i = 0; i < VERTICAL_RESOLUTION; i++)
            {
                for (int j = 0; j < HORIZONTAL_RESOLUTION; j++)
                {
                    if (spPt[i,j] == 3 && gpNm[i,j] == 0)
                    {
                        gpNm[i,j] = groupNum;
                        groupNum++;
                    }
                }
            }

            //Find the centers of the clusters
               // TextWriter nw = new StreamWriter("spTesting.txt");

            double[] nrm2 = new double[3];
            double mag2 = 0;
            //double rad = .11;

            double[] sum2 = new double[100];

            for (int i=0;i<100;i++) {
                spCr[i].X = 0;
                spCr[i].Y = 0;
                spCr[i].Z = 0;
                sum2[i] = 0;
            }

            for (int i=0;i<VERTICAL_RESOLUTION;i++) {
                for (int j=0;j<HORIZONTAL_RESOLUTION;j++) {
                    if (gpNm[i,j] > 0) {
                        spCr[gpNm[i,j]].X = (spCr[gpNm[i,j]].X + points[i,j].X);
                        spCr[gpNm[i,j]].Y = (spCr[gpNm[i,j]].Y + points[i,j].Y);
                        spCr[gpNm[i,j]].Z = (spCr[gpNm[i,j]].Z + points[i,j].Z);
                        sum2[gpNm[i,j]] = sum2[gpNm[i,j]] + 1;
                    }
                }
            }

            //Obtain average center of point clusters
            for (int i=0;i<groupNum;i++) {
                spCr[i].X = spCr[i].X/(float)(sum2[i]);
                spCr[i].Y = spCr[i].Y/(float)(sum2[i]);
                spCr[i].Z = spCr[i].Z/(float)(sum2[i]);

                mag2 = Math.Sqrt(Math.Pow(spCr[i].X,2)+Math.Pow(spCr[i].Y,2)+Math.Pow(spCr[i].Z,2));
                nrm2[0] = spCr[i].X/mag2;
                nrm2[1] = spCr[i].Y/mag2;
                nrm2[2] = spCr[i].Z/mag2;

                spCr[i].X = spCr[i].X + (float)(rad*nrm2[0]);
                spCr[i].Y = spCr[i].Y + (float)(rad*nrm2[1]);
                spCr[i].Z = spCr[i].Z + (float)(rad*nrm2[2]);

                nw.WriteLine(spCr[i].X);
                nw.WriteLine(spCr[i].Y);
                nw.WriteLine(spCr[i].Z);

            }

            nw.Close();

            //Obtain the centers of the spheres
            dis = 0;

            for (int i=0;i<100;i++) {
                spBl[i] = 0;
            }

            for (int i=0;i<groupNum;i++) {
                for (int j=0;j<groupNum;j++) {
                    dis = Math.Sqrt(Math.Pow(spCr[i].X-spCr[j].Y,2)+Math.Pow(spCr[i].Y-spCr[j].Y,2)+Math.Pow(spCr[i].Z-spCr[j].Z,2));
                    if (dis > .35 && dis < .55) {
                        spBl[i] = 1;
                        spBl[j] = 1;
                    }
                }
            }
        }
Beispiel #8
0
        private void UpdateRangeImage()
        {
            if (m_Robot.LatestRangeImage != null && m_Robot.LatestRangeImage != m_CurrentRangeImage)
              {
            //if (m_RangeImageViewer != null)
            //      m_RangeImageViewer.RangeImage = m_Robot.LatestRangeImage;
            //if (m_Navigation != null)
            //      m_Navigation.Map = m_Robot.Map;
            if (m_Control != null)
            {
            m_Control.Map = m_Robot.Map;
            m_Control.processRangeImage();
            }

            m_CurrentRangeImage = m_Robot.LatestRangeImage;
              }
        }
Beispiel #9
0
        private void toolStripButton12_Click(object sender, EventArgs e)
        {
            if (openFileDialog1.ShowDialog() == DialogResult.OK)
            {
                m_RangeImage = RangeImage.Read(openFileDialog1.FileName);
                InitRangeImage();
                GenerateFlatImage();
                Canvas.Invalidate();

            }
        }
Beispiel #10
0
        /**
         * @brief Reads the given reader.
         * \exception "InvalidOperationException" @brief Thrown when the requested operation is invalid (The RangeImage version is not recognized).
         * @param Reader A BinaryReader
         * @returns A RangeImage
        */
        public static RangeImage Read(BinaryReader Reader)
        {
            Reader.BaseStream.Position = 0;
              uint Version = Reader.ReadUInt32();
              RangeImage I = new RangeImage();
              switch (Version)
              {
              case 0x0104:

              //RangeImage I = new RangeImage();

              I.m_Width = Reader.ReadInt32();
              I.m_Height = Reader.ReadInt32();

              I.m_ScannerLocation = new PointF(Reader.ReadSingle(), Reader.ReadSingle());
              I.m_ScannerPrimaryAxis = new PointF(Reader.ReadSingle(), Reader.ReadSingle());

              I.m_HorizontalFieldOfView = Reader.ReadSingle();
              I.m_VerticalFieldOfView = Reader.ReadSingle();
              long l = Reader.ReadInt64();
              I.m_AquiredOn = DateTime.Now;
              I.m_SequenceNumber = Reader.ReadInt32();

              I.m_Range = Reader.ReadSingle();

              break;

              case 0x0200:

              //RangeImage I = new RangeImage();
              Int32 length = Reader.ReadInt32();
              Int32 bytesRead = 8;
              float locX=0.0f, locY=0.0f;
              float oriX=0.0f, oriY=0.0f;
              float temp;

              while (bytesRead < length)
              {
                  Byte key = Reader.ReadByte();
                  bytesRead++;
                  switch (key)
                  {
                      case WIDTH_KEY:
                          I.m_Width = Reader.ReadInt32();
                          bytesRead += 4;
                          break;
                      case HEIGHT_KEY:
                          I.m_Height = Reader.ReadInt32();
                          bytesRead += 4;
                          break;
                      case POSITION_X_KEY:
                          temp = Reader.ReadSingle();
                          bytesRead += 4;
                          I.m_ScannerLocation = new PointF(temp, locY);
                          break;
                      case POSITION_Y_KEY:
                          temp = Reader.ReadSingle();
                          bytesRead += 4;
                          I.m_ScannerLocation = new PointF(locX, temp);
                          break;
                      case ORIENTATION_X_KEY:
                          temp = Reader.ReadSingle();
                          I.m_ScannerPrimaryAxis = new PointF(oriX, temp);
                          bytesRead += 4;
                          break;
                      case ORIENTATION_Y_KEY:
                          temp = Reader.ReadSingle();
                          I.m_ScannerPrimaryAxis = new PointF(temp, oriY);
                          bytesRead += 4;
                          break;
                      case HORIZONTAL_FOV_KEY:
                          I.m_HorizontalFieldOfView = Reader.ReadSingle();
                          bytesRead += 4;
                          break;
                      case VERTICAL_FOV_KEY:
                          I.m_VerticalFieldOfView = Reader.ReadSingle();
                          bytesRead += 4;
                          break;
                      case ACQUISITION_TIME_KEY:
                          // XXX: should probably actually use this data
                          long discarded = Reader.ReadInt64();
                          bytesRead+=8;
                          I.m_AquiredOn = DateTime.Now;
                          break;
                      case SEQUENCE_NUMBER_KEY:
                          I.m_SequenceNumber = Reader.ReadInt32();
                          bytesRead += 4;
                          break;
                      case MAX_RANGE_VALUE_KEY:
                          I.m_Range = Reader.ReadSingle();
                          bytesRead += 4;
                          break;
                      case VERTICAL_BIAS_KEY:
                          I.m_VerticalBias = Reader.ReadSingle();
                          bytesRead += 4;
                          break;
                      case HORIZONTAL_BIAS_KEY:
                          I.m_HorizontalBias = Reader.ReadSingle();
                          bytesRead += 4;
                          break;
                  }
              }
              break;

              default:
              throw new InvalidOperationException("The RangeImage version is not recognized");
              }
              I.m_RangeValues = new float[I.m_Width * I.m_Height];

              for (int i = 0; i < I.m_RangeValues.Length; i++)
              I.m_RangeValues[i] = Reader.ReadSingle();
              return I;
        }
Beispiel #11
0
        //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;
        }