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

Act fast preparing for pull request

Browse Source
This commit is contained in:
guidoAI
2017-10-30 13:24:32 +01:00
committed by Ewoud Smeur
parent 3ff008f62e
commit 796d318d9c
12 changed files with 4180 additions and 32 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
+1
View File
@@ -112,6 +112,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
+11
View File
@@ -484,6 +484,17 @@
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"
/>
<aircraft
name="bebop_flip"
ac_id="3"
sw/airborne/modules/computer_vision/lib/vision/act_fast.c
+176
View File
@@ -0,0 +1,176 @@
/*
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 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
*/
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) {
/*
* Procedure:
* 1) initialize agent positions
* 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;
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 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
{
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);
#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
+48 -11
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,13 @@
#define OPTICFLOW_SHOW_FLOW 0
#define OPTICFLOW_SHOW_CORNERS 0
#define EXHAUSTIVE_FAST 0
#define ACT_FAST 1
uint16_t n_time_steps = 10;
uint16_t n_agents = 25;
// corner method:
#define CORNER_METHOD 0
// What methods are run to determine divergence, lateral flow, etc.
// SIZE_DIV looks at line sizes and only calculates divergence
#define SIZE_DIV 1
@@ -307,23 +315,52 @@ 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(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 (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);
}
// 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(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--;
}
}
}
}
sw/ext/opencv_bebop
+1 -1
Submodule sw/ext/opencv_bebop updated: 4e3904d2f4...517e0f9013
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: 625ed366b4...9525de7663