Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Merge pull request #2150 from guidoAI/act_fast_PR

Browse Source 
ACT-FAST
This commit is contained in:
Kirk Scheper
2017-11-02 16:08:24 +01:00
committed by GitHub
parent 5f353dac29 9e6edf925c
commit 05deb64637
11 changed files with 4239 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
+261
View File
@@ -0,0 +1,261 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="bebop2_optitrack_visionfront">
<firmware name="rotorcraft">
<target name="ap" board="bebop2"/>
<module name="telemetry" type="transparent_udp"/>
<module name="radio_control" type="datalink"/>
<module name="motor_mixing"/>
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<!-- <define name="AHRS_USE_GPS_HEADING" value="TRUE"/> -->
</module>
<define name="USE_SONAR" value="0"/>
<module name="ins" type="extended"/>
<define name="MT9F002_OUTPUT_HEIGHT" value="640" />
<define name="MT9F002_OUTPUT_WIDTH" value="640" />
<define name="MT9F002_INITIAL_OFFSET_X" value="0.15" />
<define name="MT9F002_INITIAL_OFFSET_Y" value="0.0" />
<define name="MT9F002_TARGET_EXPOSURE" value="30" />
<define name="MT9F002_GAIN_GREEN1" value="4"/>
<define name="MT9F002_GAIN_GREEN2" value="4"/>
<define name="MT9F002_GAIN_RED" value="5"/>
<define name="MT9F002_GAIN_BLUE" value="5"/>
<define name="MT9F002_OUTPUT_SCALER" value="0.25"/>
<define name="MT9F002_X_ODD_INC_VAL" value="1"/>
<define name="MT9F002_Y_ODD_INC_VAL" value="1"/>
</firmware>
<modules main_freq="512">
<module name="geo_mag"/>
<module name="air_data"/>
<module name="send_imu_mag_current"/>
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
</module>
<module name="video_thread"/>
<module name="video_capture">
<define name="VIDEO_CAPTURE_CAMERA" value="front_camera"/>
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images/"/>
</module>
<module name="pose_history">
<define name="POSE_HISTORY_SIZE" value="128" />
</module>
<!-- <define name="INS_INT_VEL_ID" value="ABI_DISABLE"/> -->
<module name="cv_opticflow">
<define name="OPTICFLOW_CAMERA" value="front_camera"/>
<define name="MAX_HORIZON" value="10"/>
<define name="OPTICFLOW_FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="OPTICFLOW_FY" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
<define name="OPTICFLOW_FOV_W" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
<define name="OPTICFLOW_FOV_H" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
<define name="OPTICFLOW_DEROTATION" value="0"/> <!-- formulas are not correct for the front cam -->
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
<define name="OPTICFLOW_FEATURE_MANAGEMENT" value="0"/> <!-- feature management still sucks -->
</module>
<module name="optical_flow_landing">
<define name="OFL_PGAIN" value="0.05"/>
<define name="OFL_IGAIN" value="0.0"/>
</module>
<module name="video_rtp_stream">
<define name="VIEWVIDEO_CAMERA" value="front_camera"/>
<define name="VIEWVIDEO_CAMERA2" value="bottom_camera"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="40"/>
</module>
<!--<module name="bebop_ae_awb"/>-->
</modules>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="6000"/>
</commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="2500" neutral="2500" max="12000"/>
<servo name="TOP_RIGHT" no="1" min="2500" neutral="2500" max="12000"/>
<servo name="BOTTOM_RIGHT" no="2" min="2500" neutral="2500" max="12000"/>
<servo name="BOTTOM_LEFT" no="3" min="2500" neutral="2500" max="12000"/>
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="REVERSE" value="TRUE"/>
<define name="TYPE" value="QUAD_X"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" value="TRUE"/>
</section>
<!-- Magnetometer still needs to be calibrated -->
<section name="IMU" prefix="IMU_">
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="0"/>
<define name="MAG_X_SENS" value="7.28514789391" integer="16"/>
<define name="MAG_Y_SENS" value="7.33022132691" integer="16"/>
<define name="MAG_Z_SENS" value="7.57102035692" integer="16"/>
</section>
<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>
<!-- Use GPS heading instead of magneto -->
<define name="USE_GPS_HEADING" value="1"/>
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</section>
<section name="INS" prefix="INS_">
<!--<define name="SONAR_MAX_RANGE" value="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/> -->
<!-- Use GPS altitude measurments and set the R gain -->
<define name="USE_GPS_ALT" value="1"/>
<define name="VFF_R_GPS" value="0.01"/>
</section>
<section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoint limits for attitude stabilization rc flight -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="300" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
</section>
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
<!-- attitude reference generation model -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.094"/>
<define name="G1_Q" value="0.094"/>
<define name="G1_R" value="0.0025"/>
<define name="G2_R" value="0.36"/>
<!-- Here it is assumed that your removed the damping from your bebop2!
The dampers do not really damp, but cause oscillation. By removing/
fixing them, the bebop2 will fly much better-->
<define name="FILTER_ROLL_RATE" value="FALSE"/>
<define name="FILTER_PITCH_RATE" value="FALSE"/>
<define name="FILTER_YAW_RATE" value="FALSE"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="600.0"/>
<define name="REF_ERR_Q" value="600.0"/>
<define name="REF_ERR_R" value="600.0"/>
<define name="REF_RATE_P" value="28.0"/>
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="350"/>
<define name="HOVER_KD" value="85"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="REF_MAX_SPEED" value="2" unit="m/s"/>
<define name="MAX_BANK" value="32" unit="deg"/>
<define name="PGAIN" value="220"/>
<define name="DGAIN" value="160"/>
<define name="IGAIN" value="15"/>
</section>
<section name="NAVIGATION" prefix="NAV_">
<define name="CLIMB_VSPEED" value="1.0"/>
<define name="DESCEND_VSPEED" value="-1.0"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
<!-- <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/> -->
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="11.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/modules/cv_opticflow.xml
+17
View File
@@ -43,6 +43,8 @@
<define name="MAX_ITERATIONS" value="10" description="Maximum number of iterations the Lucas Kanade algorithm should take"/>
<define name="THRESHOLD_VEC" value="2" description="TThreshold in subpixels when the iterations of Lucas Kanade should stop"/>
<define name="CORNER_METHOD" value="1" description="Method used to look for corners, exhaustive FAST (0) or ACT-FAST (1)."/>
<!-- FAST9 corner detection parameters -->
<define name="FAST9_ADAPTIVE" value="TRUE" description="Whether we should use and adapative FAST9 crner detection threshold"/>
<define name="FAST9_THRESHOLD" value="20" description="FAST9 default threshold"/>
@@ -50,6 +52,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>
@@ -58,6 +67,7 @@
<!-- Optical flow calculations parameters -->
<dl_settings name="vision_calc">
<dl_setting var="opticflow.method" min="0" step="1" max="1" module="computer_vision/opticflow_module" shortname="method" values="LK_Fast9|EdgeFlow" param="METHOD"/>
<dl_setting var="opticflow.corner_method" min="0" step="1" max="1" module="computer_vision/opticflow_module" shortname="corner_method" values="exhaustive-FAST|ACT-FAST" param="CORNER_METHOD"/>
<dl_setting var="opticflow.window_size" module="computer_vision/opticflow_module" min="0" step="1" max="20" shortname="window_size" param="OPTICFLOW_WINDOW_SIZE"/>
<dl_setting var="opticflow.search_distance" module="computer_vision/opticflow_module" min="0" step="1" max="50" shortname="search_distance" param="SEARCH_DISTANCE"/>
<dl_setting var="opticflow.subpixel_factor" module="computer_vision/opticflow_module" min="0" step="10" max="1000" shortname="subpixel_factor" param="OPTICFLOW_SUBPIXEL_FACTOR"/>
@@ -78,6 +88,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="0" 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>
@@ -112,6 +128,7 @@
<file name="pprz_matrix_decomp_float.c" dir="math"/>
<!-- Main vision calculations -->
<file name="act_fast.c" dir="modules/computer_vision/lib/vision"/>
<file name="fast_rosten.c" dir="modules/computer_vision/lib/vision"/>
<file name="lucas_kanade.c" dir="modules/computer_vision/lib/vision"/>
<file name="edge_flow.c" dir="modules/computer_vision/lib/vision"/>
conf/userconf/tudelft/conf.xml
+12 -1
View File
@@ -485,6 +485,18 @@
settings_modules="modules/gps_ubx_ucenter.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"
/>
<aircraft
name="bebop2_vision_front"
ac_id="217"
airframe="airframes/tudelft/bebop2_optitrack_visionfront.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_guido_optitrack.xml"
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"
ac_id="3"
@@ -577,4 +589,3 @@
release="2a6ad556f183875fa9fb8072d774beec7ba55701"
/>
</conf>
sw/airborne/modules/computer_vision/lib/vision/act_fast.c
+168
View File
@@ -0,0 +1,168 @@
/*
Copyright (c) 2017, Guido de Croon, TU Delft
All rights reserved.
*/
/**
* @file modules/computer_vision/lib/vision/act_fast.c
* @brief Finds corners in an image by actively scanning the image. This method is inspired by the work in:
* de Croon, G.C.H.E., and Nolfi, S. (2013, May). Act-corner: Active corner finding for optic flow determination. In Robotics and Automation (ICRA), 2013 IEEE International Conference on (pp. 4679-4684). IEEE.
*
* The main idea of this particular implementation, called ACT-FAST, is that actively scanning the image allows to:
* 1. Skip uniform areas in the image. If these areas are contiguous, many non-corners will never be evaluated.
* 2. Follow edges. Typically, following an edge will bring an agent to a corner.
* These two simple rules lead to a significant lower number of corner evaluations, while still detecting a reasonable number of corners.
* Moreover, since the agents scanning the image start on a grid, corners will be quite well-distributed over the image.
* Each step of the agent starts by classifying the agent location as a corner or not with FAST.
*
* For bigger images (e.g., 640 x 640), the computational advantage of ACT-FAST over the normal, exhaustive application of FAST becomes significant (in the order of a factor > 10).
*
* The code here is part of the following publication:
* de Croon, G.C.H.E. "ACT-FAST: efficiently finding corners by actively exploring images.", in submission.
*
*/
#include "fast_rosten.h"
#include "act_fast.h"
#include "math.h"
#include "image.h"
#define MAX_AGENTS 1000
struct agent_t agents[MAX_AGENTS];
/**
* Do an ACT-FAST corner detection.
* @param[in] *img The image to do the corner detection on
* @param[in] threshold The threshold which we use for FAST9
* @param[in] *num_corners reference to the amount of corners found, set by this function
* @param[in] **ret_corners pointer to the array which contains the corners that were detected.
* @param[in] n_agents The number of agents that will scan the image for corners
* @param[in] n_time_steps The maximum number of time steps allowed for scanning
* @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, int gradient_method)
{
/*
* Procedure:
* 1) initialize agent positions
* 2) loop over the agents, moving and checking for corners
*/
// ensure that n_agents is never bigger than MAX_AGENTS
n_agents = (n_agents < MAX_AGENTS) ? n_agents : MAX_AGENTS;
int border = 4;
// ***********************************
// 1) initialize the agents' positions
// ***********************************
// 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);
int a = 0;
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_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; }
}
// only initialize a maximum of n_agents agents.
if (a == n_agents) { break; }
}
/* ********************************************************
* 2) loop over the agents, moving and checking for corners
* ********************************************************/
// gradient
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;
}
}
// 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)++;
}
}
}
sw/airborne/modules/computer_vision/lib/vision/act_fast.h
+42
View File
@@ -0,0 +1,42 @@
/*
Copyright (c) 2017, Guido de Croon, TU Delft
All rights reserved.
*/
/**
* @file modules/computer_vision/lib/vision/act_fast.h
* @brief Finds corners in an image by actively scanning the image. This method is inspired by the work in:
* de Croon, G.C.H.E., and Nolfi, S. (2013, May). Act-corner: Active corner finding for optic flow determination. In Robotics and Automation (ICRA), 2013 IEEE International Conference on (pp. 4679-4684). IEEE.
*
* The main idea of this particular implementation, called ACT-FAST, is that actively scanning the image allows to:
* 1. Skip uniform areas in the image. If these areas are contiguous, many non-corners will never be evaluated.
* 2. Follow edges. Typically, following an edge will bring an agent to a corner.
* These two simple rules lead to a significant lower number of corner evaluations, while still detecting a reasonable number of corners.
* Moreover, since the agents scanning the image start on a grid, corners will be quite well-distributed over the image.
* Each step of the agent starts by classifying the agent location as a corner or not with FAST.
*
* For bigger images (e.g., 640 x 640), the computational advantage of ACT-FAST over the normal, exhaustive application of FAST becomes significant (in the order of a factor > 10).
*
* The code here is part of the following publication:
* de Croon, G.C.H.E. "ACT-FAST: efficiently finding corners by actively exploring images.", in submission.
*
*/
#ifndef ACT_FAST_H
#define ACT_FAST_H
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, int gradient_method);
#endif
sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c
+3565 -19
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/computer_vision/lib/vision/fast_rosten.h
+2
View File
@@ -38,5 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "lib/vision/image.h"
void fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uint16_t x_padding, uint16_t y_padding, uint16_t *num_corners, uint16_t *ret_corners_length, struct point_t **ret_corners, uint16_t *roi);
int fast9_detect_pixel(struct image_t *img, uint8_t threshold, uint16_t x, uint16_t y);
#endif
sw/airborne/modules/computer_vision/lib/vision/image.c
+71
View File
@@ -599,6 +599,77 @@ void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t point
}
}
/**
* Get the gradient at a pixel location
* @param[in,out] *img The image
* @param[in] loc The location at which to get the gradient
* @param[in] method 0 = {-1, 0, 1}, 1 = Sobel {-1, 0, 1; -2, 0, 2; -1, 0, 1}
* @param[in] dx The gradient in x-direction
* @param[in] dy The gradient in y-direction
*/
void image_gradient_pixel(struct image_t *img, struct point_t *loc, int method, int *dx, int* dy) {
// create the simple and sobel filter only once:
int gradient_x, gradient_y, index;
gradient_x = 0;
gradient_y = 0;
// get image buffer and take into account YUV vs. grayscale:
uint8_t *img_buf = (uint8_t *)img->buf;
uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1;
uint8_t add_ind = pixel_width - 1;
// check if all pixels will fall in the image:
if(loc->x >= 1 && loc->x < img->w-1 && loc->y >= 1 && loc->y < img->h - 1) {
if(method == 0) {
// *************
// Simple method
// *************
// dx:
index = loc->y * img->w * pixel_width + (loc->x-1) * pixel_width;
gradient_x -= (int) img_buf[index+add_ind];
index = loc->y * img->w * pixel_width + (loc->x+1) * pixel_width;
gradient_x += (int) img_buf[index+add_ind];
// dy:
index = (loc->y-1) * img->w * pixel_width + loc->x * pixel_width;
gradient_y -= (int) img_buf[index+add_ind];
index = (loc->y+1) * img->w * pixel_width + loc->x * pixel_width;
gradient_y += (int) img_buf[index+add_ind];
}
else {
// *****
// Sobel
// *****
static int Sobel[9] = {-1, 0, 1, -2, 0, 2, -1, 0, 1};
static int total_sobel = 8;
int filt_ind_y = 0;
int filt_ind_x;
for (int x = -1; x <= 1; x++) {
for (int y = -1; y <= 1; y++) {
index = (loc->y + y) * img->w * pixel_width + (loc->x+x) * pixel_width;
if(x!=0) {
filt_ind_x = (x+1)%3 + (y+1)*3;
gradient_x += Sobel[filt_ind_x] * (int) img_buf[index+add_ind];
}
if(y!=0) {
gradient_y += Sobel[filt_ind_y] * (int) img_buf[index+add_ind];
}
filt_ind_y++;
}
}
gradient_x /= total_sobel;
}
}
// TODO: more efficient would be to use dx, dy directly:
(*dx) = gradient_x;
(*dy) = gradient_y;
}
/**
* Draw a pink line on the image
* @param[in,out] *img The image to show the line on
sw/airborne/modules/computer_vision/lib/vision/image.h
+1
View File
@@ -105,5 +105,6 @@ void image_draw_line(struct image_t *img, struct point_t *from, struct point_t *
void image_draw_line_color(struct image_t *img, struct point_t *from, struct point_t *to, uint8_t *color);
void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size);
void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint16_t border_size);
void image_gradient_pixel(struct image_t *img, struct point_t *loc, int method, int *dx, int* dy);
#endif
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+92 -20
View File
@@ -39,6 +39,7 @@
#include "lib/vision/image.h"
#include "lib/vision/lucas_kanade.h"
#include "lib/vision/fast_rosten.h"
#include "lib/vision/act_fast.h"
#include "lib/vision/edge_flow.h"
#include "size_divergence.h"
#include "linear_flow_fit.h"
@@ -48,6 +49,12 @@
#define OPTICFLOW_SHOW_FLOW 0
#define OPTICFLOW_SHOW_CORNERS 0
#define EXHAUSTIVE_FAST 0
#define ACT_FAST 1
// TODO: these are now adapted, but perhaps later could be a setting:
uint16_t n_time_steps = 10;
uint16_t n_agents = 25;
// What methods are run to determine divergence, lateral flow, etc.
// SIZE_DIV looks at line sizes and only calculates divergence
#define SIZE_DIV 1
@@ -55,6 +62,12 @@
// relative velocities in x, y, z (divergence / time to contact), the slope of the surface, and the surface roughness.
#define LINEAR_FIT 1
#ifndef OPTICFLOW_CORNER_METHOD
// This can be estimated by total possible image height / total Field of view
#define OPTICFLOW_CORNER_METHOD ACT_FAST
#endif
PRINT_CONFIG_VAR(OPTICFLOW_CORNER_METHOD)
// Camera parameters (defaults are from an ARDrone 2)
#ifndef OPTICFLOW_FOV_W
#define OPTICFLOW_FOV_W 0.89360857702
@@ -187,6 +200,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
@@ -245,6 +278,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->corner_method = OPTICFLOW_CORNER_METHOD;
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);
}
@@ -307,23 +346,47 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img,
} else if (!opticflow->feature_management) {
// needs to be set to 0 because result is now static
result->corner_cnt = 0;
// 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
fast9_detect(&opticflow->prev_img_gray, opticflow->fast9_threshold, opticflow->fast9_min_distance,
opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt,
&opticflow->fast9_rsize,
&opticflow->fast9_ret_corners,
NULL);
if (opticflow->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
fast9_detect(&opticflow->prev_img_gray, opticflow->fast9_threshold, opticflow->fast9_min_distance,
opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt,
&opticflow->fast9_rsize,
&opticflow->fast9_ret_corners,
NULL);
} else if (opticflow->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:
// Decrease and increase the threshold based on previous values
if (result->corner_cnt < 40
&& opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) { // TODO: Replace 40 with OPTICFLOW_MAX_TRACK_CORNERS / 2
opticflow->fast9_threshold--;
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->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 (opticflow->corner_method == ACT_FAST && n_time_steps > 5 && n_agents > 10) {
n_time_steps--;
n_agents--;
}
}
}
}
@@ -436,8 +499,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
@@ -457,8 +522,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;
@@ -482,8 +549,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;
@@ -550,7 +619,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;
}
@@ -697,8 +767,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 ..................... //
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+8
View File
@@ -44,6 +44,7 @@ struct opticflow_t {
struct image_t prev_img_gray; ///< Previous gray image frame
uint8_t method; ///< Method to use to calculate the optical flow
uint8_t corner_method; ///< Method to use for determining where the corners are
uint16_t window_size; ///< Window size for the blockmatching algorithm (general value for all methods)
uint16_t search_distance; ///< Search distance for blockmatching alg.
bool derotation; ///< Derotation switched on or off (depended on the quality of the gyroscope measurement)
@@ -69,6 +70,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
};