Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
17
18#ifndef UNDISTORT_MIN_X_NORMALIZED
19#define UNDISTORT_MIN_X_NORMALIZED -2.0f
20#endif
22
23#ifndef UNDISTORT_MAX_X_NORMALIZED
24#define UNDISTORT_MAX_X_NORMALIZED 2.0f
25#endif
27
28#ifndef UNDISTORT_CENTER_RATIO
29#define UNDISTORT_CENTER_RATIO 1.00f
30#endif
32
37
38struct video_listener *listener = NULL;
39
40// Camera calibration matrix - will be filled in the init function and can be set in GUI:
41static 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
46static 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;
53 K[0] = camera_intrinsics.focal_x;
54 K[2] = camera_intrinsics.center_x;
55 K[4] = camera_intrinsics.focal_y;
56 K[5] = camera_intrinsics.center_y;
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 ) {
89 normalized_coords_to_distorted_pixels(x_n, y_n, &x_pd, &y_pd, camera_intrinsics.Dhane_k, K);
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;
113 K[0] = camera_intrinsics.focal_x;
114 K[2] = camera_intrinsics.center_x;
115 K[4] = camera_intrinsics.focal_y;
116 K[5] = camera_intrinsics.center_y;
117
122}
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
struct video_listener * listener
Definition colorfilter.c:43
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
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
static float K[9]
void undistort_image_init(void)
#define UNDISTORT_CENTER_RATIO
Maximal normalized coordinate that will be shown in the undistorted image.
static struct image_t * undistort_image_func(struct image_t *img, uint8_t camera_id)
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.
Functions for undistorting camera images.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
static uint8_t dest[]
Definition w5100.c:99