Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
act_fast.c
Go to the documentation of this file.
1/*
2Copyright (c) 2017, Guido de Croon, TU Delft
3All 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#include "generated/airframe.h"
31
32// ACT-FAST agents arrays
33// equal to the maximal number of corners defined by fast9_rsize in opticflow_calculator.c
34// TODO Currently hardcoded to two cameras
35#define MAX_AGENTS FAST9_MAX_CORNERS
36#ifdef OPTICFLOW_CAMERA2
37#define FAST9_MAX_CAMERAS 2
38#else
39#define FAST9_MAX_CAMERAS 1
40#endif
41
43
44
60 int gradient_method, int camera_id)
61{
62 // Input protection:
63 if (camera_id >= FAST9_MAX_CAMERAS) {
64 *num_corners = 0;
65 return;
66 }
67
68 /*
69 * Procedure:
70 * 1) initialize agent positions
71 * 2) loop over the agents, moving and checking for corners
72 */
73
74 // ensure that n_agents is never bigger than max_agents
76 // min_gradient should be bigger than 0:
78
79 int border = 4;
80
81 // ***********************************
82 // 1) initialize the agents' positions
83 // ***********************************
84
85 // grid sampling with a border:
86 int init_border = 10;
87 float GRID_ROWS = (int) ceil(sqrtf((float) n_agents));
88 float step_size_x = (img->w - 2 * init_border) / (GRID_ROWS - 1);
89 float step_size_y = (img->h - 2 * init_border) / (GRID_ROWS - 1);
90
91 int a = 0;
92 float px, py, pnorm;
93 for (int c = 0; c < GRID_ROWS; c++) {
94 for (int r = 0; r < GRID_ROWS; r++) {
95 // px, py represent the preferred direction of the agent when there is no texture
96 // here we initialize it differently for each agent:
97 // TODO: don't we have a randf function in Paparazzi?
98 px = ((float)(rand() % 10000) + 1) / 10000.0f;
99 py = ((float)(rand() % 10000) + 1) / 10000.0f;
100 pnorm = sqrtf(px * px + py * py);
101 struct agent_t ag = { (border + c * step_size_x), (border + r * step_size_y), 1, px / pnorm, py / pnorm};
102 agents[camera_id][a] = ag;
103 a++;
104 if (a == n_agents) { break; }
105 }
106
107 // only initialize a maximum of n_agents agents.
108 if (a == n_agents) { break; }
109 }
110
111 /* ********************************************************
112 * 2) loop over the agents, moving and checking for corners
113 * ********************************************************/
114
115 // gradient
116 int dx, dy;
117
118 // loop over all time steps:
119 for (int t = 0; t < n_time_steps; t++) {
120 // loop over the agents
121 for (a = 0; a < n_agents; a++) {
122 // only do something if the agent is active:
123 if (agents[camera_id][a].active) {
124 // check if this position is a corner:
125 uint16_t x = (uint16_t) agents[camera_id][a].x;
126 uint16_t y = (uint16_t) agents[camera_id][a].y;
128 // we arrived at a corner, yeah!!!
129 agents[camera_id][a].active = 0;
130 continue;
131 } else {
132 // make a step:
133 struct point_t loc = { .x = agents[camera_id][a].x, .y = agents[camera_id][a].y};
135 int gradient = (abs(dx) + abs(dy)) / 2;
136 if (abs(gradient) >= min_gradient) {
137 // determine the angle and make a step in that direction:
138 float norm_factor = sqrtf((float)(dx * dx + dy * dy));
139 agents[camera_id][a].x += (dy / norm_factor) * short_step;
140 agents[camera_id][a].y += (dx / norm_factor) * short_step;
141 } else {
142 // make a step in the preferred direction:
143 agents[camera_id][a].x += agents[camera_id][a].preferred_dir_x * long_step;
144 agents[camera_id][a].y += agents[camera_id][a].preferred_dir_y * long_step;
145 }
146 }
147
148 // let the agent move over the image in a toroid world:
149 if (agents[camera_id][a].x > img->w - border) {
150 agents[camera_id][a].x = border;
151 } else if (agents[camera_id][a].x < border) {
152 agents[camera_id][a].x = img->w - border;
153 }
154 if (agents[camera_id][a].y > img->h - border) {
155 agents[camera_id][a].y = border;
156 } else if (agents[camera_id][a].y < border) {
157 agents[camera_id][a].y = img->h - border;
158 }
159 }
160 }
161 }
162
163 // Transform agents to corners:
164 (*num_corners) = 0;
165 for (a = 0; a < n_agents; a++) {
166
167 // for active agents do a last check on the new position:
168 if (agents[camera_id][a].active) {
169 // check if the last step brought the agent to a corner:
170 uint16_t x = (uint16_t) agents[camera_id][a].x;
171 uint16_t y = (uint16_t) agents[camera_id][a].y;
173 // we arrived at a corner, yeah!!!
174 agents[camera_id][a].active = 0;
175 }
176 }
177
178 // if inactive, the agent is a corner:
179 if (!agents[camera_id][a].active) {
180 (*ret_corners)[(*num_corners)].x = (uint32_t) agents[camera_id][a].x;
181 (*ret_corners)[(*num_corners)].y = (uint32_t) agents[camera_id][a].y;
182 (*num_corners)++;
183 }
184 }
185}
186
struct agent_t agents[FAST9_MAX_CAMERAS][MAX_AGENTS]
Definition act_fast.c:42
#define FAST9_MAX_CAMERAS
Definition act_fast.c:39
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:58
#define MAX_AGENTS
Definition act_fast.c:35
Finds corners in an image by actively scanning the image.
int active
Definition act_fast.h:31
float preferred_dir_x
Definition act_fast.h:32
float y
Definition act_fast.h:30
float x
Definition act_fast.h:29
float preferred_dir_y
Definition act_fast.h:33
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.
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:777
Image helper functions like resizing, color filter, converters...
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 foo
Definition main_demo5.c:58
uint16_t n_agents[2]
uint16_t n_time_steps[2]
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.