Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
pano_unwrap.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Tom van Dijk
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  */
27 
28 #include "state.h"
30 
31 #include <stdio.h>
32 
33 #ifndef PANO_UNWRAP_CAMERA
34 #define PANO_UNWRAP_CAMERA bottom_camera
35 #endif
36 
37 #ifndef PANO_UNWRAP_CENTER_X
38 #define PANO_UNWRAP_CENTER_X 0.50
39 #endif
40 #ifndef PANO_UNWRAP_CENTER_Y
41 #define PANO_UNWRAP_CENTER_Y 0.50
42 #endif
43 #ifndef PANO_UNWRAP_RADIUS_BOTTOM
44 #define PANO_UNWRAP_RADIUS_BOTTOM 0.20
45 #endif
46 #ifndef PANO_UNWRAP_RADIUS_TOP
47 #define PANO_UNWRAP_RADIUS_TOP 0.30
48 #endif
49 #ifndef PANO_UNWRAP_FORWARD_DIRECTION
50 #define PANO_UNWRAP_FORWARD_DIRECTION 270.0
51 #endif
52 #ifndef PANO_UNWRAP_FLIP_HORIZONTAL
53 #define PANO_UNWRAP_FLIP_HORIZONTAL FALSE
54 #endif
55 
56 #ifndef PANO_UNWRAP_VERTICAL_RESOLUTION
57 #define PANO_UNWRAP_VERTICAL_RESOLUTION 0.18
58 #endif
59 #ifndef PANO_UNWRAP_DEROTATE_ATTITUDE
60 #define PANO_UNWRAP_DEROTATE_ATTITUDE FALSE
61 #endif
62 
63 #ifndef PANO_UNWRAP_WIDTH
64 #define PANO_UNWRAP_WIDTH 640
65 #endif
66 #ifndef PANO_UNWRAP_HEIGHT
67 #define PANO_UNWRAP_HEIGHT 0
68 #endif
69 
70 #ifndef PANO_UNWRAP_OVERWRITE_VIDEO_THREAD
71 #define PANO_UNWRAP_OVERWRITE_VIDEO_THREAD TRUE
72 #endif
73 
74 #ifndef PANO_UNWRAP_FPS
75 #define PANO_UNWRAP_FPS 0
76 #endif
77 
79  .center = {
82  },
83  .radius_bottom = PANO_UNWRAP_RADIUS_BOTTOM,
84  .radius_top = PANO_UNWRAP_RADIUS_TOP,
85  .forward_direction = PANO_UNWRAP_FORWARD_DIRECTION,
86  .flip_horizontal = PANO_UNWRAP_FLIP_HORIZONTAL,
87 
88  .vertical_resolution = PANO_UNWRAP_VERTICAL_RESOLUTION,
89  .derotate_attitude = PANO_UNWRAP_DEROTATE_ATTITUDE,
90 
91  .width = PANO_UNWRAP_WIDTH,
92  .height = PANO_UNWRAP_HEIGHT,
93 
94  .overwrite_video_thread = PANO_UNWRAP_OVERWRITE_VIDEO_THREAD,
95 
96  .show_calibration = FALSE,
97 };
98 
100 
101 #define PIXEL_U(img,x,y) ( ((uint8_t*)((img)->buf))[4*(int)((x)/2) + 2*(y)*(img)->w] )
102 #define PIXEL_V(img,x,y) ( ((uint8_t*)((img)->buf))[4*(int)((x)/2) + 2*(y)*(img)->w + 2] )
103 #define PIXEL_Y(img,x,y) ( ((uint8_t*)((img)->buf))[2*(x) + 1 + 2*(y)*(img)->w] )
104 
105 #define RED_Y 76
106 #define RED_U 84
107 #define RED_V 255
108 #define GREEN_Y 149
109 #define GREEN_U 43
110 #define GREEN_V 21
111 #define BLUE_Y 29
112 #define BLUE_U 255
113 #define BLUE_V 107
114 
115 struct LUT_t
116 {
117  // Unwarping LUT
118  // For each pixel (u,v) in the unwrapped image, contains pixel coordinates of
119  // the same pixel in the raw image (x(u,v), y(u,v)).
122  // Derotate LUT
123  // For each bearing/column (u), contains the direction in which sampling should be
124  // shifted based on euler angles phi and theta dxy(phi|u) dxy(theta|u).
125  struct FloatVect2 *dphi;
127  // Settings for which the LUT was generated
129 };
130 static struct LUT_t LUT; // Note: initialized NULL
131 
132 static void set_output_image_size(void)
133 {
134  // Find vertical size of output image
135  if (pano_unwrap.height == 0) {
136  pano_unwrap.height = (uint16_t) (fabsf((pano_unwrap.width / (2 * M_PI))
137  * (pano_unwrap.radius_bottom - pano_unwrap.radius_top)
138  / pano_unwrap.vertical_resolution) + 0.5);
139  printf("[pano_unwrap] Automatic output image height: %d\n",
140  pano_unwrap.height);
141  }
142  // Replace output image if required
143  if (pano_unwrapped_image.w != pano_unwrap.width ||
144  pano_unwrapped_image.h != pano_unwrap.height) {
145  printf("[pano_unwrap] Creating output image with size %d x %d\n",
146  pano_unwrap.width, pano_unwrap.height);
148  image_create(&pano_unwrapped_image, pano_unwrap.width, pano_unwrap.height,
149  IMAGE_YUV422);
150  }
151 }
152 
153 static void update_LUT(const struct image_t *img)
154 {
155  if (LUT.settings.center.x == pano_unwrap.center.x &&
156  LUT.settings.center.y == pano_unwrap.center.y &&
157  LUT.settings.radius_bottom == pano_unwrap.radius_bottom &&
158  LUT.settings.radius_top == pano_unwrap.radius_top &&
160  LUT.settings.flip_horizontal == pano_unwrap.flip_horizontal &&
161  LUT.settings.width == pano_unwrap.width &&
162  LUT.settings.height == pano_unwrap.height) {
163  // LUT is still valid, do nothing
164  return;
165  }
166 
167  printf("[pano_unwrap] Regenerating LUT... ");
168  // Remove old data
169  if (LUT.x) {
170  free(LUT.x);
171  LUT.x = NULL;
172  }
173  if (LUT.y) {
174  free(LUT.y);
175  LUT.y = NULL;
176  }
177  if (LUT.dphi) {
178  free(LUT.dphi);
179  LUT.dphi = NULL;
180  }
181  if (LUT.dtheta) {
182  free(LUT.dtheta);
183  LUT.dtheta = NULL;
184  }
185  // Generate unwarping LUT
186  LUT.x = malloc(sizeof(*LUT.x) * pano_unwrap.width * pano_unwrap.height);
187  LUT.y = malloc(sizeof(*LUT.y) * pano_unwrap.width * pano_unwrap.height);
188  if (!LUT.x || !LUT.y) {
189  printf("[pano_unwrap] ERROR could not allocate x or y lookup table!\n");
190  } else {
191  for (uint16_t u = 0; u < pano_unwrap.width; u++) {
192  float bearing = -M_PI + (float) u / pano_unwrapped_image.w * 2 * M_PI;
193  float angle = (pano_unwrap.forward_direction / 180.0 * M_PI)
194  + bearing * (pano_unwrap.flip_horizontal ? 1.f : -1.f);
195  float c = cosf(angle);
196  float s = sinf(angle);
197  for (uint16_t v = 0; v < pano_unwrap.height; v++) {
198  float radius = pano_unwrap.radius_top
199  + (pano_unwrap.radius_bottom - pano_unwrap.radius_top)
200  * ((float) v / (pano_unwrap.height - 1));
201  LUT.x[v + u * pano_unwrap.height] = (uint16_t) (img->w
202  * pano_unwrap.center.x + c * radius * img->h + 0.5);
203  LUT.y[v + u * pano_unwrap.height] = (uint16_t) (img->h
204  * pano_unwrap.center.y - s * radius * img->h + 0.5);
205  }
206  }
207  }
208  // Generate derotation LUT
209  LUT.dphi = malloc(sizeof(*LUT.dphi) * pano_unwrap.width);
210  LUT.dtheta = malloc(sizeof(*LUT.dtheta) * pano_unwrap.width);
211  if (!LUT.dphi || !LUT.dtheta) {
212  printf(
213  "[pano_unwrap] ERROR could not allocate dphi or dtheta lookup table!\n");
214  } else {
215  for (uint16_t u = 0; u < pano_unwrap.width; u++) {
216  float bearing = -M_PI + (float) u / pano_unwrapped_image.w * 2 * M_PI;
217  float angle = (pano_unwrap.forward_direction / 180.0 * M_PI)
218  + bearing * (pano_unwrap.flip_horizontal ? 1.f : -1.f);
219  LUT.dphi[u].x = sinf(bearing) * cosf(angle);
220  LUT.dphi[u].y = sinf(bearing) * -sinf(angle);
221  LUT.dtheta[u].x = cosf(bearing) * cosf(angle);
222  LUT.dtheta[u].y = cosf(bearing) * -sinf(angle);
223  }
224  }
225  // Keep track of settings for which this LUT was generated
227  printf("ok\n");
228 }
229 
230 static void unwrap_LUT(struct image_t *img_raw, struct image_t *img)
231 {
232  for (uint16_t u = 0; u < pano_unwrap.width; u++) {
233  // Derotation offset for this bearing
234  int16_t dx = 0;
235  int16_t dy = 0;
236  if (pano_unwrap.derotate_attitude) {
237  // Look up correction
238  const struct FloatRMat *R = stateGetNedToBodyRMat_f();
239  dx = (int16_t) (img_raw->h * pano_unwrap.vertical_resolution
240  * (LUT.dphi[u].x * MAT33_ELMT(*R, 1, 2)
241  + LUT.dtheta[u].x * MAT33_ELMT(*R, 0, 2)));
242  dy = (int16_t) (img_raw->h * pano_unwrap.vertical_resolution
243  * (LUT.dphi[u].y * MAT33_ELMT(*R, 1, 2)
244  + LUT.dtheta[u].y * MAT33_ELMT(*R, 0, 2)));
245  }
246  // Fill this column of unwrapped image
247  for (uint16_t v = 0; v < pano_unwrap.height; v++) {
248  // Look up sampling point
249  int16_t x = (int16_t) LUT.x[v + u * pano_unwrap.height] + dx;
250  int16_t y = (int16_t) LUT.y[v + u * pano_unwrap.height] + dy;
251  // Check bounds
252  if (x < 0) {
253  x = 0;
254  } else if (x > img_raw->w - 1) {
255  x = img_raw->w - 1;
256  }
257  if (y < 0) {
258  y = 0;
259  } else if (y > img_raw->h - 1) {
260  y = img_raw->h - 1;
261  }
262  // Draw calibration pattern
263  if (pano_unwrap.show_calibration) {
264  if (v == 0 || v == pano_unwrap.height - 1) {
265  PIXEL_U(img_raw,x,y) = BLUE_U;
266  PIXEL_V(img_raw,x,y) = BLUE_V;
267  }
268  if ((u == pano_unwrap.width / 2)
269  || (u == pano_unwrap.width / 2 + 1)) {
270  PIXEL_Y(img_raw,x,y) = RED_Y;
271  PIXEL_U(img_raw,x,y) = RED_U;
272  PIXEL_V(img_raw,x,y) = RED_V;
273  }
274  if ((u == 3 * pano_unwrap.width / 4)
275  || (u == 3 * pano_unwrap.width / 4 + 1)) {
276  PIXEL_Y(img_raw,x,y) = GREEN_Y;
277  PIXEL_U(img_raw,x,y) = GREEN_U;
278  PIXEL_V(img_raw,x,y) = GREEN_V;
279  }
280  }
281  // Copy pixel values
282  PIXEL_Y(img,u,v) = PIXEL_Y(img_raw, x, y);
283  PIXEL_U(img,u,v) = PIXEL_U(img_raw, x, y);
284  PIXEL_V(img,u,v) = PIXEL_V(img_raw, x, y);
285  }
286  }
287  // Draw lens center
288  if (pano_unwrap.show_calibration) {
289  uint16_t x = pano_unwrap.center.x * img_raw->w;
290  uint16_t y = pano_unwrap.center.y * img_raw->h;
291  for (int i = -5; i <= 5; i++) {
292  for (int j = -5; j <= 5; j++) {
293  if (i == 0 || j == 0) {
294  PIXEL_Y(img_raw, x+i, y+j) = BLUE_Y;
295  PIXEL_U(img_raw, x+i, y+j) = BLUE_U;
296  PIXEL_V(img_raw, x+i, y+j) = BLUE_V;
297  }
298  }
299  }
300  }
301 }
302 
303 static struct image_t *camera_cb(struct image_t *img)
304 {
306  update_LUT(img);
308  pano_unwrapped_image.ts = img->ts;
310  return pano_unwrap.overwrite_video_thread ? &pano_unwrapped_image : NULL;
311 }
312 
314 {
318 }
319 
float radius_top
Distance from center point to top of region of interest [fraction of image height].
Definition: pano_unwrap.h:35
struct image_t pano_unwrapped_image
Unwrapped panoramic image.
Definition: pano_unwrap.c:99
unsigned short uint16_t
Definition: types.h:16
float vertical_resolution
Vertical resolution of raw image in the region of interest, used for attitude derotation [fraction of...
Definition: pano_unwrap.h:39
bool show_calibration
Draw calibration pattern on raw image.
Definition: pano_unwrap.h:47
static uint32_t s
#define GREEN_V
Definition: pano_unwrap.c:110
#define GREEN_Y
Definition: pano_unwrap.c:108
struct pano_unwrap_t pano_unwrap
Definition: pano_unwrap.c:78
#define GREEN_U
Definition: pano_unwrap.c:109
void image_free(struct image_t *img)
Free the image.
Definition: image.c:69
#define PANO_UNWRAP_WIDTH
Definition: pano_unwrap.c:64
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
float forward_direction
Angle [deg] in raw image that corresponds to the forward direction, where 0 points right and the valu...
Definition: pano_unwrap.h:36
Definition: image.h:43
#define PANO_UNWRAP_FLIP_HORIZONTAL
Definition: pano_unwrap.c:53
#define PANO_UNWRAP_RADIUS_BOTTOM
Definition: pano_unwrap.c:44
static struct LUT_t LUT
Definition: pano_unwrap.c:130
struct FloatVect2 * dtheta
Definition: pano_unwrap.c:126
uint32_t pprz_ts
The timestamp in us since system startup.
Definition: image.h:49
bool derotate_attitude
Set to true if roll/pitch movement should be corrected.
Definition: pano_unwrap.h:40
static struct image_t * camera_cb(struct image_t *img)
Definition: pano_unwrap.c:303
#define PIXEL_U(img, x, y)
Definition: pano_unwrap.c:101
struct FloatVect2 * dphi
Definition: pano_unwrap.c:125
#define BLUE_Y
Definition: pano_unwrap.c:111
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps)
Definition: cv.c:46
#define FALSE
Definition: std.h:5
uint16_t height
Height of unwrapped image. Set to 0 (default) to determine automatically from unwrapped_width, radius_bottom, _top and vertical_resolution.
Definition: pano_unwrap.h:43
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
#define PANO_UNWRAP_FPS
Definition: pano_unwrap.c:75
#define RED_V
Definition: pano_unwrap.c:107
#define MAT33_ELMT(_m, _row, _col)
Definition: pprz_algebra.h:436
uint16_t w
Image width.
Definition: image.h:45
Computer vision framework for onboard processing.
uint16_t h
Image height.
Definition: image.h:46
signed short int16_t
Definition: types.h:17
#define PANO_UNWRAP_CENTER_X
Definition: pano_unwrap.c:38
#define PANO_UNWRAP_FORWARD_DIRECTION
Definition: pano_unwrap.c:50
#define PIXEL_V(img, x, y)
Definition: pano_unwrap.c:102
#define RED_U
Definition: pano_unwrap.c:106
struct FloatVect2 center
Center point of panoramic lens [fraction of image width, height].
Definition: pano_unwrap.h:33
void pano_unwrap_init()
Definition: pano_unwrap.c:313
#define BLUE_V
Definition: pano_unwrap.c:113
#define PANO_UNWRAP_OVERWRITE_VIDEO_THREAD
Definition: pano_unwrap.c:71
#define PANO_UNWRAP_DEROTATE_ATTITUDE
Definition: pano_unwrap.c:60
API to get/set the generic vehicle states.
#define PIXEL_Y(img, x, y)
Definition: pano_unwrap.c:103
#define PANO_UNWRAP_VERTICAL_RESOLUTION
Definition: pano_unwrap.c:57
static void update_LUT(const struct image_t *img)
Definition: pano_unwrap.c:153
struct timeval ts
The timestamp of creation.
Definition: image.h:47
#define PANO_UNWRAP_CENTER_Y
Definition: pano_unwrap.c:41
bool overwrite_video_thread
Set to true if the unwrapped image should be returned to the video thread.
Definition: pano_unwrap.h:45
#define PANO_UNWRAP_RADIUS_TOP
Definition: pano_unwrap.c:47
uint16_t * y
Definition: pano_unwrap.c:121
float radius_bottom
Distance from center point to bottom of region of interest [fraction of image height].
Definition: pano_unwrap.h:34
UYVY format (uint16 per pixel)
Definition: image.h:36
rotation matrix
static void set_output_image_size(void)
Definition: pano_unwrap.c:132
#define RED_Y
Definition: pano_unwrap.c:105
bool flip_horizontal
Set to true to horizontally flip the unwrapped image.
Definition: pano_unwrap.h:37
struct pano_unwrap_t settings
Definition: pano_unwrap.c:128
#define BLUE_U
Definition: pano_unwrap.c:112
#define PANO_UNWRAP_CAMERA
Definition: pano_unwrap.c:34
uint16_t width
Width of unwrapped image.
Definition: pano_unwrap.h:42
#define PANO_UNWRAP_HEIGHT
Definition: pano_unwrap.c:67
uint16_t * x
Definition: pano_unwrap.c:120
static void unwrap_LUT(struct image_t *img_raw, struct image_t *img)
Definition: pano_unwrap.c:230