Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
edge_flow.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Kimberly McGuire <k.n.mcguire@tudelft.nl
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  */
20 
30 #include <lib/vision/edge_flow.h>
39 void calc_previous_frame_nr(struct opticflow_result_t *result, struct opticflow_t *opticflow, uint8_t current_frame_nr,
40  uint8_t *previous_frame_offset, uint8_t *previous_frame_nr)
41 {
42  // Adaptive Time Horizon:
43  // if the flow measured in previous frame is small,
44  // the algorithm will choose an frame further away back from the
45  // current frame to detect subpixel flow
46  if (MAX_HORIZON > 2) {
47 
48  uint32_t flow_mag_x, flow_mag_y;
49  flow_mag_x = abs(result->flow_x);
50  flow_mag_y = abs(result->flow_y);
51  uint32_t min_flow = 3;
52  uint32_t max_flow = opticflow->search_distance - 3;
53 
54  uint8_t previous_frame_offset_x = previous_frame_offset[0];
55  uint8_t previous_frame_offset_y = previous_frame_offset[1];
56 
57  // IF statements which will decrement the previous frame offset
58  // if the measured flow of last loop is higher than max value (higher flow measured)
59  // and visa versa
60  if (flow_mag_x > max_flow && previous_frame_offset_x > 1) {
61  previous_frame_offset[0] = previous_frame_offset_x - 1;
62  }
63  if (flow_mag_x < min_flow && previous_frame_offset_x < MAX_HORIZON - 1) {
64  previous_frame_offset[0] = previous_frame_offset_x + 1;
65  }
66  if (flow_mag_y > max_flow && previous_frame_offset_y > 1) {
67  previous_frame_offset[1] = previous_frame_offset_y - 1;
68  }
69  if (flow_mag_y < min_flow && previous_frame_offset_y < MAX_HORIZON - 1) {
70  previous_frame_offset[1] = previous_frame_offset_y + 1;
71  }
72  }
73 
74  //Wrap index previous frame offset from current frame nr.
75  previous_frame_nr[0] = (current_frame_nr - previous_frame_offset[0] + MAX_HORIZON) %
77  previous_frame_nr[1] = (current_frame_nr - previous_frame_offset[1] + MAX_HORIZON) %
79 }
80 
88 void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
89  char direction, uint16_t edge_threshold)
90 {
91  uint8_t *img_buf = (uint8_t *)img->buf;
92 
93  // TODO use arm_conv_q31()
94  int32_t sobel_sum = 0;
95  int32_t Sobel[3] = { -1, 0, 1};
96 
97  uint32_t y = 0, x = 0;
98  int32_t c = 0;
99 
100  uint32_t idx = 0;
101 
102  uint16_t image_width = img->w;
103  uint16_t image_height = img->h;
104  uint32_t interlace;
105  if (img->type == IMAGE_GRAYSCALE) {
106  interlace = 1;
107  } else {
108  if (img->type == IMAGE_YUV422) {
109  interlace = 2;
110  } else
111  while (1); // hang to show user something isn't right
112  }
113 
114 
115  // compute edge histogram
116  if (direction == 'x') {
117  // set values that are not visited
118  edge_histogram[0] = edge_histogram[image_width - 1] = 0;
119  for (x = 1; x < image_width - 1; x++) {
120  edge_histogram[x] = 0;
121  for (y = 0; y < image_height; y++) {
122  sobel_sum = 0;
123 
124  for (c = -1; c <= 1; c++) {
125  idx = interlace * (image_width * y + (x + c)); // 2 for interlace
126 
127  sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx + 1];
128  }
129  sobel_sum = abs(sobel_sum);
130  if (sobel_sum > edge_threshold) {
131  edge_histogram[x] += sobel_sum;
132  }
133  }
134  }
135  } else if (direction == 'y') {
136  // set values that are not visited
137  edge_histogram[0] = edge_histogram[image_height - 1] = 0;
138  for (y = 1; y < image_height - 1; y++) {
139  edge_histogram[y] = 0;
140  for (x = 0; x < image_width; x++) {
141  sobel_sum = 0;
142 
143  for (c = -1; c <= 1; c++) {
144  idx = interlace * (image_width * (y + c) + x); // 2 for interlace
145 
146  sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx + 1];
147  }
148  sobel_sum = abs(sobel_sum);
149  if (sobel_sum > edge_threshold) {
150  edge_histogram[y] += sobel_sum;
151  }
152  }
153  }
154  } else
155  while (1); // hang to show user something isn't right
156 }
157 
168 void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogram_prev, int32_t *displacement,
169  uint16_t size,
170  uint8_t window, uint8_t disp_range, int32_t der_shift)
171 {
172  int32_t c = 0, r = 0;
173  uint32_t x = 0;
174  uint32_t SAD_temp[2 * DISP_RANGE_MAX + 1]; // size must be at least 2*D + 1
175 
176  int32_t W = window;
177  int32_t D = disp_range;
178 
179 
180  uint8_t SHIFT_TOO_FAR = 0;
181  memset(displacement, 0, sizeof(int32_t)*size);
182 
183  int32_t border[2];
184 
185  if (der_shift < 0) {
186  border[0] = W + D + der_shift;
187  border[1] = size - W - D;
188  } else if (der_shift > 0) {
189  border[0] = W + D;
190  border[1] = size - W - D - der_shift;
191  } else {
192  border[0] = W + D;
193  border[1] = size - W - D;
194  }
195 
196  if (border[0] >= border[1] || abs(der_shift) >= 10) {
197  SHIFT_TOO_FAR = 1;
198  }
199  {
200  // TODO: replace with arm offset subtract
201  for (x = border[0]; x < border[1]; x++) {
202  displacement[x] = 0;
203  if (!SHIFT_TOO_FAR) {
204  for (c = -D; c <= D; c++) {
205  SAD_temp[c + D] = 0;
206  for (r = -W; r <= W; r++) {
207  SAD_temp[c + D] += abs(edge_histogram[x + r] - edge_histogram_prev[x + r + c + der_shift]);
208  }
209  }
210  displacement[x] = (int32_t)getMinimum(SAD_temp, 2 * D + 1) - D;
211  } else {
212  }
213  }
214  }
215 
216 }
217 
225 {
226  uint32_t i;
227  uint32_t min_ind = 0;
228  uint32_t min_err = a[min_ind];
229  uint32_t min_err_tot = 0;
230  for (i = 1; i < n; i++) {
231  if (a[i] <= min_err) {
232  min_ind = i;
233  min_err = a[i];
234  min_err_tot += min_err;
235  }
236  }
237  //*min_error = min_err_tot;
238  return min_ind;
239 }
240 
241 
251 void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border,
252  uint16_t RES)
253 {
254  int32_t x;
255 
256  int32_t count = 0;
257  int32_t sumY = 0;
258  int32_t sumX = 0;
259  int32_t sumX2 = 0;
260  int32_t sumXY = 0;
261  int32_t xMean = 0;
262  int32_t yMean = 0;
263  int32_t divergence_int = 0;
264  int32_t border_int = (int32_t)border;
265  int32_t size_int = (int32_t)size;
266  uint32_t total_error = 0;
267 
268  *divergence = 0;
269  *flow = 0;
270 
271  // compute fixed sums
272  int32_t xend = size_int - border_int - 1;
273  sumX = xend * (xend + 1) / 2 - border_int * (border_int + 1) / 2 + border_int;
274  sumX2 = xend * (xend + 1) * (2 * xend + 1) / 6;
275  xMean = (size_int - 1) / 2;
276  count = size_int - 2 * border_int;
277 
278  for (x = border_int; x < size - border_int; x++) {
279  sumY += displacement[x];
280  sumXY += x * displacement[x];
281  }
282 
283  yMean = RES * sumY / count;
284 
285  divergence_int = (RES * sumXY - sumX * yMean) / (sumX2 - sumX * xMean); // compute slope of line ax + b
286  *divergence = divergence_int;
287  *flow = yMean - *divergence * xMean; // compute b (or y) intercept of line ax + b
288 
289  for (x = border_int; x < size - border_int; x++) {
290  total_error += abs(RES * displacement[x] - divergence_int * x + yMean);
291  }
292 }
293 
301 void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, int32_t *edge_hist_x_prev
302  , int32_t *edge_hist_x)
303 {
304  struct point_t point1;
305  struct point_t point2;
306  struct point_t point1_prev;
307  struct point_t point2_prev;
308  struct point_t point1_extra;
309  struct point_t point2_extra;
310  uint16_t i;
311 
312  for (i = 1; i < img->w - 1; i++) {
313  point1.y = -(uint16_t)edge_hist_x[i] / 100 + img->h / 3;
314  point1.x = i;
315  point2.y = -(uint16_t)edge_hist_x[i + 1] / 100 + img->h / 3;
316  point2.x = i + 1;
317 
318  point1_prev.y = -(uint16_t)edge_hist_x_prev[i] / 100 + img->h * 2 / 3;
319  point1_prev.x = i;
320  point2_prev.y = -(uint16_t)edge_hist_x_prev[i + 1] / 100 + img->h * 2 / 3;
321  point2_prev.x = i + 1;
322 
323  image_draw_line(img, &point1, &point2);
324  image_draw_line(img, &point1_prev, &point2_prev);
325  }
326 
327  point1_extra.y = (edgeflow.flow_x + edgeflow.div_x * img->w / 2) / 100 + img->h / 2;
328  point1_extra.x = 0;
329  point2_extra.y = (edgeflow.flow_x + edgeflow.div_x * img->w / 2) / 100 + img->h / 2;
330  point2_extra.x = img->w;
331  image_draw_line(img, &point1_extra, &point2_extra);
332 }
333 
342 {
343  uint32_t amountPeaks = 0;
344  uint32_t i = 0;
345 
346  for (i = 1; i < size + 1; i ++) {
347  if (edgehist[i - 1] < edgehist[i] && edgehist[i] > edgehist[i + 1] && edgehist[i] > thres) {
348  amountPeaks ++;
349  }
350  }
351  return amountPeaks;
352 }
unsigned short uint16_t
Definition: types.h:16
int16_t flow_y
Flow in y direction from the camera (in subpixels)
void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogram_prev, int32_t *displacement, uint16_t size, uint8_t window, uint8_t disp_range, int32_t der_shift)
Calculate_displacement calculates the displacement between two histograms.
Definition: edge_flow.c:168
int32_t flow_x
Definition: edge_flow.h:77
static uint32_t idx
struct opticflow_t opticflow
Opticflow calculations.
calculate optical flow with EdgeFlow
Definition: image.h:42
int32_t div_x
Definition: edge_flow.h:78
void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, int32_t *edge_hist_x_prev, int32_t *edge_hist_x)
Draws edgehistogram, displacement and linefit directly on the image for debugging (only for edgeflow ...
Definition: edge_flow.c:301
void image_draw_line(struct image_t *img, struct point_t *from, struct point_t *to)
Draw a line on the image.
Definition: image.c:599
#define RES
Definition: detect_window.c:27
uint32_t x
The x coordinate of the point.
Definition: image.h:56
void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[], char direction, uint16_t edge_threshold)
Calculate a edge/gradient histogram for each dimension of the image.
Definition: edge_flow.c:88
#define MAX_HORIZON
Definition: edge_flow.h:45
uint16_t w
Image width.
Definition: image.h:44
unsigned long uint32_t
Definition: types.h:18
uint16_t h
Image height.
Definition: image.h:45
uint32_t getMinimum(uint32_t *a, uint32_t n)
Calculate minimum of an array.
Definition: edge_flow.c:224
void * buf
Image buffer (depending on the image_type)
Definition: image.h:51
uint32_t y
The y coordinate of the point.
Definition: image.h:57
Definition: image.h:55
signed long int32_t
Definition: types.h:19
float divergence
unsigned char uint8_t
Definition: types.h:14
uint8_t direction
UYVY format (uint16 per pixel)
Definition: image.h:35
Grayscale image with only the Y part (uint8 per pixel)
Definition: image.h:36
void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border, uint16_t RES)
Fits a linear model to an array with pixel displacements with least squares.
Definition: edge_flow.c:251
void calc_previous_frame_nr(struct opticflow_result_t *result, struct opticflow_t *opticflow, uint8_t current_frame_nr, uint8_t *previous_frame_offset, uint8_t *previous_frame_nr)
Calc_previous_frame_nr; adaptive Time Horizon.
Definition: edge_flow.c:39
int16_t flow_x
Flow in x direction from the camera (in subpixels)
uint32_t getAmountPeaks(int32_t *edgehist, uint32_t thres, int32_t size)
getAmountPeaks, calculates the amount of peaks in a edge histogram
Definition: edge_flow.c:341
#define DISP_RANGE_MAX
Definition: edge_flow.h:52
uint16_t search_distance
Search distance for blockmatching alg.