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 7.0KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230
  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. bool Arene::empty() {
  19. if ((this->arene.height==0) || (this->arene.width==0)) return true;
  20. else return false;
  21. }
  22. Img::Img(ImageMat imgMatrice) {
  23. this->img = imgMatrice.clone();
  24. }
  25. string Img::ToString() {
  26. return "Image size: "+to_string(this->img.cols)+"x"+to_string(this->img.rows)+" (dim="+to_string(this->img.dims)+")";
  27. }
  28. string Img::ToBase64() {
  29. return "";
  30. }
  31. Img* Img::Copy() {
  32. return new Img(this->img);
  33. }
  34. float Img::calculAngle(Position robot) {
  35. float a = robot.direction.x - robot.center.x;
  36. float b = robot.direction.y - robot.center.y ;
  37. float angle = atan2(b,a);
  38. return angle * 180.f/M_PI;
  39. }
  40. float Img::calculAngle2(cv::Point2f pt1, cv::Point2f pt2) {
  41. float a = pt1.x - pt2.x;
  42. float b = pt1.y - pt2.y ;
  43. float angle = atan2(b,a);
  44. return angle * 180.f/M_PI;
  45. }
  46. #ifdef __WITH_ARUCO__
  47. cv::Point2f Img::find_aruco_center(std::vector<cv::Point2f> aruco) {
  48. return ((aruco[0] + aruco[2])/2);
  49. }
  50. cv::Point2f Img::find_aruco_direction(std::vector<cv::Point2f> aruco) {
  51. return ((aruco[0]+aruco[1])/2);;
  52. }
  53. std::list<Position> Img::search_aruco(Arene monArene) {
  54. ImageMat imgTraitment;
  55. std::list<Position> positionList;
  56. cv::Point2f areneCoor;
  57. std::vector<int> ids;
  58. std::vector<std::vector<cv::Point2f> > corners;
  59. if(monArene.empty())
  60. imgTraitment=this->img.clone();
  61. else{
  62. imgTraitment = cropArena(monArene);
  63. areneCoor.x = monArene.x;
  64. areneCoor.y = monArene.y;
  65. }
  66. cv::detectMarkers(imgTraitment,dictionary, corners, ids);
  67. if(ids.size()>0){
  68. for(int i = 0 ; i < ids.size() ; i++){
  69. Position newPos;
  70. newPos.center = find_aruco_center(corners[i]);
  71. newPos.direction = find_aruco_direction(corners[i]);
  72. newPos.robotId = ids[i];
  73. if(!monArene.empty()){
  74. newPos.direction += areneCoor;
  75. newPos.center += areneCoor;
  76. }
  77. newPos.angle = calculAngle2(newPos.center, newPos.direction);
  78. positionList.push_back(newPos);
  79. }
  80. }
  81. return positionList;
  82. }
  83. #endif // __WITH_ARUCO__
  84. float Img::euclideanDist(cv::Point2f p, cv::Point2f q) {
  85. cv::Point diff = p - q;
  86. return cv::sqrt(diff.x*diff.x + diff.y*diff.y);
  87. }
  88. Jpg Img::toJpg() {
  89. Jpg imgJpg;
  90. cv::imencode(".jpg",this->img,imgJpg);
  91. return imgJpg;
  92. }
  93. //string Img::ToBase64() {
  94. // string imgBase64;
  95. // Jpg imgJpg = toJpg();
  96. //
  97. // /* faire la convertion Jpg vers base 64 */
  98. // return imgBase64;
  99. //}
  100. std::list<Position> Img::search_robot(Arene monArene) {
  101. std::list<Position> robotsFind;
  102. std::vector<std::vector<cv::Point2f> > contours;
  103. std::vector<cv::Point2f> approx;
  104. std::vector<cv::Vec4i> hierarchy;
  105. ImageMat imgTraitment;
  106. if(monArene.empty())
  107. imgTraitment=this->img.clone();
  108. else
  109. imgTraitment = cropArena(monArene);
  110. cvtColor(imgTraitment,imgTraitment,CV_RGB2GRAY);
  111. threshold(imgTraitment,imgTraitment,128,255,CV_THRESH_BINARY);
  112. findContours(imgTraitment, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point2f(0, 0));
  113. for(unsigned int i = 0;i < contours.size();i++)
  114. {
  115. cv::approxPolyDP(ImageMat(contours[i]), approx, cv::arcLength(ImageMat(contours[i]), true)*0.17, true);
  116. if(approx.size() == 3 && fabs(cv::contourArea(contours[i])) > 200 && fabs(cv::contourArea(contours[i])) < 700)
  117. {
  118. cv::Point2f a,b,c;
  119. cv::Point2f center;
  120. a = approx[0];
  121. b = approx[1];
  122. c = approx[2];
  123. if(!monArene.empty()) // ajout de l'offset de l'arène
  124. {
  125. a.x += monArene.arene.x;
  126. a.y += monArene.arene.y;
  127. b.x += monArene.arene.x;
  128. b.y += monArene.arene.y;
  129. c.x += monArene.arene.x;
  130. c.y += monArene.arene.y;
  131. }
  132. center.x = (a.x + b.x + c.x)/3;
  133. center.y = (a.y + b.y + c.y)/3;
  134. Position newPos;
  135. newPos.center=center;
  136. if(euclideanDist(center,b) > euclideanDist(center,a) && euclideanDist(center,b) > euclideanDist(center,c) )
  137. {
  138. newPos.direction=b;
  139. //line(img,center,b,Scalar(0,125,0),2,8,0);
  140. }
  141. else if(euclideanDist(center,a) > euclideanDist(center,c))
  142. {
  143. newPos.direction=a;
  144. //line(img,center,a,Scalar(0,125,0),2,8,0);
  145. }
  146. else
  147. {
  148. newPos.direction=c;
  149. //line(img,center,c,Scalar(0,125,0),2,8,0);
  150. }
  151. newPos.angle=calculAngle(newPos);
  152. robotsFind.push_back(newPos);
  153. }
  154. }
  155. return robotsFind;
  156. }
  157. Arene Img::search_arena() {
  158. std::vector<std::vector<cv::Point> > contours;
  159. std::vector<cv::Point> approx;
  160. std::vector<cv::Vec4i> hierarchy;
  161. ImageMat imageTrt;
  162. cv::cvtColor(this->img,imageTrt,CV_RGB2GRAY); // conversion en niveau de gris
  163. cv::threshold(imageTrt,imageTrt,128,255,CV_THRESH_BINARY); // Threshold les éléments les plus clair
  164. cv::Canny(imageTrt, imageTrt, 100,200,3); // detection d'angle
  165. findContours(imageTrt, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0));
  166. for(unsigned int i = 0; i < contours.size();i++)
  167. {
  168. approxPolyDP(ImageMat(contours[i]), approx, cv::arcLength(ImageMat(contours[i]), true)*0.1, true);
  169. if(approx.size()==4 && fabs(cv::contourArea(contours[i])) > 100000)
  170. {
  171. Arene rectangle;
  172. rectangle.arene = cv::boundingRect(ImageMat(contours[i]));
  173. return rectangle;
  174. }
  175. }
  176. return Arene();
  177. }
  178. int Img::draw_robot(Position robot) {
  179. cv::arrowedLine(this->img, (cv::Point2f)robot.center, (cv::Point2f)robot.direction, cv::Scalar(0,0,255),3,8,0);
  180. return 0;
  181. }
  182. int Img::draw_all_robots(std::list<Position> robots) {
  183. for(Position robot : robots){
  184. cv::arrowedLine(this->img, (cv::Point2f)robot.center, (cv::Point2f)robot.direction, cv::Scalar(0,0,255),3,8,0);
  185. }
  186. return robots.size();
  187. }
  188. int Img::draw_arena(Arene areneToDraw) {
  189. cv::rectangle(this->img,areneToDraw.arene.tl(),areneToDraw.arene.br(),cv::Scalar(0,0,125),2,8,0);
  190. return 0;
  191. }
  192. ImageMat Img::cropArena(Arene arene) {
  193. return this->img(arene.arene);
  194. }