Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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  int32_t y = 0, x = 0;
98  int32_t c = 0;
99 
100  uint32_t idx = 0;
101 
102  int16_t image_width = (int16_t)img->w;
103  int16_t image_height = (int16_t)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 += 2) {
125  idx = interlace * (image_width * y + (x + c));
126 
127  sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx];
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 += 2) {
144  idx = interlace * (image_width * (y + c) + x);
145 
146  sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx];
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  int32_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  // TODO: replace with arm offset subtract
200  for (x = border[0]; x < border[1]; x++) {
201  if (!SHIFT_TOO_FAR) {
202  for (c = -D; c <= D; c++) {
203  SAD_temp[c + D] = 0;
204  for (r = -W; r <= W; r++) {
205  SAD_temp[c + D] += abs(edge_histogram[x + r] - edge_histogram_prev[x + r + c + der_shift]);
206  }
207  }
208  displacement[x] = (int32_t)getMinimum(SAD_temp, 2 * D + 1) - D;
209  } else {
210  }
211  }
212 }
213 
221 {
222  uint32_t i;
223  uint32_t min_ind = 0;
224  uint32_t min_err = a[min_ind];
225  uint32_t min_err_tot = 0;
226  for (i = 1; i < n; i++) {
227  if (a[i] <= min_err) {
228  min_ind = i;
229  min_err = a[i];
230  min_err_tot += min_err;
231  }
232  }
233  //*min_error = min_err_tot;
234  return min_ind;
235 }
236 
237 
247 void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border,
248  uint16_t RES)
249 {
250  int32_t x;
251 
252  int32_t count = 0;
253  int32_t sumY = 0;
254  int32_t sumX = 0;
255  int32_t sumX2 = 0;
256  int32_t sumXY = 0;
257  int32_t xMean = 0;
258  int32_t yMean = 0;
259  int32_t divergence_int = 0;
260  int32_t border_int = (int32_t)border;
261  int32_t size_int = (int32_t)size;
262  uint32_t total_error = 0;
263 
264  *divergence = 0;
265  *flow = 0;
266 
267  // compute fixed sums
268  int32_t xend = size_int - border_int - 1;
269  sumX = xend * (xend + 1) / 2 - border_int * (border_int + 1) / 2 + border_int;
270  sumX2 = xend * (xend + 1) * (2 * xend + 1) / 6 - border_int * (border_int + 1) * (2 * border_int + 1) / 6 + border_int * border_int;
271  xMean = (size_int - 1) / 2;
272  count = size_int - 2 * border_int;
273 
274  for (x = border_int; x < size_int - border_int; x++) {
275  sumY += displacement[x];
276  sumXY += x * displacement[x];
277  }
278 
279  yMean = RES * sumY / count;
280 
281  divergence_int = (RES * sumXY - sumX * yMean) / (sumX2 - sumX * xMean); // compute slope of line ax + b
282  *divergence = divergence_int;
283  *flow = yMean - *divergence * xMean; // compute b (or y) intercept of line ax + b
284 
285  for (x = border_int; x < size_int - border_int; x++) {
286  total_error += abs(RES * displacement[x] - divergence_int * x + yMean);
287  }
288 }
289 
297 void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, int32_t *edge_hist_x_prev
298  , int32_t *edge_hist_x)
299 {
300  struct point_t point1;
301  struct point_t point2;
302  struct point_t point1_prev;
303  struct point_t point2_prev;
304  struct point_t point1_extra;
305  struct point_t point2_extra;
306  uint16_t i;
307 
308  for (i = 1; i < img->w - 1; i++) {
309  point1.y = -(uint16_t)edge_hist_x[i] / 100 + img->h / 3;
310  point1.x = i;
311  point2.y = -(uint16_t)edge_hist_x[i + 1] / 100 + img->h / 3;
312  point2.x = i + 1;
313 
314  point1_prev.y = -(uint16_t)edge_hist_x_prev[i] / 100 + img->h * 2 / 3;
315  point1_prev.x = i;
316  point2_prev.y = -(uint16_t)edge_hist_x_prev[i + 1] / 100 + img->h * 2 / 3;
317  point2_prev.x = i + 1;
318 
319  image_draw_line(img, &point1, &point2);
320  image_draw_line(img, &point1_prev, &point2_prev);
321  }
322 
323  point1_extra.y = (edgeflow.flow_x + edgeflow.div_x * img->w / 2) / 100 + img->h / 2;
324  point1_extra.x = 0;
325  point2_extra.y = (edgeflow.flow_x + edgeflow.div_x * img->w / 2) / 100 + img->h / 2;
326  point2_extra.x = img->w;
327  image_draw_line(img, &point1_extra, &point2_extra);
328 }
329 
338 {
339  uint32_t amountPeaks = 0;
340  int32_t i = 0;
341 
342  for (i = 1; i < size + 1; i ++) {
343  if (edgehist[i - 1] < edgehist[i] && edgehist[i] > edgehist[i + 1] && edgehist[i] > thres) {
344  amountPeaks ++;
345  }
346  }
347  return amountPeaks;
348 }
#define RES
Definition: detect_window.c:27
uint32_t getMinimum(uint32_t *a, uint32_t n)
Calculate minimum of an array.
Definition: edge_flow.c:220
uint32_t getAmountPeaks(int32_t *edgehist, int32_t thres, int32_t size)
getAmountPeaks, calculates the amount of peaks in a edge histogram
Definition: edge_flow.c:337
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
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:297
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
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:247
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
calculate optical flow with EdgeFlow
#define DISP_RANGE_MAX
Definition: edge_flow.h:53
int32_t div_x
Definition: edge_flow.h:79
#define MAX_HORIZON
Definition: edge_flow.h:46
int32_t flow_x
Definition: edge_flow.h:78
void image_draw_line(struct image_t *img, struct point_t *from, struct point_t *to)
Draw a pink line on the image.
Definition: image.c:917
void * buf
Image buffer (depending on the image_type)
Definition: image.h:54
uint32_t x
The x coordinate of the point.
Definition: image.h:59
uint32_t y
The y coordinate of the point.
Definition: image.h:60
uint16_t h
Image height.
Definition: image.h:47
uint16_t w
Image width.
Definition: image.h:46
@ IMAGE_GRAYSCALE
Grayscale image with only the Y part (uint8 per pixel)
Definition: image.h:37
@ IMAGE_YUV422
UYVY format (uint16 per pixel)
Definition: image.h:36
Definition: image.h:44
Definition: image.h:58
int16_t flow_x
Flow in x direction from the camera (in subpixels) with X positive to the right.
int16_t flow_y
Flow in y direction from the camera (in subpixels) with Y positive to the bottom.
static uint32_t idx
uint8_t direction
uint16_t search_distance
Search distance for blockmatching alg.
struct opticflow_t opticflow[ACTIVE_CAMERAS]
Opticflow calculations.
static float D
Definition: trilateration.c:35
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98