mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 13:55:40 +08:00
[modules] added settings to switch between edgeflow and LK, and some extra variables
This commit is contained in:
k.n.mcguire@tudelft.nl
@@ -24,10 +24,14 @@
|
||||
<define name="FX" value="343.1211" description="Field in the x direction of the camera (Defaults are from an ARDrone 2)"/>
|
||||
<define name="FY" value="348.5053" description="Field in the y direction of the camera (Defaults are from an ARDrone 2)"/>
|
||||
|
||||
<!-- General optical flow calculation parameters -->
|
||||
<define name="METHOD" value="0" description="Method used to calculate optical flow"/>
|
||||
<define name="WINDOW_SIZE" value="10" description="Window size used for block matching (pixels)"/>
|
||||
<define name="SEARCH_DISTANCE" value="10" description="Maximum search distance for blockmatching (pixels)"/>
|
||||
<define name="SUBPIXEL_FACTOR" value="100" description="Amount of subpixels per pixel, used for more precise (subpixel) calculations of the flow"/>
|
||||
|
||||
<!-- Lucas Kanade optical flow calculation parameters -->
|
||||
<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"/>
|
||||
|
||||
@@ -35,23 +39,30 @@
|
||||
<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>
|
||||
</doc>
|
||||
|
||||
<settings>
|
||||
<dl_settings NAME="Vision stabilization">
|
||||
|
||||
<!-- Optical flow calculations parameters for Lucas Kanade and FAST9 -->
|
||||
<!-- Optical flow calculations parameters -->
|
||||
<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.method" min="0" step="1" max="1" module="computer_vision/opticflow_module" shortname="method" values="LK_Fast9|EdgeFlow" param="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="1" max="100" shortname="subpixel_factor" param="OPTICFLOW_SUBPIXEL_FACTOR"/>
|
||||
|
||||
<!-- Specifically for Lucas Kanade and FAST9 -->
|
||||
<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.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>
|
||||
|
||||
@@ -21,7 +21,7 @@ static uint32_t timeval_diff2(struct timeval *starttime, struct timeval *finisht
|
||||
static uint32_t getMinimum(uint32_t *a, uint32_t n);
|
||||
void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border,
|
||||
uint16_t RES);
|
||||
|
||||
uint32_t getAmountPeaks(int32_t *edgehist, uint32_t median, int32_t size);
|
||||
/**
|
||||
* Run the optical flow with EDGEFLOW on a new image frame
|
||||
* @param[in] *opticflow The opticalflow structure that keeps track of previous images
|
||||
@@ -29,6 +29,8 @@ void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_
|
||||
* @param[in] *img The image frame to calculate the optical flow from
|
||||
* @param[out] *result The optical flow result
|
||||
*/
|
||||
|
||||
|
||||
void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
|
||||
struct opticflow_result_t *result)
|
||||
{
|
||||
@@ -40,8 +42,24 @@ void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t
|
||||
|
||||
// Define Normal variables
|
||||
struct edgeflow_displacement_t displacement;
|
||||
uint8_t disp_range = DISP_RANGE_MAX;
|
||||
uint16_t RES = 100;
|
||||
uint16_t disp_range;
|
||||
if (opticflow->search_distance < DISP_RANGE_MAX) {
|
||||
disp_range = opticflow->search_distance;
|
||||
} else {
|
||||
disp_range = DISP_RANGE_MAX;
|
||||
}
|
||||
|
||||
uint16_t window_size;
|
||||
|
||||
if (opticflow->window_size < MAX_WINDOW_SIZE) {
|
||||
window_size = opticflow->window_size;
|
||||
} else {
|
||||
window_size = MAX_WINDOW_SIZE;
|
||||
}
|
||||
|
||||
uint16_t RES = opticflow->subpixel_factor;
|
||||
|
||||
//......................Calculating EdgeFlow..................... //
|
||||
|
||||
// Calculate current frame's edge histogram
|
||||
int32_t *edge_hist_x = edge_hist[current_frame_nr].x;
|
||||
@@ -105,45 +123,71 @@ void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t
|
||||
// Estimate pixel wise displacement of the edge histograms for x and y direction
|
||||
calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x,
|
||||
displacement.x, img->w,
|
||||
opticflow->window_size, disp_range, der_shift_x);
|
||||
window_size, disp_range, der_shift_x);
|
||||
calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
|
||||
displacement.y, img->h,
|
||||
opticflow->window_size, disp_range, der_shift_y);
|
||||
window_size, disp_range, der_shift_y);
|
||||
|
||||
// Fit a line on the pixel displacement to estimate
|
||||
// the global pixel flow and divergence (RES is resolution)
|
||||
line_fit(displacement.x, &edgeflow.div_x,
|
||||
&edgeflow.flow_x, img->w,
|
||||
opticflow->window_size + disp_range, RES);
|
||||
window_size + disp_range, RES);
|
||||
line_fit(displacement.y, &edgeflow.div_y,
|
||||
&edgeflow.flow_y, img->h,
|
||||
opticflow->window_size + disp_range, RES);
|
||||
window_size + disp_range, RES);
|
||||
|
||||
// Save Resulting flow in results
|
||||
result->flow_x = (int16_t)edgeflow.flow_x / (previous_frame_offset[0] * RES);
|
||||
result->flow_y = (int16_t)edgeflow.flow_y / (previous_frame_offset[1] * RES);
|
||||
/* Save Resulting flow in results
|
||||
* Warning: The flow detected here is different in sign
|
||||
* and size, therefore this will be multiplied with
|
||||
* the same subpixel factor and -1 to make it on par with
|
||||
* the LK algorithm of t opticalflow_calculator.c
|
||||
* */
|
||||
edgeflow.flow_x = -1 * edgeflow.flow_x;
|
||||
edgeflow.flow_y = -1 * edgeflow.flow_y;
|
||||
|
||||
// VELOCITY //
|
||||
result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0];
|
||||
result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1];
|
||||
|
||||
//Estimate fps per direction
|
||||
//Fill up the results optic flow to be on par with LK_fast9
|
||||
result->flow_der_x = result->flow_x;
|
||||
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 = (float)edgeflow.flow_x / RES;
|
||||
result->div_size = 0.0f;
|
||||
result->noise_measurement = 0.0f;
|
||||
result->surface_roughness = 0.0f;
|
||||
|
||||
//......................Calculating VELOCITY ..................... //
|
||||
|
||||
/*Estimate fps per direction
|
||||
* This is the fps with adaptive horizon for subpixel flow, which is not similar
|
||||
* to the loop speed of the algorithm. The faster the quadcopter flies
|
||||
* the higher it becomes
|
||||
*/
|
||||
float fps_x = 0;
|
||||
float fps_y = 0;
|
||||
float time_diff_x = (float)(timeval_diff2(&edge_hist[previous_frame_x].frame_time, &img->ts)) / 1000.;
|
||||
float time_diff_y = (float)(timeval_diff2(&edge_hist[previous_frame_y].frame_time, &img->ts)) / 1000.;
|
||||
fps_x = 1 / (time_diff_x);
|
||||
fps_y = 1 / (time_diff_y);
|
||||
|
||||
result->fps = fps_x;
|
||||
|
||||
//Calculate velocity
|
||||
// Calculate velocity
|
||||
float vel_x = edgeflow.flow_x * fps_x * state->agl * OPTICFLOW_FOV_W / (img->w * RES);
|
||||
float vel_y = edgeflow.flow_y * fps_y * state->agl * OPTICFLOW_FOV_H / (img->h * RES);
|
||||
result->vel_x = vel_x;
|
||||
result->vel_y = vel_y;
|
||||
|
||||
// Rotate velocities from camera frame coordinates to body coordinates.
|
||||
// IMPORTANT since these values are used for control! This the case on the ARDrone and bebop, but on other systems this might be different!
|
||||
result->vel_body_x = vel_y;
|
||||
result->vel_body_y = - vel_x;
|
||||
/* Rotate velocities from camera frame coordinates to body coordinates.
|
||||
* IMPORTANT This frame to body orientation should bethe case for the parrot
|
||||
* ARdrone and Bebop, however this can be different for other quadcopters
|
||||
* ALWAYS double check!
|
||||
*/
|
||||
result->vel_body_x = - vel_y;
|
||||
result->vel_body_y = vel_x;
|
||||
|
||||
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
|
||||
draw_edgeflow_img(img, edgeflow, displacement, *edge_hist_x)
|
||||
@@ -270,8 +314,6 @@ void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogra
|
||||
if (border[0] >= border[1] || abs(der_shift) >= 10) {
|
||||
SHIFT_TOO_FAR = 1;
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
// TODO: replace with arm offset subtract
|
||||
for (x = border[0]; x < border[1]; x++) {
|
||||
@@ -420,5 +462,22 @@ void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, struct
|
||||
image_draw_line(img, &point1_extra, &point2_extra);
|
||||
}
|
||||
|
||||
/**
|
||||
* getAmountPeaks, calculates the amount of peaks in a edge histogram
|
||||
* @param[in] *edgehist Horizontal edge_histogram
|
||||
* @param[in] thres The threshold from which a peak is considered significant peak or not
|
||||
* @param[in] size Size of the array
|
||||
* @param[return] amount of peaks
|
||||
*/
|
||||
uint32_t getAmountPeaks(int32_t *edgehist, uint32_t thres, int32_t size)
|
||||
{
|
||||
uint32_t amountPeaks = 0;
|
||||
uint32_t i = 0;
|
||||
|
||||
|
||||
for (i = 1; i < size + 1; i ++) {
|
||||
if (edgehist[i - 1] < edgehist[i] && edgehist[i] > edgehist[i + 1] && edgehist[i] > thres) {
|
||||
amountPeaks ++;
|
||||
}
|
||||
}
|
||||
return amountPeaks;
|
||||
}
|
||||
|
||||
@@ -23,7 +23,10 @@
|
||||
#define MAX_HORIZON 10
|
||||
#endif
|
||||
#ifndef DISP_RANGE_MAX
|
||||
#define DISP_RANGE_MAX 20
|
||||
#define DISP_RANGE_MAX 50
|
||||
#endif
|
||||
#ifndef MAX_WINDOW_SIZE
|
||||
#define MAX_WINDOW_SIZE 20
|
||||
#endif
|
||||
#ifndef IMAGE_HEIGHT
|
||||
#define IMAGE_HEIGHT 240
|
||||
|
||||
@@ -82,8 +82,13 @@ PRINT_CONFIG_VAR(OPTICFLOW_MAX_TRACK_CORNERS)
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_WINDOW_SIZE)
|
||||
|
||||
#ifndef OPTICFLOW_MAX_SEARCH_DISTANCE
|
||||
#define OPTICFLOW_MAX_SEARCH_DISTANCE 20
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_MAX_SEARCH_DISTANCE)
|
||||
|
||||
#ifndef OPTICFLOW_SUBPIXEL_FACTOR
|
||||
#define OPTICFLOW_SUBPIXEL_FACTOR 10
|
||||
#define OPTICFLOW_SUBPIXEL_FACTOR 100
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_SUBPIXEL_FACTOR)
|
||||
|
||||
@@ -112,6 +117,13 @@ PRINT_CONFIG_VAR(OPTICFLOW_FAST9_THRESHOLD)
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_FAST9_MIN_DISTANCE)
|
||||
|
||||
#ifndef OPTICFLOW_METHOD
|
||||
#define OPTICFLOW_METHOD 0
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_METHOD)
|
||||
|
||||
|
||||
|
||||
/* 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);
|
||||
@@ -134,8 +146,11 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
|
||||
opticflow->prev_theta = 0.0;
|
||||
|
||||
/* Set the default values */
|
||||
opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
|
||||
opticflow->method = 0; //0 = LK_fast9, 1 = Edgeflow
|
||||
opticflow->window_size = OPTICFLOW_WINDOW_SIZE;
|
||||
opticflow->search_distance = OPTICFLOW_MAX_SEARCH_DISTANCE;
|
||||
|
||||
opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
|
||||
opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR;
|
||||
opticflow->max_iterations = OPTICFLOW_MAX_ITERATIONS;
|
||||
opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC;
|
||||
@@ -143,6 +158,7 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
|
||||
opticflow->fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE;
|
||||
opticflow->fast9_threshold = OPTICFLOW_FAST9_THRESHOLD;
|
||||
opticflow->fast9_min_distance = OPTICFLOW_FAST9_MIN_DISTANCE;
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -42,20 +42,25 @@ struct opticflow_t {
|
||||
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 method; ///< Method to use to calculate the optical flow
|
||||
uint16_t window_size; ///< Window size for the blockmatching algorithm (general value for all methods)
|
||||
uint16_t search_distance; ///< Search distance for blockmatchin alg. (
|
||||
|
||||
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
|
||||
|
||||
uint8_t max_track_corners; ///< Maximum amount of corners Lucas Kanade should track
|
||||
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
|
||||
|
||||
};
|
||||
|
||||
|
||||
void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h);
|
||||
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result);
|
||||
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
|
||||
struct opticflow_result_t *result);
|
||||
|
||||
#endif /* OPTICFLOW_CALCULATOR_H */
|
||||
|
||||
|
||||
@@ -266,16 +266,13 @@ static void *opticflow_module_calc(void *data __attribute__((unused)))
|
||||
// Do the optical flow calculation
|
||||
struct opticflow_result_t temp_result;
|
||||
|
||||
#if USE_LK
|
||||
opticflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
|
||||
#else
|
||||
#if USE_EDGEFLOW
|
||||
edgeflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
|
||||
#else
|
||||
PRINT_CONFIG_MSG("Both edgeflow and Lukas kanade is not turned on. Define either USE_LK or use_EDGEFLOW on TRUE!");
|
||||
#endif
|
||||
#endif
|
||||
|
||||
if (opticflow.method == 0) {
|
||||
opticflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
|
||||
} else {
|
||||
if (opticflow.method == 1) {
|
||||
edgeflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
|
||||
} else { PRINT_CONFIG_MSG("Both edgeflow and Lukas kanade is not turned on. Define either USE_LK or use_EDGEFLOW on TRUE!"); }
|
||||
}
|
||||
// Copy the result if finished
|
||||
pthread_mutex_lock(&opticflow_mutex);
|
||||
memcpy(&opticflow_result, &temp_result, sizeof(struct opticflow_result_t));
|
||||
|
||||
Reference in New Issue
Block a user