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);
}
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);
}
