Exemplo n.º 1
0
        static void FindContours(ref Mat result, ref Mat image, ref string resultstring)
        {
            //Подгружаем размеры робота из соответствующего xml файла
            NXTProperties nxt = new NXTProperties();
            XmlSerializer xmlNXTSerializer = new XmlSerializer(typeof(NXTProperties));
            FileStream    xmlNXT           = new FileStream("NXT.xml", FileMode.Open);

            nxt = (NXTProperties)xmlNXTSerializer.Deserialize(xmlNXT);
            xmlNXT.Close();

            //resultstring += String.Format("{0}*{1}", robodrom.length, nxt.LCDlength);

            Point[][]        contours;
            HierarchyIndex[] hierarchyIndexes;
            Cv2.FindContours(result, out contours, out hierarchyIndexes, RetrievalModes.Tree, ContourApproximationModes.ApproxNone);

            RotatedRect[] rectangles = new RotatedRect[contours.Length];

            Point2f[] centerOfBigRectangle = new Point2f[contours.Length];
            Point2f[] centerOfSmallRectangle = new Point2f[contours.Length];
            int       bigRectangles = 0, smallRectangles = 0;

            for (int i = 0; i < contours.Length; i++)
            {
                rectangles[i] = Cv2.MinAreaRect(contours[i]);
                int boxLength = (int)Math.Max(rectangles[i].Size.Height, rectangles[i].Size.Width);
                int boxWidth  = (int)Math.Min(rectangles[i].Size.Height, rectangles[i].Size.Width);

                //Ищем контур NXT блока
                if (boxLength > 30 && boxLength < 45 && boxWidth > 25 && boxWidth < 40)
                {
                    centerOfBigRectangle[bigRectangles] = rectangles[i].Center;
                    bigRectangles++;
                }

                //Ищем контур экрана NXT блока
                if (boxLength > 5 && boxLength < 20 && boxWidth > 5 && boxWidth < 15)
                {
                    centerOfSmallRectangle[smallRectangles] = rectangles[i].Center;
                    smallRectangles++;
                }
            }

            int    numOfRobots = 0;
            double prevxBigRectangle = 0;
            double prevyBigRectangle = 0;

            //Определяем правильность и угол
            for (int i = 0; i < bigRectangles; i++)
            {
                for (int j = 0; j < smallRectangles; j++)
                {
                    //Проверяем расстояние между центрами прямоугольников
                    double xDistance = centerOfBigRectangle[i].X - centerOfSmallRectangle[j].X;
                    double yDistance = centerOfBigRectangle[i].Y - centerOfSmallRectangle[j].Y;
                    double distance  = Math.Sqrt(Math.Pow(xDistance, 2) + Math.Pow(yDistance, 2));
                    double angle;
                    if (distance < 10)
                    {
                        //Избегаем повторного детектирования
                        if (Math.Abs(prevxBigRectangle - centerOfBigRectangle[i].X) <= 5 && Math.Abs(prevyBigRectangle - centerOfBigRectangle[i].Y) <= 5)
                        {
                            continue;
                        }
                        prevxBigRectangle = centerOfBigRectangle[i].X;
                        prevyBigRectangle = centerOfBigRectangle[i].Y;

                        //Определяем углы и рисуем
                        angle = Math.Atan2(xDistance, yDistance);

                        int thickness              = 2;
                        int robotLength            = 150;
                        int robotWeigth            = 90;
                        int fromNXTCentertoForward = 95;

                        //Определяем угловые точки прямоугольника, описывающего робот
                        Point2f[] corners = new Point2f[4];
                        corners[0].X = (float)(centerOfBigRectangle[i].X - fromNXTCentertoForward * Math.Sin(angle) + robotWeigth * Math.Cos(angle) / 2);
                        corners[0].Y = (float)(centerOfBigRectangle[i].Y - fromNXTCentertoForward * Math.Cos(angle) - robotWeigth * Math.Sin(angle) / 2);
                        corners[1].X = (float)(corners[0].X - robotWeigth * Math.Cos(angle));
                        corners[1].Y = (float)(corners[0].Y + robotWeigth * Math.Sin(angle));
                        corners[2].X = (float)(corners[1].X + robotLength * Math.Sin(angle));
                        corners[2].Y = (float)(corners[1].Y + robotLength * Math.Cos(angle));
                        corners[3].X = (float)(corners[2].X + robotWeigth * Math.Cos(angle));
                        corners[3].Y = (float)(corners[2].Y - robotWeigth * Math.Sin(angle));

                        //Находим центр робота
                        Point2f centerOfRobot;
                        centerOfRobot.X = (float)(centerOfBigRectangle[i].X - (robotLength - fromNXTCentertoForward) / 4 * Math.Sin(angle));
                        centerOfRobot.Y = (float)(centerOfBigRectangle[i].Y - (robotLength - fromNXTCentertoForward) / 4 * Math.Cos(angle));
                        //						Cv2.Circle(image, centerOfRobot, 75, Scalar.Red);

                        //Cv2.Circle(image, new Point(10, 10), 5, Scalar.Green);
                        Cv2.Line(image, corners[0], corners[1], Scalar.Red, thickness);
                        Cv2.Line(image, corners[1], corners[2], Scalar.Yellow, thickness);
                        Cv2.Line(image, corners[2], corners[3], Scalar.Green, thickness);
                        Cv2.Line(image, corners[3], corners[0], Scalar.Gold, thickness);



                        //Формируем строку для вывода
                        resultstring += String.Format("Координаты {0}-го робота: ({1:F0}; {2:F0})[{7}] и ({5:F0}; {6:F0})[{8}]\r\nУгол поворота: {3:F2} рад ({4:F0} град)\r\n",
                                                      numOfRobots + 1,
                                                      //centerOfRobot.X, centerOfRobot.Y,
                                                      centerOfBigRectangle[i].X, centerOfBigRectangle[i].Y, angle, angle * 180 / Math.PI, centerOfSmallRectangle[j].X, centerOfSmallRectangle[j].Y, i, j);
                        numOfRobots++;
                    }
                }
            }

            resultstring += String.Format("Обнаружено роботов:{0}\r\n", numOfRobots);


            //resultstring += String.Format("{0}*{1}*{2}", robodrom.height, robodrom.length, robodrom.width);
        }
Exemplo n.º 2
0
        static void FindContours(ref Mat result, ref Mat image, ref string resultstring)
        {
            //Подгружаем параметры робота из соответствующего xml файла
            NXTProperties nxt = new NXTProperties();
            XmlSerializer xmlNXTSerializer = new XmlSerializer(typeof(NXTProperties));
            FileStream    xmlNXT           = new FileStream("NXT.xml", FileMode.Open);

            nxt = (NXTProperties)xmlNXTSerializer.Deserialize(xmlNXT);
            xmlNXT.Close();

            Point[][]        contours;
            HierarchyIndex[] hierarchyIndexes;
            Cv2.FindContours(result, out contours, out hierarchyIndexes, RetrievalModes.Tree, ContourApproximationModes.ApproxSimple);
            RotatedRect[,] rectangles = new RotatedRect[contours.Length, 2];

            //Определяем соотношение площадей NXT блока и его экрана
            float squaresProportion = (nxt.NXTlength * nxt.NXTwidth) / (nxt.LCDlength * nxt.LCDwidth);
            int   numOfRobots       = 0;

            //В данный массив заносим номера вложенного прямоугольника и описывающего его прямоугольника
            int[,] childsAndParents = new int[contours.Length, 2];

            /*Ищем вложенные прямоугольники (родитель родителя необходим, чтобы определить не внешнюю сторону внутреннего прямоугольника,
             * а внутреннюю сторону следующего по иерархии)
             */
            for (int i = 0, childnum = 0; i < contours.Length; i++)
            {
                if ((hierarchyIndexes[i].Child == -1) && (hierarchyIndexes[i].Parent != -1))
                {
                    //Определяем наличие прямоугольника, описанного вокруг вложенного
                    int parentnum = hierarchyIndexes[hierarchyIndexes[i].Parent].Parent;
                    if (parentnum != -1)
                    {
                        childsAndParents[childnum, 0] = i;
                        childsAndParents[childnum, 1] = parentnum;
                        childnum++;
                    }
                }
            }

            //Нашли хотя бы один вложенный
            for (int i = 0; i < childsAndParents.Length; i++)
            {
                int childnum  = childsAndParents[i, 0];
                int parentnum = childsAndParents[i, 1];

                if (childnum > 0)
                {
                    rectangles[i, 0] = Cv2.MinAreaRect(contours[childnum]);
                    rectangles[i, 1] = Cv2.MinAreaRect(contours[parentnum]);

                    //Определяем коэффициент соотношения между размером робота и его снимком
                    float coef = Math.Max(rectangles[i, 0].Size.Height, rectangles[i, 0].Size.Width) / nxt.LCDlength;

                    //Проверяем соотношение площадей
                    float rectanglesProportion = (rectangles[i, 1].Size.Height * rectangles[i, 1].Size.Width)
                                                 / (rectangles[i, 0].Size.Height * rectangles[i, 0].Size.Width);

                    if (rectanglesProportion >= 0.5 * squaresProportion && rectanglesProportion <= 1.5 * squaresProportion)
                    {
                        double xDistance = rectangles[i, 0].Center.X - rectangles[i, 1].Center.X;
                        double yDistance = rectangles[i, 1].Center.Y - rectangles[i, 0].Center.Y;

                        //Определяем расстояние между центрами прямоугольников
                        double centersDistance = Math.Sqrt(Math.Pow(xDistance, 2) + Math.Pow(yDistance, 2));

                        //Проверяем соответствие расстояние между центрами прямоугольника реальным
                        double distance = coef * (nxt.NXTlength / 2 - nxt.fromFrontNXTtoLCD - nxt.LCDwidth / 2);
                        if (centersDistance >= 0.5 * distance && centersDistance <= 1.5 * distance)
                        {
                            //Определяем угол
                            double angle = Math.Atan2(yDistance, xDistance);

                            //Определяем центр робота
                            Point2f centerOfRobot;
                            centerOfRobot.X = (float)(rectangles[i, 0].Center.X + (nxt.LCDwidth / 2 + nxt.fromFrontToNXT + nxt.fromFrontNXTtoLCD - nxt.length / 2) * coef * Math.Cos(angle));
                            centerOfRobot.Y = (float)(rectangles[i, 0].Center.Y - (nxt.LCDlength / 2 + nxt.fromLeftToNXT + nxt.fromLeftNXTtoLCD - nxt.width / 2) * coef * Math.Sin(angle));

                            //Определяем угловые точки прямоугольника, описывающего робот
                            Point2f[] corners = new Point2f[4];
                            corners[0].X = (float)(centerOfRobot.X + nxt.length / 2 * coef * Math.Cos(angle) + nxt.width / 2 * coef * Math.Sin(angle));
                            corners[0].Y = (float)(centerOfRobot.Y - nxt.length / 2 * coef * Math.Sin(angle) + nxt.width / 2 * coef * Math.Cos(angle));

                            corners[1].X = (float)(corners[0].X - nxt.length * coef * Math.Cos(angle));
                            corners[1].Y = (float)(corners[0].Y + nxt.length * coef * Math.Sin(angle));
                            corners[2].X = (float)(corners[1].X - nxt.width * coef * Math.Sin(angle));
                            corners[2].Y = (float)(corners[1].Y - nxt.width * coef * Math.Cos(angle));
                            corners[3].X = (float)(corners[2].X + nxt.length * coef * Math.Cos(angle));
                            corners[3].Y = (float)(corners[2].Y - nxt.length * coef * Math.Sin(angle));

                            //И рисуем его
                            int thickness = 2;
                            Cv2.Line(image, corners[0], corners[1], Scalar.Red, thickness);
                            Cv2.Line(image, corners[1], corners[2], Scalar.Red, thickness);
                            Cv2.Line(image, corners[2], corners[3], Scalar.Red, thickness);
                            Cv2.Line(image, corners[3], corners[0], Scalar.Red, thickness);

                            //Формируем строку для вывода
                            resultstring += String.Format("Координаты {0}-го робота: ({1:F0}; {2:F0}) \r\nУгол поворота: {3:F2} рад ({4:F0} град)\r\n",
                                                          numOfRobots + 1, rectangles[i, 0].Center.X, rectangles[i, 0].Center.Y, angle, angle * 180 / Math.PI);
                            numOfRobots++;
                        }
                    }
                }
                else
                {
                    break;
                }
            }
            resultstring += String.Format("Обнаружено роботов:{0}\r\n", numOfRobots);
        }