Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
opencv_contour.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) Roland Meertens and Peng Lu
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
26 #include "opencv_contour.h"
27 #include <opencv2/core/core.hpp>
28 #include <opencv2/imgproc/imgproc.hpp>
29 #include "opencv_image_functions.h"
30 
31 using namespace cv;
32 using namespace std;
33 
36 
37 RNG rng(12345);
38 
39 // YUV in opencv convert to YUV on Bebop
40 void yuv_opencv_to_yuv422(Mat image, char *img, int width, int height)
41 {
42 //Turn the opencv RGB colored image back in a YUV colored image for the drone
43  for (int row = 0; row < height; row++) {
44  for (int col = 0; col < width; col++) {
45  // Extract pixel color from image
46  cv::Vec3b &c = image.at<cv::Vec3b>(row, col);
47 
48  // Set image buffer values
49  int i = row * width + col;
50  img[2 * i + 1] = c[0]; // y;
51  img[2 * i] = col % 2 ? c[1] : c[2]; // u or v
52  }
53  }
54 }
55 
56 void uyvy_opencv_to_yuv_opencv(Mat image, Mat image_in, int width, int height)
57 {
58 //Turn the opencv RGB colored image back in a YUV colored image for the drone
59  for (int row = 0; row < height; row++) {
60  for (int col = 0; col < width; col++) {
61  // Extract pixel color from image
62  cv::Vec3b c = image_in.at<cv::Vec3b>(row, col);
63  cv::Vec3b c_m1 = image_in.at<cv::Vec3b>(row, col);
64  cv::Vec3b c_p1 = image_in.at<cv::Vec3b>(row, col);
65  if (col > 0) {
66  c_m1 = image_in.at<cv::Vec3b>(row, col - 1);
67  }
68  if (col < width) {
69  c_p1 = image_in.at<cv::Vec3b>(row, col + 1);
70  }
71  image.at<cv::Vec3b>(row, col)[0] = c[1] ;
72  image.at<cv::Vec3b>(row, col)[1] = col % 2 ? c[0] : c_m1[0];
73  image.at<cv::Vec3b>(row, col)[2] = col % 2 ? c_p1[0] : c[0];
74 
75  }
76  }
77 }
78 
79 void find_contour(char *img, int width, int height)
80 {
81  // Create a new image, using the original bebop image.
82  Mat M(width, height, CV_8UC2, img); // original
83  Mat image, edge_image, thresh_image;
84 
85  // convert UYVY in paparazzi to YUV in opencv
86  cvtColor(M, M, CV_YUV2RGB_Y422);
87  cvtColor(M, M, CV_RGB2YUV);
88 
89  // Threshold all values within the indicted YUV values.
91  cont_thres.upper_u, cont_thres.upper_v), thresh_image);
92 
94  vector<vector<Point> > contours;
95  vector<Vec4i> hierarchy;
96  edge_image = thresh_image;
97  int edgeThresh = 35;
98  Canny(edge_image, edge_image, edgeThresh, edgeThresh * 3);
99  findContours(edge_image, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
100 
101  // Get the moments
102  vector<Moments> mu(contours.size());
103  for (unsigned int i = 0; i < contours.size(); i++) {
104  mu[i] = moments(contours[i], false);
105  }
106 
107  // Get the mass centers:
108  vector<Point2f> mc(contours.size());
109  for (unsigned int i = 0; i < contours.size(); i++) {
110  mc[i] = Point2f(mu[i].m10 / mu[i].m00 , mu[i].m01 / mu[i].m00);
111  }
112 
114  Mat drawing = Mat::zeros(edge_image.size(), CV_8UC3);
115  for (unsigned int i = 0; i < contours.size(); i++) {
116  Scalar color = Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));
117  drawContours(drawing, contours, i, color, 2, 8, hierarchy, 0, Point());
118  circle(drawing, mc[i], 4, color, -1, 8, 0);
119  }
120 
121  // Find Largest Contour
122  int largest_contour_index = 0;
123  int largest_area = 0;
124  Rect bounding_rect;
125 
126  // iterate through each contour.
127  for (unsigned int i = 0; i < contours.size(); i++) {
128  // Find the area of contour
129  double a = contourArea(contours[i], false);
130  if (a > largest_area) {
131  largest_area = a;
132  // Store the index of largest contour
133  largest_contour_index = i;
134  // Find the bounding rectangle for biggest contour
135  bounding_rect = boundingRect(contours[i]);
136  }
137  }
138  Scalar color(255, 255, 255);
139  // Draw the contour and rectangle
140  drawContours(M, contours, largest_contour_index, color, CV_FILLED, 8, hierarchy);
141 
142  rectangle(M, bounding_rect, Scalar(0, 255, 0), 2, 8, 0);
143 
144  // some figure can cause there are no largest circles, in this case, do not draw circle
145  circle(M, mc[largest_contour_index], 4, Scalar(0, 255, 0), -1, 8, 0);
146  Point2f rect_center(bounding_rect.x + bounding_rect.width / 2 , bounding_rect.y + bounding_rect.height / 2);
147  circle(image, rect_center, 4, Scalar(0, 0, 255), -1, 8, 0);
148 
149  // Convert back to YUV422, and put it in place of the original image
150  grayscale_opencv_to_yuv422(M, img, width, height);
151  float contour_distance_est;
152  //estimate the distance in X, Y and Z direction
153  float area = bounding_rect.width * bounding_rect.height;
154  if (area > 28000.) {
155  contour_distance_est = 0.1;
156  }
157  if ((area > 16000.) && (area < 28000.)) {
158  contour_distance_est = 0.5;
159  }
160  if ((area > 11000.) && (area < 16000.)) {
161  contour_distance_est = 1;
162  }
163  if ((area > 3000.) && (area < 11000.)) {
164  contour_distance_est = 1.5;
165  }
166  if (area < 3000.) {
167  contour_distance_est = 2.0;
168  }
169  cont_est.contour_d_x = contour_distance_est;
170  float Im_center_w = width / 2.;
171  float Im_center_h = height / 2.;
172  float real_size = 1.; // real size of the object
173  cont_est.contour_d_y = -(rect_center.x - Im_center_w) * real_size / float(bounding_rect.width); // right hand
174  cont_est.contour_d_z = -(rect_center.y - Im_center_h) * real_size / float(bounding_rect.height); // point downwards
175 }
176 
177 
178 
179 
180 
181 
182 
183 
184 
185 
186 
187 
188 
189 
void grayscale_opencv_to_yuv422(Mat image, char *img, int width, int height)
void uyvy_opencv_to_yuv_opencv(Mat image, Mat image_in, int width, int height)
A small library with functions to convert between the Paparazzi used YUV422 arrays and the opencv ima...
Detects contours of an obstacle used in the autonomous drone racing.
RNG rng(12345)
struct contour_threshold cont_thres
void yuv_opencv_to_yuv422(Mat image, char *img, int width, int height)
void find_contour(char *img, int width, int height)
struct contour_estimation cont_est