Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
size_divergence.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Guido de Croon <guido.de.croon@gmail.com>
3  *
4  * From:
5  * Characterization of Flow Field Divergence for Vertical Landing Control of MAVs
6  * by H.W. Ho and G.C.H.E. de Croon (submitted)
7  *
8  * This file is part of Paparazzi.
9  *
10  * Paparazzi is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2, or (at your option)
13  * any later version.
14  *
15  * Paparazzi is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with Paparazzi; see the file COPYING. If not, see
22  * <http://www.gnu.org/licenses/>.
23  */
24 
32 #include "size_divergence.h"
33 #include <stdlib.h>
34 
42 float get_size_divergence(struct flow_t *vectors, int count, int n_samples)
43 {
44  float distance_1, distance_2;
45  float divs_sum = 0.f;
46  uint32_t used_samples = 0;
47  float dx, dy;
48  int32_t i, j;
49 
50  int32_t max_samples = (count * count - count) / 2;
51 
52  if (count < 2) {
53  return 0.f;
54  } else if (count >= max_samples) {
55  n_samples = 0;
56  }
57 
58  if (n_samples == 0) {
59  // go through all possible lines:
60  for (i = 0; i < count; i++) {
61  for (j = i + 1; j < count; j++) {
62  // distance in previous image:
63  dx = (float)vectors[i].pos.x - (float)vectors[j].pos.x;
64  dy = (float)vectors[i].pos.y - (float)vectors[j].pos.y;
65  distance_1 = sqrtf(dx * dx + dy * dy);
66 
67  if (distance_1 < 1E-5) {
68  continue;
69  }
70 
71  // distance in current image:
72  dx = (float)vectors[i].pos.x + (float)vectors[i].flow_x - (float)vectors[j].pos.x - (float)vectors[j].flow_x;
73  dy = (float)vectors[i].pos.y + (float)vectors[i].flow_y - (float)vectors[j].pos.y - (float)vectors[j].flow_y;
74  distance_2 = sqrtf(dx * dx + dy * dy);
75 
76  divs_sum += (distance_2 - distance_1) / distance_1;
77  used_samples++;
78  }
79  }
80  } else {
81  // take random samples:
82  for (uint16_t sample = 0; sample < n_samples; sample++) {
83  // take two random indices:
84  i = rand() % count;
85  j = rand() % count;
86  // ensure it is not the same index:
87  while (i == j) {
88  j = rand() % count;
89  }
90 
91  // distance in previous image:
92  dx = (float)vectors[i].pos.x - (float)vectors[j].pos.x;
93  dy = (float)vectors[i].pos.y - (float)vectors[j].pos.y;
94  distance_1 = sqrtf(dx * dx + dy * dy);
95 
96  if (distance_1 < 1E-5) {
97  continue;
98  }
99 
100  // distance in current image:
101  dx = (float)vectors[i].pos.x + (float)vectors[i].flow_x - (float)vectors[j].pos.x - (float)vectors[j].flow_x;
102  dy = (float)vectors[i].pos.y + (float)vectors[i].flow_y - (float)vectors[j].pos.y - (float)vectors[j].flow_y;
103  distance_2 = sqrtf(dx * dx + dy * dy);
104 
105  divs_sum += (distance_2 - distance_1) / distance_1;
106  used_samples++;
107  }
108  }
109 
110  if (used_samples < 1){
111  return 0.f;
112  }
113 
114  // return the calculated mean divergence:
115  return divs_sum / used_samples;
116 }
int n_samples
Definition: detect_gate.c:85
#define E
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
struct point_t pos
The original position the flow comes from in subpixels.
Definition: image.h:79
int32_t flow_y
The y direction flow in subpixels.
Definition: image.h:81
int32_t flow_x
The x direction flow in subpixels.
Definition: image.h:80
Definition: image.h:78
float get_size_divergence(struct flow_t *vectors, int count, int n_samples)
Get divergence from optical flow vectors based on line sizes between corners.
Calculate divergence from flow vectors by looking at line sizes beteween the points.
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