Paparazzi UAS  v6.0_unstable-20-gfdbd467-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
wedgebug_opencv.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of paparazzi
3  *
4  * paparazzi is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published by
6  * the Free Software Foundation; either version 2, or (at your option)
7  * any later version.
8  *
9  * paparazzi is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with paparazzi; see the file COPYING. If not, see
16  * <http://www.gnu.org/licenses/>.
17  */
18 
19 
20 
21 #include "wedgebug_opencv.h"
22 
23 #include <opencv2/core/core.hpp>
24 #include <opencv2/imgproc/imgproc.hpp>
25 #include <opencv2/imgproc/types_c.h> // needed for CV_MOP_CLOSE
26 #include <opencv2/imgcodecs/imgcodecs.hpp>
27 #include <opencv2/calib3d/calib3d.hpp>
28 #include <iostream>
29 #include <stdint.h>
30 
31 using namespace cv;
32 
33 
34 
35 // Local declarations
36 int transfer(const Mat *from, const image_t *to);
37 
38 
39 // Local functions
40 int transfer(const Mat *from, const image_t *to)
41 {
42  // Determining type of supplied image
43  // 1) If image is of type uint16_t
44  if (to->type == IMAGE_INT16) {
45  typedef uint16_t img_dip_type;
46  for (int i = 0; i < (from->rows * from->cols); i++) {
47  ((img_dip_type *)to->buf)[i] = from->at<img_dip_type>
48  (i); // Using ".at" here, as accessing buffer is problematic with a cropped image as it maintains a connection to original image
49  }
50  }
51  // 2) If image is of type uint8_t
52  else if (to->type == IMAGE_GRAYSCALE) { // If image is of type uint8_t
53  typedef uint8_t img_dip_type;
54  for (int i = 0; i < (from->rows * from->cols); i++) {
55  ((img_dip_type *)to->buf)[i] = from->at<img_dip_type>
56  (i); // Using ".at" here, as accessing buffer is problematic with a cropped image as it maintains a connection to original image
57  }
58  }
59 
60  return 0;
61 }
62 
63 
64 
65 
66 
67 
68 // Global functions
69 int save_image_gray(struct image_t *img, char *myString)
70 {
71  // Create a new image, using the original bebop image.
72 
73  if (img->type == IMAGE_INT16) {
74  Mat M(img->h, img->w, CV_16SC1, img->buf);
75  Mat Mcopy = M.clone();
76  //normalize(M, M, 0, 255, NORM_MINMAX);
77  imwrite(myString, Mcopy);
78 
79  } else if (img->type == IMAGE_GRAYSCALE) {
80  Mat M(img->h, img->w, CV_8UC1, img->buf);
81  Mat Mcopy = M.clone();
82 
83  imwrite(myString, Mcopy);
84  } else {
85  std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
86  std::endl;
87  return -1;
88  }
89  return 0;
90 }
91 
92 
93 
94 int save_image_color(struct image_t *img, char *myString)
95 {
96  // Create a new image, using the original bebop image.
97  Mat M(img->h, img->w, CV_8UC2, img->buf); // CV_8UC2 is the openCV format for UYVY images
98  // Definition of Mat: Mat::Mat(int _rows, int _cols, int _type, void* _data, size_t _step)
99  // Remember that void takes any data type including char
100  Mat image;
101 
102  cvtColor(M, image, CV_YUV2GRAY_Y422);
103  imwrite(myString, image);
104 
105  // Code below is for testing
106  //double minVal;
107  //double maxVal;
108  //Point minLoc;
109  //Point maxLoc;
110  //minMaxLoc(image ,&minVal, &maxVal, &minLoc, &maxLoc);
111  //std::cout << "Left1: Min=" << minVal << "; Max=" << maxVal << std::endl;
112  return 0;
113 }
114 
115 
116 int save_image_HM(struct image_t *img, char *myString, int const heatmap)
117 {
118  {
119  // Create a new image, using the original bebop image.
120 
121  /*
122  * Heat maps:
123  * 0 = COLORMAP_AUTUMN
124  * 1 = COLORMAP_BONE
125  * 2 = COLORMAP_JET
126  * 3 = COLORMAP_WINTER
127  * 4 = COLORMAP_RAINBOW
128  * 5 = COLORMAP_OCEAN
129  * 6 = COLORMAP_SUMMER
130  * 7 = COLORMAP_SPRING
131  * 8 = COLORMAP_COOL
132  * 9 = COLORMAP_HSV
133  * 10 = COLORMAP_PINK
134  * 11 = COLORMAP_HOT
135  */
136 
137 
138  if (img->type == IMAGE_INT16) {
139  Mat M(img->h, img->w, CV_16SC1, img->buf);
140  Mat Mcopy = M.clone();
141 
142  //normalize(Mcopy, Mcopy, 0, 255, NORM_MINMAX);
143  Mcopy.convertTo(Mcopy, CV_8UC1); //Converting image to 8 bit image
144  applyColorMap(Mcopy, Mcopy, heatmap);
145 
146  imwrite(myString, Mcopy);
147 
148  } else if (img->type == IMAGE_GRAYSCALE) {
149  Mat M(img->h, img->w, CV_8UC1, img->buf);
150  Mat Mcopy = M.clone();
151 
152  applyColorMap(Mcopy, Mcopy, heatmap);
153 
154  imwrite(myString, Mcopy);
155  } else {
156  std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
157  std::endl;
158  return -1;
159  }
160  return 0;
161  }
162 
163 }
164 
165 
166 
167 
168 int SBM_OCV(struct image_t *img_disp, const struct image_t *img_left, const struct image_t *img_right,
169  const int ndisparities, const int SADWindowSize, const bool cropped)
170 {
171  // Defining variables
172  Mat img_left_OCV(img_left->h, img_left->w, CV_8UC1, img_left->buf);
173  Mat img_right_OCV(img_right->h, img_right->w, CV_8UC1, img_right->buf);
174  Mat img_disp_OCV;
175  Mat img_disp_2_OCV;
176 
177  //std::cout << "OpenCV version : " << CV_VERSION << std::endl;
178  //std::cout << "Major version : " << CV_MAJOR_VERSION << std::endl;
179  //std::cout << "Minor version : " << CV_MINOR_VERSION << std::endl;
180  //std::cout << "Subminor version : " << CV_SUBMINOR_VERSION << std::endl;
181 
182 
183 
184  // Block matching
185  Ptr<StereoBM> sbm = StereoBM::create(ndisparities, SADWindowSize);
186  sbm->compute(img_left_OCV, img_right_OCV, img_disp_OCV); //type of img_disp_OCV is CV_16S i.e. int16_t
187 
188 
189  // Determining type of supplied image
190  // 1) If image is of type int16_t
191  if (img_disp->type == IMAGE_INT16) {
192  //std::cout << "int16_t" << std::endl;
193  typedef int16_t img_dip_type;
194 
195  // Cropping or not cropping:
196  if (cropped == 0) { // If image should not be cropped
197  for (int i = 0; i < (img_disp_OCV.rows * img_disp_OCV.cols); i++) {
198  ((img_dip_type *)img_disp->buf)[i] = img_disp_OCV.at<img_dip_type>
199  (i); // Using ".at" here are accessing buffer is problematic with a cropped image as it maintains a connection to oriinal image
200  }
201  } else if (cropped == 1) { // If image should be cropped
202  struct crop_t img_cropped_info;
203  struct img_size_t original_img_dims = {img_left->w, img_left->h};
204 
205  post_disparity_crop_rect(&img_cropped_info, &original_img_dims, ndisparities,
206  SADWindowSize); // Function from wedebug.h
207  Rect crop_area = Rect(img_cropped_info.x, img_cropped_info.y, img_cropped_info.w, img_cropped_info.h);
208  Mat img_cropped = img_disp_OCV(crop_area);// <---
209 
210  for (int i = 0; i < (img_cropped.rows * img_cropped.cols); i++) {
211  ((img_dip_type *)img_disp->buf)[i] = img_cropped.at<img_dip_type>
212  (i); // Using ".at" here are accessing buffer is problematic with a cropped image as it maintains a connection to oriinal image
213  }
214 
215  } else {return -1;}
216  }
217 
218 
219  // 2) If image is of type uint8_t
220  else if (img_disp->type == IMAGE_GRAYSCALE) { // If image is of type uint8_t
221 
222  //std::cout << "uint8_t" << std::endl;
223  typedef uint8_t img_dip_type;
224 
225  // THe followig code is to display the depth map with depth capped at 256/
226  // This is for debugging only such that the depth image has moe contrast when observing it
227  //imwrite("/home/dureade/Documents/paparazzi_images/img_disp_int8_cropped_capped.bmp", img_disp_OCV);
228 
229 
230 
231  img_disp_OCV = img_disp_OCV / 16; // We divide by 16 to get actual disparity values
232  img_disp_OCV.convertTo(img_disp_OCV, CV_8UC1); // Now we convert 16bit image to 8 bit image
233 
234 
235 
236  // Cropping or not cropping:
237  if (cropped == 0) { // If image should not be cropped
238  for (int i = 0; i < (img_disp_OCV.rows * img_disp_OCV.cols); i++) {
239  ((img_dip_type *)img_disp->buf)[i] = img_disp_OCV.data[i];
240  }
241  } else if (cropped == 1) { // If image should be cropped
242  struct crop_t img_cropped_info;
243  struct img_size_t original_img_dims = {img_left->w, img_left->h};
244 
245 
246  post_disparity_crop_rect(&img_cropped_info, &original_img_dims, ndisparities,
247  SADWindowSize); // Function from wedebug.h
248 
249  Rect crop_area = Rect(img_cropped_info.x, img_cropped_info.y, img_cropped_info.w, img_cropped_info.h);
250  Mat img_cropped = img_disp_OCV(crop_area); // Problem here
251 
252 
253  // Test code for saving cropped left image
254  //Mat img_left_cropped =img_left_OCV(crop_area);
255  //imwrite("/home/dureade/Documents/paparazzi_images/for_report/img_left_int8_cropped.bmp", img_left_cropped);
256 
257  for (int i = 0; i < (img_cropped.rows * img_cropped.cols); i++) {
258  ((img_dip_type *)img_disp->buf)[i] = img_cropped.at<img_dip_type>
259  (i); // Using ".at" here are accessing buffer is problematic with a cropped image as it maintains a connection to oriinal image
260  }
261  } else {return -1;}
262 
263  }
264 
265 
266  // 3) If image is of unsupported type
267  else {
268  std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
269  std::endl;
270  return -1;
271  }
272 
273  return 0;
274 }
275 
276 
277 int opening_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
278 {
279  Mat img_input_OCV;
280  Mat img_output_OCV;
281  Mat kernel = getStructuringElement(MORPH_RECT, Size(SE_size, SE_size));
282 
283  if (img_input->type == IMAGE_INT16) {
284  img_input_OCV = Mat(img_input->h, img_input->w, CV_16S, img_input->buf);
285  } else if (img_input->type == IMAGE_GRAYSCALE) {
286  img_input_OCV = Mat(img_input->h, img_input->w, CV_8UC1, img_input->buf);
287  } else {
288  std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
289  std::endl;
290  return -1;
291  }
292 
293 
294  morphologyEx(img_input_OCV, img_output_OCV, CV_MOP_OPEN, kernel, Point(-1, -1), iteration);
295  //erode(img_input_OCV, img_output_OCV, kernel);
296  transfer(&img_output_OCV, img_output);
297 
298  return 0;
299 }
300 
301 
302 
303 
304 int closing_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
305 {
306  Mat img_input_OCV;
307  Mat img_output_OCV;
308  Mat kernel = getStructuringElement(MORPH_RECT, Size(SE_size, SE_size));
309 
310  if (img_input->type == IMAGE_INT16) {
311  img_input_OCV = Mat(img_input->h, img_input->w, CV_16S, img_input->buf);
312 
313  } else if (img_input->type == IMAGE_GRAYSCALE) {
314  img_input_OCV = Mat(img_input->h, img_input->w, CV_8UC1, img_input->buf);
315  } else {
316  std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
317  std::endl;
318  return -1;
319  }
320 
321 
322  morphologyEx(img_input_OCV, img_output_OCV, CV_MOP_CLOSE, kernel, Point(-1, -1), iteration);
323  //erode(img_input_OCV, img_output_OCV, kernel);
324  transfer(&img_output_OCV, img_output);
325  return 0;
326 }
327 
328 
329 
330 int dilation_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
331 {
332  Mat img_input_OCV;
333  Mat img_output_OCV;
334  Mat kernel = getStructuringElement(MORPH_RECT, Size(SE_size, SE_size));
335 
336  if (img_input->type == IMAGE_INT16) {
337  img_input_OCV = Mat(img_input->h, img_input->w, CV_16S, img_input->buf);
338 
339  } else if (img_input->type == IMAGE_GRAYSCALE) {
340  img_input_OCV = Mat(img_input->h, img_input->w, CV_8UC1, img_input->buf);
341  } else {
342  std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
343  std::endl;
344  return -1;
345  }
346 
347  dilate(img_input_OCV, img_output_OCV, kernel, Point(-1, -1), iteration);
348  //erode(img_input_OCV, img_output_OCV, kernel);
349  transfer(&img_output_OCV, img_output);
350  return 0;
351 }
352 
353 
354 
355 int erosion_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
356 {
357  Mat img_input_OCV;
358  Mat img_output_OCV;
359  Mat kernel = getStructuringElement(MORPH_RECT, Size(SE_size, SE_size));
360 
361  if (img_input->type == IMAGE_INT16) {
362  img_input_OCV = Mat(img_input->h, img_input->w, CV_16S, img_input->buf);
363 
364  } else if (img_input->type == IMAGE_GRAYSCALE) {
365  img_input_OCV = Mat(img_input->h, img_input->w, CV_8UC1, img_input->buf);
366  } else {
367  std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
368  std::endl;
369  return -1;
370  }
371 
372  erode(img_input_OCV, img_output_OCV, kernel, Point(-1, -1), iteration);
373  //erode(img_input_OCV, img_output_OCV, kernel);
374  transfer(&img_output_OCV, img_output);
375  return 0;
376 }
377 
378 
379 
380 int sobel_OCV(struct image_t *img_input, const struct image_t *img_output, const int kernel_size, const int thr)
381 {
382 
383  Mat img_input_OCV;
384  Mat img_grad_x, img_grad_y, img_grad_mag;
385  int ddepth = CV_32F;//CV_16S; // Format of gradient image output (CV_32F needed for normalization function)
386  int delta = 0; // Value added to each gradient pixel
387  int scale = 1; // Factor by which gradient pixels is increased
388 
389 
390  if (img_input->type == IMAGE_INT16) {
391  //std::cout << "IMAGE_OPEN_DISP" << std::endl;
392  img_input_OCV = Mat(img_input->h, img_input->w, CV_16S, img_input->buf);
393  Sobel(img_input_OCV, img_grad_x, ddepth, 1, 0, kernel_size, scale, delta, BORDER_DEFAULT); // Horizontal gradient
394  //imwrite("/home/dureade/Documents/paparazzi_images/img_grad_x.bmp", img_grad_x*-1);
395  Sobel(img_input_OCV, img_grad_y, ddepth, 0, 1, kernel_size, scale, delta, BORDER_DEFAULT); // Vertical gradient
396  //imwrite("/home/dureade/Documents/paparazzi_images/img_grad_y.bmp", img_grad_y);
397  magnitude(img_grad_x, img_grad_y, img_grad_mag); // Calculating magnitude
398 
399  //imwrite("/home/dureade/Documents/paparazzi_images/for_report/img_grad_mag.bmp", img_grad_mag);
400  threshold(img_grad_mag, img_grad_mag, thr, 127, THRESH_BINARY);
401  } else if (img_input->type == IMAGE_GRAYSCALE) {
402  //std::cout << "IMAGE_GRAYSCALE" << std::endl;
403  img_input_OCV = Mat(img_input->h, img_input->w, CV_8UC1, img_input->buf);
404  Sobel(img_input_OCV, img_grad_x, ddepth, 1, 0, kernel_size, scale, delta, BORDER_DEFAULT); // Horizontal gradient
405  //imwrite("/home/dureade/Documents/paparazzi_images/img_grad_x.bmp", img_grad_x*-1);
406  Sobel(img_input_OCV, img_grad_y, ddepth, 0, 1, kernel_size, scale, delta, BORDER_DEFAULT); // Vertical gradient
407  //imwrite("/home/dureade/Documents/paparazzi_images/img_grad_y.bmp", img_grad_y);
408  magnitude(img_grad_x, img_grad_y, img_grad_mag); // Calculating magnitude
409  //imwrite("/home/dureade/Documents/paparazzi_images/for_report/img_grad_mag.bmp", img_grad_mag);
410  threshold(img_grad_mag, img_grad_mag, thr, 127, THRESH_BINARY);
411  } else {
412  std::cout << "This function only worked with images of type IMAGE_GRAYSCALE and IAMGE_OPENCV_DISP. Leaving function." <<
413  std::endl;
414  return -1;
415  }
416 
417 
418 
419  //normalize(img_grad_mag, img_grad_mag, 0, 255, NORM_MINMAX); // Normalizing magnitude between 0 and 255
420 
421  img_grad_mag.convertTo(img_grad_mag, CV_8UC1); //Converting image to 8 bit image
422 
423  transfer(&img_grad_mag, img_output); // Saving image into output images
424 
425  Mat(img_output->h, img_output->w, CV_8UC1, img_output->buf);
426 
427 
428  /*
429  // Size of variables
430  std::cout << "img_input_OCV = " << img_input_OCV.total() * img_input_OCV.elemSize() << std::endl;
431  std::cout << "img_grad_x = " << img_grad_x.total() * img_grad_x.elemSize() << std::endl;
432  std::cout << "img_grad_y = " << img_grad_y.total() * img_grad_y.elemSize() << std::endl;
433  std::cout << "img_grad_mag = " << img_grad_mag.total() * img_grad_mag.elemSize() << std::endl;
434  */
435  /*
436  double minVal;
437  double maxVal;
438  Point minLoc;
439  Point maxLoc;
440  minMaxLoc(img_grad_mag ,&minVal, &maxVal, &minLoc, &maxLoc);
441  std::cout << "grad_x: Min=" << minVal << "; Max=" << maxVal << std::endl;
442 
443 
444  //imwrite("/home/dureade/Documents/paparazzi_images/img_grad_mag.bmp", img_grad_mag);
445  //imwrite("/home/dureade/Documents/paparazzi_images/abs_grad_y.bmp", abs_grad_y);
446  //imwrite("/home/dureade/Documents/paparazzi_images/grad.bmp", grad);
447  //imwrite("/home/dureade/Documents/paparazzi_images/img_input_OCV.bmp", img_input_OCV);
448  *
449  */
450 
451 
452 
453  return 1;
454 }
int closing_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
Definition: image.h:92
int save_image_gray(struct image_t *img, char *myString)
uint16_t h
height of the cropped area
Definition: image.h:96
Definition: image.h:44
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
int transfer(const Mat *from, const image_t *to)
struct image_t img_right
Image obtained from left camera (UYVY format)
Definition: wedgebug.c:75
uint16_t y
Start position y (vertical)
Definition: image.h:94
uint16_t x
Start position x (horizontal)
Definition: image.h:93
uint16_t w
Width of the cropped area.
Definition: image.h:95
int dilation_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
An image to hold disparity image data from openCV (int16 per pixel)
Definition: image.h:40
int save_image_color(struct image_t *img, char *myString)
uint16_t w
Image width.
Definition: image.h:46
uint16_t h
Image height.
Definition: image.h:47
void * buf
Image buffer (depending on the image_type)
Definition: image.h:54
int SBM_OCV(struct image_t *img_disp, const struct image_t *img_left, const struct image_t *img_right, const int ndisparities, const int SADWindowSize, const bool cropped)
static const float scale[]
void post_disparity_crop_rect(struct crop_t *_img_cropped_info, struct img_size_t *_original_img_dims, const int disp_n, const int block_size)
Definition: wedgebug.c:401
struct image_t img_left
Definition: wedgebug.c:74
int save_image_HM(struct image_t *img, char *myString, int const heatmap)
Grayscale image with only the Y part (uint8 per pixel)
Definition: image.h:37
int sobel_OCV(struct image_t *img_input, const struct image_t *img_output, const int kernel_size, const int thr)
int opening_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
int erosion_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
enum image_type type
The image type.
Definition: image.h:45