48#include "generated/airframe.h"
54#define OPTICFLOW_SHOW_CORNERS 0
56#define EXHAUSTIVE_FAST 0
69#ifndef OPTICFLOW_CORNER_METHOD
70#define OPTICFLOW_CORNER_METHOD ACT_FAST
73#ifndef OPTICFLOW_CORNER_METHOD_CAMERA2
74#define OPTICFLOW_CORNER_METHOD_CAMERA2 ACT_FAST
80#ifndef OPTICFLOW_MAX_TRACK_CORNERS
81#define OPTICFLOW_MAX_TRACK_CORNERS 25
84#ifndef OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2
85#define OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2 25
90#ifndef OPTICFLOW_WINDOW_SIZE
91#define OPTICFLOW_WINDOW_SIZE 10
94#ifndef OPTICFLOW_WINDOW_SIZE_CAMERA2
95#define OPTICFLOW_WINDOW_SIZE_CAMERA2 10
100#ifndef OPTICFLOW_SEARCH_DISTANCE
101#define OPTICFLOW_SEARCH_DISTANCE 20
104#ifndef OPTICFLOW_SEARCH_DISTANCE_CAMERA2
105#define OPTICFLOW_SEARCH_DISTANCE_CAMERA2 20
110#ifndef OPTICFLOW_SUBPIXEL_FACTOR
111#define OPTICFLOW_SUBPIXEL_FACTOR 10
114#ifndef OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2
115#define OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2 10
120#ifndef OPTICFLOW_RESOLUTION_FACTOR
121#define OPTICFLOW_RESOLUTION_FACTOR 100
124#ifndef OPTICFLOW_RESOLUTION_FACTOR_CAMERA2
125#define OPTICFLOW_RESOLUTION_FACTOR_CAMERA2 100
130#ifndef OPTICFLOW_MAX_ITERATIONS
131#define OPTICFLOW_MAX_ITERATIONS 10
134#ifndef OPTICFLOW_MAX_ITERATIONS_CAMERA2
135#define OPTICFLOW_MAX_ITERATIONS_CAMERA2 10
140#ifndef OPTICFLOW_THRESHOLD_VEC
141#define OPTICFLOW_THRESHOLD_VEC 2
144#ifndef OPTICFLOW_THRESHOLD_VEC_CAMERA2
145#define OPTICFLOW_THRESHOLD_VEC_CAMERA2 2
150#ifndef OPTICFLOW_PYRAMID_LEVEL
151#define OPTICFLOW_PYRAMID_LEVEL 2
154#ifndef OPTICFLOW_PYRAMID_LEVEL_CAMERA2
155#define OPTICFLOW_PYRAMID_LEVEL_CAMERA2 2
160#ifndef OPTICFLOW_FAST9_ADAPTIVE
161#define OPTICFLOW_FAST9_ADAPTIVE TRUE
164#ifndef OPTICFLOW_FAST9_ADAPTIVE_CAMERA2
165#define OPTICFLOW_FAST9_ADAPTIVE_CAMERA2 TRUE
170#ifndef OPTICFLOW_FAST9_THRESHOLD
171#define OPTICFLOW_FAST9_THRESHOLD 20
174#ifndef OPTICFLOW_FAST9_THRESHOLD_CAMERA2
175#define OPTICFLOW_FAST9_THRESHOLD_CAMERA2 20
180#ifndef OPTICFLOW_FAST9_MIN_DISTANCE
181#define OPTICFLOW_FAST9_MIN_DISTANCE 10
184#ifndef OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2
185#define OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2 10
190#ifndef OPTICFLOW_FAST9_PADDING
191#define OPTICFLOW_FAST9_PADDING 20
194#ifndef OPTICFLOW_FAST9_PADDING_CAMERA2
195#define OPTICFLOW_FAST9_PADDING_CAMERA2 20
201#define FAST9_LOW_THRESHOLD 5
202#define FAST9_HIGH_THRESHOLD 60
205#ifndef OPTICFLOW_METHOD
206#define OPTICFLOW_METHOD 0
209#ifndef OPTICFLOW_METHOD_CAMERA2
210#define OPTICFLOW_METHOD_CAMERA2 0
215#if OPTICFLOW_METHOD > 1
216#error WARNING: Both Lukas Kanade and EdgeFlow are NOT selected camera1
219#if OPTICFLOW_METHOD_CAMERA2 > 1
220#error WARNING: Both Lukas Kanade and EdgeFlow are NOT selected camera2
223#ifndef OPTICFLOW_DEROTATION
224#define OPTICFLOW_DEROTATION TRUE
227#ifndef OPTICFLOW_DEROTATION_CAMERA2
228#define OPTICFLOW_DEROTATION_CAMERA2 TRUE
233#ifndef OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X
234#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X 1.0
237#ifndef OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2
238#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2 1.0
243#ifndef OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y
244#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y 1.0
247#ifndef OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2
248#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2 1.0
253#ifndef OPTICFLOW_MEDIAN_FILTER
254#define OPTICFLOW_MEDIAN_FILTER FALSE
257#ifndef OPTICFLOW_MEDIAN_FILTER_CAMERA2
258#define OPTICFLOW_MEDIAN_FILTER_CAMERA2 FALSE
263#ifndef OPTICFLOW_FEATURE_MANAGEMENT
264#define OPTICFLOW_FEATURE_MANAGEMENT 0
267#ifndef OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2
268#define OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2 0
273#ifndef OPTICFLOW_FAST9_REGION_DETECT
274#define OPTICFLOW_FAST9_REGION_DETECT 1
277#ifndef OPTICFLOW_FAST9_REGION_DETECT_CAMERA2
278#define OPTICFLOW_FAST9_REGION_DETECT_CAMERA2 1
283#ifndef OPTICFLOW_FAST9_NUM_REGIONS
284#define OPTICFLOW_FAST9_NUM_REGIONS 9
287#ifndef OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2
288#define OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2 9
293#ifndef OPTICFLOW_ACTFAST_LONG_STEP
294#define OPTICFLOW_ACTFAST_LONG_STEP 10
297#ifndef OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2
298#define OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2 10
303#ifndef OPTICFLOW_ACTFAST_SHORT_STEP
304#define OPTICFLOW_ACTFAST_SHORT_STEP 2
307#ifndef OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2
308#define OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2 2
313#ifndef OPTICFLOW_ACTFAST_GRADIENT_METHOD
314#define OPTICFLOW_ACTFAST_GRADIENT_METHOD 1
317#ifndef OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2
318#define OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2 1
323#ifndef OPTICFLOW_ACTFAST_MIN_GRADIENT
324#define OPTICFLOW_ACTFAST_MIN_GRADIENT 10
327#ifndef OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2
328#define OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2 10
334#ifndef OPTICFLOW_BODY_TO_CAM_PHI
335#define OPTICFLOW_BODY_TO_CAM_PHI 0
337#ifndef OPTICFLOW_BODY_TO_CAM_THETA
338#define OPTICFLOW_BODY_TO_CAM_THETA 0
340#ifndef OPTICFLOW_BODY_TO_CAM_PSI
341#define OPTICFLOW_BODY_TO_CAM_PSI -M_PI_2
344#ifndef OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2
345#define OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2 0
347#ifndef OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2
348#define OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2 0
350#ifndef OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2
351#define OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2 -M_PI_2
356#ifndef OPTICFLOW_TRACK_BACK
357#define OPTICFLOW_TRACK_BACK FALSE
360#ifndef OPTICFLOW_TRACK_BACK_CAMERA2
361#define OPTICFLOW_TRACK_BACK_CAMERA2 FALSE
368#ifndef OPTICFLOW_SHOW_FLOW
369#define OPTICFLOW_SHOW_FLOW FALSE
372#ifndef OPTICFLOW_SHOW_FLOW_CAMERA2
373#define OPTICFLOW_SHOW_FLOW_CAMERA2 FALSE
380#include "filters/median_filter.h"
386static int cmp_flow(
const void *a,
const void *
b);
387static int cmp_array(
const void *a,
const void *
b);
441#ifdef OPTICFLOW_CAMERA2
477 opticflow[1].camera = &OPTICFLOW_CAMERA2;
519 float error_threshold;
520 int n_iterations_RANSAC, n_samples_RANSAC, success_fit;
526 result->
fps = 1000.f / dt;
538 }
else if (!
opticflow->feature_management) {
586#if OPTICFLOW_SHOW_CORNERS
636 int32_t back_x, back_y, diff_x, diff_y, dist_squared;
637 int32_t back_track_threshold = 200;
643 diff_x = back_x - vectors[i].
pos.
x;
644 diff_y = back_y - vectors[i].
pos.
y;
645 dist_squared = diff_x * diff_x + diff_y * diff_y;
647 if (dist_squared > back_track_threshold) {
659 uint8_t color[4] = {0, 0, 0, 0};
660 uint8_t bad_color[4] = {0, 0, 0, 0};
674 error_threshold = 10.0f;
675 n_iterations_RANSAC = 20;
676 n_samples_RANSAC = 5;
678 n_samples_RANSAC, img->
w, img->
h, &fit_info);
689 error_threshold = 10.0f;
690 n_iterations_RANSAC = 20;
691 n_samples_RANSAC = 5;
693 n_samples_RANSAC, img->
w, img->
h, OPTICFLOW_CAMERA.camera_intrinsics.focal_x, &fit_info);
726 float diff_flow_x = 0.f;
727 float diff_flow_y = 0.f;
731 float rotation_threshold = M_PI / 180.0f;
732 if (fabs(
opticflow->img_gray.eulers.phi -
opticflow->prev_img_gray.eulers.phi) > rotation_threshold
733 || fabs(
opticflow->img_gray.eulers.theta -
opticflow->prev_img_gray.eulers.theta) > rotation_threshold) {
742 float phi_diff =
opticflow->img_gray.eulers.phi -
opticflow->prev_img_gray.eulers.phi;
743 float theta_diff =
opticflow->img_gray.eulers.theta -
opticflow->prev_img_gray.eulers.theta;
744 float psi_diff =
opticflow->img_gray.eulers.psi -
opticflow->prev_img_gray.eulers.psi;
748 diff_flow_x = phi_diff *
749 opticflow->camera->camera_intrinsics.focal_x;
750 diff_flow_y = theta_diff *
opticflow->camera->camera_intrinsics.focal_y;
752 opticflow->derotation_correction_factor_x;
754 opticflow->derotation_correction_factor_y;
760 uint8_t color[4] = {255, 255, 255, 255};
761 uint8_t bad_color[4] = {255, 255, 255, 255};
837 struct flow_t *predicted_flow_vectors = malloc(
sizeof(
struct flow_t) * n_points);
839 float K[9] = {
opticflow->camera->camera_intrinsics.focal_x, 0.0f,
opticflow->camera->camera_intrinsics.center_x,
840 0.0f,
opticflow->camera->camera_intrinsics.focal_y,
opticflow->camera->camera_intrinsics.center_y,
844 float k =
opticflow->camera->camera_intrinsics.Dhane_k;
860 float x_n_new, y_n_new, x_pix_new, y_pix_new;
861 float predicted_flow_x, predicted_flow_y;
862 for (
uint16_t i = 0; i < n_points; i++) {
864 predicted_flow_vectors[i].
pos.
x = flow_vectors[i].
pos.
x;
865 predicted_flow_vectors[i].
pos.
y = flow_vectors[i].
pos.
y;
868 (
float)flow_vectors[i].
pos.
y /
opticflow->subpixel_factor, &x_n, &y_n, k,
K);
871 predicted_flow_x =
A * x_n * y_n -
B * x_n * x_n -
B + C * y_n;
872 predicted_flow_y = -C * x_n +
A +
A * y_n * y_n -
B * x_n * y_n;
874 x_n_new = x_n + predicted_flow_x;
875 y_n_new = y_n + predicted_flow_y;
882 predicted_flow_vectors[i].
error = 0;
884 predicted_flow_vectors[i].
flow_x = 0;
885 predicted_flow_vectors[i].
flow_y = 0;
889 predicted_flow_vectors[i].
flow_x = 0;
890 predicted_flow_vectors[i].
flow_y = 0;
894 return predicted_flow_vectors;
929 if (!exists) {
c1++; }
943 region_count[i] = calloc(2,
sizeof(
uint16_t));
944 region_count[i][0] = 0;
945 region_count[i][1] = i;
950 region_index = (
opticflow->fast9_ret_corners[i].x * root_regions / img->
w
951 + root_regions * (
opticflow->fast9_ret_corners[i].y * root_regions / img->
h));
952 region_index = (region_index <
opticflow->fast9_num_regions) ? region_index :
opticflow->fast9_num_regions - 1;
953 region_count[region_index][0]++;
957 qsort(region_count,
opticflow->fast9_num_regions,
sizeof(region_count[0]),
cmp_array);
963 roi[0] = (region_count[i][1] % root_regions) * (img->
w / root_regions);
964 roi[1] = (region_count[i][1] / root_regions) * (img->
h / root_regions);
965 roi[2] = roi[0] + (img->
w / root_regions);
966 roi[3] = roi[1] + (img->
h / root_regions);
973 &
opticflow->fast9_rsize, &new_corners, roi);
976 for (
uint16_t j = 0; j < new_count; j++) {
1003 free(region_count[i]);
1022 static uint8_t current_frame_nr = 0;
1024 static uint8_t previous_frame_offset[2] = {1, 1};
1028 displacement.
x = calloc(img->
w,
sizeof(
int32_t));
1029 displacement.
y = calloc(img->
h,
sizeof(
int32_t));
1032 if (
opticflow->just_switched_method == 1 && edge_hist[0].
x == NULL) {
1035 edge_hist[i].
x = calloc(img->
w,
sizeof(
int32_t));
1036 edge_hist[i].
y = calloc(img->
h,
sizeof(
int32_t));
1042 disp_range =
opticflow->search_distance;
1060 int32_t *edge_hist_x = edge_hist[current_frame_nr].
x;
1061 int32_t *edge_hist_y = edge_hist[current_frame_nr].
y;
1075 int32_t *prev_edge_histogram_x = edge_hist[previous_frame_nr[0]].
x;
1076 int32_t *prev_edge_histogram_y = edge_hist[previous_frame_nr[1]].
y;
1082 der_shift_x = (
int16_t)((edge_hist[current_frame_nr].eulers.phi - edge_hist[previous_frame_nr[0]].
eulers.
phi) *
1083 opticflow->camera->camera_intrinsics.focal_x *
opticflow->derotation_correction_factor_x);
1084 der_shift_y = (
int16_t)((edge_hist[current_frame_nr].eulers.theta - edge_hist[previous_frame_nr[1]].
eulers.
theta) *
1085 opticflow->camera->camera_intrinsics.focal_y *
opticflow->derotation_correction_factor_y);
1090 displacement.
x, img->
w,
1091 window_size, disp_range, der_shift_x);
1093 displacement.
y, img->
h,
1094 window_size, disp_range, der_shift_y);
1100 window_size + disp_range,
RES);
1103 window_size + disp_range,
RES);
1140 float time_diff_x = (float)(
timeval_diff(&edge_hist[previous_frame_nr[0]].frame_time, &img->
ts)) / 1000.;
1141 float time_diff_y = (float)(
timeval_diff(&edge_hist[previous_frame_nr[1]].frame_time, &img->
ts)) / 1000.;
1142 fps_x = 1 / (time_diff_x);
1143 fps_y = 1 / (time_diff_y);
1145 result->
fps = fps_x;
1166 current_frame_nr = (current_frame_nr + 1) %
MAX_HORIZON;
1168 free(displacement.
x);
1169 free(displacement.
y);
1184 bool flow_successful =
false;
1187 static int8_t switch_counter[2] = {-1, -1};
1194 opticflow->just_switched_method =
false;
1210 return flow_successful;
1222 msec = (finishtime->tv_sec - starttime->tv_sec) * 1000;
1223 msec += (finishtime->tv_usec - starttime->tv_usec) / 1000;
1253 return pa[0] - pb[0];
void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners, uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient, int gradient_method, int camera_id)
Do an ACT-FAST corner detection.
Finds corners in an image by actively scanning the image.
float agl_dist_value_filtered
Bind to agl ABI message and provide a filtered value to be used in flight plans.
uint32_t getAmountPeaks(int32_t *edgehist, int32_t thres, int32_t size)
getAmountPeaks, calculates the amount of peaks in a edge histogram
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)
Calculate_displacement calculates the displacement between two histograms.
void draw_edgeflow_img(struct image_t *img, struct edge_flow_t edgeflow, int32_t *edge_hist_x_prev, int32_t *edge_hist_x)
Draws edgehistogram, displacement and linefit directly on the image for debugging (only for edgeflow ...
void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[], char direction, uint16_t edge_threshold)
Calculate a edge/gradient histogram for each dimension of the image.
void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border, uint16_t RES)
Fits a linear model to an array with pixel displacements with least squares.
void calc_previous_frame_nr(struct opticflow_result_t *result, struct opticflow_t *opticflow, uint8_t current_frame_nr, uint8_t *previous_frame_offset, uint8_t *previous_frame_nr)
Calc_previous_frame_nr; adaptive Time Horizon.
calculate optical flow with EdgeFlow
struct FloatEulers eulers
struct timeval frame_time
void fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uint16_t x_padding, uint16_t y_padding, uint16_t *num_corners, uint16_t *ret_corners_length, struct point_t **ret_corners, uint16_t *roi)
Do a FAST9 corner detection.
#define FLOAT_EULERS_ZERO(_e)
#define float_rmat_of_eulers
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
#define VECT3_ASSIGN(_a, _x, _y, _z)
void image_copy(struct image_t *input, struct image_t *output)
Copy an image from inut to output This will only work if the formats are the same.
void image_switch(struct image_t *a, struct image_t *b)
This will switch image *a and *b This is faster as image_copy because it doesn't copy the whole image...
void image_show_points(struct image_t *img, struct point_t *points, uint16_t points_cnt)
Show points in an image by coloring them through giving the pixels the maximum value.
void image_to_grayscale(struct image_t *input, struct image_t *output)
Convert an image to grayscale.
void image_show_flow_color(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor, const uint8_t *color, const uint8_t *bad_color)
Shows the flow from a specific point to a new point This works on YUV422 and Grayscale images.
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Image helper functions like resizing, color filter, converters...
struct timeval ts
The timestamp of creation.
uint32_t x
The x coordinate of the point.
uint32_t y
The y coordinate of the point.
uint16_t count
Number of times the point has been tracked successfully.
struct point_t pos
The original position the flow comes from in subpixels.
struct FloatEulers eulers
Euler Angles at time of image.
uint32_t error
The matching error in the tracking process in subpixels.
int32_t flow_y
The y direction flow in subpixels.
int32_t flow_x
The x direction flow in subpixels.
@ IMAGE_GRAYSCALE
Grayscale image with only the Y part (uint8 per pixel).
int16_t flow_der_x
The derotated flow calculation in the x direction (in subpixels).
float div_size
Divergence as determined with the size_divergence script.
float surface_roughness
Surface roughness as determined with a linear optical flow fit.
uint16_t tracked_cnt
The amount of tracked corners.
float divergence
Divergence as determined with a linear flow fit.
uint16_t corner_cnt
The amount of coners found by FAST9.
float fps
Frames per second of the optical flow calculation.
float noise_measurement
noise of measurement, for state filter
struct FloatVect3 vel_body
The velocity in body frame (m/s) with X positive to the front of the aircraft, Y positive to the righ...
int16_t flow_x
Flow in x direction from the camera (in subpixels) with X positive to the right.
uint8_t camera_id
Camera id as passed to cv_add_to_device.
struct FloatVect3 vel_cam
The velocity in camera frame (m/s).
int16_t flow_y
Flow in y direction from the camera (in subpixels) with Y positive to the bottom.
int16_t flow_der_y
The derotated flow calculation in the y direction (in subpixels).
bool analyze_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, int im_width, int im_height, float focal_length, struct linear_flow_fit_info *info)
Analyze a flow field, retrieving information on relative velocities and rotations,...
bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info)
Analyze a linear flow field, retrieving information such as divergence, surface roughness,...
float divergence
Basically, relative_velocity_z. Actual divergence of a 2D flow field is 2 * relative_velocity_z.
float surface_roughness
The error of the linear fit is a measure of surface roughness.
struct flow_t * opticFlowLK(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt, uint16_t half_window_size, uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint8_t max_points, uint8_t pyramid_level, uint8_t keep_bad_points)
efficient fixed-point optical-flow calculation
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2
#define OPTICFLOW_MEDIAN_FILTER
#define OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2
#define OPTICFLOW_FAST9_ADAPTIVE_CAMERA2
#define OPTICFLOW_ACTFAST_LONG_STEP
static void manage_flow_features(struct image_t *img, struct opticflow_t *opticflow, struct opticflow_result_t *result)
#define OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2
#define OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2
#define OPTICFLOW_MEDIAN_FILTER_CAMERA2
#define OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2
#define OPTICFLOW_SHOW_FLOW
#define OPTICFLOW_ACTFAST_SHORT_STEP
#define OPTICFLOW_THRESHOLD_VEC
#define OPTICFLOW_PYRAMID_LEVEL_CAMERA2
struct FloatRMat body_to_cam[2]
#define OPTICFLOW_SUBPIXEL_FACTOR
#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X
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)
#define OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2
static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime)
Calculate the difference from start till finish.
#define OPTICFLOW_FAST9_MIN_DISTANCE
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.
#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2
#define OPTICFLOW_MAX_ITERATIONS
#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y
#define OPTICFLOW_ACTFAST_GRADIENT_METHOD
#define OPTICFLOW_SEARCH_DISTANCE_CAMERA2
#define OPTICFLOW_THRESHOLD_VEC_CAMERA2
#define OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2
#define OPTICFLOW_BODY_TO_CAM_PSI
#define OPTICFLOW_FAST9_THRESHOLD_CAMERA2
#define OPTICFLOW_CORNER_METHOD
#define OPTICFLOW_RESOLUTION_FACTOR
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.
#define OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2
#define OPTICFLOW_FEATURE_MANAGEMENT
#define OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2
static int cmp_flow(const void *a, const void *b)
Compare two flow vectors based on flow distance Used for sorting.
#define OPTICFLOW_WINDOW_SIZE
#define OPTICFLOW_METHOD_CAMERA2
#define OPTICFLOW_TRACK_BACK_CAMERA2
void opticflow_calc_init(struct opticflow_t opticflow[])
Initialize the opticflow calculator.
#define OPTICFLOW_MAX_ITERATIONS_CAMERA2
#define OPTICFLOW_SHOW_FLOW_CAMERA2
#define OPTICFLOW_PYRAMID_LEVEL
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.
#define OPTICFLOW_TRACK_BACK
#define OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2
struct MedianFilter3Float vel_filt
#define OPTICFLOW_ACTFAST_MIN_GRADIENT
#define FAST9_LOW_THRESHOLD
#define OPTICFLOW_FAST9_NUM_REGIONS
#define OPTICFLOW_MAX_TRACK_CORNERS
#define OPTICFLOW_FAST9_PADDING
#define OPTICFLOW_SEARCH_DISTANCE
#define OPTICFLOW_FAST9_PADDING_CAMERA2
#define OPTICFLOW_BODY_TO_CAM_THETA
#define OPTICFLOW_FAST9_REGION_DETECT
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.
#define FAST9_HIGH_THRESHOLD
#define OPTICFLOW_DEROTATION
#define OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2
#define OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2
#define OPTICFLOW_FAST9_REGION_DETECT_CAMERA2
#define OPTICFLOW_RESOLUTION_FACTOR_CAMERA2
#define OPTICFLOW_BODY_TO_CAM_PHI
#define OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2
#define OPTICFLOW_FAST9_THRESHOLD
#define OPTICFLOW_DEROTATION_CAMERA2
#define OPTICFLOW_WINDOW_SIZE_CAMERA2
#define OPTICFLOW_FAST9_ADAPTIVE
#define OPTICFLOW_CORNER_METHOD_CAMERA2
Calculate velocity from optic flow.
#define FAST9_MAX_CORNERS
struct opticflow_t opticflow[ACTIVE_CAMERAS]
Opticflow calculations.
float get_size_divergence(struct flow_t *vectors, int count, int n_samples)
Get divergence from optical flow vectors based on line sizes between corners.
Calculate divergence from flow vectors by looking at line sizes beteween the points.
bool distorted_pixels_to_normalized_coords(float x_pd, float y_pd, float *x_n, float *y_n, float k, const float *K)
Transform distorted pixel coordinates to normalized coordinates.
bool normalized_coords_to_distorted_pixels(float x_n, float y_n, float *x_pd, float *y_pd, float k, const float *K)
Transform normalized coordinates to distorted pixel coordinates.
Functions for undistorting camera images.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.