diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml
index 499bebab52..0d3da40530 100644
--- a/conf/modules/cv_opticflow.xml
+++ b/conf/modules/cv_opticflow.xml
@@ -50,6 +50,13 @@
+
+
+
+
+
+
+
@@ -78,6 +85,12 @@
+
+
+
+
+
+
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index bcc451f4ab..9b16640cc6 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -494,6 +494,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/video_rtp_stream.xml modules/optical_flow_landing.xml modules/cv_opticflow.xml modules/video_capture.xml modules/air_data.xml modules/geo_mag.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="#ffffbf17bf17"
+ release=""
/>
-
diff --git a/sw/airborne/modules/computer_vision/lib/vision/act_fast.c b/sw/airborne/modules/computer_vision/lib/vision/act_fast.c
index a56ece6b55..8b52ff1027 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/act_fast.c
+++ b/sw/airborne/modules/computer_vision/lib/vision/act_fast.c
@@ -30,7 +30,7 @@ All rights reserved.
#define MAX_AGENTS 1000
-struct agent agents[MAX_AGENTS];
+struct agent_t agents[MAX_AGENTS];
/**
* Do an ACT-FAST corner detection.
@@ -43,8 +43,11 @@ struct agent agents[MAX_AGENTS];
* @param[in] long_step When there is not enough texture, the agent will take a long step to a next point of this length in pixels
* @param[in] short_step When there is texture, the agent will follow the edge with this short step in pixels
* @param[in] min_gradient The minimum gradient, in order to determine when to take a long or short step
+ * @param[in] gradient_method: 0 = simple {-1, 0, 1}, 1 = Sobel {-1,0,1,-2,0,2,-1,0,1}
*/
-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) {
+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)
+{
/*
* Procedure:
@@ -52,11 +55,6 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
* 2) loop over the agents, moving and checking for corners
*/
- // method to determine the gradient:
- // 0 = simple (-1, 0, 1)
- // 1 = Sobel
- int gradient_method = 1;
-
// ensure that n_agents is never bigger than MAX_AGENTS
n_agents = (n_agents < MAX_AGENTS) ? n_agents : MAX_AGENTS;
@@ -68,30 +66,28 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
// grid sampling with a border:
int init_border = 10;
- float GRID_ROWS = (int) ceil( sqrtf((float) n_agents) );
- float step_size_x = (img->w - 2*init_border) / (GRID_ROWS-1);
- float step_size_y = (img->h - 2*init_border) / (GRID_ROWS-1);
+ float GRID_ROWS = (int) ceil(sqrtf((float) n_agents));
+ float step_size_x = (img->w - 2 * init_border) / (GRID_ROWS - 1);
+ float step_size_y = (img->h - 2 * init_border) / (GRID_ROWS - 1);
int a = 0;
- float px,py,pnorm;
- for(int c = 0; c < GRID_ROWS; c++)
- {
- for(int r = 0; r < GRID_ROWS; r++)
- {
+ float px, py, pnorm;
+ for (int c = 0; c < GRID_ROWS; c++) {
+ for (int r = 0; r < GRID_ROWS; r++) {
// px, py represent the preferred direction of the agent when there is no texture
// here we initialize it differently for each agent:
// TODO: don't we have a randf function in Paparazzi?
- px = ((float) (rand() % 10000)) / 10000.0f;
- py = ((float) (rand() % 10000)) / 10000.0f;
- pnorm = sqrtf(px*px+py*py);
- struct agent ag = { (border + c * step_size_x), (border + r * step_size_y), 1, px/pnorm, py/pnorm};
+ px = ((float)(rand() % 10000)) / 10000.0f;
+ py = ((float)(rand() % 10000)) / 10000.0f;
+ pnorm = sqrtf(px * px + py * py);
+ struct agent_t ag = { (border + c * step_size_x), (border + r * step_size_y), 1, px / pnorm, py / pnorm};
agents[a] = ag;
a++;
- if(a == n_agents) break;
+ if (a == n_agents) { break; }
}
// only initialize a maximum of n_agents agents.
- if(a == n_agents) break;
+ if (a == n_agents) { break; }
}
/* ********************************************************
@@ -102,75 +98,71 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
int dx, dy;
// loop over all time steps:
- for(int t = 0; t < n_time_steps; t++) {
- // loop over the agents
- for(a = 0; a < n_agents; a++) {
- // only do something if the agent is active:
- if(agents[a].active) {
- // check if this position is a corner:
- uint16_t x = (uint16_t) agents[a].x;
- uint16_t y = (uint16_t) agents[a].y;
- if(fast9_detect_pixel(img, fast_threshold, x, y)) {
- // we arrived at a corner, yeah!!!
- agents[a].active = 0;
- break;
- }
- else {
- // make a step:
- struct point_t loc = {agents[a].x, agents[a].y};
- image_gradient_pixel(img, &loc, gradient_method, &dx, &dy);
- int gradient = (abs(dx) + abs(dy)) / 2;
- if(abs(gradient) >= min_gradient) {
- // determine the angle and make a step in that direction:
- float norm_factor = sqrtf((float) (dx*dx + dy*dy));
- agents[a].x += (dy / norm_factor) * short_step;
- agents[a].y += (dx / norm_factor) * short_step;
- }
- else {
- // make a step in the preferred direction:
- agents[a].x += agents[a].preferred_dir_x * long_step;
- agents[a].y += agents[a].preferred_dir_y * long_step;
- }
- }
-
- // let the agent move over the image in a toroid world:
- if(agents[a].x > img->w - border) {
- agents[a].x = border;
- }
- else if(agents[a].x < border) {
- agents[a].x = img->w - border;
- }
- if(agents[a].y > img->h - border) {
- agents[a].y = border;
- }
- else if(agents[a].y < border) {
- agents[a].y = img->h - border;
- }
- }
- }
- }
-
- // Transform agents to corners:
- (*num_corners) = 0;
- for(a = 0; a < n_agents; a++) {
-
- // for active agents do a last check on the new position:
- if(agents[a].active) {
- // check if the last step brought the agent to a corner:
- uint16_t x = (uint16_t) agents[a].x;
- uint16_t y = (uint16_t) agents[a].y;
- if(fast9_detect_pixel(img, fast_threshold, x, y)) {
- // we arrived at a corner, yeah!!!
- agents[a].active = 0;
+ for (int t = 0; t < n_time_steps; t++) {
+ // loop over the agents
+ for (a = 0; a < n_agents; a++) {
+ // only do something if the agent is active:
+ if (agents[a].active) {
+ // check if this position is a corner:
+ uint16_t x = (uint16_t) agents[a].x;
+ uint16_t y = (uint16_t) agents[a].y;
+ if (fast9_detect_pixel(img, fast_threshold, x, y)) {
+ // we arrived at a corner, yeah!!!
+ agents[a].active = 0;
+ break;
+ } else {
+ // make a step:
+ struct point_t loc = {agents[a].x, agents[a].y};
+ image_gradient_pixel(img, &loc, gradient_method, &dx, &dy);
+ int gradient = (abs(dx) + abs(dy)) / 2;
+ if (abs(gradient) >= min_gradient) {
+ // determine the angle and make a step in that direction:
+ float norm_factor = sqrtf((float)(dx * dx + dy * dy));
+ agents[a].x += (dy / norm_factor) * short_step;
+ agents[a].y += (dx / norm_factor) * short_step;
+ } else {
+ // make a step in the preferred direction:
+ agents[a].x += agents[a].preferred_dir_x * long_step;
+ agents[a].y += agents[a].preferred_dir_y * long_step;
}
- }
+ }
- // if inactive, the agent is a corner:
- if(!agents[a].active) {
- (*ret_corners)[(*num_corners)].x = (uint32_t) agents[a].x;
- (*ret_corners)[(*num_corners)].y = (uint32_t) agents[a].y;
- (*num_corners)++;
- }
- }
+ // let the agent move over the image in a toroid world:
+ if (agents[a].x > img->w - border) {
+ agents[a].x = border;
+ } else if (agents[a].x < border) {
+ agents[a].x = img->w - border;
+ }
+ if (agents[a].y > img->h - border) {
+ agents[a].y = border;
+ } else if (agents[a].y < border) {
+ agents[a].y = img->h - border;
+ }
+ }
+ }
+ }
+
+ // Transform agents to corners:
+ (*num_corners) = 0;
+ for (a = 0; a < n_agents; a++) {
+
+ // for active agents do a last check on the new position:
+ if (agents[a].active) {
+ // check if the last step brought the agent to a corner:
+ uint16_t x = (uint16_t) agents[a].x;
+ uint16_t y = (uint16_t) agents[a].y;
+ if (fast9_detect_pixel(img, fast_threshold, x, y)) {
+ // we arrived at a corner, yeah!!!
+ agents[a].active = 0;
+ }
+ }
+
+ // if inactive, the agent is a corner:
+ if (!agents[a].active) {
+ (*ret_corners)[(*num_corners)].x = (uint32_t) agents[a].x;
+ (*ret_corners)[(*num_corners)].y = (uint32_t) agents[a].y;
+ (*num_corners)++;
+ }
+ }
}
diff --git a/sw/airborne/modules/computer_vision/lib/vision/act_fast.h b/sw/airborne/modules/computer_vision/lib/vision/act_fast.h
index b957cf3a86..2e5802714b 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/act_fast.h
+++ b/sw/airborne/modules/computer_vision/lib/vision/act_fast.h
@@ -25,18 +25,18 @@ All rights reserved.
#ifndef ACT_FAST_H
#define ACT_FAST_H
-struct agent
-{
- float x;
- float y;
- int active;
- float preferred_dir_x;
- float preferred_dir_y;
+struct agent_t {
+ float x;
+ float y;
+ int active;
+ float preferred_dir_x;
+ float preferred_dir_y;
};
#include "std.h"
#include "lib/vision/image.h"
-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);
+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);
#endif
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
index 289ec0c19f..2d949ff969 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
@@ -195,6 +195,26 @@ PRINT_CONFIG_VAR(OPTICFLOW_FAST9_REGION_DETECT)
#endif
PRINT_CONFIG_VAR(OPTICFLOW_FAST9_NUM_REGIONS)
+#ifndef OPTICFLOW_ACTFAST_LONG_STEP
+#define OPTICFLOW_ACTFAST_LONG_STEP 10
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_LONG_STEP)
+
+#ifndef OPTICFLOW_ACTFAST_SHORT_STEP
+#define OPTICFLOW_ACTFAST_SHORT_STEP 2
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_SHORT_STEP)
+
+#ifndef OPTICFLOW_ACTFAST_GRADIENT_METHOD
+#define OPTICFLOW_ACTFAST_GRADIENT_METHOD 1
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_GRADIENT_METHOD)
+
+#ifndef OPTICFLOW_ACTFAST_MIN_GRADIENT
+#define OPTICFLOW_ACTFAST_MIN_GRADIENT 10
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_MIN_GRADIENT)
+
// Defaults for ARdrone
#ifndef OPTICFLOW_BODY_TO_CAM_PHI
#define OPTICFLOW_BODY_TO_CAM_PHI 0
@@ -253,6 +273,12 @@ void opticflow_calc_init(struct opticflow_t *opticflow)
opticflow->fast9_rsize = 512;
opticflow->fast9_ret_corners = calloc(opticflow->fast9_rsize, sizeof(struct point_t));
+
+ opticflow->actfast_long_step = OPTICFLOW_ACTFAST_LONG_STEP;
+ opticflow->actfast_short_step = OPTICFLOW_ACTFAST_SHORT_STEP;
+ opticflow->actfast_min_gradient = OPTICFLOW_ACTFAST_MIN_GRADIENT;
+ opticflow->actfast_gradient_method = OPTICFLOW_ACTFAST_GRADIENT_METHOD;
+
struct FloatEulers euler = {OPTICFLOW_BODY_TO_CAM_PHI, OPTICFLOW_BODY_TO_CAM_THETA, OPTICFLOW_BODY_TO_CAM_PSI};
float_rmat_of_eulers(&body_to_cam, &euler);
}
@@ -316,7 +342,7 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img,
// needs to be set to 0 because result is now static
result->corner_cnt = 0;
- if(CORNER_METHOD == EXHAUSTIVE_FAST) {
+ if (CORNER_METHOD == EXHAUSTIVE_FAST) {
// FAST corner detection
// TODO: There is something wrong with fast9_detect destabilizing FPS. This problem is reduced with putting min_distance
// to 0 (see defines), however a more permanent solution should be considered
@@ -326,40 +352,35 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img,
&opticflow->fast9_ret_corners,
NULL);
- }
- else if (CORNER_METHOD == ACT_FAST) {
- // ACT-FAST corner detection:
- // TODO: all relevant things should be settings:
- float long_step = 10; // 5
- float short_step = 2; // 2
- int min_gradient = 10; // 10
- printf("opticflow->fast9_threshold = %d, n_agents = %d, n_time_steps = %d\n", opticflow->fast9_threshold, n_agents, n_time_steps);
- act_fast(&opticflow->prev_img_gray, opticflow->fast9_threshold, &result->corner_cnt,
- &opticflow->fast9_ret_corners, n_agents, n_time_steps,
- long_step, short_step, min_gradient);
+ } else if (CORNER_METHOD == ACT_FAST) {
+ // ACT-FAST corner detection:
+ act_fast(&opticflow->prev_img_gray, opticflow->fast9_threshold, &result->corner_cnt,
+ &opticflow->fast9_ret_corners, n_agents, n_time_steps,
+ opticflow->actfast_long_step, opticflow->actfast_short_step, opticflow->actfast_min_gradient,
+ opticflow->actfast_gradient_method);
}
// Adaptive threshold
if (opticflow->fast9_adaptive) {
- // This works well for exhaustive FAST, but drives the threshold to the minimum for ACT-FAST:
+ // This works well for exhaustive FAST, but drives the threshold to the minimum for ACT-FAST:
// Decrease and increase the threshold based on previous values
if (result->corner_cnt < 40) { // TODO: Replace 40 with OPTICFLOW_MAX_TRACK_CORNERS / 2
// make detections easier:
- if(opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) {
- opticflow->fast9_threshold--;
+ if (opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) {
+ opticflow->fast9_threshold--;
}
- if(CORNER_METHOD == ACT_FAST) {
- n_time_steps++;
- n_agents++;
+ if (CORNER_METHOD == ACT_FAST) {
+ n_time_steps++;
+ n_agents++;
}
} else if (result->corner_cnt > OPTICFLOW_MAX_TRACK_CORNERS * 2 && opticflow->fast9_threshold < FAST9_HIGH_THRESHOLD) {
opticflow->fast9_threshold++;
- if(CORNER_METHOD == ACT_FAST && n_time_steps > 5 && n_agents > 10) {
- n_time_steps--;
- n_agents--;
+ if (CORNER_METHOD == ACT_FAST && n_time_steps > 5 && n_agents > 10) {
+ n_time_steps--;
+ n_agents--;
}
}
}
@@ -473,8 +494,10 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img,
// Velocity calculation
// Right now this formula is under assumption that the flow only exist in the center axis of the camera.
// TODO Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane.
- result->vel_cam.x = (float)result->flow_der_x * result->fps * agl_dist_value_filtered / (opticflow->subpixel_factor * OPTICFLOW_FX);
- result->vel_cam.y = (float)result->flow_der_y * result->fps * agl_dist_value_filtered / (opticflow->subpixel_factor * OPTICFLOW_FY);
+ result->vel_cam.x = (float)result->flow_der_x * result->fps * agl_dist_value_filtered /
+ (opticflow->subpixel_factor * OPTICFLOW_FX);
+ result->vel_cam.y = (float)result->flow_der_y * result->fps * agl_dist_value_filtered /
+ (opticflow->subpixel_factor * OPTICFLOW_FY);
result->vel_cam.z = result->divergence * result->fps * agl_dist_value_filtered;
//Apply a median filter to the velocity if wanted
@@ -494,8 +517,10 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img,
result->corner_cnt = result->tracked_cnt;
//get the new positions of the corners and the "residual" subpixel positions
for (uint16_t i = 0; i < result->tracked_cnt; i++) {
- opticflow->fast9_ret_corners[i].x = (uint32_t)((vectors[i].pos.x + (float)vectors[i].flow_x) / opticflow->subpixel_factor);
- opticflow->fast9_ret_corners[i].y = (uint32_t)((vectors[i].pos.y + (float)vectors[i].flow_y) / opticflow->subpixel_factor);
+ opticflow->fast9_ret_corners[i].x = (uint32_t)((vectors[i].pos.x + (float)vectors[i].flow_x) /
+ opticflow->subpixel_factor);
+ opticflow->fast9_ret_corners[i].y = (uint32_t)((vectors[i].pos.y + (float)vectors[i].flow_y) /
+ opticflow->subpixel_factor);
opticflow->fast9_ret_corners[i].x_sub = (uint16_t)((vectors[i].pos.x + vectors[i].flow_x) % opticflow->subpixel_factor);
opticflow->fast9_ret_corners[i].y_sub = (uint16_t)((vectors[i].pos.y + vectors[i].flow_y) % opticflow->subpixel_factor);
opticflow->fast9_ret_corners[i].count = vectors[i].pos.count;
@@ -519,8 +544,10 @@ static void manage_flow_features(struct image_t *img, struct opticflow_t *opticf
while (c1 < (int16_t)result->corner_cnt - 1) {
bool exists = false;
for (int16_t i = c1 + 1; i < result->corner_cnt; i++) {
- if (abs((int16_t)opticflow->fast9_ret_corners[c1].x - (int16_t)opticflow->fast9_ret_corners[i].x) < opticflow->fast9_min_distance / 2
- && abs((int16_t)opticflow->fast9_ret_corners[c1].y - (int16_t)opticflow->fast9_ret_corners[i].y) < opticflow->fast9_min_distance / 2) {
+ if (abs((int16_t)opticflow->fast9_ret_corners[c1].x - (int16_t)opticflow->fast9_ret_corners[i].x) <
+ opticflow->fast9_min_distance / 2
+ && abs((int16_t)opticflow->fast9_ret_corners[c1].y - (int16_t)opticflow->fast9_ret_corners[i].y) <
+ opticflow->fast9_min_distance / 2) {
// if too close, replace the corner with the last one in the list:
opticflow->fast9_ret_corners[c1].x = opticflow->fast9_ret_corners[result->corner_cnt - 1].x;
opticflow->fast9_ret_corners[c1].y = opticflow->fast9_ret_corners[result->corner_cnt - 1].y;
@@ -587,7 +614,8 @@ static void manage_flow_features(struct image_t *img, struct opticflow_t *opticf
bool exists = false;
for (uint16_t k = 0; k < result->corner_cnt; k++) {
if (abs((int16_t)new_corners[j].x - (int16_t)opticflow->fast9_ret_corners[k].x) < (int16_t)opticflow->fast9_min_distance
- && abs((int16_t)new_corners[j].y - (int16_t)opticflow->fast9_ret_corners[k].y) < (int16_t)opticflow->fast9_min_distance) {
+ && abs((int16_t)new_corners[j].y - (int16_t)opticflow->fast9_ret_corners[k].y) < (int16_t)
+ opticflow->fast9_min_distance) {
exists = true;
break;
}
@@ -734,8 +762,10 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img,
result->flow_der_y = result->flow_y;
result->corner_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
result->tracked_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
- result->divergence = -1.0 * (float)edgeflow.div_x / RES; // Also multiply the divergence with -1.0 to make it on par with the LK algorithm of
- result->div_size = result->divergence; // Fill the div_size with the divergence to atleast get some divergenge measurement when switching from LK to EF
+ result->divergence = -1.0 * (float)edgeflow.div_x /
+ RES; // Also multiply the divergence with -1.0 to make it on par with the LK algorithm of
+ result->div_size =
+ result->divergence; // Fill the div_size with the divergence to atleast get some divergenge measurement when switching from LK to EF
result->surface_roughness = 0.0f;
//......................Calculating VELOCITY ..................... //
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
index 34569d7032..0bf54ba531 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
@@ -69,6 +69,13 @@ struct opticflow_t {
bool feature_management; ///< Decides whether to keep track corners in memory for the next frame instead of re-detecting every time
bool fast9_region_detect; ///< Decides whether to detect fast9 corners in specific regions of interest or the whole image (only for feature management)
uint8_t fast9_num_regions; ///< The number of regions of interest the image is split into
+
+ float actfast_long_step; ///< Step size to take when there is no texture
+ float actfast_short_step; ///< Step size to take when there is an edge to be followed
+ int actfast_min_gradient; ///< Threshold that decides when there is sufficient texture for edge following
+ int actfast_gradient_method; ///< Whether to use a simple or Sobel filter
+
+
};