Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[modules] edgeflow added copyright

Browse Source
This commit is contained in:
k.n.mcguire@tudelft.nl
2016-03-09 17:51:08 +01:00
parent 3c91b3e264
commit e3b3185085
4 changed files with 119 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/computer_vision/lib/vision/edge_flow.c
+75 -4
View File
@@ -1,11 +1,82 @@
/*
* edge_flow.c
* Copyright (C) 2016 Kimberly McGuire <k.n.mcguire@tudelft.nl
*
* Created on: Feb 22, 2016
* Author: knmcguire
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
#include <lib/vision/edge_flow.h>
/**
* @file modules/computer_vision/lib/vision/edge_flow.ch
* @brief calculate optical flow with EdgeFlow
*
* Edge-histogram matching, implementation by K. N. McGuire
* Publication: Local Histogram Matching for Efficient Optical Flow Computation Applied to Velocity Estimation on Pocket Drones
* by K.N. McGuire et al. (2016), ICRA 2016
*/
#include <lib/vision/edge_flow.h>
/**
* Calc_previous_frame_nr; adaptive Time Horizon
* @param[in] *opticflow The opticalflow structure
* @param[in] *result The optical flow result
* @param[in] *current_frame_nr: The current frame number of the circular array of the edge_hist struct
* @param[in] *previous_frame_offset: previous frame offset of how far the method should compare the edgehistogram
* @param[out] *previous_frame_nr: previous frame index of the edgehist struct
*/
void calc_previous_frame_nr(struct opticflow_result_t *result, struct opticflow_t *opticflow, uint8_t current_frame_nr,
uint8_t *previous_frame_offset, uint8_t *previous_frame_nr)
{
// Adaptive Time Horizon:
// if the flow measured in previous frame is small,
// the algorithm will choose an frame further away back from the
// current frame to detect subpixel flow
if (MAX_HORIZON > 2) {
uint32_t flow_mag_x, flow_mag_y;
flow_mag_x = abs(result->flow_x);
flow_mag_y = abs(result->flow_y);
uint32_t min_flow = 3;
uint32_t max_flow = opticflow->search_distance - 3;
uint8_t previous_frame_offset_x = previous_frame_offset[0];
uint8_t previous_frame_offset_y = previous_frame_offset[1];
// IF statements which will decrement the previous frame offset
// if the measured flow of last loop is higher than max value (higher flow measured)
// and visa versa
if (flow_mag_x > max_flow && previous_frame_offset_x > 1) {
previous_frame_offset[0] = previous_frame_offset_x - 1;
}
if (flow_mag_x < min_flow && previous_frame_offset_x < MAX_HORIZON - 1) {
previous_frame_offset[0] = previous_frame_offset_x + 1;
}
if (flow_mag_y > max_flow && previous_frame_offset_y > 1) {
previous_frame_offset[1] = previous_frame_offset_y - 1;
}
if (flow_mag_y < min_flow && previous_frame_offset_y < MAX_HORIZON - 1) {
previous_frame_offset[1] = previous_frame_offset_y + 1;
}
}
//Wrap index previous frame offset from current frame nr.
previous_frame_nr[0] = (current_frame_nr - previous_frame_offset[0] + MAX_HORIZON) %
MAX_HORIZON;
previous_frame_nr[1] = (current_frame_nr - previous_frame_offset[1] + MAX_HORIZON) %
MAX_HORIZON;
}
/**
* Calculate a edge/gradient histogram for each dimension of the image
sw/airborne/modules/computer_vision/lib/vision/edge_flow.h
+28 -5
View File
@@ -1,8 +1,30 @@
/*
* edge_flow.h
* Copyright (C) 2016 Kimberly McGuire <k.n.mcguire@tudelft.nl
*
* Created on: Feb 22, 2016
* Author: knmcguire
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/lib/vision/edge_flow.ch
* @brief calculate optical flow with EdgeFlow
*
* Edge-histogram matching, implementation by K. N. McGuire
* Publication: Local Histogram Matching for Efficient Optical Flow Computation Applied to Velocity Estimation on Pocket Drones
* by K.N. McGuire et al. (2016), ICRA 2016
*/
#ifndef EDGE_FLOW_H_
@@ -61,11 +83,12 @@ struct edge_flow_t {
int32_t div_y;
};
void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result);
// Local functions of the EDGEFLOW algorithm
void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, struct edgeflow_displacement_t displacement,
int32_t *edge_hist_x);
void calc_previous_frame_nr(struct opticflow_result_t *result, struct opticflow_t *opticflow, uint8_t current_frame_nr,
uint8_t *previous_frame_offset, uint8_t *previous_frame_nr);
void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
char direction, uint16_t edge_threshold);
void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogram_prev, int32_t *displacement,
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+12 -45
View File
@@ -1,6 +1,7 @@
/*
* Copyright (C) 2014 Hann Woei Ho
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
* 2016 Kimberly McGuire <k.n.mcguire@tudelft.nl
*
* This file is part of Paparazzi.
*
@@ -333,7 +334,7 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
* @param[in] *img The image frame to calculate the optical flow from
* @param[out] *result The optical flow result
*/
void calc_edgeflow(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result)
{
// Define Static Variables
@@ -374,52 +375,18 @@ void calc_edgeflow(struct opticflow_t *opticflow, struct opticflow_state_t *stat
edge_hist[current_frame_nr].pitch = state->theta;
edge_hist[current_frame_nr].roll = state->phi;
// Adaptive Time Horizon:
// if the flow measured in previous frame is small,
// the algorithm will choose an frame further away back from the
// current frame to detect subpixel flow
if (MAX_HORIZON > 2) {
uint32_t flow_mag_x, flow_mag_y;
flow_mag_x = abs(edgeflow.flow_x);
flow_mag_y = abs(edgeflow.flow_y);
uint32_t min_flow = 3;
uint32_t max_flow = disp_range * RES - 3 * RES;
uint8_t previous_frame_offset_x = previous_frame_offset[0];
uint8_t previous_frame_offset_y = previous_frame_offset[1];
// IF statements which will decrement the previous frame offset
// if the measured flow of last loop is higher than max value (higher flow measured)
// and visa versa
if (flow_mag_x > max_flow && previous_frame_offset_x > 1) {
previous_frame_offset[0] = previous_frame_offset_x - 1;
}
if (flow_mag_x < min_flow && previous_frame_offset_x < MAX_HORIZON - 1) {
previous_frame_offset[0] = previous_frame_offset_x + 1;
}
if (flow_mag_y > max_flow && previous_frame_offset_y > 1) {
previous_frame_offset[1] = previous_frame_offset_y - 1;
}
if (flow_mag_y < min_flow && previous_frame_offset_y < MAX_HORIZON - 1) {
previous_frame_offset[1] = previous_frame_offset_y + 1;
}
}
//Wrap index previous frame offset from current frame nr.
uint8_t previous_frame_x = (current_frame_nr - previous_frame_offset[0] + MAX_HORIZON) %
MAX_HORIZON;
uint8_t previous_frame_y = (current_frame_nr - previous_frame_offset[1] + MAX_HORIZON) %
MAX_HORIZON;
// Calculate which previous edge_hist to compare with the current
uint8_t previous_frame_nr[2];
calc_previous_frame_nr(result, opticflow, current_frame_nr, previous_frame_offset, previous_frame_nr);
//Select edge histogram from the previous frame nr
int32_t *prev_edge_histogram_x = edge_hist[previous_frame_x].x;
int32_t *prev_edge_histogram_y = edge_hist[previous_frame_y].y;
int32_t *prev_edge_histogram_x = edge_hist[previous_frame_nr[0]].x;
int32_t *prev_edge_histogram_y = edge_hist[previous_frame_nr[1]].y;
//Calculate the corrosponding derotation of the two frames
int16_t der_shift_x = -(int16_t)((edge_hist[previous_frame_x].roll - edge_hist[current_frame_nr].roll) *
int16_t der_shift_x = -(int16_t)((edge_hist[previous_frame_nr[0]].roll - edge_hist[current_frame_nr].roll) *
(float)img->w / (OPTICFLOW_FOV_W));
int16_t der_shift_y = -(int16_t)((edge_hist[previous_frame_x].pitch - edge_hist[current_frame_nr].pitch) *
int16_t der_shift_y = -(int16_t)((edge_hist[previous_frame_nr[1]].pitch - edge_hist[current_frame_nr].pitch) *
(float)img->h / (OPTICFLOW_FOV_H));
// Estimate pixel wise displacement of the edge histograms for x and y direction
@@ -470,8 +437,8 @@ void calc_edgeflow(struct opticflow_t *opticflow, struct opticflow_state_t *stat
*/
float fps_x = 0;
float fps_y = 0;
float time_diff_x = (float)(timeval_diff(&edge_hist[previous_frame_x].frame_time, &img->ts)) / 1000.;
float time_diff_y = (float)(timeval_diff(&edge_hist[previous_frame_y].frame_time, &img->ts)) / 1000.;
float time_diff_x = (float)(timeval_diff(&edge_hist[previous_frame_nr[0]].frame_time, &img->ts)) / 1000.;
float time_diff_y = (float)(timeval_diff(&edge_hist[previous_frame_nr[1]].frame_time, &img->ts)) / 1000.;
fps_x = 1 / (time_diff_x);
fps_y = 1 / (time_diff_y);
@@ -513,7 +480,7 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
calc_fast9_lukas_kanade(opticflow, state, img, result);
} else {
if (opticflow->method == 1) {
calc_edgeflow(opticflow, state, img, result);
calc_edgeflow_tot(opticflow, state, img, result);
} else {
PRINT_CONFIG_MSG("Both edgeflow and Lukas kanade is not turned on. Define either USE_LK or use_EDGEFLOW on TRUE!");
}
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+2 -1
View File
@@ -1,6 +1,7 @@
/*
* Copyright (C) 2014 Hann Woei Ho
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
* 2016 Kimberly McGuire <k.n.mcguire@tudelft.nl
*
* This file is part of Paparazzi.
*
@@ -63,7 +64,7 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
struct opticflow_result_t *result);
void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result);
void calc_edgeflow(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result);
#endif /* OPTICFLOW_CALCULATOR_H */