Paparazzi UAS  v5.15_devel-164-g81d4ceb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
act_fast.c
Go to the documentation of this file.
1 /*
2 Copyright (c) 2017, Guido de Croon, TU Delft
3 All rights reserved.
4 */
5 
6 
26 #include "fast_rosten.h"
27 #include "act_fast.h"
28 #include "math.h"
29 #include "image.h"
30 #include "../../opticflow/opticflow_calculator.h"
31 
32 // equal to the maximal number of corners defined by fast9_rsize in opticflow_calculator.c
33 #define MAX_AGENTS FAST9_MAX_CORNERS
35 
49 void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners,
50  uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient, int gradient_method)
51 {
52 
53  /*
54  * Procedure:
55  * 1) initialize agent positions
56  * 2) loop over the agents, moving and checking for corners
57  */
58 
59  // ensure that n_agents is never bigger than MAX_AGENTS
60  n_agents = (n_agents < MAX_AGENTS) ? n_agents : MAX_AGENTS;
61  // min_gradient should be bigger than 0:
62  min_gradient = (min_gradient == 0) ? 1 : min_gradient;
63 
64  int border = 4;
65 
66  // ***********************************
67  // 1) initialize the agents' positions
68  // ***********************************
69 
70  // grid sampling with a border:
71  int init_border = 10;
72  float GRID_ROWS = (int) ceil(sqrtf((float) n_agents));
73  float step_size_x = (img->w - 2 * init_border) / (GRID_ROWS - 1);
74  float step_size_y = (img->h - 2 * init_border) / (GRID_ROWS - 1);
75 
76  int a = 0;
77  float px, py, pnorm;
78  for (int c = 0; c < GRID_ROWS; c++) {
79  for (int r = 0; r < GRID_ROWS; r++) {
80  // px, py represent the preferred direction of the agent when there is no texture
81  // here we initialize it differently for each agent:
82  // TODO: don't we have a randf function in Paparazzi?
83  px = ((float)(rand() % 10000) + 1) / 10000.0f;
84  py = ((float)(rand() % 10000) + 1) / 10000.0f;
85  pnorm = sqrtf(px * px + py * py);
86  struct agent_t ag = { (border + c * step_size_x), (border + r * step_size_y), 1, px / pnorm, py / pnorm};
87  agents[a] = ag;
88  a++;
89  if (a == n_agents) { break; }
90  }
91 
92  // only initialize a maximum of n_agents agents.
93  if (a == n_agents) { break; }
94  }
95 
96  /* ********************************************************
97  * 2) loop over the agents, moving and checking for corners
98  * ********************************************************/
99 
100  // gradient
101  int dx, dy;
102 
103  // loop over all time steps:
104  for (int t = 0; t < n_time_steps; t++) {
105  // loop over the agents
106  for (a = 0; a < n_agents; a++) {
107  // only do something if the agent is active:
108  if (agents[a].active) {
109  // check if this position is a corner:
110  uint16_t x = (uint16_t) agents[a].x;
111  uint16_t y = (uint16_t) agents[a].y;
112  if (fast9_detect_pixel(img, fast_threshold, x, y)) {
113  // we arrived at a corner, yeah!!!
114  agents[a].active = 0;
115  continue;
116  } else {
117  // make a step:
118  struct point_t loc = { .x = agents[a].x, .y = agents[a].y};
119  image_gradient_pixel(img, &loc, gradient_method, &dx, &dy);
120  int gradient = (abs(dx) + abs(dy)) / 2;
121  if (abs(gradient) >= min_gradient) {
122  // determine the angle and make a step in that direction:
123  float norm_factor = sqrtf((float)(dx * dx + dy * dy));
124  agents[a].x += (dy / norm_factor) * short_step;
125  agents[a].y += (dx / norm_factor) * short_step;
126  } else {
127  // make a step in the preferred direction:
128  agents[a].x += agents[a].preferred_dir_x * long_step;
129  agents[a].y += agents[a].preferred_dir_y * long_step;
130  }
131  }
132 
133  // let the agent move over the image in a toroid world:
134  if (agents[a].x > img->w - border) {
135  agents[a].x = border;
136  } else if (agents[a].x < border) {
137  agents[a].x = img->w - border;
138  }
139  if (agents[a].y > img->h - border) {
140  agents[a].y = border;
141  } else if (agents[a].y < border) {
142  agents[a].y = img->h - border;
143  }
144  }
145  }
146  }
147 
148  // Transform agents to corners:
149  (*num_corners) = 0;
150  for (a = 0; a < n_agents; a++) {
151 
152  // for active agents do a last check on the new position:
153  if (agents[a].active) {
154  // check if the last step brought the agent to a corner:
155  uint16_t x = (uint16_t) agents[a].x;
156  uint16_t y = (uint16_t) agents[a].y;
157  if (fast9_detect_pixel(img, fast_threshold, x, y)) {
158  // we arrived at a corner, yeah!!!
159  agents[a].active = 0;
160  }
161  }
162 
163  // if inactive, the agent is a corner:
164  if (!agents[a].active) {
165  (*ret_corners)[(*num_corners)].x = (uint32_t) agents[a].x;
166  (*ret_corners)[(*num_corners)].y = (uint32_t) agents[a].y;
167  (*num_corners)++;
168  }
169  }
170 }
171 
unsigned short uint16_t
Definition: types.h:16
uint16_t n_agents
Definition: image.h:43
uint16_t n_time_steps
int active
Definition: act_fast.h:31
uint32_t x
The x coordinate of the point.
Definition: image.h:58
Finds corners in an image by actively scanning the image.
int fast9_detect_pixel(struct image_t *img, uint8_t threshold, uint16_t x, uint16_t y)
Do a FAST9 corner detection for a single pixel.
Definition: fast_rosten.c:3709
float x
Definition: act_fast.h:29
Image helper functions like resizing, color filter, converters...
void image_gradient_pixel(struct image_t *img, struct point_t *loc, int method, int *dx, int *dy)
Get the gradient at a pixel location.
Definition: image.c:760
uint16_t w
Image width.
Definition: image.h:45
unsigned long uint32_t
Definition: types.h:18
uint16_t h
Image height.
Definition: image.h:46
float preferred_dir_y
Definition: act_fast.h:33
void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners, uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient, int gradient_method)
Do an ACT-FAST corner detection.
Definition: act_fast.c:49
uint32_t y
The y coordinate of the point.
Definition: image.h:59
Definition: image.h:57
struct agent_t agents[MAX_AGENTS]
Definition: act_fast.c:34
float preferred_dir_x
Definition: act_fast.h:32
unsigned char uint8_t
Definition: types.h:14
float y
Definition: act_fast.h:30
#define MAX_AGENTS
Definition: act_fast.c:33