No Description
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

img.cpp 8.6KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295
  1. /*
  2. * Copyright (C) 2018 dimercur
  3. *
  4. * This program is free software: you can redistribute it and/or modify
  5. * it under the terms of the GNU General Public License as published by
  6. * the Free Software Foundation, either version 3 of the License, or
  7. * (at your option) any later version.
  8. *
  9. * This program is distributed in the hope that it will be useful,
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. * GNU General Public License for more details.
  13. *
  14. * You should have received a copy of the GNU General Public License
  15. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  16. */
  17. #include "img.h"
  18. /**
  19. * Tell if arena is empty (not found) or not
  20. * @return true if no arena found, false otherwise
  21. */
  22. bool Arena::IsEmpty() {
  23. if ((this->arena.height == 0) || (this->arena.width == 0)) return true;
  24. else return false;
  25. }
  26. /**
  27. * Create new Img object based on image data
  28. *
  29. * @param imgMatrice Image data to be stored (raw data)
  30. */
  31. Img::Img(ImageMat imgMatrice) {
  32. this->img = imgMatrice.clone();
  33. #ifdef __WITH_ARUCO__
  34. this->dictionary=cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(3));
  35. #endif // __WITH_ARUCO__
  36. }
  37. /**
  38. * Convert object to a string representation
  39. *
  40. * @return String containing information on contained image (size and number of channel)
  41. */
  42. string Img::ToString() {
  43. return "Image size: " + to_string(this->img.cols) + "x" + to_string(this->img.rows) + " (dim=" + to_string(this->img.dims) + ")";
  44. }
  45. /**
  46. * Create a copy of current object
  47. *
  48. * @return New Img object, copy of current
  49. */
  50. Img* Img::Copy() {
  51. return new Img(this->img);
  52. }
  53. /**
  54. * Function for computing angle
  55. * @param robots Position of robot
  56. * @return Angle
  57. */
  58. float Img::CalculAngle(Position robot) {
  59. float a = robot.direction.x - robot.center.x;
  60. float b = robot.direction.y - robot.center.y;
  61. float angle = atan2(b, a);
  62. return angle * 180.f / M_PI;
  63. }
  64. /**
  65. * Function for computing angle
  66. * @param pt1 ???
  67. * @param pt2 ???
  68. * @return Angle
  69. */
  70. float Img::CalculAngle2(cv::Point2f pt1, cv::Point2f pt2) {
  71. float a = pt1.x - pt2.x;
  72. float b = pt1.y - pt2.y;
  73. float angle = atan2(b, a);
  74. return angle * 180.f / M_PI;
  75. }
  76. #ifdef __WITH_ARUCO__
  77. /**
  78. * Find center point of given aruco
  79. * @param aruco Aruco coordinates
  80. * @return Center point coordinate
  81. */
  82. cv::Point2f Img::FindArucoCenter(std::vector<cv::Point2f> aruco) {
  83. return ((aruco[0] + aruco[2]) / 2);
  84. }
  85. /**
  86. * Find direction of given aruco
  87. * @param aruco Aruco coordinates
  88. * @return Orientation of aruco
  89. */
  90. cv::Point2f Img::FindArucoDirection(std::vector<cv::Point2f> aruco) {
  91. return ((aruco[0] + aruco[1]) / 2);
  92. }
  93. #endif // __WITH_ARUCO__
  94. /**
  95. * Used for computing distance
  96. * @param p ???
  97. * @param q ???
  98. * @return Distance
  99. */
  100. float Img::EuclideanDistance(cv::Point2f p, cv::Point2f q) {
  101. cv::Point diff = p - q;
  102. return cv::sqrt(diff.x * diff.x + diff.y * diff.y);
  103. }
  104. /**
  105. * Compress current image to JPEG
  106. * @return Image compressed as JPEG
  107. */
  108. Jpg Img::ToJpg() {
  109. Jpg imgJpg;
  110. cv::imencode(".jpg", this->img, imgJpg);
  111. return imgJpg;
  112. }
  113. /**
  114. * Search available robots in an image
  115. * @param arena Arena position for cropping image
  116. * @return list of position, empty if no robot found
  117. */
  118. std::list<Position> Img::SearchRobot(Arena arena) {
  119. #ifdef __WITH_ARUCO__
  120. ImageMat imgTraitment;
  121. std::list<Position> positionList;
  122. cv::Point2f areneCoor;
  123. std::vector<int> ids;
  124. std::vector<std::vector<cv::Point2f> > corners;
  125. if (arena.IsEmpty())
  126. imgTraitment = this->img.clone();
  127. else {
  128. imgTraitment = CropArena(arena);
  129. areneCoor.x = arena.arena.x;
  130. areneCoor.y = arena.arena.y;
  131. }
  132. cv::aruco::detectMarkers(imgTraitment, dictionary, corners, ids);
  133. if (ids.size() > 0) {
  134. for (int i = 0; i < ids.size(); i++) {
  135. Position newPos;
  136. newPos.center = FindArucoCenter(corners[i]);
  137. newPos.direction = FindArucoDirection(corners[i]);
  138. newPos.robotId = ids[i];
  139. if (!arena.IsEmpty()) {
  140. newPos.direction += areneCoor;
  141. newPos.center += areneCoor;
  142. }
  143. newPos.angle = CalculAngle2(newPos.center, newPos.direction);
  144. positionList.push_back(newPos);
  145. }
  146. }
  147. return positionList;
  148. #else
  149. std::list<Position> robotsFind;
  150. std::vector<std::vector<cv::Point> > contours;
  151. std::vector<cv::Point> approx;
  152. std::vector<cv::Vec4i> hierarchy;
  153. ImageMat imgTraitment;
  154. if (arena.IsEmpty()) {
  155. imgTraitment = this->img.clone();
  156. } else {
  157. imgTraitment = this->img(arena.arena);
  158. }
  159. cvtColor(imgTraitment, imgTraitment, CV_RGB2GRAY);
  160. threshold(imgTraitment, imgTraitment, 128, 255, CV_THRESH_BINARY);
  161. findContours(imgTraitment, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0));
  162. for (unsigned int i = 0; i < contours.size(); i++) {
  163. ImageMat m(contours[i]);
  164. cv::approxPolyDP(m, approx, cv::arcLength(ImageMat(contours[i]), true)*0.17, true);
  165. if (approx.size() == 3 && fabs(cv::contourArea(contours[i])) > 200 && fabs(cv::contourArea(contours[i])) < 700) {
  166. cv::Point a, b, c;
  167. cv::Point center;
  168. a = approx[0];
  169. b = approx[1];
  170. c = approx[2];
  171. if (!arena.IsEmpty()) { // ajout de l'offset de l'arène
  172. a.x += arena.arena.x;
  173. a.y += arena.arena.y;
  174. b.x += arena.arena.x;
  175. b.y += arena.arena.y;
  176. c.x += arena.arena.x;
  177. c.y += arena.arena.y;
  178. }
  179. center.x = (a.x + b.x + c.x) / 3;
  180. center.y = (a.y + b.y + c.y) / 3;
  181. Position newPos;
  182. newPos.center = center;
  183. if (EuclideanDistance(center, b) > EuclideanDistance(center, a) && EuclideanDistance(center, b) > EuclideanDistance(center, c)) {
  184. newPos.direction = b;
  185. //line(img,center,b,Scalar(0,125,0),2,8,0);
  186. } else if (EuclideanDistance(center, a) > EuclideanDistance(center, c)) {
  187. newPos.direction = a;
  188. //line(img,center,a,Scalar(0,125,0),2,8,0);
  189. } else {
  190. newPos.direction = c;
  191. //line(img,center,c,Scalar(0,125,0),2,8,0);
  192. }
  193. newPos.angle = CalculAngle(newPos);
  194. newPos.robotId = -1; // dumb identifier
  195. robotsFind.push_back(newPos);
  196. }
  197. }
  198. return robotsFind;
  199. #endif // __WITH_ARUCO__
  200. }
  201. /**
  202. * Search arena outline in current image
  203. * @return Arena object with coordinate of outline, empty if no arena found
  204. */
  205. Arena Img::SearchArena() {
  206. std::vector<std::vector<cv::Point> > contours;
  207. std::vector<cv::Point> approx;
  208. std::vector<cv::Vec4i> hierarchy;
  209. ImageMat imageTrt;
  210. cv::cvtColor(this->img, imageTrt, CV_RGB2GRAY); // conversion en niveau de gris
  211. cv::threshold(imageTrt, imageTrt, 128, 255, CV_THRESH_BINARY); // Threshold les éléments les plus clair
  212. cv::Canny(imageTrt, imageTrt, 100, 200, 3); // detection d'angle
  213. findContours(imageTrt, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0));
  214. for (unsigned int i = 0; i < contours.size(); i++) {
  215. approxPolyDP(ImageMat(contours[i]), approx, cv::arcLength(ImageMat(contours[i]), true)*0.1, true);
  216. if (approx.size() == 4 && fabs(cv::contourArea(contours[i])) > 100000) {
  217. Arena rectangle;
  218. rectangle.arena = cv::boundingRect(ImageMat(contours[i]));
  219. return rectangle;
  220. }
  221. }
  222. return Arena();
  223. }
  224. /**
  225. * Draw an oriented arrow at robot position
  226. * @param robot Position of robot
  227. */
  228. void Img::DrawRobot(Position robot) {
  229. cv::arrowedLine(this->img, (cv::Point2f)robot.center, (cv::Point2f)robot.direction, cv::Scalar(0, 0, 255), 3, 8, 0);
  230. }
  231. /**
  232. * Draw an oriented arrow for each position provided
  233. * @param robots List of robot positions
  234. * @return Number of position drawn
  235. */
  236. int Img::DrawAllRobots(std::list<Position> robots) {
  237. for (Position robot : robots) {
  238. cv::arrowedLine(this->img, (cv::Point2f)robot.center, (cv::Point2f)robot.direction, cv::Scalar(0, 0, 255), 3, 8, 0);
  239. }
  240. return robots.size();
  241. }
  242. /**
  243. * Draw arena outline
  244. * @param arenaToDraw Arena position
  245. */
  246. void Img::DrawArena(Arena arenaToDraw) {
  247. cv::rectangle(this->img, arenaToDraw.arena.tl(), arenaToDraw.arena.br(), cv::Scalar(0, 0, 125), 2, 8, 0);
  248. }
  249. /**
  250. * Crop image around detected arena
  251. * @param arena Coordinate of arena
  252. * @return Reduced image, focused on arena
  253. */
  254. ImageMat Img::CropArena(Arena arena) {
  255. return this->img(arena.arena);
  256. }