mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 13:55:40 +08:00
[modules] cleaned up opticalflow modules and edgeflow
This commit is contained in:
k.n.mcguire@tudelft.nl
@@ -31,6 +31,8 @@
|
|||||||
<load name="cv_opticflow.xml">
|
<load name="cv_opticflow.xml">
|
||||||
<define name="OPTICFLOW_DEBUG" value = "FALSE"/>
|
<define name="OPTICFLOW_DEBUG" value = "FALSE"/>
|
||||||
<define name="OPTICFLOW_SHOW_FLOW" value = "FALSE"/>
|
<define name="OPTICFLOW_SHOW_FLOW" value = "FALSE"/>
|
||||||
|
<define name="USE_EDGEFLOW" value = "TRUE"/>
|
||||||
|
<define name="USE_LK" value = "FALSE"/>
|
||||||
</load>
|
</load>
|
||||||
<load name="video_thread.xml">
|
<load name="video_thread.xml">
|
||||||
<define name="VIDEO_THREAD_CAMERA" value="front_camera"/>
|
<define name="VIDEO_THREAD_CAMERA" value="front_camera"/>
|
||||||
|
|||||||
@@ -79,10 +79,12 @@
|
|||||||
<file name="pprz_algebra_float.c" dir="math"/>
|
<file name="pprz_algebra_float.c" dir="math"/>
|
||||||
<file name="pprz_matrix_decomp_float.c" dir="math"/>
|
<file name="pprz_matrix_decomp_float.c" dir="math"/>
|
||||||
|
|
||||||
|
<file name="edge_flow.c" dir="modules/computer_vision/opticflow"/>
|
||||||
|
|
||||||
<!-- Main vision calculations -->
|
<!-- Main vision calculations -->
|
||||||
<file name="fast_rosten.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="lucas_kanade.c" dir="modules/computer_vision/lib/vision"/>
|
||||||
<file name="edge_flow.c" dir="modules/computer_vision/lib/vision"/>
|
|
||||||
<raw>
|
<raw>
|
||||||
include $(CFG_SHARED)/udp.makefile
|
include $(CFG_SHARED)/udp.makefile
|
||||||
|
|
||||||
|
|||||||
@@ -1,369 +0,0 @@
|
|||||||
/*
|
|
||||||
* edge_flow.c
|
|
||||||
*
|
|
||||||
* Created on: Feb 22, 2016
|
|
||||||
* Author: knmcguire
|
|
||||||
*/
|
|
||||||
#include <lib/vision/edge_flow.h>
|
|
||||||
|
|
||||||
static uint32_t timeval_diff2(struct timeval *starttime, struct timeval *finishtime);
|
|
||||||
|
|
||||||
void test_function(struct image_t *img,struct image_t *img_gray)
|
|
||||||
{
|
|
||||||
image_to_grayscale(img, img_gray);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// calculate_edge_histogram calculates the image gradient of the images and makes a edge feature histogram
|
|
||||||
void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
|
|
||||||
char direction, uint16_t edge_threshold)
|
|
||||||
{
|
|
||||||
uint8_t *img_buf = (uint8_t *)img->buf;
|
|
||||||
|
|
||||||
// TODO use arm_conv_q31()
|
|
||||||
int32_t sobel_sum = 0;
|
|
||||||
int32_t Sobel[3] = { -1, 0, 1};
|
|
||||||
|
|
||||||
uint32_t y = 0, x = 0;
|
|
||||||
int32_t c = 0;
|
|
||||||
|
|
||||||
uint32_t idx = 0;
|
|
||||||
|
|
||||||
uint16_t image_width = img->w;
|
|
||||||
uint16_t image_height = img->h;
|
|
||||||
uint32_t interlace;
|
|
||||||
if(img->type == IMAGE_GRAYSCALE)
|
|
||||||
interlace = 1;
|
|
||||||
else {
|
|
||||||
if(img->type == IMAGE_YUV422)
|
|
||||||
interlace = 2;
|
|
||||||
else
|
|
||||||
while (1); // hang to show user something isn't right
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// compute edge histogram
|
|
||||||
if (direction == 'x') {
|
|
||||||
// set values that are not visited
|
|
||||||
edge_histogram[0] = edge_histogram[image_width - 1] = 0;
|
|
||||||
for (x = 1; x < image_width - 1; x++) {
|
|
||||||
edge_histogram[x] = 0;
|
|
||||||
for (y = 0; y < image_height; y++) {
|
|
||||||
sobel_sum = 0;
|
|
||||||
|
|
||||||
for (c = -1; c <= 1; c++) {
|
|
||||||
idx = interlace * (image_width * y + (x + c)); // 2 for interlace
|
|
||||||
|
|
||||||
sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx+1];
|
|
||||||
}
|
|
||||||
sobel_sum = abs(sobel_sum);
|
|
||||||
if (sobel_sum > edge_threshold) {
|
|
||||||
edge_histogram[x] += sobel_sum;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
} else if (direction == 'y') {
|
|
||||||
// set values that are not visited
|
|
||||||
edge_histogram[0] = edge_histogram[image_height - 1] = 0;
|
|
||||||
for (y = 1; y < image_height - 1; y++) {
|
|
||||||
edge_histogram[y] = 0;
|
|
||||||
for (x = 0; x < image_width; x++) {
|
|
||||||
sobel_sum = 0;
|
|
||||||
|
|
||||||
for (c = -1; c <= 1; c++) {
|
|
||||||
idx = interlace * (image_width * (y + c) + x); // 2 for interlace
|
|
||||||
|
|
||||||
sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx+1];
|
|
||||||
}
|
|
||||||
sobel_sum = abs(sobel_sum);
|
|
||||||
if (sobel_sum > edge_threshold) {
|
|
||||||
edge_histogram[y] += sobel_sum;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
} else
|
|
||||||
while (1); // hang to show user something isn't right
|
|
||||||
}
|
|
||||||
|
|
||||||
// Calculate_displacement calculates the displacement between two histograms
|
|
||||||
// D should be half the search disparity range
|
|
||||||
// W is local search window
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
int32_t c = 0, r = 0;
|
|
||||||
uint32_t x = 0;
|
|
||||||
uint32_t SAD_temp[2 * DISP_RANGE_MAX + 1]; // size must be at least 2*D + 1
|
|
||||||
|
|
||||||
int32_t W = window;
|
|
||||||
int32_t D = disp_range;
|
|
||||||
|
|
||||||
|
|
||||||
uint8_t SHIFT_TOO_FAR = 0;
|
|
||||||
memset(displacement, 0, size);
|
|
||||||
|
|
||||||
int32_t border[2];
|
|
||||||
|
|
||||||
if (der_shift < 0)
|
|
||||||
{
|
|
||||||
border[0] = W + D + der_shift;
|
|
||||||
border[1] = size - W - D;
|
|
||||||
}
|
|
||||||
else if(der_shift > 0)
|
|
||||||
{
|
|
||||||
border[0] = W + D;
|
|
||||||
border[1] = size - W - D - der_shift;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
border[0] = W + D;
|
|
||||||
border[1] = size - W - D;
|
|
||||||
}
|
|
||||||
|
|
||||||
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++) {
|
|
||||||
displacement[x] = 0;
|
|
||||||
for (c = -D; c <= D; c++) {
|
|
||||||
SAD_temp[c + D] = 0;
|
|
||||||
for (r = -W; r <= W; r++) {
|
|
||||||
SAD_temp[c + D] += abs(edge_histogram[x + r] - edge_histogram_prev[x + r + c + der_shift]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if(!SHIFT_TOO_FAR)
|
|
||||||
displacement[x] = (int32_t)getMinimum(SAD_temp, 2 * D + 1) - D;
|
|
||||||
else
|
|
||||||
displacement[x]=0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Small supporting functions
|
|
||||||
|
|
||||||
uint32_t getMinimum(uint32_t *a, uint32_t n)
|
|
||||||
{
|
|
||||||
uint32_t i;
|
|
||||||
uint32_t min_ind = 0;
|
|
||||||
uint32_t min_err = a[min_ind];
|
|
||||||
uint32_t min_err_tot = 0;
|
|
||||||
for (i = 1; i < n; i++) {
|
|
||||||
if (a[i] <= min_err) {
|
|
||||||
min_ind = i;
|
|
||||||
min_err = a[i];
|
|
||||||
min_err_tot += min_err;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
//*min_error = min_err_tot;
|
|
||||||
return min_ind;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Line_fit fits a line using least squares to the histogram disparity map
|
|
||||||
void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border,
|
|
||||||
uint16_t RES)
|
|
||||||
{
|
|
||||||
int32_t x;
|
|
||||||
|
|
||||||
int32_t count = 0;
|
|
||||||
int32_t sumY = 0;
|
|
||||||
int32_t sumX = 0;
|
|
||||||
int32_t sumX2 = 0;
|
|
||||||
int32_t sumXY = 0;
|
|
||||||
int32_t xMean = 0;
|
|
||||||
int32_t yMean = 0;
|
|
||||||
int32_t divergence_int = 0;
|
|
||||||
int32_t border_int = (int32_t)border;
|
|
||||||
int32_t size_int = (int32_t)size;
|
|
||||||
uint32_t total_error = 0;
|
|
||||||
|
|
||||||
*divergence = 0;
|
|
||||||
*flow = 0;
|
|
||||||
|
|
||||||
// compute fixed sums
|
|
||||||
int32_t xend = size_int - border_int - 1;
|
|
||||||
sumX = xend * (xend + 1) / 2 - border_int * (border_int + 1) / 2 + border_int;
|
|
||||||
sumX2 = xend * (xend + 1) * (2 * xend + 1) / 6;
|
|
||||||
xMean = (size_int - 1) / 2;
|
|
||||||
count = size_int - 2 * border_int;
|
|
||||||
|
|
||||||
for (x = border_int; x < size - border_int; x++) {
|
|
||||||
sumY += displacement[x];
|
|
||||||
sumXY += x * displacement[x];
|
|
||||||
}
|
|
||||||
|
|
||||||
yMean = RES * sumY / count;
|
|
||||||
|
|
||||||
divergence_int = (RES * sumXY - sumX * yMean) / (sumX2 - sumX * xMean); // compute slope of line ax + b
|
|
||||||
*divergence = divergence_int;
|
|
||||||
*flow = yMean - *divergence * xMean; // compute b (or y) intercept of line ax + b
|
|
||||||
|
|
||||||
for (x = border_int; x < size - border_int; x++) {
|
|
||||||
total_error += abs(RES * displacement[x] - divergence_int * x + yMean);
|
|
||||||
}
|
|
||||||
|
|
||||||
//return total_error / size;
|
|
||||||
}
|
|
||||||
void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, struct edgeflow_displacement_t displacement, int32_t *edge_hist_x)
|
|
||||||
{
|
|
||||||
struct point_t point1;
|
|
||||||
struct point_t point2;
|
|
||||||
struct point_t point1_prev;
|
|
||||||
struct point_t point2_prev;
|
|
||||||
struct point_t point1_extra;
|
|
||||||
struct point_t point2_extra;
|
|
||||||
uint16_t i;
|
|
||||||
|
|
||||||
for(i = 120; i<240;i++)
|
|
||||||
{
|
|
||||||
point1.y = -(uint16_t)edge_hist_x[i]/100 + img->h/3;
|
|
||||||
point1.x = i;
|
|
||||||
point2.y = -(uint16_t)edge_hist_x[i+1]/100 + img->h/3;
|
|
||||||
point2.x = i+1;
|
|
||||||
|
|
||||||
point1_prev.y = -(uint16_t)displacement.horizontal[i]*5 + img->h*2/3;
|
|
||||||
point1_prev.x = i;
|
|
||||||
point2_prev.y = -(uint16_t)displacement.horizontal[i+1]*5 + img->h*2/3;
|
|
||||||
point2_prev.x = i+1;
|
|
||||||
|
|
||||||
image_draw_line(img, &point1,&point2);
|
|
||||||
image_draw_line(img, &point1_prev,&point2_prev);
|
|
||||||
}
|
|
||||||
|
|
||||||
point1_extra.y = (edgeflow.horizontal_flow+edgeflow.horizontal_div * -180 )/ 100+ img->h/2;
|
|
||||||
point1_extra.x = 0;
|
|
||||||
point2_extra.y = (edgeflow.horizontal_flow+edgeflow.horizontal_div * 180 )/ 100 + img->h/2;
|
|
||||||
point2_extra.x = 360;
|
|
||||||
image_draw_line(img, &point1_extra,&point2_extra);
|
|
||||||
}
|
|
||||||
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;
|
|
||||||
uint8_t disp_range = DISP_RANGE_MAX;
|
|
||||||
uint16_t RES = 100;
|
|
||||||
|
|
||||||
// Calculate current frame's edge histogram
|
|
||||||
int32_t *edge_hist_x = edge_hist[current_frame_nr].horizontal;
|
|
||||||
int32_t *edge_hist_y = edge_hist[current_frame_nr].vertical;
|
|
||||||
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.horizontal_flow);
|
|
||||||
flow_mag_y = abs(edgeflow.vertical_flow);
|
|
||||||
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].horizontal;
|
|
||||||
int32_t *prev_edge_histogram_y = edge_hist[previous_frame_y].vertical;
|
|
||||||
|
|
||||||
//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.horizontal, img->w,
|
|
||||||
opticflow->window_size, disp_range, der_shift_x);
|
|
||||||
calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
|
|
||||||
displacement.vertical, img->h,
|
|
||||||
opticflow->window_size, disp_range, der_shift_y);
|
|
||||||
|
|
||||||
line_fit(displacement.horizontal, &edgeflow.horizontal_div,
|
|
||||||
&edgeflow.horizontal_flow, img->w,
|
|
||||||
opticflow->window_size + disp_range, RES);
|
|
||||||
line_fit(displacement.vertical, &edgeflow.vertical_div,
|
|
||||||
&edgeflow.vertical_flow, img->h,
|
|
||||||
opticflow->window_size + disp_range, RES);
|
|
||||||
|
|
||||||
result->flow_x = (int16_t)edgeflow.horizontal_flow/(previous_frame_offset[0]*RES);
|
|
||||||
result->flow_y = (int16_t)edgeflow.vertical_flow/(previous_frame_offset[1]*RES);
|
|
||||||
|
|
||||||
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;
|
|
||||||
float vel_hor = edgeflow.horizontal_flow * fps_x* state->agl * OPTICFLOW_FOV_W / (img->w * RES);
|
|
||||||
float vel_ver = edgeflow.vertical_flow * fps_y * state->agl * OPTICFLOW_FOV_H / (img->h * RES);
|
|
||||||
|
|
||||||
result->vel_x = vel_ver;
|
|
||||||
result->vel_y = - vel_hor;
|
|
||||||
|
|
||||||
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
|
|
||||||
draw_edgeflow_img(img, edgeflow,displacement, *edge_hist_x)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON;
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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
|
|
||||||
* @return The difference in milliseconds
|
|
||||||
*/
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
@@ -1,68 +0,0 @@
|
|||||||
/*
|
|
||||||
* edge_flow.h
|
|
||||||
*
|
|
||||||
* Created on: Feb 22, 2016
|
|
||||||
* Author: knmcguire
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef EDGE_FLOW_H_
|
|
||||||
#define EDGE_FLOW_H_
|
|
||||||
|
|
||||||
|
|
||||||
#include "std.h"
|
|
||||||
#include "opticflow/inter_thread_data.h"
|
|
||||||
#include "lib/vision/image.h"
|
|
||||||
#include "lib/v4l/v4l2.h"
|
|
||||||
#include "opticflow/opticflow_calculator.h"
|
|
||||||
#include <string.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
|
|
||||||
#include "mcu_periph/sys_time.h"
|
|
||||||
|
|
||||||
#define MAX_HORIZON 10
|
|
||||||
#define IMAGE_HEIGHT 240
|
|
||||||
#define IMAGE_WIDTH 320
|
|
||||||
#define DISP_RANGE_MAX 20
|
|
||||||
#ifndef OPTICFLOW_FOV_W
|
|
||||||
#define OPTICFLOW_FOV_W 0.89360857702
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef OPTICFLOW_FOV_H
|
|
||||||
#define OPTICFLOW_FOV_H 0.67020643276
|
|
||||||
#endif
|
|
||||||
|
|
||||||
struct edge_hist_t {
|
|
||||||
int32_t horizontal[IMAGE_WIDTH];
|
|
||||||
int32_t vertical[IMAGE_HEIGHT];
|
|
||||||
struct timeval frame_time;
|
|
||||||
float roll;
|
|
||||||
float pitch;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct edgeflow_displacement_t {
|
|
||||||
int32_t horizontal[IMAGE_WIDTH];
|
|
||||||
int32_t vertical[IMAGE_HEIGHT];
|
|
||||||
};
|
|
||||||
|
|
||||||
struct edge_flow_t {
|
|
||||||
int32_t horizontal_flow;
|
|
||||||
int32_t horizontal_div;
|
|
||||||
int32_t vertical_flow;
|
|
||||||
int32_t vertical_div;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border,
|
|
||||||
uint16_t RES);
|
|
||||||
void test_function(struct image_t *image,struct image_t *image_gray);
|
|
||||||
void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
|
|
||||||
struct opticflow_result_t *result);
|
|
||||||
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);
|
|
||||||
uint32_t getMinimum(uint32_t *a, uint32_t n);
|
|
||||||
void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, struct edgeflow_displacement_t displacement, int32_t *edge_hist_x);
|
|
||||||
|
|
||||||
#endif /* EDGE_FLOW_H_ */
|
|
||||||
@@ -0,0 +1,424 @@
|
|||||||
|
/*
|
||||||
|
* edge_flow.c
|
||||||
|
*
|
||||||
|
* Created on: Feb 22, 2016
|
||||||
|
* Author: knmcguire
|
||||||
|
*/
|
||||||
|
#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);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 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;
|
||||||
|
uint8_t disp_range = DISP_RANGE_MAX;
|
||||||
|
uint16_t RES = 100;
|
||||||
|
|
||||||
|
// 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,
|
||||||
|
opticflow->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);
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
line_fit(displacement.y, &edgeflow.div_y,
|
||||||
|
&edgeflow.flow_y, img->h,
|
||||||
|
opticflow->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);
|
||||||
|
|
||||||
|
// VELOCITY //
|
||||||
|
|
||||||
|
//Estimate fps per direction
|
||||||
|
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 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;
|
||||||
|
|
||||||
|
#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
|
||||||
|
* @param[in] *img The image frame to calculate the edge histogram from
|
||||||
|
* @param[out] *edge_histogram The edge histogram from the current frame_step
|
||||||
|
* @param[in] direction Indicating if the histogram is made in either x or y direction
|
||||||
|
* @param[in] edge_threshold A threshold if a gradient is considered a edge or not
|
||||||
|
*/
|
||||||
|
void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
|
||||||
|
char direction, uint16_t edge_threshold)
|
||||||
|
{
|
||||||
|
uint8_t *img_buf = (uint8_t *)img->buf;
|
||||||
|
|
||||||
|
// TODO use arm_conv_q31()
|
||||||
|
int32_t sobel_sum = 0;
|
||||||
|
int32_t Sobel[3] = { -1, 0, 1};
|
||||||
|
|
||||||
|
uint32_t y = 0, x = 0;
|
||||||
|
int32_t c = 0;
|
||||||
|
|
||||||
|
uint32_t idx = 0;
|
||||||
|
|
||||||
|
uint16_t image_width = img->w;
|
||||||
|
uint16_t image_height = img->h;
|
||||||
|
uint32_t interlace;
|
||||||
|
if (img->type == IMAGE_GRAYSCALE) {
|
||||||
|
interlace = 1;
|
||||||
|
} else {
|
||||||
|
if (img->type == IMAGE_YUV422) {
|
||||||
|
interlace = 2;
|
||||||
|
} else
|
||||||
|
while (1); // hang to show user something isn't right
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// compute edge histogram
|
||||||
|
if (direction == 'x') {
|
||||||
|
// set values that are not visited
|
||||||
|
edge_histogram[0] = edge_histogram[image_width - 1] = 0;
|
||||||
|
for (x = 1; x < image_width - 1; x++) {
|
||||||
|
edge_histogram[x] = 0;
|
||||||
|
for (y = 0; y < image_height; y++) {
|
||||||
|
sobel_sum = 0;
|
||||||
|
|
||||||
|
for (c = -1; c <= 1; c++) {
|
||||||
|
idx = interlace * (image_width * y + (x + c)); // 2 for interlace
|
||||||
|
|
||||||
|
sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx + 1];
|
||||||
|
}
|
||||||
|
sobel_sum = abs(sobel_sum);
|
||||||
|
if (sobel_sum > edge_threshold) {
|
||||||
|
edge_histogram[x] += sobel_sum;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else if (direction == 'y') {
|
||||||
|
// set values that are not visited
|
||||||
|
edge_histogram[0] = edge_histogram[image_height - 1] = 0;
|
||||||
|
for (y = 1; y < image_height - 1; y++) {
|
||||||
|
edge_histogram[y] = 0;
|
||||||
|
for (x = 0; x < image_width; x++) {
|
||||||
|
sobel_sum = 0;
|
||||||
|
|
||||||
|
for (c = -1; c <= 1; c++) {
|
||||||
|
idx = interlace * (image_width * (y + c) + x); // 2 for interlace
|
||||||
|
|
||||||
|
sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx + 1];
|
||||||
|
}
|
||||||
|
sobel_sum = abs(sobel_sum);
|
||||||
|
if (sobel_sum > edge_threshold) {
|
||||||
|
edge_histogram[y] += sobel_sum;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else
|
||||||
|
while (1); // hang to show user something isn't right
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Calculate_displacement calculates the displacement between two histograms
|
||||||
|
* @param[in] *edge_histogram The edge histogram from the current frame_step
|
||||||
|
* @param[in] *edge_histogram_prev The edge histogram from the previous frame_step
|
||||||
|
* @param[in] *displacement array with pixel displacement of the sequential edge histograms
|
||||||
|
* @param[in] size Indicating the size of the displacement array
|
||||||
|
* @param[in] window Indicating the search window size
|
||||||
|
* @param[in] disp_range Indicating the maximum disparity range for the block matching
|
||||||
|
* @param[in] der_shift The pixel shift estimated by the angle rate of the IMU
|
||||||
|
*/
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
int32_t c = 0, r = 0;
|
||||||
|
uint32_t x = 0;
|
||||||
|
uint32_t SAD_temp[2 * DISP_RANGE_MAX + 1]; // size must be at least 2*D + 1
|
||||||
|
|
||||||
|
int32_t W = window;
|
||||||
|
int32_t D = disp_range;
|
||||||
|
|
||||||
|
|
||||||
|
uint8_t SHIFT_TOO_FAR = 0;
|
||||||
|
memset(displacement, 0, size);
|
||||||
|
|
||||||
|
int32_t border[2];
|
||||||
|
|
||||||
|
if (der_shift < 0) {
|
||||||
|
border[0] = W + D + der_shift;
|
||||||
|
border[1] = size - W - D;
|
||||||
|
} else if (der_shift > 0) {
|
||||||
|
border[0] = W + D;
|
||||||
|
border[1] = size - W - D - der_shift;
|
||||||
|
} else {
|
||||||
|
border[0] = W + D;
|
||||||
|
border[1] = size - W - D;
|
||||||
|
}
|
||||||
|
|
||||||
|
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++) {
|
||||||
|
displacement[x] = 0;
|
||||||
|
for (c = -D; c <= D; c++) {
|
||||||
|
SAD_temp[c + D] = 0;
|
||||||
|
for (r = -W; r <= W; r++) {
|
||||||
|
SAD_temp[c + D] += abs(edge_histogram[x + r] - edge_histogram_prev[x + r + c + der_shift]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (!SHIFT_TOO_FAR) {
|
||||||
|
displacement[x] = (int32_t)getMinimum(SAD_temp, 2 * D + 1) - D;
|
||||||
|
} else {
|
||||||
|
displacement[x] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Calculate minimum of an array
|
||||||
|
* @param[in] *a Array containing values
|
||||||
|
* @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 i;
|
||||||
|
uint32_t min_ind = 0;
|
||||||
|
uint32_t min_err = a[min_ind];
|
||||||
|
uint32_t min_err_tot = 0;
|
||||||
|
for (i = 1; i < n; i++) {
|
||||||
|
if (a[i] <= min_err) {
|
||||||
|
min_ind = i;
|
||||||
|
min_err = a[i];
|
||||||
|
min_err_tot += min_err;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//*min_error = min_err_tot;
|
||||||
|
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
|
||||||
|
* @param[in] *displacements Array with Pixel Displacements
|
||||||
|
* @param[out] *divergence Global divergence of pixel displacements
|
||||||
|
* @param[out] *flow Global translational flow from pixel displacements
|
||||||
|
* @param[in] *size Size of displacement array
|
||||||
|
* @param[in] border A border offset of the array that should not be considerd for the line fit
|
||||||
|
* @param[in] RES Resolution to be used for the integer based linefit
|
||||||
|
*/
|
||||||
|
void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border,
|
||||||
|
uint16_t RES)
|
||||||
|
{
|
||||||
|
int32_t x;
|
||||||
|
|
||||||
|
int32_t count = 0;
|
||||||
|
int32_t sumY = 0;
|
||||||
|
int32_t sumX = 0;
|
||||||
|
int32_t sumX2 = 0;
|
||||||
|
int32_t sumXY = 0;
|
||||||
|
int32_t xMean = 0;
|
||||||
|
int32_t yMean = 0;
|
||||||
|
int32_t divergence_int = 0;
|
||||||
|
int32_t border_int = (int32_t)border;
|
||||||
|
int32_t size_int = (int32_t)size;
|
||||||
|
uint32_t total_error = 0;
|
||||||
|
|
||||||
|
*divergence = 0;
|
||||||
|
*flow = 0;
|
||||||
|
|
||||||
|
// compute fixed sums
|
||||||
|
int32_t xend = size_int - border_int - 1;
|
||||||
|
sumX = xend * (xend + 1) / 2 - border_int * (border_int + 1) / 2 + border_int;
|
||||||
|
sumX2 = xend * (xend + 1) * (2 * xend + 1) / 6;
|
||||||
|
xMean = (size_int - 1) / 2;
|
||||||
|
count = size_int - 2 * border_int;
|
||||||
|
|
||||||
|
for (x = border_int; x < size - border_int; x++) {
|
||||||
|
sumY += displacement[x];
|
||||||
|
sumXY += x * displacement[x];
|
||||||
|
}
|
||||||
|
|
||||||
|
yMean = RES * sumY / count;
|
||||||
|
|
||||||
|
divergence_int = (RES * sumXY - sumX * yMean) / (sumX2 - sumX * xMean); // compute slope of line ax + b
|
||||||
|
*divergence = divergence_int;
|
||||||
|
*flow = yMean - *divergence * xMean; // compute b (or y) intercept of line ax + b
|
||||||
|
|
||||||
|
for (x = border_int; x < size - border_int; x++) {
|
||||||
|
total_error += abs(RES * displacement[x] - divergence_int * x + yMean);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Draws edgehistogram, displacement and linefit directly on the image for debugging (only for edgeflow in horizontal direction!!)
|
||||||
|
* @param[out] *img The image structure where will be drawn on
|
||||||
|
* @param[in] edgeflow Information structure for flow information
|
||||||
|
* @param[in] Displacement Pixel wise Displacement array
|
||||||
|
* @param[in] *edge_hist_x Horizontal edge_histogram
|
||||||
|
*/
|
||||||
|
void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, struct edgeflow_displacement_t displacement,
|
||||||
|
int32_t *edge_hist_x)
|
||||||
|
{
|
||||||
|
struct point_t point1;
|
||||||
|
struct point_t point2;
|
||||||
|
struct point_t point1_prev;
|
||||||
|
struct point_t point2_prev;
|
||||||
|
struct point_t point1_extra;
|
||||||
|
struct point_t point2_extra;
|
||||||
|
uint16_t i;
|
||||||
|
|
||||||
|
for (i = 120; i < 240; i++) {
|
||||||
|
point1.y = -(uint16_t)edge_hist_x[i] / 100 + img->h / 3;
|
||||||
|
point1.x = i;
|
||||||
|
point2.y = -(uint16_t)edge_hist_x[i + 1] / 100 + img->h / 3;
|
||||||
|
point2.x = i + 1;
|
||||||
|
|
||||||
|
point1_prev.y = -(uint16_t)displacement.x[i] * 5 + img->h * 2 / 3;
|
||||||
|
point1_prev.x = i;
|
||||||
|
point2_prev.y = -(uint16_t)displacement.x[i + 1] * 5 + img->h * 2 / 3;
|
||||||
|
point2_prev.x = i + 1;
|
||||||
|
|
||||||
|
image_draw_line(img, &point1, &point2);
|
||||||
|
image_draw_line(img, &point1_prev, &point2_prev);
|
||||||
|
}
|
||||||
|
|
||||||
|
point1_extra.y = (edgeflow.flow_x + edgeflow.div_x * -180) / 100 + img->h / 2;
|
||||||
|
point1_extra.x = 0;
|
||||||
|
point2_extra.y = (edgeflow.flow_x + edgeflow.div_x * 180) / 100 + img->h / 2;
|
||||||
|
point2_extra.x = 360;
|
||||||
|
image_draw_line(img, &point1_extra, &point2_extra);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@@ -0,0 +1,64 @@
|
|||||||
|
/*
|
||||||
|
* edge_flow.h
|
||||||
|
*
|
||||||
|
* Created on: Feb 22, 2016
|
||||||
|
* Author: knmcguire
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef EDGE_FLOW_H_
|
||||||
|
#define EDGE_FLOW_H_
|
||||||
|
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
#include "opticflow/inter_thread_data.h"
|
||||||
|
#include "lib/vision/image.h"
|
||||||
|
#include "lib/v4l/v4l2.h"
|
||||||
|
#include "opticflow/opticflow_calculator.h"
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef MAX_HORIZON
|
||||||
|
#define MAX_HORIZON 10
|
||||||
|
#endif
|
||||||
|
#ifndef DISP_RANGE_MAX
|
||||||
|
#define DISP_RANGE_MAX 20
|
||||||
|
#endif
|
||||||
|
#ifndef IMAGE_HEIGHT
|
||||||
|
#define IMAGE_HEIGHT 240
|
||||||
|
#endif
|
||||||
|
#ifndef IMAGE_WIDTH
|
||||||
|
#define IMAGE_WIDTH 360
|
||||||
|
#endif
|
||||||
|
#ifndef OPTICFLOW_FOV_W
|
||||||
|
#define OPTICFLOW_FOV_W 0.89360857702
|
||||||
|
#endif
|
||||||
|
#ifndef OPTICFLOW_FOV_H
|
||||||
|
#define OPTICFLOW_FOV_H 0.67020643276
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct edge_hist_t {
|
||||||
|
int32_t x[IMAGE_WIDTH];
|
||||||
|
int32_t y[IMAGE_HEIGHT];
|
||||||
|
struct timeval frame_time;
|
||||||
|
float roll;
|
||||||
|
float pitch;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct edgeflow_displacement_t {
|
||||||
|
int32_t x[IMAGE_WIDTH];
|
||||||
|
int32_t y[IMAGE_HEIGHT];
|
||||||
|
};
|
||||||
|
|
||||||
|
struct edge_flow_t {
|
||||||
|
int32_t flow_x;
|
||||||
|
int32_t div_x;
|
||||||
|
int32_t flow_y;
|
||||||
|
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);
|
||||||
|
|
||||||
|
#endif /* EDGE_FLOW_H_ */
|
||||||
@@ -40,8 +40,11 @@ struct opticflow_result_t {
|
|||||||
int16_t flow_der_x; ///< The derotated flow calculation in the x direction (in subpixels)
|
int16_t flow_der_x; ///< The derotated flow calculation in the x direction (in subpixels)
|
||||||
int16_t flow_der_y; ///< The derotated flow calculation in the y direction (in subpixels)
|
int16_t flow_der_y; ///< The derotated flow calculation in the y direction (in subpixels)
|
||||||
|
|
||||||
float vel_x; ///< The velocity in the x direction
|
float vel_x; ///< The velocity in the x direction (image coordinates)
|
||||||
float vel_y; ///< The velocity in the y direction
|
float vel_y; ///< The velocity in the y direction (image coordinates)
|
||||||
|
|
||||||
|
float vel_body_x; ///< The velocity in the x direction (body fixed coordinates)
|
||||||
|
float vel_body_y; ///< The velocity in the y direction (body fixed coordinates)
|
||||||
|
|
||||||
float div_size; ///< Divergence as determined with the size_divergence script
|
float div_size; ///< Divergence as determined with the size_divergence script
|
||||||
|
|
||||||
|
|||||||
@@ -280,16 +280,19 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
|
|||||||
// Velocity calculation
|
// Velocity calculation
|
||||||
// Right now this formula is under assumption that the flow only exist in the center axis of the camera.
|
// Right now this formula is under assumption that the flow only exist in the center axis of the camera.
|
||||||
// TODO Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane.
|
// TODO Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane.
|
||||||
float vel_hor = result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FX;
|
float vel_x = result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FX;
|
||||||
float vel_ver = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FY;
|
float vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FY;
|
||||||
|
result->vel_x = vel_x;
|
||||||
|
result->vel_y = vel_y;
|
||||||
|
|
||||||
// Velocity calculation: uncomment if focal length of the camera is not known or incorrect.
|
// Velocity calculation: uncomment if focal length of the camera is not known or incorrect.
|
||||||
// result->vel_x = - result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_W / img->w
|
// result->vel_x = - result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_W / img->w
|
||||||
// result->vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_H / img->h
|
// result->vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_H / img->h
|
||||||
|
|
||||||
// Rotate velocities from camera frame coordinates to body coordinates.
|
// Rotate velocities from camera frame coordinates to body coordinates.
|
||||||
result->vel_x = vel_ver;
|
// IMPORTANT for control! This the case on the ARDrone and bebop, but on other systems this might be different!
|
||||||
result->vel_y = - vel_hor;
|
result->vel_body_x = vel_y;
|
||||||
|
result->vel_body_y = - vel_x;
|
||||||
|
|
||||||
// Determine quality of noise measurement for state filter
|
// Determine quality of noise measurement for state filter
|
||||||
//TODO Experiment with multiple noise measurement models
|
//TODO Experiment with multiple noise measurement models
|
||||||
|
|||||||
@@ -36,7 +36,6 @@
|
|||||||
#include "lib/v4l/v4l2.h"
|
#include "lib/v4l/v4l2.h"
|
||||||
#include "lib/encoding/jpeg.h"
|
#include "lib/encoding/jpeg.h"
|
||||||
#include "lib/encoding/rtp.h"
|
#include "lib/encoding/rtp.h"
|
||||||
#include "lib/vision/edge_flow.h"
|
|
||||||
|
|
||||||
|
|
||||||
/* Default sonar/agl to use in opticflow visual_estimator */
|
/* Default sonar/agl to use in opticflow visual_estimator */
|
||||||
@@ -69,6 +68,16 @@ PRINT_CONFIG_MSG("OPTICFLOW_DEVICE_SIZE = " _SIZE_HELPER(OPTICFLOW_DEVICE_SIZE))
|
|||||||
#endif
|
#endif
|
||||||
PRINT_CONFIG_VAR(OPTICFLOW_DEVICE_BUFFERS)
|
PRINT_CONFIG_VAR(OPTICFLOW_DEVICE_BUFFERS)
|
||||||
|
|
||||||
|
#ifndef USE_EDGEFLOW
|
||||||
|
#define USE_EDGEFLOW FALSE
|
||||||
|
#endif
|
||||||
|
PRINT_CONFIG_VAR(USE_EDGEFLOW)
|
||||||
|
|
||||||
|
#ifndef USE_LK
|
||||||
|
#define USE_LK TRUE ///< The video device buffers (the amount of V4L2 buffers)
|
||||||
|
#endif
|
||||||
|
PRINT_CONFIG_VAR(USE_LK)
|
||||||
|
|
||||||
/* The main opticflow variables */
|
/* The main opticflow variables */
|
||||||
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_result_t opticflow_result; ///< The opticflow result
|
||||||
@@ -177,8 +186,8 @@ void opticflow_module_run(void)
|
|||||||
//TODO Find an appropiate quality measure for the noise model in the state filter, for now it is tracked_cnt
|
//TODO Find an appropiate quality measure for the noise model in the state filter, for now it is tracked_cnt
|
||||||
if (opticflow_result.tracked_cnt > 0) {
|
if (opticflow_result.tracked_cnt > 0) {
|
||||||
AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
|
AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
|
||||||
opticflow_result.vel_x,
|
opticflow_result.vel_body_x,
|
||||||
opticflow_result.vel_y,
|
opticflow_result.vel_body_y,
|
||||||
0.0f,
|
0.0f,
|
||||||
opticflow_result.noise_measurement
|
opticflow_result.noise_measurement
|
||||||
);
|
);
|
||||||
@@ -256,11 +265,16 @@ static void *opticflow_module_calc(void *data __attribute__((unused)))
|
|||||||
|
|
||||||
// Do the optical flow calculation
|
// Do the optical flow calculation
|
||||||
struct opticflow_result_t temp_result;
|
struct opticflow_result_t temp_result;
|
||||||
//opticflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
|
|
||||||
edgeflow_calc_frame(&opticflow, &temp_state, &img, &temp_result);
|
|
||||||
|
|
||||||
//test_function(&img,&img_gray);
|
#if USE_LK
|
||||||
//image_to_grayscale(&img, &img_gray);
|
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
|
||||||
|
|
||||||
// Copy the result if finished
|
// Copy the result if finished
|
||||||
pthread_mutex_lock(&opticflow_mutex);
|
pthread_mutex_lock(&opticflow_mutex);
|
||||||
@@ -274,7 +288,7 @@ static void *opticflow_module_calc(void *data __attribute__((unused)))
|
|||||||
&video_sock, // UDP device
|
&video_sock, // UDP device
|
||||||
&img_jpeg,
|
&img_jpeg,
|
||||||
0, // Format 422
|
0, // Format 422
|
||||||
50, // Jpeg-Quality
|
50, // Jpeg-Quality
|
||||||
0, // DRI Header
|
0, // DRI Header
|
||||||
0 // 90kHz time increment
|
0 // 90kHz time increment
|
||||||
);
|
);
|
||||||
|
|||||||
@@ -29,6 +29,7 @@
|
|||||||
|
|
||||||
// Include opticflow calculator
|
// Include opticflow calculator
|
||||||
#include "opticflow/opticflow_calculator.h"
|
#include "opticflow/opticflow_calculator.h"
|
||||||
|
#include "opticflow/edge_flow.h"
|
||||||
|
|
||||||
// Needed for settings
|
// Needed for settings
|
||||||
extern struct opticflow_t opticflow;
|
extern struct opticflow_t opticflow;
|
||||||
|
|||||||
Reference in New Issue
Block a user