Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- if (contour_index >= 0) {
- // Moments
- cv::Moments moments = cv::moments(contours[contour_index], true);
- double marker_y = (int)(moments.m01 / moments.m00);
- double marker_x = (int)(moments.m10 / moments.m00);
- // Measurements
- cv::Mat measurement = (cv::Mat1f(2, 1) << marker_x, marker_y);
- // Correction
- cv::Mat estimated = kalman.correct(measurement);
- // Show result
- cv::Rect rect = cv::boundingRect(contours[contour_index]);
- cv::rectangle(image, rect, cv::Scalar(0, 255, 0));
- }
- // Prediction
- cv::Mat1f prediction = kalman.predict();
- int radius = 1e+3 * kalman.errorCovPre.at<float>(0, 0);
- // Calculate object heading fraction
- float heading = -((image.cols/2)-prediction(0, 0))/(image.cols/2);
- sprintf(textBuffer, "heading = %+3.2f", heading);
- putText(image, textBuffer, cvPoint(30,30), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
- // Sample the object color
- if(learnMode) {
- // Crosshairs
- cv::line(image, cvPoint(image.cols/2, 0), cvPoint(image.cols/2, image.rows/2 - 2), green); //top vertical crosshair
- cv::line(image, cvPoint(image.cols/2, image.rows/2 + 2), cvPoint(image.cols/2, image.rows), green); //bottom vertical crosshair
- cv::line(image, cvPoint(0, image.rows/2), cvPoint(image.cols/2 - 2, image.rows/2), green); //left horizontal crosshair
- cv::line(image, cvPoint(image.cols/2 + 2, image.rows/2), cvPoint(image.cols, image.rows/2), green); //right horizontal crosshair
- cv::Vec3b hsvSample = hsv.at<cv::Vec3b>(cvPoint(image.cols/2, image.rows/2));
- sprintf(textBuffer, "hsvSample = %3d, %3d, %3d", hsvSample[0], hsvSample[1], hsvSample[2]);
- putText(image, textBuffer, cvPoint(30,120), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
- cv::setTrackbarPos("H max", "binalized", hsvSample[0]+20);
- cv::setTrackbarPos("H min", "binalized", hsvSample[0]-20);
- cv::setTrackbarPos("S max", "binalized", hsvSample[1]+20);
- cv::setTrackbarPos("S min", "binalized", hsvSample[1]-20);
- cv::setTrackbarPos("V max", "binalized", hsvSample[2]+20);
- cv::setTrackbarPos("V min", "binalized", hsvSample[2]-20);
- }
- // Show predicted position
- cv::circle(image, cv::Point(prediction(0, 0), prediction(0, 1)), radius, green, 2);
- //Speed
- if ((key >= '0') && (key <= '9'))
- {
- speed = (key-'0')*0.1;
- //printf("speed = %3.2fn", speed);
- }
- sprintf(textBuffer, "speed = %3.2f", speed);
- putText(image, textBuffer, cvPoint(30,60), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
- // Drone control
- double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
- // Auto-follow
- vx = speed;
- vr = -heading;
- // Manual control override
- if (key == 0x260000) vx = 1.0;
- if (key == 0x280000) vx = -1.0;
- if (key == 0x250000) vr = 1.0;
- if (key == 0x270000) vr = -1.0;
- if (key == 'q') vz = 1.0;
- if (key == 'a') vz = -1.0;
- if (key == 'l') learnMode = !learnMode;
- ardrone.move3D(vx, vy, vz, vr);
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement