mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 13:55:40 +08:00
[modules] made a seperate function for opticflow by fast9 and LK and added edgflow to opticflow_calculator
This commit is contained in:
k.n.mcguire@tudelft.nl
@@ -6,195 +6,6 @@
|
||||
*/
|
||||
#include <opticflow/edge_flow.h>
|
||||
|
||||
// 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 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,
|
||||
uint16_t size,
|
||||
uint8_t window, uint8_t disp_range, int32_t der_shift);
|
||||
|
||||
// Local assisting functions (only used here)
|
||||
// TODO: find a way to incorperate/find these functions in paparazzi
|
||||
static uint32_t timeval_diff2(struct timeval *starttime, struct timeval *finishtime);
|
||||
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
|
||||
* @param[in] *state The state of the drone
|
||||
* @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)
|
||||
{
|
||||
// Define Static Variables
|
||||
static struct edge_hist_t edge_hist[MAX_HORIZON];
|
||||
static uint8_t current_frame_nr = 0;
|
||||
static struct edge_flow_t edgeflow;
|
||||
static uint8_t previous_frame_offset[2] = {1, 1};
|
||||
|
||||
// Define Normal variables
|
||||
struct edgeflow_displacement_t displacement;
|
||||
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;
|
||||
int32_t *edge_hist_y = edge_hist[current_frame_nr].y;
|
||||
calculate_edge_histogram(img, edge_hist_x, 'x', 0);
|
||||
calculate_edge_histogram(img, edge_hist_y, 'y', 0);
|
||||
|
||||
// Copy frame time and angles of image to calculated edge histogram
|
||||
memcpy(&edge_hist[current_frame_nr].frame_time, &img->ts, sizeof(struct timeval));
|
||||
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;
|
||||
|
||||
//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;
|
||||
|
||||
//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) *
|
||||
(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) *
|
||||
(float)img->h / (OPTICFLOW_FOV_H));
|
||||
|
||||
// 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,
|
||||
window_size, disp_range, der_shift_x);
|
||||
calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
|
||||
displacement.y, img->h,
|
||||
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,
|
||||
window_size + disp_range, RES);
|
||||
line_fit(displacement.y, &edgeflow.div_y,
|
||||
&edgeflow.flow_y, img->h,
|
||||
window_size + disp_range, 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;
|
||||
|
||||
result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0];
|
||||
result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1];
|
||||
|
||||
//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
|
||||
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 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)
|
||||
#endif
|
||||
// Increment and wrap current time frame
|
||||
current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate a edge/gradient histogram for each dimension of the image
|
||||
@@ -340,7 +151,7 @@ void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogra
|
||||
* @param[in] *n The size of the array
|
||||
* @return The index of the smallest value of the array
|
||||
*/
|
||||
static uint32_t getMinimum(uint32_t *a, uint32_t n)
|
||||
uint32_t getMinimum(uint32_t *a, uint32_t n)
|
||||
{
|
||||
uint32_t i;
|
||||
uint32_t min_ind = 0;
|
||||
@@ -357,18 +168,6 @@ static uint32_t getMinimum(uint32_t *a, uint32_t n)
|
||||
return min_ind;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the difference from start till finish
|
||||
* @param[in] *starttime The start time to calculate the difference from
|
||||
* @param[in] *finishtime The finish time to calculate the difference from
|
||||
*/
|
||||
static uint32_t timeval_diff2(struct timeval *starttime, struct timeval *finishtime)
|
||||
{
|
||||
uint32_t msec;
|
||||
msec = (finishtime->tv_sec - starttime->tv_sec) * 1000;
|
||||
msec += (finishtime->tv_usec - starttime->tv_usec) / 1000;
|
||||
return msec;
|
||||
}
|
||||
|
||||
/**
|
||||
* Fits a linear model to an array with pixel displacements with least squares
|
||||
|
||||
@@ -63,5 +63,22 @@ struct edge_flow_t {
|
||||
|
||||
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 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,
|
||||
uint16_t size,
|
||||
uint8_t window, uint8_t disp_range, int32_t der_shift);
|
||||
|
||||
// Local assisting functions (only used here)
|
||||
// TODO: find a way to incorperate/find these functions in paparazzi
|
||||
uint32_t timeval_diff2(struct timeval *starttime, struct timeval *finishtime);
|
||||
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);
|
||||
|
||||
|
||||
#endif /* EDGE_FLOW_H_ */
|
||||
|
||||
@@ -35,11 +35,12 @@
|
||||
// Own Header
|
||||
#include "opticflow_calculator.h"
|
||||
|
||||
|
||||
// Computer Vision
|
||||
#include "lib/vision/image.h"
|
||||
#include "lib/vision/lucas_kanade.h"
|
||||
#include "lib/vision/fast_rosten.h"
|
||||
|
||||
#include "edge_flow.h"
|
||||
#include "size_divergence.h"
|
||||
#include "linear_flow_fit.h"
|
||||
|
||||
@@ -82,8 +83,8 @@ 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
|
||||
#ifndef OPTICFLOW_SEARCH_DISTANCE
|
||||
#define OPTICFLOW_SEARCH_DISTANCE 20
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_MAX_SEARCH_DISTANCE)
|
||||
|
||||
@@ -148,7 +149,7 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
|
||||
/* Set the default values */
|
||||
opticflow->method = 0; //0 = LK_fast9, 1 = Edgeflow
|
||||
opticflow->window_size = OPTICFLOW_WINDOW_SIZE;
|
||||
opticflow->search_distance = OPTICFLOW_MAX_SEARCH_DISTANCE;
|
||||
opticflow->search_distance = OPTICFLOW_SEARCH_DISTANCE;
|
||||
|
||||
opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
|
||||
opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR;
|
||||
@@ -161,15 +162,8 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Run the optical flow on a new image frame
|
||||
* @param[in] *opticflow The opticalflow structure that keeps track of previous images
|
||||
* @param[in] *state The state of the drone
|
||||
* @param[in] *img The image frame to calculate the optical flow from
|
||||
* @param[out] *result The optical flow result
|
||||
*/
|
||||
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
|
||||
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)
|
||||
{
|
||||
// variables for size_divergence:
|
||||
float size_divergence; int n_samples;
|
||||
@@ -326,6 +320,201 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
|
||||
image_switch(&opticflow->img_gray, &opticflow->prev_img_gray);
|
||||
}
|
||||
|
||||
/**
|
||||
* Run the optical flow with EDGEFLOW on a new image frame
|
||||
* @param[in] *opticflow The opticalflow structure that keeps track of previous images
|
||||
* @param[in] *state The state of the drone
|
||||
* @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,
|
||||
struct opticflow_result_t *result)
|
||||
{
|
||||
// Define Static Variables
|
||||
static struct edge_hist_t edge_hist[MAX_HORIZON];
|
||||
static uint8_t current_frame_nr = 0;
|
||||
static struct edge_flow_t edgeflow;
|
||||
static uint8_t previous_frame_offset[2] = {1, 1};
|
||||
|
||||
// Define Normal variables
|
||||
struct edgeflow_displacement_t displacement;
|
||||
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;
|
||||
int32_t *edge_hist_y = edge_hist[current_frame_nr].y;
|
||||
calculate_edge_histogram(img, edge_hist_x, 'x', 0);
|
||||
calculate_edge_histogram(img, edge_hist_y, 'y', 0);
|
||||
|
||||
// Copy frame time and angles of image to calculated edge histogram
|
||||
memcpy(&edge_hist[current_frame_nr].frame_time, &img->ts, sizeof(struct timeval));
|
||||
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;
|
||||
|
||||
//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;
|
||||
|
||||
//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) *
|
||||
(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) *
|
||||
(float)img->h / (OPTICFLOW_FOV_H));
|
||||
|
||||
// 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,
|
||||
window_size, disp_range, der_shift_x);
|
||||
calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
|
||||
displacement.y, img->h,
|
||||
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,
|
||||
window_size + disp_range, RES);
|
||||
line_fit(displacement.y, &edgeflow.div_y,
|
||||
&edgeflow.flow_y, img->h,
|
||||
window_size + disp_range, 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;
|
||||
|
||||
result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0];
|
||||
result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1];
|
||||
|
||||
//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_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.;
|
||||
fps_x = 1 / (time_diff_x);
|
||||
fps_y = 1 / (time_diff_y);
|
||||
|
||||
result->fps = fps_x;
|
||||
|
||||
// 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 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)
|
||||
#endif
|
||||
// Increment and wrap current time frame
|
||||
current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Run the optical flow on a new image frame
|
||||
* @param[in] *opticflow The opticalflow structure that keeps track of previous images
|
||||
* @param[in] *state The state of the drone
|
||||
* @param[in] *img The image frame to calculate the optical flow from
|
||||
* @param[out] *result The optical flow result
|
||||
*/
|
||||
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
|
||||
struct opticflow_result_t *result)
|
||||
{
|
||||
if (opticflow->method == 0) {
|
||||
calc_fast9_lukas_kanade(opticflow, state, img, result);
|
||||
} else {
|
||||
if (opticflow->method == 1) {
|
||||
calc_edgeflow(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!");
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the difference from start till finish
|
||||
* @param[in] *starttime The start time to calculate the difference from
|
||||
|
||||
@@ -61,7 +61,10 @@ struct opticflow_t {
|
||||
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 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,
|
||||
struct opticflow_result_t *result);
|
||||
#endif /* OPTICFLOW_CALCULATOR_H */
|
||||
|
||||
|
||||
|
||||
@@ -265,14 +265,8 @@ static void *opticflow_module_calc(void *data __attribute__((unused)))
|
||||
|
||||
// Do the optical flow calculation
|
||||
struct opticflow_result_t temp_result;
|
||||
opticflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
|
||||
|
||||
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