API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Sep 20th, 2019
81
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 11.94 KB | None | 0 0
raw download clone embed print report
  1. ////////////////////////////////////////Поиск центра круга, угол на четырехугольник, бинаризация и настройка ползунками + функции поворота на угол и установка глубины/////////////////////////
  2. #include <murAPI.hpp>
  3. #include <cmath>
  4. #include <iostream>
  5. #include<vector>
  6. using namespace std;
  7. //using namespace cv;
  8. float absoluteYaw = mur.getYaw();
  9. float errorOld=0,error=0,errorIntegral=0,errorBalance,errorBalanceOld,ErrorBalanceIntegral,uOld;
  10. //Регуляторные коэффиценты
  11. float kpdepth = 5;
  12. float kddepth = 1;
  13. float kidepth = 0;
  14. float kpbalance = 4;
  15. float kdbalance = 0.0333;
  16. float kibalance = 0;
  17. float u;
  18. float filtre[3];
  19. cv::Mat frame;
  20. cv::Mat hsv_and;
  21. int Hmin = 0;
  22. int Hmax = 256;
  23. int Smin = 0;
  24. int Smax = 256;
  25. int Vmin = 0;
  26. int Vmax = 256;
  27. int HSVmax = 256;
  28. //Функции обработки ползунков
  29. void myTrackbarHmin(int pos){ Hmin = pos;}
  30. void myTrackbarHmax(int pos){ Hmax = pos;}
  31. void myTrackbarSmin(int pos){ Smin = pos;}
  32. void myTrackbarSmax(int pos){ Smax = pos;}
  33. void myTrackbarVmin(int pos){ Vmin = pos;}
  34. void myTrackbarVmax(int pos){Vmax = pos;}
  35. Timer t;
  36. bool simul = 0;
  37. //Сюда пишем координаты центров круга
  38. int targetx,targety;
  39. int counter = 0;
  40. float max_r = 0;
  41. int findCircle = 0;
  42. //Режим отладки DEBUG = 1 - включить, DEBUG = 0 - выключить
  43. int DEBUG = 1;
  44. char ch;
  45. int areaBall=0;
  46. float height,width;
  47. //Получение глубины со усредненным скользящим фильтром
  48. float getdepth()
  49. {
  50. filtre[2]=filtre[1];
  51. filtre[1]=filtre[0];
  52. filtre[0]=mur.getInputAOne();
  53. return (filtre[0]+filtre[1]+filtre[2])/3;
  54. }
  55. //Установка глубины
  56. void setdepth(float depthToSet){
  57. float depth = getdepth();
  58. error=depthToSet-depth;
  59. errorIntegral+=error;
  60. mur.setPortC((depthToSet-depth)*kpdepth+(error-errorOld)*kddepth+errorIntegral*kidepth);
  61. errorOld=error;
  62. if(errorIntegral>3000){
  63. errorIntegral=3000;}
  64. if(errorIntegral>-3000){
  65. errorIntegral=-3000;}
  66.  
  67. }
  68. //Сыда угол на четырехугольник
  69. float angleToTarget = 0;
  70. //Размытие или сжатие изображения, 1 аргумент 0 - размытие, 1 - сжатие, 2 аргумент - радиус полей размытия, 3 - само изображение
  71. cv::Mat erodeDilate(bool type,int radi,cv::Mat img)
  72. {
  73.  
  74. cv::Mat elem = cv::getStructuringElement(2,cv::Size(2*radi+1,2*radi+1),cv::Point(radi,radi));
  75. if(type == 0)
  76. cv::dilate(img,img,elem);
  77. else
  78. cv::erode(img,img,elem);
  79. return img;
  80. }
  81. //Поиск прямоугольников mode - по умолчанию 0, если 1 учитываются только четырехугольнике расположенные по примерному центру экрана
  82. bool searchRectangle(bool mode=0)
  83. {
  84. int findRect = 0;
  85. cv::RotatedRect fitEll;
  86. cv::Mat forRect = hsv_and;
  87. cv::GaussianBlur(forRect,forRect,cv::Size(5,5),0,0);
  88. forRect = erodeDilate(1,1,forRect);
  89. double perim = 0,per;
  90. vector<vector<cv::Point>> contours;
  91. vector<cv::Point> maxContour;
  92. cv::findContours(forRect,contours,CV_RETR_TREE,CV_CHAIN_APPROX_NONE,cvPoint(0,0));
  93. if(contours.size()!=0)
  94. {
  95.  
  96. for(int i = 0;i<contours.size();i++)
  97. {
  98. per = cv::arcLength(contours.at(i),1);
  99. if(per>perim&&per>50)//Игнорируем контура с периметром меньше 50
  100. {
  101. maxContour = contours.at(i);
  102. perim = per;
  103. }
  104. }
  105. if(maxContour.size()!=0)
  106. {
  107. if(mode)
  108. {
  109. cv::RotatedRect fitEll = cv::fitEllipse(maxContour);
  110. if(abs(160-(int)fitEll.center.x)<40 && abs(120-(int)fitEll.center.y)<40)//+-40 по центру экрана
  111. {
  112. cv::approxPolyDP(maxContour,maxContour,20,1);
  113. if(maxContour.size() == 4)
  114. {
  115. findRect = 1;
  116. angleToTarget = (int)fitEll.angle;
  117. if(DEBUG==1)
  118. cout<<"ANGLE: "<<angleToTarget<<endl;
  119. }
  120. }
  121. }
  122. else
  123. {
  124. fitEll = cv::fitEllipse(maxContour);
  125. cv::approxPolyDP(maxContour,maxContour,20,1);
  126. if(maxContour.size() == 4)
  127. {
  128. findRect = 1;
  129. angleToTarget = (int)fitEll.angle;
  130. if(DEBUG==1)
  131. cout<<"ANGLE: "<<angleToTarget<<endl;
  132. }
  133. }
  134. }
  135.  
  136. }
  137. if(DEBUG == 1)
  138. {
  139. cv::circle(frame,cvPoint((int)fitEll.center.x,(int)fitEll.center.y),(int)fitEll.size.width,cv::Scalar(255,0,0), 5, 8);
  140. imshow("main",frame);
  141. }
  142. return findRect;
  143. }
  144. //Поиск контура с максимальным радиусом, должна быть запущена после функции binary
  145. bool searchObruch()
  146. {
  147. int findCircle = 0;
  148. float radius = 0;
  149. cv::Mat forCircle = hsv_and;
  150. cv::GaussianBlur(forCircle,forCircle,cv::Size(5,5),0,0);
  151. forCircle = erodeDilate(0,1,forCircle);
  152. double perim = 0,per;
  153. vector<vector<cv::Point>> contours;
  154. vector<cv::Point> maxContour;
  155. cv::findContours(forCircle,contours,CV_RETR_TREE,CV_CHAIN_APPROX_NONE,cvPoint(0,0));
  156. if(contours.size()!=0)
  157. {
  158. findCircle = 1;
  159. for(int i = 0;i<contours.size();i++)
  160. {
  161. per = cv::arcLength(contours.at(i),1);
  162. if(per>perim&&per>50)
  163. {
  164. maxContour = contours.at(i);
  165. perim = per;
  166. }
  167. }
  168. if(maxContour.size()!=0)
  169. {
  170. cv::Point2f point;
  171. cv::minEnclosingCircle((cv::Mat)maxContour,point,radius);
  172. targetx = point.x;
  173. targety = point.y;
  174. }
  175. }
  176. if(DEBUG == 1)
  177. {
  178. cv::circle(frame,cvPoint(targetx,targety),radius,cv::Scalar(0,255,255), 5, 8);
  179. imshow("main",frame);
  180. }
  181. return findCircle;
  182. }
  183. //Поиск круга, должна быть запущена после binary
  184. bool searchCircle()
  185. {
  186. bool findCircle = 0;
  187. cv::Mat forCircle = hsv_and;
  188. cv::blur(forCircle,forCircle,cv::Size(5,5),cv::Point(0,0));
  189. forCircle = erodeDilate(0,1,forCircle);
  190. if(DEBUG == 1)
  191. cv::imshow("binary",forCircle);
  192. vector<cv::Vec3f> contours;
  193. cv::HoughCircles(forCircle,contours,CV_HOUGH_GRADIENT,2,forCircle.rows/4,200,100,30,320);//4 аргумент - качество поиска круга, чем больше тем больше диапазон, последние 2 аргумента максимальный и минимальный радиусы
  194. int max_r = 0,radius;
  195. for(int i = 0;i<contours.size();i++)
  196. {
  197. radius = contours[i][2];
  198. if(radius>max_r)
  199. {
  200. max_r = radius;
  201. targetx = (int)contours[i][0];
  202. targety = (int)contours[i][1];
  203. }
  204. }
  205.  
  206. if(DEBUG == 1)
  207. {
  208. cv::circle(frame,cvPoint(targetx,targety),max_r,cv::Scalar(0,255,255), 5, 8);
  209. imshow("main",frame);
  210. }
  211. return findCircle;
  212. }
  213.  
  214.  
  215. //Функция бинаризации записывает бинаризованые параметры в hsv_and, 1 аргумент - номер камеры, остальные для настройки диапазона
  216. void binary(bool camNum,int hsv1,int hsv2,int hsv3,int hsv4,int hsv5,int hsv6)
  217. {
  218. if(camNum==0)
  219. frame = mur.getCameraOneFrame();
  220. else
  221. frame = mur.getCameraTwoFrame();
  222. //height = frame.height;
  223. //width = frame.width;
  224. cv::Scalar low(hsv1,hsv3,hsv5);
  225. cv::Scalar max(hsv2,hsv4,hsv6);
  226. cv::cvtColor(frame,frame,CV_BGR2HSV);
  227. cv::inRange(frame,low,max,hsv_and);
  228. if(DEBUG==1)
  229. cv::waitKey(33);
  230.  
  231. }
  232. //Балансировка на угол
  233. void balance(float yaw ){
  234. uOld=u;
  235. u =((int)(yaw-mur.getYaw()+540)%360-180)*kpbalance;
  236. mur.setPortA(-u-(u-uOld)*kdbalance);
  237. mur.setPortB(u+(u-uOld)*kdbalance);
  238. }
  239. float yaw=0;
  240. float setyaw = 0;
  241. bool flag=1;
  242. bool onGo = 0;
  243. int yawCenter = 0;
  244. //orange poloska 0,20,121,255,0,255
  245. //Yellow sqr 13,255,193,255,0,255
  246. int main()
  247. {
  248. int FLAG = 0;
  249. std::cout <<"start";
  250. int deta=100;
  251. int yaww;
  252. simul = 0;
  253. int dep = 0;
  254. int targ1;
  255. bool rec;
  256. //Если равен 0 запускается основной алгоритм, если 1 режим настройки ползунками
  257. bool bin_mode = 0;
  258. int mis = 0;
  259. bool toggle = 0,obruch = 0,toBall = 0;
  260. //Инициализируем камеру
  261. mur.initCamera(1);
  262. if(!bin_mode)
  263. {
  264. while(1)
  265. {
  266. //Простой пример программы снимаем показание с фронтальной камеры, бинаризуемся на красный, если есть четырехугольник крутим мотором
  267. binary(1,0,3,88,255,0,255);
  268. if(searchRectangle()==1)
  269. mur.setPortB(-50);
  270. else
  271. mur.setPortB(0);
  272. cout<<"ANGLE: "<<searchCircle()<<endl;
  273. }
  274.  
  275. }
  276.  
  277.  
  278. else
  279. {
  280.  
  281. IplImage* onlyBinary = 0;
  282. IplImage* hsv2= 0;
  283. IplImage* hsv_and2;
  284. IplImage* h_plane2 = 0;
  285. IplImage* s_plane2 = 0;
  286. IplImage* v_plane2 = 0;
  287. IplImage* frame1;
  288. //Подгружаем файл для настройки с помощью ползунков
  289. frame1 = cvLoadImage("D:/khams/image777.png");
  290. hsv2 = cvCreateImage( cvGetSize(frame1), IPL_DEPTH_8U, 3 );
  291. h_plane2 = cvCreateImage( cvGetSize(frame1), IPL_DEPTH_8U, 1 );
  292. s_plane2 = cvCreateImage( cvGetSize(frame1), IPL_DEPTH_8U, 1 );
  293. v_plane2 = cvCreateImage( cvGetSize(frame1), IPL_DEPTH_8U, 1 );
  294. hsv_and2 = cvCreateImage( cvGetSize(frame1), IPL_DEPTH_8U, 1 );
  295. cvNamedWindow("original",CV_WINDOW_AUTOSIZE);
  296. cvNamedWindow("hsv_and",CV_WINDOW_AUTOSIZE);
  297. cvCreateTrackbar("Hmin", "hsv_and", &Hmin, HSVmax, myTrackbarHmin);
  298. cvCreateTrackbar("Hmax", "hsv_and", &Hmax, HSVmax, myTrackbarHmax);
  299. cvCreateTrackbar("Smin", "hsv_and", &Smin, HSVmax, myTrackbarSmin);
  300. cvCreateTrackbar("Smax", "hsv_and", &Smax, HSVmax, myTrackbarSmax);
  301. cvCreateTrackbar("Vmin", "hsv_and", &Vmin, HSVmax, myTrackbarVmin);
  302. cvCreateTrackbar("Vmax", "hsv_and", &Vmax, HSVmax, myTrackbarVmax);
  303. while(true){
  304. cvCvtColor( frame1, hsv2, CV_BGR2HSV );
  305. cvSplit( hsv2, h_plane2, s_plane2, v_plane2, 0 );
  306. cvInRangeS(h_plane2, cvScalar(Hmin), cvScalar(Hmax), h_plane2);
  307. cvInRangeS(s_plane2, cvScalar(Smin), cvScalar(Smax), s_plane2);
  308. cvInRangeS(v_plane2, cvScalar(Vmin), cvScalar(Vmax), v_plane2);
  309. cvAnd(h_plane2, s_plane2, hsv_and2);
  310. cvAnd(hsv_and2, v_plane2, hsv_and2);
  311. cvShowImage( "hsv_and", hsv_and2 );
  312. cvShowImage( "original", frame1 );
  313. char c = cvWaitKey(33);
  314. if(c == 115)
  315. cvSaveImage("HSV.jpg",hsv_and2);
  316. //Выйти на escape
  317. if (c == 27) {
  318. break;
  319. }
  320. }
  321.  
  322.  
  323. cvReleaseImage(&hsv2);
  324. cvReleaseImage(&h_plane2);
  325. cvReleaseImage(&s_plane2);
  326. cvReleaseImage(&v_plane2);
  327. cvReleaseImage(&hsv_and2);
  328. cvDestroyAllWindows();
  329. }
  330. return 0;
  331. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!