Paparazzi UAS
v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
|
Estimate velocity from optic flow. More...
#include "std.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "opticflow_calculator.h"
#include "lib/vision/image.h"
#include "lib/vision/lucas_kanade.h"
#include "lib/vision/fast_rosten.h"
#include "lib/vision/edge_flow.h"
#include "size_divergence.h"
#include "linear_flow_fit.h"
#include "lib/filters/kalman_filter_vision.h"
#include "subsystems/imu.h"
#include "filters/median_filter.h"
Go to the source code of this file.
Functions | |
static uint32_t | timeval_diff (struct timeval *starttime, struct timeval *finishtime) |
Calculate the difference from start till finish. More... | |
static int | cmp_flow (const void *a, const void *b) |
Compare two flow vectors based on flow distance Used for sorting. More... | |
static int | cmp_array (const void *a, const void *b) |
Compare the rows of an integer (uint16_t) 2D array based on the first column. More... | |
void | opticflow_calc_init (struct opticflow_t *opticflow) |
Initialize the opticflow calculator. More... | |
void | calc_fast9_lukas_kanade (struct opticflow_t *opticflow, struct opticflow_state_t *cam_state, struct image_t *img, struct opticflow_result_t *result) |
Run the optical flow with fast9 and lukaskanade on a new image frame. More... | |
void | calc_edgeflow_tot (struct opticflow_t *opticflow, struct opticflow_state_t *cam_state, struct image_t *img, struct opticflow_result_t *result) |
Run the optical flow with EDGEFLOW on a new image frame. More... | |
void | opticflow_calc_frame (struct opticflow_t *opticflow, struct opticflow_state_t *cam_state, struct image_t *img, struct opticflow_result_t *result) |
Run the optical flow on a new image frame. More... | |
void | kalman_filter_opticflow_velocity (float *velocity_x, float *velocity_y, float *acceleration_measurement, float fps, float *measurement_noise, float kalman_process_noise, bool reinitialize_kalman) |
Filter the velocity with a simple linear kalman filter, together with the accelerometers. More... | |
Variables | |
struct MedianFilterInt vel_x_filt | vel_y_filt |
Estimate velocity from optic flow.
Using images from a vertical camera and IMU sensor data.
Definition in file opticflow_calculator.c.
#define FAST9_HIGH_THRESHOLD 60 |
Definition at line 147 of file opticflow_calculator.c.
Referenced by calc_fast9_lukas_kanade().
#define FAST9_LOW_THRESHOLD 5 |
Definition at line 146 of file opticflow_calculator.c.
Referenced by calc_fast9_lukas_kanade().
#define LINEAR_FIT 1 |
Definition at line 61 of file opticflow_calculator.c.
Referenced by calc_fast9_lukas_kanade().
#define OPTICFLOW_DEROTATION TRUE |
Definition at line 159 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X 1.0 |
Definition at line 164 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y 1.0 |
Definition at line 169 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_FAST9_ADAPTIVE TRUE |
Definition at line 126 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_FAST9_MIN_DISTANCE 10 |
Definition at line 136 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_FAST9_NUM_REGIONS 9 |
Definition at line 199 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_FAST9_PADDING 20 |
Definition at line 141 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_FAST9_REGION_DETECT 1 |
Definition at line 194 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_FAST9_THRESHOLD 20 |
Definition at line 131 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_FEATURE_MANAGEMENT 1 |
Definition at line 189 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_FOV_H 0.67020643276 |
Definition at line 70 of file opticflow_calculator.c.
Referenced by calc_edgeflow_tot(), and calc_fast9_lukas_kanade().
#define OPTICFLOW_FOV_W 0.89360857702 |
Definition at line 65 of file opticflow_calculator.c.
Referenced by calc_edgeflow_tot(), and calc_fast9_lukas_kanade().
#define OPTICFLOW_FX 343.1211 |
Definition at line 75 of file opticflow_calculator.c.
Referenced by calc_fast9_lukas_kanade().
#define OPTICFLOW_FY 348.5053 |
Definition at line 80 of file opticflow_calculator.c.
Referenced by calc_fast9_lukas_kanade().
#define OPTICFLOW_KALMAN_FILTER TRUE |
Definition at line 179 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE 0.01 |
Definition at line 184 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_MAX_ITERATIONS 10 |
Definition at line 111 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_MAX_TRACK_CORNERS 25 |
Definition at line 86 of file opticflow_calculator.c.
Referenced by calc_fast9_lukas_kanade(), and opticflow_calc_init().
#define OPTICFLOW_MEDIAN_FILTER FALSE |
Definition at line 174 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_METHOD 0 |
Definition at line 150 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_PYRAMID_LEVEL 2 |
Definition at line 121 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_RESOLUTION_FACTOR 100 |
Definition at line 106 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_SEARCH_DISTANCE 20 |
Definition at line 96 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_SHOW_CORNERS 0 |
Definition at line 54 of file opticflow_calculator.c.
#define OPTICFLOW_SHOW_FLOW 0 |
Definition at line 53 of file opticflow_calculator.c.
#define OPTICFLOW_SUBPIXEL_FACTOR 10 |
Definition at line 101 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_THRESHOLD_VEC 2 |
Definition at line 116 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define OPTICFLOW_WINDOW_SIZE 10 |
Definition at line 91 of file opticflow_calculator.c.
Referenced by opticflow_calc_init().
#define SIZE_DIV 1 |
Definition at line 58 of file opticflow_calculator.c.
Referenced by calc_fast9_lukas_kanade().
void calc_edgeflow_tot | ( | struct opticflow_t * | opticflow, |
struct opticflow_state_t * | cam_state, | ||
struct image_t * | img, | ||
struct opticflow_result_t * | result | ||
) |
Run the optical flow with EDGEFLOW on a new image frame.
[in] | *opticflow | The opticalflow structure that keeps track of previous images |
[in] | *state | The state of the drone |
[in] | *img | The image frame to calculate the optical flow from |
[out] | *result | The optical flow result |
Definition at line 519 of file opticflow_calculator.c.
References opticflow_state_t::agl, calc_previous_frame_nr(), calculate_edge_displacement(), calculate_edge_histogram(), opticflow_result_t::corner_cnt, opticflow_t::derotation, opticflow_t::derotation_correction_factor_x, opticflow_t::derotation_correction_factor_y, DISP_RANGE_MAX, opticflow_result_t::div_size, edge_flow_t::div_x, edge_flow_t::div_y, opticflow_result_t::divergence, draw_edgeflow_img(), FLOAT_RATES_ZERO, opticflow_result_t::flow_der_x, opticflow_result_t::flow_der_y, opticflow_result_t::flow_x, edge_flow_t::flow_x, opticflow_result_t::flow_y, edge_flow_t::flow_y, opticflow_result_t::fps, edge_hist_t::frame_time, getAmountPeaks(), image_t::h, opticflow_t::just_switched_method, line_fit(), MAX_HORIZON, MAX_WINDOW_SIZE, opticflow_t::median_filter, opticflow_result_t::noise_measurement, OPTICFLOW_FOV_H, OPTICFLOW_FOV_W, opticflow_state_t::rates, edge_hist_t::rates, RES, opticflow_t::resolution_factor, opticflow_t::search_distance, opticflow_result_t::surface_roughness, timeval_diff(), opticflow_result_t::tracked_cnt, image_t::ts, update_median_filter_i(), opticflow_result_t::vel_x, opticflow_result_t::vel_y, vel_y_filt, image_t::w, opticflow_t::window_size, edge_hist_t::x, edgeflow_displacement_t::x, edge_hist_t::y, and edgeflow_displacement_t::y.
Referenced by opticflow_calc_frame().
void calc_fast9_lukas_kanade | ( | struct opticflow_t * | opticflow, |
struct opticflow_state_t * | cam_state, | ||
struct image_t * | img, | ||
struct opticflow_result_t * | result | ||
) |
Run the optical flow with fast9 and lukaskanade on a new image frame.
[in] | *opticflow | The opticalflow structure that keeps track of previous images |
[in] | *state | The state of the drone |
[in] | *img | The image frame to calculate the optical flow from |
[out] | *result | The optical flow result |
Definition at line 256 of file opticflow_calculator.c.
References opticflow_state_t::agl, analyze_linear_flow_field(), cmp_array(), cmp_flow(), opticflow_result_t::corner_cnt, point_t::count, opticflow_t::derotation, opticflow_t::derotation_correction_factor_x, opticflow_t::derotation_correction_factor_y, opticflow_result_t::div_size, linear_flow_fit_info::divergence, opticflow_result_t::divergence, opticflow_t::fast9_adaptive, fast9_detect(), FAST9_HIGH_THRESHOLD, FAST9_LOW_THRESHOLD, opticflow_t::fast9_min_distance, opticflow_t::fast9_num_regions, opticflow_t::fast9_padding, opticflow_t::fast9_region_detect, opticflow_t::fast9_ret_corners, opticflow_t::fast9_rsize, opticflow_t::fast9_threshold, opticflow_t::feature_management, FLOAT_RATES_ZERO, opticflow_result_t::flow_der_x, opticflow_result_t::flow_der_y, opticflow_result_t::flow_x, flow_t::flow_x, opticflow_result_t::flow_y, flow_t::flow_y, opticflow_result_t::fps, get_size_divergence(), opticflow_t::got_first_img, image_t::h, image_copy(), image_create(), IMAGE_GRAYSCALE, image_show_flow(), image_show_points(), image_switch(), image_to_grayscale(), opticflow_t::img_gray, init_median_filter_i(), opticflow_t::just_switched_method, LINEAR_FIT, opticflow_t::max_iterations, opticflow_t::max_track_corners, MEDIAN_DEFAULT_SIZE, opticflow_t::median_filter, opticflow_result_t::noise_measurement, OPTICFLOW_FOV_H, OPTICFLOW_FOV_W, OPTICFLOW_FX, OPTICFLOW_FY, OPTICFLOW_MAX_TRACK_CORNERS, opticFlowLK(), FloatRates::p, flow_t::pos, opticflow_t::prev_img_gray, opticflow_t::prev_rates, opticflow_t::prev_timestamp, opticflow_t::pyramid_level, FloatRates::q, opticflow_state_t::rates, SIZE_DIV, opticflow_t::subpixel_factor, linear_flow_fit_info::surface_roughness, opticflow_result_t::surface_roughness, opticflow_t::threshold_vec, timeval_diff(), opticflow_result_t::tracked_cnt, image_t::ts, update_median_filter_i(), opticflow_result_t::vel_x, opticflow_result_t::vel_y, vel_y_filt, image_t::w, opticflow_t::window_size, point_t::x, point_t::x_sub, point_t::y, and point_t::y_sub.
Referenced by opticflow_calc_frame().
|
static |
Compare the rows of an integer (uint16_t) 2D array based on the first column.
Used for sorting.
[in] | *a | The first row (should be *uint16_t) |
[in] | *b | The second flow vector (should be *uint16_t) |
Definition at line 856 of file opticflow_calculator.c.
Referenced by calc_fast9_lukas_kanade().
|
static |
Compare two flow vectors based on flow distance Used for sorting.
[in] | *a | The first flow vector (should be vect flow_t) |
[in] | *b | The second flow vector (should be vect flow_t) |
Definition at line 841 of file opticflow_calculator.c.
References flow_t::flow_x, and flow_t::flow_y.
Referenced by calc_fast9_lukas_kanade().
void kalman_filter_opticflow_velocity | ( | float * | velocity_x, |
float * | velocity_y, | ||
float * | acceleration_measurement, | ||
float | fps, | ||
float * | measurement_noise, | ||
float | kalman_process_noise, | ||
bool | reinitialize_kalman | ||
) |
Filter the velocity with a simple linear kalman filter, together with the accelerometers.
[in] | *velocity_x | Velocity in x direction of body fixed coordinates |
[in] | *velocity_y | Belocity in y direction of body fixed coordinates |
[in] | *acceleration_measurement | Measurements of the accelerometers |
[in] | fps | Frames per second |
[in] | *measurement_noise | Expected variance of the noise of the measurements |
[in] | *measurement_noise | Expected variance of the noise of the model prediction |
[in] | reinitialize_kalman | Boolean to reinitialize the kalman filter |
Definition at line 772 of file opticflow_calculator.c.
References kalman_filter_linear_2D_float(), and model.
Referenced by opticflow_calc_frame().
void opticflow_calc_frame | ( | struct opticflow_t * | opticflow, |
struct opticflow_state_t * | cam_state, | ||
struct image_t * | img, | ||
struct opticflow_result_t * | result | ||
) |
Run the optical flow on a new image frame.
[in] | *opticflow | The opticalflow structure that keeps track of previous images |
[in] | *state | The state of the drone |
[in] | *img | The image frame to calculate the optical flow from |
[out] | *result | The optical flow result |
Definition at line 689 of file opticflow_calculator.c.
References opticflow_state_t::accel_imu_meas, ACCELS_FLOAT_OF_BFP, calc_edgeflow_tot(), calc_fast9_lukas_kanade(), float_quat_vmult(), opticflow_result_t::fps, opticflow_state_t::imu_to_body_quat, opticflow_t::just_switched_method, opticflow_t::kalman_filter, kalman_filter_opticflow_velocity(), opticflow_t::kalman_filter_process_noise, opticflow_t::method, opticflow_result_t::noise_measurement, opticflow_result_t::vel_body_x, opticflow_result_t::vel_body_y, opticflow_result_t::vel_x, opticflow_result_t::vel_y, FloatVect3::x, and FloatVect3::y.
Referenced by opticflow_module_calc().
void opticflow_calc_init | ( | struct opticflow_t * | opticflow | ) |
Initialize the opticflow calculator.
[out] | *opticflow | The new optical flow calculator |
Definition at line 219 of file opticflow_calculator.c.
References opticflow_t::derotation, opticflow_t::derotation_correction_factor_x, opticflow_t::derotation_correction_factor_y, opticflow_t::fast9_adaptive, opticflow_t::fast9_min_distance, opticflow_t::fast9_num_regions, opticflow_t::fast9_padding, opticflow_t::fast9_region_detect, opticflow_t::fast9_ret_corners, opticflow_t::fast9_rsize, opticflow_t::fast9_threshold, opticflow_t::feature_management, opticflow_t::kalman_filter, opticflow_t::kalman_filter_process_noise, opticflow_t::max_iterations, opticflow_t::max_track_corners, opticflow_t::median_filter, opticflow_t::method, OPTICFLOW_DEROTATION, OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X, OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y, OPTICFLOW_FAST9_ADAPTIVE, OPTICFLOW_FAST9_MIN_DISTANCE, OPTICFLOW_FAST9_NUM_REGIONS, OPTICFLOW_FAST9_PADDING, OPTICFLOW_FAST9_REGION_DETECT, OPTICFLOW_FAST9_THRESHOLD, OPTICFLOW_FEATURE_MANAGEMENT, OPTICFLOW_KALMAN_FILTER, OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE, OPTICFLOW_MAX_ITERATIONS, OPTICFLOW_MAX_TRACK_CORNERS, OPTICFLOW_MEDIAN_FILTER, OPTICFLOW_METHOD, OPTICFLOW_PYRAMID_LEVEL, OPTICFLOW_RESOLUTION_FACTOR, OPTICFLOW_SEARCH_DISTANCE, OPTICFLOW_SUBPIXEL_FACTOR, OPTICFLOW_THRESHOLD_VEC, OPTICFLOW_WINDOW_SIZE, opticflow_t::pyramid_level, opticflow_t::resolution_factor, opticflow_t::search_distance, opticflow_t::subpixel_factor, opticflow_t::threshold_vec, and opticflow_t::window_size.
Referenced by opticflow_module_init().
|
static |
Calculate the difference from start till finish.
[in] | *starttime | The start time to calculate the difference from |
[in] | *finishtime | The finish time to calculate the difference from |
Definition at line 826 of file opticflow_calculator.c.
Referenced by calc_edgeflow_tot(), and calc_fast9_lukas_kanade().
struct MedianFilterInt vel_x_filt vel_y_filt |
Definition at line 205 of file opticflow_calculator.c.
Referenced by calc_edgeflow_tot(), and calc_fast9_lukas_kanade().