Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
31using namespace cv;
32
33
34
35// Local declarations
36int transfer(const Mat *from, const image_t *to);
37
38
39// Local functions
40int 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
69int 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
94int 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
116int 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
168int 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
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
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
277int 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
304int 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
330int 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
355int 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
380int 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}
#define Ptr(x)
macro to use pointers in flight plans XML
void * buf
Image buffer (depending on the image_type).
Definition image.h:54
uint16_t h
Image height.
Definition image.h:47
uint16_t w
Image width.
Definition image.h:46
enum image_type type
The image type.
Definition image.h:45
@ IMAGE_GRAYSCALE
Grayscale image with only the Y part (uint8 per pixel).
Definition image.h:37
@ IMAGE_INT16
An image to hold disparity image data from openCV (int16 per pixel).
Definition image.h:40
Definition image.h:92
static const float scale[]
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
struct image_t img_left
Definition wedgebug.c:74
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 crop_t img_cropped_info
Definition wedgebug.c:92
struct image_t img_right
Image obtained from left camera (UYVY format).
Definition wedgebug.c:75
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)
int closing_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
int sobel_OCV(struct image_t *img_input, const struct image_t *img_output, const int kernel_size, const int thr)
int save_image_gray(struct image_t *img, char *myString)
int dilation_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
int transfer(const Mat *from, const image_t *to)
int save_image_color(struct image_t *img, char *myString)
int erosion_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
int opening_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
int save_image_HM(struct image_t *img, char *myString, int const heatmap)