Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
float moments[MOMENT_DELAY]
Definition: ins_flow.c:174
static float mu
Physical model parameters.
Definition: nps_fdm_rover.c:60
struct contour_threshold cont_thres
RNG rng(12345)
void find_contour(char *img, int width, int height)
void uyvy_opencv_to_yuv_opencv(Mat image, Mat image_in, int width, int height)
void yuv_opencv_to_yuv422(Mat image, char *img, int width, int height)
struct contour_estimation cont_est
Detects contours of an obstacle used in the autonomous drone racing.
void grayscale_opencv_to_yuv422(Mat image, char *img, int width, int height)
A small library with functions to convert between the Paparazzi used YUV422 arrays and the opencv ima...