Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
Macros | Functions | Variables
opticflow_calculator.c File Reference

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/act_fast.h"
#include "lib/vision/edge_flow.h"
#include "lib/vision/undistortion.h"
#include "size_divergence.h"
#include "linear_flow_fit.h"
#include "modules/sonar/agl_dist.h"
#include <BOARD_CONFIG>
#include "filters/median_filter.h"
+ Include dependency graph for opticflow_calculator.c:

Go to the source code of this file.

Macros

#define OPTICFLOW_SHOW_CORNERS   0
 
#define EXHAUSTIVE_FAST   0
 
#define ACT_FAST   1
 
#define SIZE_DIV   1
 
#define LINEAR_FIT   1
 
#define OPTICFLOW_CORNER_METHOD   ACT_FAST
 
#define OPTICFLOW_CORNER_METHOD_CAMERA2   ACT_FAST
 
#define OPTICFLOW_MAX_TRACK_CORNERS   25
 
#define OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2   25
 
#define OPTICFLOW_WINDOW_SIZE   10
 
#define OPTICFLOW_WINDOW_SIZE_CAMERA2   10
 
#define OPTICFLOW_SEARCH_DISTANCE   20
 
#define OPTICFLOW_SEARCH_DISTANCE_CAMERA2   20
 
#define OPTICFLOW_SUBPIXEL_FACTOR   10
 
#define OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2   10
 
#define OPTICFLOW_RESOLUTION_FACTOR   100
 
#define OPTICFLOW_RESOLUTION_FACTOR_CAMERA2   100
 
#define OPTICFLOW_MAX_ITERATIONS   10
 
#define OPTICFLOW_MAX_ITERATIONS_CAMERA2   10
 
#define OPTICFLOW_THRESHOLD_VEC   2
 
#define OPTICFLOW_THRESHOLD_VEC_CAMERA2   2
 
#define OPTICFLOW_PYRAMID_LEVEL   2
 
#define OPTICFLOW_PYRAMID_LEVEL_CAMERA2   2
 
#define OPTICFLOW_FAST9_ADAPTIVE   TRUE
 
#define OPTICFLOW_FAST9_ADAPTIVE_CAMERA2   TRUE
 
#define OPTICFLOW_FAST9_THRESHOLD   20
 
#define OPTICFLOW_FAST9_THRESHOLD_CAMERA2   20
 
#define OPTICFLOW_FAST9_MIN_DISTANCE   10
 
#define OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2   10
 
#define OPTICFLOW_FAST9_PADDING   20
 
#define OPTICFLOW_FAST9_PADDING_CAMERA2   20
 
#define FAST9_LOW_THRESHOLD   5
 
#define FAST9_HIGH_THRESHOLD   60
 
#define OPTICFLOW_METHOD   0
 
#define OPTICFLOW_METHOD_CAMERA2   0
 
#define OPTICFLOW_DEROTATION   TRUE
 
#define OPTICFLOW_DEROTATION_CAMERA2   TRUE
 
#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X   1.0
 
#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2   1.0
 
#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y   1.0
 
#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2   1.0
 
#define OPTICFLOW_MEDIAN_FILTER   FALSE
 
#define OPTICFLOW_MEDIAN_FILTER_CAMERA2   FALSE
 
#define OPTICFLOW_FEATURE_MANAGEMENT   0
 
#define OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2   0
 
#define OPTICFLOW_FAST9_REGION_DETECT   1
 
#define OPTICFLOW_FAST9_REGION_DETECT_CAMERA2   1
 
#define OPTICFLOW_FAST9_NUM_REGIONS   9
 
#define OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2   9
 
#define OPTICFLOW_ACTFAST_LONG_STEP   10
 
#define OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2   10
 
#define OPTICFLOW_ACTFAST_SHORT_STEP   2
 
#define OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2   2
 
#define OPTICFLOW_ACTFAST_GRADIENT_METHOD   1
 
#define OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2   1
 
#define OPTICFLOW_ACTFAST_MIN_GRADIENT   10
 
#define OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2   10
 
#define OPTICFLOW_BODY_TO_CAM_PHI   0
 
#define OPTICFLOW_BODY_TO_CAM_THETA   0
 
#define OPTICFLOW_BODY_TO_CAM_PSI   -M_PI_2
 
#define OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2   0
 
#define OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2   0
 
#define OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2   -M_PI_2
 
#define OPTICFLOW_TRACK_BACK   FALSE
 
#define OPTICFLOW_TRACK_BACK_CAMERA2   FALSE
 
#define OPTICFLOW_SHOW_FLOW   FALSE
 
#define OPTICFLOW_SHOW_FLOW_CAMERA2   FALSE
 

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...
 
static void manage_flow_features (struct image_t *img, struct opticflow_t *opticflow, struct opticflow_result_t *result)
 
static struct flow_t * predict_flow_vectors (struct flow_t *flow_vectors, uint16_t n_points, float phi_diff, float theta_diff, float psi_diff, struct opticflow_t *opticflow)
 
void opticflow_calc_init (struct opticflow_t opticflow[])
 Initialize the opticflow calculator. More...
 
bool calc_fast9_lukas_kanade (struct opticflow_t *opticflow, struct image_t *img, struct opticflow_result_t *result)
 Run the optical flow with fast9 and lukaskanade on a new image frame. More...
 
bool calc_edgeflow_tot (struct opticflow_t *opticflow, struct image_t *img, struct opticflow_result_t *result)
 Run the optical flow with EDGEFLOW on a new image frame. More...
 
bool opticflow_calc_frame (struct opticflow_t *opticflow, struct image_t *img, struct opticflow_result_t *result)
 Run the optical flow on a new image frame. More...
 

Variables

uint16_t n_time_steps [2] = {10, 10}
 
uint16_t n_agents [2] = {25, 25}
 
struct MedianFilter3Float vel_filt
 
struct FloatRMat body_to_cam [2]
 

Detailed Description

Estimate velocity from optic flow.

Definition in file opticflow_calculator.c.

Macro Definition Documentation

◆ ACT_FAST

#define ACT_FAST   1

Definition at line 56 of file opticflow_calculator.c.

◆ EXHAUSTIVE_FAST

#define EXHAUSTIVE_FAST   0

Definition at line 55 of file opticflow_calculator.c.

◆ FAST9_HIGH_THRESHOLD

#define FAST9_HIGH_THRESHOLD   60

Definition at line 201 of file opticflow_calculator.c.

◆ FAST9_LOW_THRESHOLD

#define FAST9_LOW_THRESHOLD   5

Definition at line 200 of file opticflow_calculator.c.

◆ LINEAR_FIT

#define LINEAR_FIT   1

Definition at line 66 of file opticflow_calculator.c.

◆ OPTICFLOW_ACTFAST_GRADIENT_METHOD

#define OPTICFLOW_ACTFAST_GRADIENT_METHOD   1

Definition at line 313 of file opticflow_calculator.c.

◆ OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2

#define OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2   1

Definition at line 317 of file opticflow_calculator.c.

◆ OPTICFLOW_ACTFAST_LONG_STEP

#define OPTICFLOW_ACTFAST_LONG_STEP   10

Definition at line 293 of file opticflow_calculator.c.

◆ OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2

#define OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2   10

Definition at line 297 of file opticflow_calculator.c.

◆ OPTICFLOW_ACTFAST_MIN_GRADIENT

#define OPTICFLOW_ACTFAST_MIN_GRADIENT   10

Definition at line 323 of file opticflow_calculator.c.

◆ OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2

#define OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2   10

Definition at line 327 of file opticflow_calculator.c.

◆ OPTICFLOW_ACTFAST_SHORT_STEP

#define OPTICFLOW_ACTFAST_SHORT_STEP   2

Definition at line 303 of file opticflow_calculator.c.

◆ OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2

#define OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2   2

Definition at line 307 of file opticflow_calculator.c.

◆ OPTICFLOW_BODY_TO_CAM_PHI

#define OPTICFLOW_BODY_TO_CAM_PHI   0

Definition at line 334 of file opticflow_calculator.c.

◆ OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2

#define OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2   0

Definition at line 344 of file opticflow_calculator.c.

◆ OPTICFLOW_BODY_TO_CAM_PSI

#define OPTICFLOW_BODY_TO_CAM_PSI   -M_PI_2

Definition at line 340 of file opticflow_calculator.c.

◆ OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2

#define OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2   -M_PI_2

Definition at line 350 of file opticflow_calculator.c.

◆ OPTICFLOW_BODY_TO_CAM_THETA

#define OPTICFLOW_BODY_TO_CAM_THETA   0

Definition at line 337 of file opticflow_calculator.c.

◆ OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2

#define OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2   0

Definition at line 347 of file opticflow_calculator.c.

◆ OPTICFLOW_CORNER_METHOD

#define OPTICFLOW_CORNER_METHOD   ACT_FAST

Definition at line 69 of file opticflow_calculator.c.

◆ OPTICFLOW_CORNER_METHOD_CAMERA2

#define OPTICFLOW_CORNER_METHOD_CAMERA2   ACT_FAST

Definition at line 73 of file opticflow_calculator.c.

◆ OPTICFLOW_DEROTATION

#define OPTICFLOW_DEROTATION   TRUE

Definition at line 223 of file opticflow_calculator.c.

◆ OPTICFLOW_DEROTATION_CAMERA2

#define OPTICFLOW_DEROTATION_CAMERA2   TRUE

Definition at line 227 of file opticflow_calculator.c.

◆ OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X

#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X   1.0

Definition at line 233 of file opticflow_calculator.c.

◆ OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2

#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2   1.0

Definition at line 237 of file opticflow_calculator.c.

◆ OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y

#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y   1.0

Definition at line 243 of file opticflow_calculator.c.

◆ OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2

#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2   1.0

Definition at line 247 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_ADAPTIVE

#define OPTICFLOW_FAST9_ADAPTIVE   TRUE

Definition at line 160 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_ADAPTIVE_CAMERA2

#define OPTICFLOW_FAST9_ADAPTIVE_CAMERA2   TRUE

Definition at line 164 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_MIN_DISTANCE

#define OPTICFLOW_FAST9_MIN_DISTANCE   10

Definition at line 180 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2

#define OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2   10

Definition at line 184 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_NUM_REGIONS

#define OPTICFLOW_FAST9_NUM_REGIONS   9

Definition at line 283 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2

#define OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2   9

Definition at line 287 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_PADDING

#define OPTICFLOW_FAST9_PADDING   20

Definition at line 190 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_PADDING_CAMERA2

#define OPTICFLOW_FAST9_PADDING_CAMERA2   20

Definition at line 194 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_REGION_DETECT

#define OPTICFLOW_FAST9_REGION_DETECT   1

Definition at line 273 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_REGION_DETECT_CAMERA2

#define OPTICFLOW_FAST9_REGION_DETECT_CAMERA2   1

Definition at line 277 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_THRESHOLD

#define OPTICFLOW_FAST9_THRESHOLD   20

Definition at line 170 of file opticflow_calculator.c.

◆ OPTICFLOW_FAST9_THRESHOLD_CAMERA2

#define OPTICFLOW_FAST9_THRESHOLD_CAMERA2   20

Definition at line 174 of file opticflow_calculator.c.

◆ OPTICFLOW_FEATURE_MANAGEMENT

#define OPTICFLOW_FEATURE_MANAGEMENT   0

Definition at line 263 of file opticflow_calculator.c.

◆ OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2

#define OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2   0

Definition at line 267 of file opticflow_calculator.c.

◆ OPTICFLOW_MAX_ITERATIONS

#define OPTICFLOW_MAX_ITERATIONS   10

Definition at line 130 of file opticflow_calculator.c.

◆ OPTICFLOW_MAX_ITERATIONS_CAMERA2

#define OPTICFLOW_MAX_ITERATIONS_CAMERA2   10

Definition at line 134 of file opticflow_calculator.c.

◆ OPTICFLOW_MAX_TRACK_CORNERS

#define OPTICFLOW_MAX_TRACK_CORNERS   25

Definition at line 80 of file opticflow_calculator.c.

◆ OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2

#define OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2   25

Definition at line 84 of file opticflow_calculator.c.

◆ OPTICFLOW_MEDIAN_FILTER

#define OPTICFLOW_MEDIAN_FILTER   FALSE

Definition at line 253 of file opticflow_calculator.c.

◆ OPTICFLOW_MEDIAN_FILTER_CAMERA2

#define OPTICFLOW_MEDIAN_FILTER_CAMERA2   FALSE

Definition at line 257 of file opticflow_calculator.c.

◆ OPTICFLOW_METHOD

#define OPTICFLOW_METHOD   0

Definition at line 205 of file opticflow_calculator.c.

◆ OPTICFLOW_METHOD_CAMERA2

#define OPTICFLOW_METHOD_CAMERA2   0

Definition at line 209 of file opticflow_calculator.c.

◆ OPTICFLOW_PYRAMID_LEVEL

#define OPTICFLOW_PYRAMID_LEVEL   2

Definition at line 150 of file opticflow_calculator.c.

◆ OPTICFLOW_PYRAMID_LEVEL_CAMERA2

#define OPTICFLOW_PYRAMID_LEVEL_CAMERA2   2

Definition at line 154 of file opticflow_calculator.c.

◆ OPTICFLOW_RESOLUTION_FACTOR

#define OPTICFLOW_RESOLUTION_FACTOR   100

Definition at line 120 of file opticflow_calculator.c.

◆ OPTICFLOW_RESOLUTION_FACTOR_CAMERA2

#define OPTICFLOW_RESOLUTION_FACTOR_CAMERA2   100

Definition at line 124 of file opticflow_calculator.c.

◆ OPTICFLOW_SEARCH_DISTANCE

#define OPTICFLOW_SEARCH_DISTANCE   20

Definition at line 100 of file opticflow_calculator.c.

◆ OPTICFLOW_SEARCH_DISTANCE_CAMERA2

#define OPTICFLOW_SEARCH_DISTANCE_CAMERA2   20

Definition at line 104 of file opticflow_calculator.c.

◆ OPTICFLOW_SHOW_CORNERS

#define OPTICFLOW_SHOW_CORNERS   0

Definition at line 53 of file opticflow_calculator.c.

◆ OPTICFLOW_SHOW_FLOW

#define OPTICFLOW_SHOW_FLOW   FALSE

Definition at line 368 of file opticflow_calculator.c.

◆ OPTICFLOW_SHOW_FLOW_CAMERA2

#define OPTICFLOW_SHOW_FLOW_CAMERA2   FALSE

Definition at line 372 of file opticflow_calculator.c.

◆ OPTICFLOW_SUBPIXEL_FACTOR

#define OPTICFLOW_SUBPIXEL_FACTOR   10

Definition at line 110 of file opticflow_calculator.c.

◆ OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2

#define OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2   10

Definition at line 114 of file opticflow_calculator.c.

◆ OPTICFLOW_THRESHOLD_VEC

#define OPTICFLOW_THRESHOLD_VEC   2

Definition at line 140 of file opticflow_calculator.c.

◆ OPTICFLOW_THRESHOLD_VEC_CAMERA2

#define OPTICFLOW_THRESHOLD_VEC_CAMERA2   2

Definition at line 144 of file opticflow_calculator.c.

◆ OPTICFLOW_TRACK_BACK

#define OPTICFLOW_TRACK_BACK   FALSE

Definition at line 356 of file opticflow_calculator.c.

◆ OPTICFLOW_TRACK_BACK_CAMERA2

#define OPTICFLOW_TRACK_BACK_CAMERA2   FALSE

Definition at line 360 of file opticflow_calculator.c.

◆ OPTICFLOW_WINDOW_SIZE

#define OPTICFLOW_WINDOW_SIZE   10

Definition at line 90 of file opticflow_calculator.c.

◆ OPTICFLOW_WINDOW_SIZE_CAMERA2

#define OPTICFLOW_WINDOW_SIZE_CAMERA2   10

Definition at line 94 of file opticflow_calculator.c.

◆ SIZE_DIV

#define SIZE_DIV   1

Definition at line 63 of file opticflow_calculator.c.

Function Documentation

◆ calc_edgeflow_tot()

bool calc_edgeflow_tot ( struct opticflow_t *  opticflow,
struct image_t *  img,
struct opticflow_result_t *  result 
)

Run the optical flow with EDGEFLOW on a new image frame.

Parameters
[in]*opticflowThe opticalflow structure that keeps track of previous images
[in]*stateThe state of the drone
[in]*imgThe image frame to calculate the optical flow from
[out]*resultThe optical flow result
computationsuccessful

Definition at line 1006 of file opticflow_calculator.c.

References agl_dist_value_filtered, calc_previous_frame_nr(), calculate_edge_displacement(), calculate_edge_histogram(), opticflow_t::camera, opticflow_result_t::camera_id, video_config_t::camera_intrinsics, 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(), image_t::eulers, edge_hist_t::eulers, FLOAT_EULERS_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, camera_intrinsics_t::focal_x, camera_intrinsics_t::focal_y, opticflow_result_t::fps, edge_hist_t::frame_time, getAmountPeaks(), image_t::h, opticflow_t::id, opticflow_t::just_switched_method, line_fit(), MAX_HORIZON, MAX_WINDOW_SIZE, opticflow_t::median_filter, opticflow_result_t::noise_measurement, opticflow, FloatEulers::phi, RES, opticflow_t::resolution_factor, opticflow_t::search_distance, opticflow_t::show_flow, opticflow_result_t::surface_roughness, FloatEulers::theta, timeval_diff(), opticflow_result_t::tracked_cnt, image_t::ts, UpdateMedianFilterVect3Float, opticflow_result_t::vel_cam, vel_filt, image_t::w, opticflow_t::window_size, FloatVect3::x, edge_hist_t::x, edgeflow_displacement_t::x, FloatVect3::y, edge_hist_t::y, edgeflow_displacement_t::y, and FloatVect3::z.

Referenced by opticflow_calc_frame().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ calc_fast9_lukas_kanade()

bool calc_fast9_lukas_kanade ( struct opticflow_t *  opticflow,
struct image_t *  img,
struct opticflow_result_t *  result 
)

Run the optical flow with fast9 and lukaskanade on a new image frame.

Parameters
[in]*opticflowThe opticalflow structure that keeps track of previous images
[in]*stateThe state of the drone
[in]*imgThe image frame to calculate the optical flow from
[out]*resultThe optical flow result
Returns
Was optical flow successful

Definition at line 492 of file opticflow_calculator.c.

References act_fast(), ACT_FAST, opticflow_t::actfast_gradient_method, opticflow_t::actfast_long_step, opticflow_t::actfast_min_gradient, opticflow_t::actfast_short_step, agl_dist_value_filtered, analyze_linear_flow_field(), bottom_camera, opticflow_t::camera, opticflow_result_t::camera_id, video_config_t::camera_intrinsics, cmp_flow(), opticflow_result_t::corner_cnt, opticflow_t::corner_method, point_t::count, opticflow_t::derotation, opticflow_t::derotation_correction_factor_x, opticflow_t::derotation_correction_factor_y, video_config_t::dev_name, opticflow_result_t::div_size, linear_flow_fit_info::divergence, opticflow_result_t::divergence, flow_t::error, image_t::eulers, EXHAUSTIVE_FAST, f, opticflow_t::fast9_adaptive, fast9_detect(), FAST9_HIGH_THRESHOLD, FAST9_LOW_THRESHOLD, opticflow_t::fast9_min_distance, opticflow_t::fast9_padding, opticflow_t::fast9_ret_corners, opticflow_t::fast9_rsize, opticflow_t::fast9_threshold, opticflow_t::feature_management, 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, camera_intrinsics_t::focal_x, camera_intrinsics_t::focal_y, opticflow_result_t::fps, get_size_divergence(), opticflow_t::got_first_img, image_t::h, opticflow_t::id, image_copy(), image_create(), IMAGE_GRAYSCALE, image_show_flow_color(), image_show_points(), image_switch(), image_to_grayscale(), opticflow_t::img_gray, InitMedianFilterVect3Float, opticflow_t::just_switched_method, LARGE_FLOW_ERROR, LINEAR_FIT, manage_flow_features(), opticflow_t::max_iterations, opticflow_t::max_track_corners, MEDIAN_DEFAULT_SIZE, opticflow_t::median_filter, n_agents, n_samples, n_time_steps, opticflow_result_t::noise_measurement, opticflow, opticFlowLK(), FloatEulers::phi, flow_t::pos, predict_flow_vectors(), opticflow_t::prev_img_gray, FloatEulers::psi, opticflow_t::pyramid_level, opticflow_t::show_flow, SIZE_DIV, opticflow_t::subpixel_factor, linear_flow_fit_info::surface_roughness, opticflow_result_t::surface_roughness, FloatEulers::theta, opticflow_t::threshold_vec, timeval_diff(), opticflow_t::track_back, opticflow_result_t::tracked_cnt, image_t::ts, UpdateMedianFilterVect3Float, VECT3_ASSIGN, opticflow_result_t::vel_body, opticflow_result_t::vel_cam, vel_filt, image_t::w, opticflow_t::window_size, FloatVect3::x, point_t::x, point_t::x_sub, FloatVect3::y, point_t::y, point_t::y_sub, and FloatVect3::z.

Referenced by opticflow_calc_frame().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ cmp_array()

static int cmp_array ( const void *  a,
const void *  b 
)
static

Compare the rows of an integer (uint16_t) 2D array based on the first column.

Used for sorting.

Parameters
[in]*aThe first row (should be *uint16_t)
[in]*bThe second flow vector (should be *uint16_t)
Returns
Negative if a[0] < b[0],0 if a[0] == b[0] and positive if a[0] > b[0]

Definition at line 1237 of file opticflow_calculator.c.

References b.

Referenced by manage_flow_features().

+ Here is the caller graph for this function:

◆ cmp_flow()

static int cmp_flow ( const void *  a,
const void *  b 
)
static

Compare two flow vectors based on flow distance Used for sorting.

Parameters
[in]*aThe first flow vector (should be vect flow_t)
[in]*bThe second flow vector (should be vect flow_t)
Returns
Negative if b has more flow than a, 0 if the same and positive if a has more flow than b

Definition at line 1222 of file opticflow_calculator.c.

References b, flow_t::flow_x, and flow_t::flow_y.

Referenced by calc_fast9_lukas_kanade().

+ Here is the caller graph for this function:

◆ manage_flow_features()

static void manage_flow_features ( struct image_t *  img,
struct opticflow_t *  opticflow,
struct opticflow_result_t *  result 
)
static

Definition at line 892 of file opticflow_calculator.c.

References c1, cmp_array(), opticflow_result_t::corner_cnt, point_t::count, fast9_detect(), 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, image_t::h, opticflow_t::max_track_corners, opticflow, opticflow_t::prev_img_gray, image_t::w, point_t::x, point_t::x_sub, point_t::y, and point_t::y_sub.

Referenced by calc_fast9_lukas_kanade().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ opticflow_calc_frame()

bool opticflow_calc_frame ( struct opticflow_t *  opticflow,
struct image_t *  img,
struct opticflow_result_t *  result 
)

Run the optical flow on a new image frame.

Parameters
[in]*opticflowThe opticalflow structure that keeps track of previous images
[in]*stateThe state of the drone
[in]*imgThe image frame to calculate the optical flow from
[out]*resultThe optical flow result

Definition at line 1169 of file opticflow_calculator.c.

References body_to_cam, calc_edgeflow_tot(), calc_fast9_lukas_kanade(), float_rmat_transp_vmult(), opticflow_t::id, opticflow_t::just_switched_method, opticflow_t::method, opticflow, opticflow_result_t::vel_body, and opticflow_result_t::vel_cam.

Referenced by opticflow_module_calc().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ opticflow_calc_init()

void opticflow_calc_init ( struct opticflow_t  opticflow[])

Initialize the opticflow calculator.

Parameters
[out]*opticflowThe new optical flow calculator

Definition at line 396 of file opticflow_calculator.c.

References opticflow_t::actfast_gradient_method, opticflow_t::actfast_long_step, opticflow_t::actfast_min_gradient, opticflow_t::actfast_short_step, body_to_cam, opticflow_t::camera, opticflow_t::corner_method, opticflow_t::derotation, opticflow_t::derotation_correction_factor_x, opticflow_t::derotation_correction_factor_y, opticflow_t::fast9_adaptive, FAST9_MAX_CORNERS, 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_rmat_of_eulers, opticflow_t::id, opticflow_t::max_iterations, opticflow_t::max_track_corners, opticflow_t::median_filter, opticflow_t::method, opticflow, OPTICFLOW_ACTFAST_GRADIENT_METHOD, OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2, OPTICFLOW_ACTFAST_LONG_STEP, OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2, OPTICFLOW_ACTFAST_MIN_GRADIENT, OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2, OPTICFLOW_ACTFAST_SHORT_STEP, OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2, OPTICFLOW_BODY_TO_CAM_PHI, OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2, OPTICFLOW_BODY_TO_CAM_PSI, OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2, OPTICFLOW_BODY_TO_CAM_THETA, OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2, OPTICFLOW_CORNER_METHOD, OPTICFLOW_CORNER_METHOD_CAMERA2, OPTICFLOW_DEROTATION, OPTICFLOW_DEROTATION_CAMERA2, OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X, OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2, OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y, OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2, OPTICFLOW_FAST9_ADAPTIVE, OPTICFLOW_FAST9_ADAPTIVE_CAMERA2, OPTICFLOW_FAST9_MIN_DISTANCE, OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2, OPTICFLOW_FAST9_NUM_REGIONS, OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2, OPTICFLOW_FAST9_PADDING, OPTICFLOW_FAST9_PADDING_CAMERA2, OPTICFLOW_FAST9_REGION_DETECT, OPTICFLOW_FAST9_REGION_DETECT_CAMERA2, OPTICFLOW_FAST9_THRESHOLD, OPTICFLOW_FAST9_THRESHOLD_CAMERA2, OPTICFLOW_FEATURE_MANAGEMENT, OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2, OPTICFLOW_MAX_ITERATIONS, OPTICFLOW_MAX_ITERATIONS_CAMERA2, OPTICFLOW_MAX_TRACK_CORNERS, OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2, OPTICFLOW_MEDIAN_FILTER, OPTICFLOW_MEDIAN_FILTER_CAMERA2, OPTICFLOW_METHOD, OPTICFLOW_METHOD_CAMERA2, OPTICFLOW_PYRAMID_LEVEL, OPTICFLOW_PYRAMID_LEVEL_CAMERA2, OPTICFLOW_RESOLUTION_FACTOR, OPTICFLOW_RESOLUTION_FACTOR_CAMERA2, OPTICFLOW_SEARCH_DISTANCE, OPTICFLOW_SEARCH_DISTANCE_CAMERA2, OPTICFLOW_SHOW_FLOW, OPTICFLOW_SHOW_FLOW_CAMERA2, OPTICFLOW_SUBPIXEL_FACTOR, OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2, OPTICFLOW_THRESHOLD_VEC, OPTICFLOW_THRESHOLD_VEC_CAMERA2, OPTICFLOW_TRACK_BACK, OPTICFLOW_TRACK_BACK_CAMERA2, OPTICFLOW_WINDOW_SIZE, OPTICFLOW_WINDOW_SIZE_CAMERA2, opticflow_t::pyramid_level, opticflow_t::resolution_factor, opticflow_t::search_distance, opticflow_t::show_flow, opticflow_t::subpixel_factor, opticflow_t::threshold_vec, opticflow_t::track_back, and opticflow_t::window_size.

Referenced by opticflow_module_init().

+ Here is the caller graph for this function:

◆ predict_flow_vectors()

static struct flow_t * predict_flow_vectors ( struct flow_t *  flow_vectors,
uint16_t  n_points,
float  phi_diff,
float  theta_diff,
float  psi_diff,
struct opticflow_t *  opticflow 
)
static

Definition at line 821 of file opticflow_calculator.c.

References A, B, opticflow_t::camera, video_config_t::camera_intrinsics, camera_intrinsics_t::center_x, camera_intrinsics_t::center_y, video_config_t::dev_name, camera_intrinsics_t::Dhane_k, distorted_pixels_to_normalized_coords(), flow_t::error, flow_t::flow_x, flow_t::flow_y, camera_intrinsics_t::focal_x, camera_intrinsics_t::focal_y, front_camera, K, LARGE_FLOW_ERROR, normalized_coords_to_distorted_pixels(), opticflow, flow_t::pos, opticflow_t::subpixel_factor, point_t::x, and point_t::y.

Referenced by calc_fast9_lukas_kanade().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ timeval_diff()

static uint32_t timeval_diff ( struct timeval *  starttime,
struct timeval *  finishtime 
)
static

Calculate the difference from start till finish.

Parameters
[in]*starttimeThe start time to calculate the difference from
[in]*finishtimeThe finish time to calculate the difference from
Returns
The difference in milliseconds

Definition at line 1207 of file opticflow_calculator.c.

Referenced by calc_edgeflow_tot(), and calc_fast9_lukas_kanade().

+ Here is the caller graph for this function:

Variable Documentation

◆ body_to_cam

struct FloatRMat body_to_cam[2]

Definition at line 381 of file opticflow_calculator.c.

Referenced by opticflow_calc_frame(), and opticflow_calc_init().

◆ n_agents

uint16_t n_agents[2] = {25, 25}

Definition at line 59 of file opticflow_calculator.c.

Referenced by act_fast(), and calc_fast9_lukas_kanade().

◆ n_time_steps

uint16_t n_time_steps[2] = {10, 10}

Definition at line 58 of file opticflow_calculator.c.

Referenced by act_fast(), and calc_fast9_lukas_kanade().

◆ vel_filt

struct MedianFilter3Float vel_filt

Definition at line 380 of file opticflow_calculator.c.

Referenced by calc_edgeflow_tot(), and calc_fast9_lukas_kanade().