Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
undistort_image.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018, Guido de Croon
3  *
4  * @file modules/computer_vision/undistort_image.c
5  */
6 
7 // Own header
9 #include <stdio.h>
12 
13 #ifndef UNDISTORT_FPS
14 #define UNDISTORT_FPS 0
15 #endif
16 PRINT_CONFIG_VAR(UNDISTORT_FPS)
17 
18 #ifndef UNDISTORT_MIN_X_NORMALIZED
19 #define UNDISTORT_MIN_X_NORMALIZED -2.0f
20 #endif
21 PRINT_CONFIG_VAR(UNDISTORT_MIN_X_NORMALIZED)
22 
23 #ifndef UNDISTORT_MAX_X_NORMALIZED
24 #define UNDISTORT_MAX_X_NORMALIZED 2.0f
25 #endif
26 PRINT_CONFIG_VAR(UNDISTORT_MAX_X_NORMALIZED)
27 
28 #ifndef UNDISTORT_CENTER_RATIO
29 #define UNDISTORT_CENTER_RATIO 1.00f
30 #endif
31 PRINT_CONFIG_VAR(UNDISTORT_CENTER_RATIO)
32 
37 
38 struct video_listener *listener = NULL;
39 
40 // Camera calibration matrix - will be filled in the init function and can be set in GUI:
41 static float K[9] = {0.0f, 0.0f, 0.0f,
42  0.0f, 0.0f, 0.0f,
43  0.0f, 0.0f, 1.0f};
44 
45 // Function
46 static struct image_t *undistort_image_func(struct image_t *img, uint8_t camera_id)
47 {
48  // TODO: These commands could actually only be run when the parameters or image size are changed
49  float normalized_step = (max_x_normalized - min_x_normalized) / img->w;
50  float h_w_ratio = img->h / (float) img->w;
51  float min_y_normalized = h_w_ratio * min_x_normalized;
52  float max_y_normalized = h_w_ratio * max_x_normalized;
57 
58  // create an image of the same size:
59  struct image_t img_distorted;
60  image_create(&img_distorted, img->w, img->h, img->type);
61 
62  uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1;
63 
64  image_copy(img, &img_distorted);
65 
66  uint8_t *dest = (uint8_t *)img->buf;
67  uint8_t *source = (uint8_t *)img_distorted.buf;
68  uint32_t index_dest, index_src;
69 
70  // set all pixels to black:
71  for(uint32_t x = 0; x < img->w; x++) {
72  for(uint32_t y = 0; y < img->h; y++) {
73  index_dest = pixel_width*(y*img->w+x);
74  dest[index_dest] = 128; // grey
75  dest[index_dest+1] = 0; // black
76  }
77  }
78 
79  // fill the image again, now with the undistorted image:
80  float x_pd, y_pd;
81  uint32_t x_pd_ind, y_pd_ind;
82  uint32_t x = 0;
83  for(float x_n = min_x_normalized; x_n < max_x_normalized; x_n += normalized_step, x++) {
84  uint32_t y = 0;
85  for(float y_n = min_y_normalized; y_n < max_y_normalized; y_n += normalized_step, y++) {
86  if(center_ratio == 1.0f ||
87  (x_n > center_ratio * min_x_normalized && x_n < center_ratio * max_x_normalized && y_n > center_ratio * min_y_normalized && y_n < center_ratio * max_y_normalized)
88  ) {
90  if(x_pd > 0.0f && y_pd > 0.0f) {
91  x_pd_ind = (uint32_t) x_pd;
92  y_pd_ind = (uint32_t) y_pd;
93  if(x_pd_ind < img->w && y_pd_ind < img->h) {
94  // Assuming UY VY (2 bytes per pixel, and U for even indices, V for odd indices)
95  index_dest = pixel_width*(y*img->w+x);
96  index_src = pixel_width*(y_pd_ind*img_distorted.w+x_pd_ind);
97  dest[index_dest] = 128; // source[index_src]; // Colors will be a pain for undistortion...
98  dest[index_dest+1] = source[index_src+1]; // no interpolation or anything, just the basics for now.
99  }
100  }
101  }
102  }
103  }
104 
105  image_free(&img_distorted);
106  return img;
107 }
108 
110 {
111  // set the calibration matrix
112  camera_intrinsics = UNDISTORT_CAMERA.camera_intrinsics;
117 
122 }
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id)
Definition: cv.c:46
void image_copy(struct image_t *input, struct image_t *output)
Copy an image from inut to output This will only work if the formats are the same.
Definition: image.c:89
void image_free(struct image_t *img)
Free the image.
Definition: image.c:75
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:43
Image helper functions like resizing, color filter, converters...
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_YUV422
UYVY format (uint16 per pixel)
Definition: image.h:36
Definition: image.h:44
struct video_listener * listener
static float K[9]
static struct image_t * undistort_image_func(struct image_t *img, uint8_t camera_id)
void undistort_image_init(void)
#define UNDISTORT_CENTER_RATIO
Maximal normalized coordinate that will be shown in the undistorted image.
float center_ratio
struct camera_intrinsics_t camera_intrinsics
#define UNDISTORT_MAX_X_NORMALIZED
Maximal normalized coordinate that will be shown in the undistorted image.
#define UNDISTORT_MIN_X_NORMALIZED
Minimal normalized coordinate that will be shown in the undistorted image.
float max_x_normalized
#define UNDISTORT_FPS
Default FPS (zero means run at camera fps)
float min_x_normalized
bool normalized_coords_to_distorted_pixels(float x_n, float y_n, float *x_pd, float *y_pd, float k, const float *K)
Transform normalized coordinates to distorted pixel coordinates.
Definition: undistortion.c:146
Functions for undistorting camera images.
float focal_y
focal length in the y-direction in pixels
Definition: video_device.h:48
float center_x
center image coordinate in the x-direction
Definition: video_device.h:49
float focal_x
focal length in the x-direction in pixels
Definition: video_device.h:47
float Dhane_k
(un)distortion parameter for a fish-eye lens
Definition: video_device.h:51
float center_y
center image coordinate in the y-direction
Definition: video_device.h:50
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
static uint8_t dest[]
Definition: w5100.c:99