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
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 
35 #define NO_DIV 0.0
36 
44 float get_size_divergence(struct flow_t *vectors, int count, int n_samples)
45 {
46  float distance_1;
47  float distance_2;
48  float *divs;
49  unsigned int sample;
50  float dx;
51  float dy;
52  float mean_divergence;
53  int n_elements;
54  unsigned int i, j;
55 
56  // TODO: if count!/(2!*count-2!) lower than n_samples, do full sampling
57  if (count < 2) {
58  return NO_DIV;
59  }
60 
61  if (n_samples == 0) {
62  // divs will contain the individual divergence estimates:
63  n_elements = (count * count - count) / 2;
64  divs = (float *) malloc(sizeof(float) * n_elements);
65 
66  // go through all possible lines:
67  sample = 0;
68  for (i = 0; i < count; i++) {
69  for (j = i + 1; j < count; j++) {
70  // distance in previous image:
71  dx = (float)vectors[i].pos.x - (float)vectors[j].pos.x;
72  dy = (float)vectors[i].pos.y - (float)vectors[j].pos.y;
73  distance_1 = sqrt(dx * dx + dy * dy);
74 
75  // distance in current image:
76  dx = (float)vectors[i].pos.x + (float)vectors[i].flow_x - (float)vectors[j].pos.x - (float)vectors[j].flow_x;
77  dy = (float)vectors[i].pos.y + (float)vectors[i].flow_y - (float)vectors[j].pos.y - (float)vectors[j].flow_y;
78  distance_2 = sqrt(dx * dx + dy * dy);
79 
80  // calculate divergence for this sample:
81  divs[sample] = (distance_2 - distance_1) / distance_1;
82  sample++;
83  }
84  }
85 
86  // calculate the mean divergence:
87  mean_divergence = get_mean(divs, n_elements);
88 
89  // free the memory of divs:
90  free(divs);
91  } else {
92  // vector that will contain individual divergence estimates:
93  divs = (float *) malloc(sizeof(float) * n_samples);
94 
95  // take random samples:
96  for (sample = 0; sample < n_samples; sample++) {
97  // take two random indices:
98  i = rand() % count;
99  j = rand() % count;
100  // ensure it is not the same index:
101  while (i == j) {
102  j = rand() % count;
103  }
104 
105  // distance in previous image:
106  dx = (float)vectors[i].pos.x - (float)vectors[j].pos.x;
107  dy = (float)vectors[i].pos.y - (float)vectors[j].pos.y;
108  distance_1 = sqrt(dx * dx + dy * dy);
109 
110  // distance in current image:
111  dx = (float)vectors[i].pos.x + (float)vectors[i].flow_x - (float)vectors[j].pos.x - (float)vectors[j].flow_x;
112  dy = (float)vectors[i].pos.y + (float)vectors[i].flow_y - (float)vectors[j].pos.y - (float)vectors[j].flow_y;
113  distance_2 = sqrt(dx * dx + dy * dy);
114 
115 
116  // calculate divergence for this sample:
117  divs[sample] = (distance_2 - distance_1) / distance_1;
118  }
119 
120  // calculate the mean divergence:
121  mean_divergence = get_mean(divs, n_samples);
122  // free the memory of divs:
123  free(divs);
124  }
125 
126  // return the calculated divergence:
127  return mean_divergence;
128 }
129 
136 float get_mean(float *numbers, int n_elements)
137 {
138  int i = 0;
139  float mean = 0;
140  for (i = 0; i < n_elements; i++) {
141  mean += numbers[i];
142  }
143  mean /= n_elements;
144  return mean;
145 }
146 
147 
float get_mean(float *numbers, int n_elements)
Get the sample mean of a vector of floats.
Definition: image.h:61
Calculate divergence from flow vectors by looking at line sizes beteween the points.
uint32_t x
The x coordinate of the point.
Definition: image.h:56
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.
int16_t flow_x
The x direction flow in subpixels.
Definition: image.h:63
struct point_t pos
The original position the flow comes from.
Definition: image.h:62
uint32_t y
The y coordinate of the point.
Definition: image.h:57
#define NO_DIV
int16_t flow_y
The y direction flow in subpixels.
Definition: image.h:64