Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
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 
25 #include "fast_rosten.h"
26 #include "act_fast.h"
27 #include "math.h"
28 #include "image.h"
29 #include "../../opticflow/opticflow_calculator.h"
30 
31 // ACT-FAST agents arrays
32 // equal to the maximal number of corners defined by fast9_rsize in opticflow_calculator.c
33 // TODO Currently hardcoded to two cameras
34 #define MAX_AGENTS FAST9_MAX_CORNERS
35 #ifndef OPTICFLOW_CAMERA2
37 #else
38  struct agent_t agents[2][MAX_AGENTS];
39 #endif
40 
54 void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners,
55  uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient,
56  int gradient_method, int camera_id)
57 {
58 
59  /*
60  * Procedure:
61  * 1) initialize agent positions
62  * 2) loop over the agents, moving and checking for corners
63  */
64 
65  // ensure that n_agents is never bigger than max_agents
66  n_agents = (n_agents < MAX_AGENTS) ? n_agents : MAX_AGENTS;
67  // min_gradient should be bigger than 0:
68  min_gradient = (min_gradient == 0) ? 1 : min_gradient;
69 
70  int border = 4;
71 
72  // ***********************************
73  // 1) initialize the agents' positions
74  // ***********************************
75 
76  // grid sampling with a border:
77  int init_border = 10;
78  float GRID_ROWS = (int) ceil(sqrtf((float) n_agents));
79  float step_size_x = (img->w - 2 * init_border) / (GRID_ROWS - 1);
80  float step_size_y = (img->h - 2 * init_border) / (GRID_ROWS - 1);
81 
82  int a = 0;
83  float px, py, pnorm;
84  for (int c = 0; c < GRID_ROWS; c++) {
85  for (int r = 0; r < GRID_ROWS; r++) {
86  // px, py represent the preferred direction of the agent when there is no texture
87  // here we initialize it differently for each agent:
88  // TODO: don't we have a randf function in Paparazzi?
89  px = ((float)(rand() % 10000) + 1) / 10000.0f;
90  py = ((float)(rand() % 10000) + 1) / 10000.0f;
91  pnorm = sqrtf(px * px + py * py);
92  struct agent_t ag = { (border + c * step_size_x), (border + r * step_size_y), 1, px / pnorm, py / pnorm};
93  agents[camera_id][a] = ag;
94  a++;
95  if (a == n_agents) { break; }
96  }
97 
98  // only initialize a maximum of n_agents agents.
99  if (a == n_agents) { break; }
100  }
101 
102  /* ********************************************************
103  * 2) loop over the agents, moving and checking for corners
104  * ********************************************************/
105 
106  // gradient
107  int dx, dy;
108 
109  // loop over all time steps:
110  for (int t = 0; t < n_time_steps; t++) {
111  // loop over the agents
112  for (a = 0; a < n_agents; a++) {
113  // only do something if the agent is active:
114  if (agents[camera_id][a].active) {
115  // check if this position is a corner:
116  uint16_t x = (uint16_t) agents[camera_id][a].x;
117  uint16_t y = (uint16_t) agents[camera_id][a].y;
118  if (fast9_detect_pixel(img, fast_threshold, x, y)) {
119  // we arrived at a corner, yeah!!!
120  agents[camera_id][a].active = 0;
121  continue;
122  } else {
123  // make a step:
124  struct point_t loc = { .x = agents[camera_id][a].x, .y = agents[camera_id][a].y};
125  image_gradient_pixel(img, &loc, gradient_method, &dx, &dy);
126  int gradient = (abs(dx) + abs(dy)) / 2;
127  if (abs(gradient) >= min_gradient) {
128  // determine the angle and make a step in that direction:
129  float norm_factor = sqrtf((float)(dx * dx + dy * dy));
130  agents[camera_id][a].x += (dy / norm_factor) * short_step;
131  agents[camera_id][a].y += (dx / norm_factor) * short_step;
132  } else {
133  // make a step in the preferred direction:
134  agents[camera_id][a].x += agents[camera_id][a].preferred_dir_x * long_step;
135  agents[camera_id][a].y += agents[camera_id][a].preferred_dir_y * long_step;
136  }
137  }
138 
139  // let the agent move over the image in a toroid world:
140  if (agents[camera_id][a].x > img->w - border) {
141  agents[camera_id][a].x = border;
142  } else if (agents[camera_id][a].x < border) {
143  agents[camera_id][a].x = img->w - border;
144  }
145  if (agents[camera_id][a].y > img->h - border) {
146  agents[camera_id][a].y = border;
147  } else if (agents[camera_id][a].y < border) {
148  agents[camera_id][a].y = img->h - border;
149  }
150  }
151  }
152  }
153 
154  // Transform agents to corners:
155  (*num_corners) = 0;
156  for (a = 0; a < n_agents; a++) {
157 
158  // for active agents do a last check on the new position:
159  if (agents[camera_id][a].active) {
160  // check if the last step brought the agent to a corner:
161  uint16_t x = (uint16_t) agents[camera_id][a].x;
162  uint16_t y = (uint16_t) agents[camera_id][a].y;
163  if (fast9_detect_pixel(img, fast_threshold, x, y)) {
164  // we arrived at a corner, yeah!!!
165  agents[camera_id][a].active = 0;
166  }
167  }
168 
169  // if inactive, the agent is a corner:
170  if (!agents[camera_id][a].active) {
171  (*ret_corners)[(*num_corners)].x = (uint32_t) agents[camera_id][a].x;
172  (*ret_corners)[(*num_corners)].y = (uint32_t) agents[camera_id][a].y;
173  (*num_corners)++;
174  }
175  }
176 }
177 
uint16_t n_agents[2]
uint16_t n_time_steps[2]
Definition: image.h:44
int active
Definition: act_fast.h:31
uint32_t x
The x coordinate of the point.
Definition: image.h:59
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 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, int camera_id)
Do an ACT-FAST corner detection.
Definition: act_fast.c:54
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:766
uint16_t w
Image width.
Definition: image.h:46
uint16_t h
Image height.
Definition: image.h:47
float preferred_dir_y
Definition: act_fast.h:33
uint32_t y
The y coordinate of the point.
Definition: image.h:60
Definition: image.h:58
float preferred_dir_x
Definition: act_fast.h:32
float y
Definition: act_fast.h:30
#define MAX_AGENTS
Definition: act_fast.c:34
struct agent_t agents[1][MAX_AGENTS]
Definition: act_fast.c:36
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78