Added settings etc.

This commit is contained in:
guidoAI
2017-11-02 13:23:35 +01:00
parent 796d318d9c
commit 044ba0dc11
6 changed files with 170 additions and 128 deletions
+13
View File
@@ -50,6 +50,13 @@
<define name="FAST9_PADDING" value="20" description="The outer border in which no corners will be searched"/>
<define name="FAST9_REGION_DETECT" value="1" description="Whether to detect fast9 corners in regions of interest or the whole image (only works with feature management)"/>
<define name="FAST9_NUM_REGIONS" value="9" description="The number of regions of interest to split the image into"/>
<!-- ACT-FAST parameters -->
<define name="ACTFAST_LONG_STEP" value="10" description="Step size to take when there is no texture"/>
<define name="ACTFAST_SHORT_STEP" value="2" description="Step size to take when there is an edge to be followed"/>
<define name="ACTFAST_MIN_GRADIENT" value="10" description="Threshold that decides when there is sufficient texture for edge following"/>
<define name="ACTFAST_GRADIENT_METHOD" value="1" description="Whether to use a simple (0) or Sobel (1) filter"/>
</section>
</doc>
@@ -78,6 +85,12 @@
<dl_setting var="opticflow.fast9_region_detect" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="fast9_region_detect" param="OPTICFLOW_FAST9_REGION_DETECT"/>
<dl_setting var="opticflow.fast9_num_regions" module="computer_vision/opticflow_module" min="1" step="1" max="25" shortname="fast9_num_regions" param="OPTICFLOW_FAST9_NUM_REGIONS"/>
<!-- ACT-FAST settings -->
<dl_setting var="opticflow.actfast_long_step" module="computer_vision/opticflow_module" min="1" step="1" max="100" shortname="actfast_long_step" param="OPTICFLOW_ACTFAST_LONG_STEP"/>
<dl_setting var="opticflow.actfast_short_step" module="computer_vision/opticflow_module" min="1" step="1" max="10" shortname="actfast_short_step" param="OPTICFLOW_ACTFAST_SHORT_STEP"/>
<dl_setting var="opticflow.actfast_min_gradient" module="computer_vision/opticflow_module" min="1" step="1" max="255" shortname="actfast_min_gradient" param="OPTICFLOW_ACTFAST_MIN_GRADIENT"/>
<dl_setting var="opticflow.actfast_gradient_method" module="computer_vision/opticflow_module" min="1" step="1" max="1" values="SIMPLE|SOBEL" shortname="actfast_gradient_method" param="OPTICFLOW_ACTFAST_GRADIENT_METHOD"/>
<!-- Changes pyramid level of lucas kanade optical flow. -->
<dl_setting var="opticflow.pyramid_level" module="computer_vision/opticflow_module" min="0" step="1" max="10" shortname="pyramid_level" param="OPTICFLOW_PYRAMID_LEVEL"/>
</dl_settings>
+1 -1
View File
@@ -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=""
/>
<aircraft
name="bebop_flip"
@@ -586,4 +587,3 @@
release="2a6ad556f183875fa9fb8072d774beec7ba55701"
/>
</conf>
@@ -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)++;
}
}
}
@@ -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
@@ -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 ..................... //
@@ -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
};