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 31 Packages Projects Releases Wiki Activity

[vision] Add more settings to optical flow

Browse Source
This commit is contained in:
Freek van Tienen
2015-03-31 10:37:50 +02:00
parent 38f7c98ba1
commit 5f49f54faa
5 changed files with 117 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/cv_opticflow.xml
+34 -9
View File
@@ -13,18 +13,31 @@
</description>
<section name="VISION" prefix="VISION_">
<define name="PHI_PGAIN" value="500" description="optic flow pgain"/>
<define name="PHI_IGAIN" value="10" description="optic flow igain"/>
<define name="THETA_PGAIN" value="500" description="optic flow pgain"/>
<define name="THETA_IGAIN" value="10" description="optic flow igain"/>
<define name="DESIRED_VX" value="0" description="feedforward optic flow vx"/>
<define name="DESIRED_VY" value="0" description="feedforward optic flow vy"/>
<define name="PHI_PGAIN" value="400" description="Optic flow proportional gain on the roll velocity error"/>
<define name="PHI_IGAIN" value="20" description="Optic flow integrated gain on the summed roll velocity error"/>
<define name="THETA_PGAIN" value="400" description="Optic flow proportional gain on the pitch velocity error"/>
<define name="THETA_IGAIN" value="20" description="Optic flow integrated gain on the summed pitch velocity error"/>
<define name="DESIRED_VX" value="0" description="The desired velocity in the body frame x direction"/>
<define name="DESIRED_VY" value="0" description="The desired velocity in the body frame y direction"/>
</section>
<section name="OPTICFLOW" prefix="OPTICFLOW_">
<define name="AGL_ID" value="ABI_SENDER_ID" description="ABI sender id for AGL message (sonar measurement) (default: ABI_BROADCAST)"/>
<define name="MAX_TRACK_CORNERS" value="25" description="The maximum amount of corners the Lucas Kanade algorithm is tracking between two frames"/>
<define name="WINDOW_SIZE" value="10" description="Window size used in Lucas Kanade algorithm"/>
<define name="SUBPIXEL_FACTOR" value="10" description="Amount of subpixels per pixel, used for more precise (subpixel) calculations of the flow"/>
<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="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"/>
<define name="FAST9_MIN_DISTANCE" value="10" description="The amount of pixels between corners that should be detected"/>
</section>
<define name="OPTICFLOW_AGL_ID" value="ABI_SENDER_ID" description="ABI sender id for AGL message (sonar measurement) (default: ABI_BROADCAST)"/>
</doc>
<settings>
<dl_settings NAME="Vision Loop">
<dl_settings name="vision">
<dl_settings NAME="Vision stabilization">
<dl_settings name="vision_stab">
<dl_setting var="opticflow_stab.phi_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
<dl_setting var="opticflow_stab.phi_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_phi" param="VISION_PHI_IGAIN"/>
<dl_setting var="opticflow_stab.theta_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_theta" param="VISION_THETA_PGAIN"/>
@@ -32,6 +45,18 @@
<dl_setting var="opticflow_stab.desired_vx" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vx" param="VISION_DESIRED_VX"/>
<dl_setting var="opticflow_stab.desired_vy" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vy" param="VISION_DESIRED_VY"/>
</dl_settings>
<dl_settings name="vision_calc">
<dl_setting var="opticflow.max_track_corners" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="max_trck_corners" param="OPTICFLOW_MAX_TRACK_CORNERS"/>
<dl_setting var="opticflow.window_size" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="window_size" param="OPTICFLOW_WINDOW_SIZE"/>
<dl_setting var="opticflow.subpixel_factor" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="subpixel_factor" param="OPTICFLOW_SUBPIXEL_FACTOR"/>
<dl_setting var="opticflow.max_iterations" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="max_iterations" param="OPTICFLOW_MAX_ITERATIONS"/>
<dl_setting var="opticflow.threshold_vec" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="threshold_vec" param="OPTICFLOW_THRESHOLD_VEC"/>
<dl_setting var="opticflow.fast9_adaptive" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="TRUE|FALSE" shortname="fast9_adaptive" param="OPTICFLOW_FAST9_ADAPTIVE"/>
<dl_setting var="opticflow.fast9_threshold" module="computer_vision/opticflow_module" min="0" step="1" max="255" shortname="fast9_threshold" param="OPTICFLOW_FAST9_THRESHOLD"/>
<dl_setting var="opticflow.fast9_min_distance" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="fast9_min_distance" param="OPTICFLOW_FAST9_MIN_DISTANCE"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+69 -17
View File
@@ -46,6 +46,47 @@
#define Fx_ARdrone 343.1211
#define Fy_ARdrone 348.5053
/* Set the default values */
#ifndef OPTICFLOW_MAX_TRACK_CORNERS
#define OPTICFLOW_MAX_TRACK_CORNERS 25
#endif
PRINT_CONFIG_VAR(OPTICFLOW_MAX_TRACK_CORNERS);
#ifndef OPTICFLOW_WINDOW_SIZE
#define OPTICFLOW_WINDOW_SIZE 10
#endif
PRINT_CONFIG_VAR(OPTICFLOW_WINDOW_SIZE);
#ifndef OPTICFLOW_SUBPIXEL_FACTOR
#define OPTICFLOW_SUBPIXEL_FACTOR 10
#endif
PRINT_CONFIG_VAR(OPTICFLOW_SUBPIXEL_FACTOR);
#ifndef OPTICFLOW_MAX_ITERATIONS
#define OPTICFLOW_MAX_ITERATIONS 10
#endif
PRINT_CONFIG_VAR(OPTICFLOW_MAX_ITERATIONS);
#ifndef OPTICFLOW_THRESHOLD_VEC
#define OPTICFLOW_THRESHOLD_VEC 2
#endif
PRINT_CONFIG_VAR(OPTICFLOW_THRESHOLD_VEC);
#ifndef OPTICFLOW_FAST9_ADAPTIVE
#define OPTICFLOW_FAST9_ADAPTIVE TRUE
#endif
PRINT_CONFIG_VAR(OPTICFLOW_FAST9_ADAPTIVE);
#ifndef OPTICFLOW_FAST9_THRESHOLD
#define OPTICFLOW_FAST9_THRESHOLD 20
#endif
PRINT_CONFIG_VAR(OPTICFLOW_FAST9_THRESHOLD);
#ifndef OPTICFLOW_FAST9_MIN_DISTANCE
#define OPTICFLOW_FAST9_MIN_DISTANCE 10
#endif
PRINT_CONFIG_VAR(OPTICFLOW_FAST9_MIN_DISTANCE);
/* Functions only used here */
static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime);
static int cmp_flow(const void *a, const void *b);
@@ -66,6 +107,17 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
opticflow->got_first_img = FALSE;
opticflow->prev_phi = 0.0;
opticflow->prev_theta = 0.0;
/* Set the default values */
opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
opticflow->window_size = OPTICFLOW_WINDOW_SIZE;
opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR;
opticflow->max_iterations = OPTICFLOW_MAX_ITERATIONS;
opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC;
opticflow->fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE;
opticflow->fast9_threshold = OPTICFLOW_FAST9_THRESHOLD;
opticflow->fast9_min_distance = OPTICFLOW_FAST9_MIN_DISTANCE;
}
/**
@@ -95,14 +147,18 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
// *************************************************************************************
// FAST corner detection (TODO: non fixed threashold)
static uint8_t threshold = 20;
struct point_t *corners = fast9_detect(img, threshold, 10, 20, 20, &result->corner_cnt);
struct point_t *corners = fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
20, 20, &result->corner_cnt);
// Adaptive threshold
if(result->corner_cnt < 40 && threshold > 5)
threshold--;
else if(result->corner_cnt > 50 && threshold < 60)
threshold++;
if(opticflow->fast9_adaptive) {
// Decrease and increase the threshold based on previous values
if(result->corner_cnt < 40 && opticflow->fast9_threshold > 5)
opticflow->fast9_threshold--;
else if(result->corner_cnt > 50 && opticflow->fast9_threshold < 60)
opticflow->fast9_threshold++;
}
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_CORNERS
image_show_points(img, corners, result->corner_cnt);
@@ -119,18 +175,14 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
// Corner Tracking
// *************************************************************************************
#define MAX_TRACK_CORNERS 25
#define HALF_WINDOW_SIZE 5
#define SUBPIXEL_FACTOR 10
#define MAX_ITERATIONS 10
#define THRESHOLD_VEC 2
// Execute a Lucas Kanade optical flow
result->tracked_cnt = result->corner_cnt;
struct flow_t *vectors = opticFlowLK(&opticflow->img_gray, &opticflow->prev_img_gray, corners, &result->tracked_cnt,
HALF_WINDOW_SIZE, SUBPIXEL_FACTOR, MAX_ITERATIONS, THRESHOLD_VEC, MAX_TRACK_CORNERS);
opticflow->window_size/2, opticflow->subpixel_factor, opticflow->max_iterations,
opticflow->threshold_vec, opticflow->max_track_corners);
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
image_show_flow(img, vectors, result->tracked_cnt, SUBPIXEL_FACTOR);
image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor);
#endif
// Get the median flow
@@ -158,14 +210,14 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
// Flow Derotation
float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / FOV_W;
float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / FOV_H;
result->flow_der_x = result->flow_x - diff_flow_x * SUBPIXEL_FACTOR;
result->flow_der_y = result->flow_y - diff_flow_y * SUBPIXEL_FACTOR;
result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor;
result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor;
opticflow->prev_phi = state->phi;
opticflow->prev_theta = state->theta;
// Velocity calculation
result->vel_x = -result->flow_der_x * result->fps / SUBPIXEL_FACTOR * img->w / Fx_ARdrone;
result->vel_y = result->flow_der_y * result->fps / SUBPIXEL_FACTOR * img->h / Fy_ARdrone;
result->vel_x = -result->flow_der_x * result->fps / opticflow->subpixel_factor * img->w / Fx_ARdrone;
result->vel_y = result->flow_der_y * result->fps / opticflow->subpixel_factor * img->h / Fy_ARdrone;
// *************************************************************************************
// Next Loop Preparation
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+10
View File
@@ -42,6 +42,16 @@ struct opticflow_t
struct image_t img_gray; //< Current gray image frame
struct image_t prev_img_gray; //< Previous gray image frame
struct timeval prev_timestamp; //< Timestamp of the previous frame, used for FPS calculation
uint8_t max_track_corners; //< Maximum amount of corners Lucas Kanade should track
uint16_t window_size; //< Window size of the Lucas Kanade calculation (needs to be even)
uint8_t subpixel_factor; //< The amount of subpixels per pixel
uint8_t max_iterations; //< The maximum amount of iterations the Lucas Kanade algorithm should do
uint8_t threshold_vec; //< The threshold in x, y subpixels which the algorithm should stop
bool_t fast9_adaptive; //< Whether the FAST9 threshold should be adaptive
uint8_t fast9_threshold; //< FAST9 corner detection threshold
uint16_t fast9_min_distance; //< Minimum distance in pixels between corners
};
sw/airborne/modules/computer_vision/opticflow_module.c
+1 -1
View File
@@ -64,7 +64,7 @@ PRINT_CONFIG_MSG("OPTICFLOW_DEVICE_SIZE = " _SIZE_HELPER(OPTICFLOW_DEVICE_SIZE))
PRINT_CONFIG_VAR(VIEWVIDEO_DEVICE_BUFFERS);
/* The main opticflow variables */
static struct opticflow_t opticflow; //< Opticflow calculations
struct opticflow_t opticflow; //< Opticflow calculations
static struct opticflow_result_t opticflow_result; //< The opticflow result
static struct opticflow_state_t opticflow_state; //< State of the drone to communicate with the opticflow
static struct v4l2_device *opticflow_dev; //< The opticflow camera V4L2 device
sw/airborne/modules/computer_vision/opticflow_module.h
+3
View File
@@ -32,6 +32,9 @@
#include "opticflow/opticflow_calculator.h"
#include "opticflow/stabilization_opticflow.h"
// Needed for settings
extern struct opticflow_t opticflow;
// Module functions
extern void opticflow_module_init(void);
extern void opticflow_module_run(void);