daily pastebin goal
47%
SHARE
TWEET

Untitled

a guest Oct 12th, 2017 55 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. if (contour_index >= 0) {
  2.     // Moments
  3.  
  4.     cv::Moments moments = cv::moments(contours[contour_index], true);
  5.  
  6.     double marker_y = (int)(moments.m01 / moments.m00);
  7.  
  8.     double marker_x = (int)(moments.m10 / moments.m00);
  9.  
  10.  
  11.  
  12.     // Measurements
  13.  
  14.     cv::Mat measurement = (cv::Mat1f(2, 1) << marker_x, marker_y);
  15.  
  16.  
  17.  
  18.     // Correction
  19.  
  20.     cv::Mat estimated = kalman.correct(measurement);
  21.  
  22.  
  23.  
  24.     // Show result
  25.  
  26.     cv::Rect rect = cv::boundingRect(contours[contour_index]);
  27.  
  28.     cv::rectangle(image, rect, cv::Scalar(0, 255, 0));
  29.  
  30. }
  31.  
  32.  
  33.  
  34. // Prediction
  35.  
  36. cv::Mat1f prediction = kalman.predict();
  37.  
  38. int radius = 1e+3 * kalman.errorCovPre.at<float>(0, 0);
  39.  
  40.  
  41.  
  42. // Calculate object heading fraction
  43.  
  44. float heading = -((image.cols/2)-prediction(0, 0))/(image.cols/2);
  45.  
  46. sprintf(textBuffer, "heading = %+3.2f", heading);
  47.  
  48. putText(image, textBuffer, cvPoint(30,30), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
  49.  
  50.  
  51.  
  52. // Sample the object color
  53.  
  54. if(learnMode) {
  55.  
  56.  
  57.  
  58.     // Crosshairs
  59.  
  60.     cv::line(image, cvPoint(image.cols/2, 0), cvPoint(image.cols/2, image.rows/2 - 2), green); //top vertical crosshair
  61.  
  62.     cv::line(image, cvPoint(image.cols/2, image.rows/2 + 2), cvPoint(image.cols/2, image.rows), green); //bottom vertical crosshair
  63.  
  64.     cv::line(image, cvPoint(0, image.rows/2), cvPoint(image.cols/2 - 2, image.rows/2), green); //left horizontal crosshair
  65.  
  66.     cv::line(image, cvPoint(image.cols/2 + 2, image.rows/2), cvPoint(image.cols, image.rows/2), green); //right horizontal crosshair
  67.  
  68.  
  69.  
  70.     cv::Vec3b hsvSample = hsv.at<cv::Vec3b>(cvPoint(image.cols/2, image.rows/2));
  71.  
  72.     sprintf(textBuffer, "hsvSample = %3d, %3d, %3d", hsvSample[0], hsvSample[1], hsvSample[2]);
  73.  
  74.     putText(image, textBuffer, cvPoint(30,120), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
  75.  
  76.  
  77.  
  78.     cv::setTrackbarPos("H max", "binalized", hsvSample[0]+20);
  79.  
  80.     cv::setTrackbarPos("H min", "binalized", hsvSample[0]-20);
  81.  
  82.  
  83.  
  84.     cv::setTrackbarPos("S max", "binalized", hsvSample[1]+20);
  85.  
  86.     cv::setTrackbarPos("S min", "binalized", hsvSample[1]-20);
  87.  
  88.  
  89.  
  90.     cv::setTrackbarPos("V max", "binalized", hsvSample[2]+20);
  91.  
  92.     cv::setTrackbarPos("V min", "binalized", hsvSample[2]-20);
  93.  
  94. }
  95.  
  96.  
  97.  
  98. // Show predicted position
  99.  
  100. cv::circle(image, cv::Point(prediction(0, 0), prediction(0, 1)), radius, green, 2);
  101.  
  102.  
  103.  
  104. //Speed
  105.  
  106. if ((key >= '0') && (key <= '9'))
  107.  
  108. {
  109.  
  110.     speed = (key-'0')*0.1;
  111.  
  112.     //printf("speed = %3.2fn", speed);
  113.  
  114. }
  115.  
  116. sprintf(textBuffer, "speed = %3.2f", speed);
  117.  
  118. putText(image, textBuffer, cvPoint(30,60), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
  119.  
  120. // Drone control
  121.  
  122. double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
  123.  
  124.  
  125.  
  126. // Auto-follow
  127.  
  128. vx = speed;
  129.  
  130. vr = -heading;
  131.  
  132.  
  133.  
  134. // Manual control override
  135.  
  136. if (key == 0x260000) vx =  1.0;
  137.  
  138. if (key == 0x280000) vx = -1.0;
  139.  
  140. if (key == 0x250000) vr =  1.0;
  141.  
  142. if (key == 0x270000) vr = -1.0;
  143.  
  144. if (key == 'q')      vz =  1.0;
  145.  
  146. if (key == 'a')      vz = -1.0;
  147.  
  148. if (key == 'l')      learnMode = !learnMode;
  149.  
  150.  
  151.  
  152. ardrone.move3D(vx, vy, vz, vr);
RAW Paste Data
Top