Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 05:42:49 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[modules]edgeflow calculate velocity, however fps is still wrong

Browse Source
This commit is contained in:
k.n.mcguire@tudelft.nl
2016-02-23 16:40:14 +01:00
parent d08d7aa5d8
commit a7226ee743
3 changed files with 109 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_edgeflow.xml
+2 -2
View File
@@ -29,8 +29,8 @@
<load name="send_imu_mag_current.xml"/> <load name="send_imu_mag_current.xml"/>
<load name="logger_file.xml"/> <load name="logger_file.xml"/>
<load name="cv_opticflow.xml"> <load name="cv_opticflow.xml">
<define name="OPTICFLOW_DEBUG" value = "TRUE"/> <define name="OPTICFLOW_DEBUG" value = "FALSE"/>
<define name="OPTICFLOW_SHOW_FLOW" value = "TRUE"/> <define name="OPTICFLOW_SHOW_FLOW" 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"/>
sw/airborne/modules/computer_vision/lib/vision/edge_flow.c
+96 -15
View File
@@ -20,7 +20,7 @@ void test_function(struct image_t *img,struct image_t *img_gray)
void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[], void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
char direction, uint16_t edge_threshold) char direction, uint16_t edge_threshold)
{ {
uint8_t *img_buf = (uint8_t *)img->buf; uint8_t *img_buf = (uint8_t *)img->buf;
// TODO use arm_conv_q31() // TODO use arm_conv_q31()
int32_t sobel_sum = 0; int32_t sobel_sum = 0;
@@ -138,7 +138,7 @@ void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogra
} }
} }
if(!SHIFT_TOO_FAR) if(!SHIFT_TOO_FAR)
displacement[x] = (int32_t)getMinimum(SAD_temp, 2 * D + 1) - D; displacement[x] = (int32_t)getMinimum(SAD_temp, 2 * D + 1) - D;
else else
displacement[x]=0; displacement[x]=0;
} }
@@ -210,14 +210,46 @@ void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_
//return total_error / size; //return total_error / size;
} }
/**
* 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
*/
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;
}
void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result) struct opticflow_result_t *result)
{ {
static struct edge_hist_t edge_hist[MAX_HORIZON]; static struct edge_hist_t edge_hist[MAX_HORIZON];
static uint8_t current_frame_nr = 1; static uint8_t current_frame_nr = 1;
struct edge_flow_t edgeflow; struct edge_flow_t edgeflow;
static uint8_t previous_frame_offset[2] = {0,0};
static uint8_t counter_check = 0;
/*static uint32_t previous_time = 0;//sys_time.nb_tick;
static uint16_t freq_counter = 0;
static uint8_t frequency = 0;
freq_counter++;
if ((sys_time.nb_tick - previous_time) > sys_time.ticks_per_sec) { // 1s has past
frequency = (uint8_t)((freq_counter * (sys_time.nb_tick - previous_time)) / sys_time.ticks_per_sec);
freq_counter = 0;
previous_time = sys_time.nb_tick;
}
printf("freq %d\n",frequency);*/
// printf("freq %d\n",frequency);
uint8_t disp_range = DISP_RANGE_MAX;
int32_t *edge_hist_x = edge_hist[current_frame_nr].horizontal; int32_t *edge_hist_x = edge_hist[current_frame_nr].horizontal;
int32_t *edge_hist_y = edge_hist[current_frame_nr].vertical; int32_t *edge_hist_y = edge_hist[current_frame_nr].vertical;
@@ -225,20 +257,44 @@ void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t
calculate_edge_histogram(img, edge_hist_x, 'x',0); calculate_edge_histogram(img, edge_hist_x, 'x',0);
calculate_edge_histogram(img, edge_hist_y, 'y',0); calculate_edge_histogram(img, edge_hist_y, 'y',0);
uint8_t previous_frame_x = (current_frame_nr - 1 + MAX_HORIZON) % //edge_hist[current_frame_nr].frame_time = img->ts;
memcpy(&edge_hist[current_frame_nr].frame_time, &img->ts, sizeof(struct timeval));
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 = disp_range - 3;
uint8_t previous_frame_offset_x = previous_frame_offset[0];
uint8_t previous_frame_offset_y = previous_frame_offset[1];
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;
}
uint8_t previous_frame_x = (current_frame_nr - previous_frame_offset[0] + MAX_HORIZON) %
MAX_HORIZON; // wrap index MAX_HORIZON; // wrap index
uint8_t previous_frame_y = (current_frame_nr - 1 + MAX_HORIZON) % uint8_t previous_frame_y = (current_frame_nr - previous_frame_offset[1] + MAX_HORIZON) %
MAX_HORIZON; // wrap index MAX_HORIZON; // wrap index
int32_t *prev_edge_histogram_x = edge_hist[previous_frame_x].horizontal; int32_t *prev_edge_histogram_x = edge_hist[previous_frame_x].horizontal;
int32_t *prev_edge_histogram_y = edge_hist[previous_frame_y].vertical; int32_t *prev_edge_histogram_y = edge_hist[previous_frame_y].vertical;
struct edgeflow_displacement_t displacement; struct edgeflow_displacement_t displacement;
uint8_t disp_range = DISP_RANGE_MAX; calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x,
calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x,
displacement.horizontal, img->w, displacement.horizontal, img->w,
opticflow->window_size, disp_range, 0); opticflow->window_size, disp_range, 0);
calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y, calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
displacement.vertical, img->h, displacement.vertical, img->h,
opticflow->window_size, disp_range, 0); opticflow->window_size, disp_range, 0);
@@ -252,10 +308,35 @@ void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t
uint16_t i; uint16_t i;
result->flow_x = (int16_t)edgeflow.horizontal_flow/RES; result->flow_x = (int16_t)edgeflow.horizontal_flow/(previous_frame_offset[0]*RES);
result->flow_y = (int16_t)edgeflow.vertical_flow/RES; result->flow_y = (int16_t)edgeflow.vertical_flow/(previous_frame_offset[1]*RES);
float fps_x = 0;
float fps_y = 0;
if(counter_check>MAX_HORIZON)
{
printf("%d\n",img->ts.tv_sec);
printf("%d %d %d %d \n",current_frame_nr,previous_frame_x, img->ts.tv_usec, edge_hist[previous_frame_x].frame_time.tv_usec);
printf("1: %d\n",timeval_diff2(&edge_hist[previous_frame_x].frame_time, &img->ts));
printf("2: %d\n",timeval_diff2(&edge_hist[previous_frame_y].frame_time, &img->ts));
float time_diff_x = (float)(timeval_diff2(&edge_hist[previous_frame_x].frame_time, &edge_hist[current_frame_nr].frame_time)) / 1000;
float time_diff_y = (float)(timeval_diff2(&edge_hist[previous_frame_y].frame_time, &edge_hist[current_frame_nr].frame_time)) / 1000;
fps_x = 1/(time_diff_x);
fps_y= 1/(time_diff_x);
result->fps = (fps_x + fps_y)/ 2;
}
else
counter_check++;
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;
struct point_t point1; struct point_t point1;
struct point_t point2; struct point_t point2;
struct point_t point1_prev; struct point_t point1_prev;
@@ -282,10 +363,10 @@ void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t
} }
point1_extra.y = (edgeflow.horizontal_flow+edgeflow.horizontal_div * -180 )/ 100+ img->h/2; point1_extra.y = (edgeflow.horizontal_flow+edgeflow.horizontal_div * -180 )/ 100+ img->h/2;
point1_extra.x = 0; point1_extra.x = 0;
point2_extra.y = (edgeflow.horizontal_flow+edgeflow.horizontal_div * 180 )/ 100 + img->h/2; point2_extra.y = (edgeflow.horizontal_flow+edgeflow.horizontal_div * 180 )/ 100 + img->h/2;
point2_extra.x = 360; point2_extra.x = 360;
image_draw_line(img, &point1_extra,&point2_extra); image_draw_line(img, &point1_extra,&point2_extra);
current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON; current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON;
sw/airborne/modules/computer_vision/lib/vision/edge_flow.h
+11 -1
View File
@@ -16,16 +16,26 @@
#include "opticflow/opticflow_calculator.h" #include "opticflow/opticflow_calculator.h"
#include <string.h> #include <string.h>
#include <stdlib.h> #include <stdlib.h>
#include <stdio.h>
#include "mcu_periph/sys_time.h"
#define MAX_HORIZON 10 #define MAX_HORIZON 10
#define IMAGE_HEIGHT 240 #define IMAGE_HEIGHT 240
#define IMAGE_WIDTH 320 #define IMAGE_WIDTH 320
#define DISP_RANGE_MAX 20 #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 { struct edge_hist_t {
int32_t horizontal[IMAGE_WIDTH]; int32_t horizontal[IMAGE_WIDTH];
int32_t vertical[IMAGE_HEIGHT]; int32_t vertical[IMAGE_HEIGHT];
int32_t frame_time; struct timeval frame_time;
int16_t roll; int16_t roll;
int16_t pitch; int16_t pitch;
}; };