/**
* Run the optical flow with fast9 and lukaskanade 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 calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result)
{
if (opticflow->just_switched_method) {
opticflow_calc_init(opticflow, img->w, img->h);
}
// variables for size_divergence:
float size_divergence; int n_samples;
// variables for linear flow fit:
float error_threshold;
int n_iterations_RANSAC, n_samples_RANSAC, success_fit;
struct linear_flow_fit_info fit_info;
// Update FPS for information
result->fps = 1 / (timeval_diff(&opticflow->prev_timestamp, &img->ts) / 1000.);
opticflow->prev_timestamp = img->ts;
// Convert image to grayscale
image_to_grayscale(img, &opticflow->img_gray);
// Copy to previous image if not set
if (!opticflow->got_first_img) {
image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
opticflow->got_first_img = true;
}
// *************************************************************************************
// Corner detection
// *************************************************************************************
// FAST corner detection
// TODO: There is something wrong with fast9_detect destabilizing FPS. This problem is reduced with putting min_distance
// to 0 (see defines), however a more permanent solution should be considered
fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt,
&opticflow->fast9_rsize,
opticflow->fast9_ret_corners);
// Adaptive threshold
if (opticflow->fast9_adaptive) {
// Decrease and increase the threshold based on previous values
if (result->corner_cnt < 40
&& opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) { // TODO: Replace 40 with OPTICFLOW_MAX_TRACK_CORNERS / 2
opticflow->fast9_threshold--;
} else if (result->corner_cnt > OPTICFLOW_MAX_TRACK_CORNERS * 2 && opticflow->fast9_threshold < FAST9_HIGH_THRESHOLD) {
opticflow->fast9_threshold++;
}
}
#if OPTICFLOW_SHOW_CORNERS
image_show_points(img, opticflow->fast9_ret_corners, result->corner_cnt);
#endif
// Check if we found some corners to track
if (result->corner_cnt < 1) {
image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
return;
}
// *************************************************************************************
// Corner Tracking
// *************************************************************************************
// Execute a Lucas Kanade optical flow
result->tracked_cnt = result->corner_cnt;
struct flow_t *vectors = opticFlowLK(&opticflow->img_gray, &opticflow->prev_img_gray, opticflow->fast9_ret_corners,
&result->tracked_cnt,
opticflow->window_size / 2, opticflow->subpixel_factor, opticflow->max_iterations,
opticflow->threshold_vec, opticflow->max_track_corners, opticflow->pyramid_level);
#if OPTICFLOW_SHOW_FLOW
printf("show: n tracked = %d\n", result->tracked_cnt);
image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor);
#endif
// Estimate size divergence:
if (SIZE_DIV) {
n_samples = 100;
size_divergence = get_size_divergence(vectors, result->tracked_cnt, n_samples);
result->div_size = size_divergence;
} else {
result->div_size = 0.0f;
}
if (LINEAR_FIT) {
// Linear flow fit (normally derotation should be performed before):
error_threshold = 10.0f;
n_iterations_RANSAC = 20;
n_samples_RANSAC = 5;
success_fit = analyze_linear_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC,
n_samples_RANSAC, img->w, img->h, &fit_info);
if (!success_fit) {
fit_info.divergence = 0.0f;
fit_info.surface_roughness = 0.0f;
}
result->divergence = fit_info.divergence;
result->surface_roughness = fit_info.surface_roughness;
} else {
result->divergence = 0.0f;
result->surface_roughness = 0.0f;
}
// Get the median flow
qsort(vectors, result->tracked_cnt, sizeof(struct flow_t), cmp_flow);
if (result->tracked_cnt == 0) {
// We got no flow
result->flow_x = 0;
result->flow_y = 0;
} else if (result->tracked_cnt > 3) {
// Take the average of the 3 median points
result->flow_x = vectors[result->tracked_cnt / 2 - 1].flow_x;
result->flow_y = vectors[result->tracked_cnt / 2 - 1].flow_y;
result->flow_x += vectors[result->tracked_cnt / 2].flow_x;
result->flow_y += vectors[result->tracked_cnt / 2].flow_y;
result->flow_x += vectors[result->tracked_cnt / 2 + 1].flow_x;
result->flow_y += vectors[result->tracked_cnt / 2 + 1].flow_y;
result->flow_x /= 3;
result->flow_y /= 3;
} else {
// Take the median point
result->flow_x = vectors[result->tracked_cnt / 2].flow_x;
result->flow_y = vectors[result->tracked_cnt / 2].flow_y;
}
// Flow Derotation
float diff_flow_x = 0;
float diff_flow_y = 0;
/*// Flow Derotation TODO:
float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;*/
if (opticflow->derotation && result->tracked_cnt > 5) {
diff_flow_x = (state->rates.p) / result->fps * img->w /
OPTICFLOW_FOV_W;// * img->w / OPTICFLOW_FOV_W;
diff_flow_y = (state->rates.q) / result->fps * img->h /
OPTICFLOW_FOV_H;// * img->h / OPTICFLOW_FOV_H;
}
result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor *
opticflow->derotation_correction_factor_x;
result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor *
opticflow->derotation_correction_factor_y;
opticflow->prev_rates = state->rates;
// Velocity calculation
// 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.
float vel_x = result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FX;
float vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FY;
//Apply a median filter to the velocity if wanted
if (opticflow->median_filter == true) {
result->vel_x = (float)update_median_filter(&vel_x_filt, (int32_t)(vel_x * 1000)) / 1000;
result->vel_y = (float)update_median_filter(&vel_y_filt, (int32_t)(vel_y * 1000)) / 1000;
} else {
result->vel_x = vel_x;
result->vel_y = vel_y;
}
// 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_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_H / img->h
// Determine quality of noise measurement for state filter
//TODO develop a noise model based on groundtruth
float noise_measurement_temp = (1 - ((float)result->tracked_cnt / ((float)opticflow->max_track_corners * 1.25)));
result->noise_measurement = noise_measurement_temp;
// *************************************************************************************
// Next Loop Preparation
// *************************************************************************************
free(vectors);
image_switch(&opticflow->img_gray, &opticflow->prev_img_gray);
}
/**
* Run the optical flow with fast9 and lukaskanade 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 calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result)
{
// variables for size_divergence:
float size_divergence;
int n_samples;
// variables for linear flow fit:
float error_threshold;
int n_iterations_RANSAC, n_samples_RANSAC, success_fit;
struct linear_flow_fit_info fit_info;
// Update FPS for information
result->fps = 1 / (timeval_diff(&opticflow->prev_timestamp, &img->ts) / 1000.);
memcpy(&opticflow->prev_timestamp, &img->ts, sizeof(struct timeval));
// Convert image to grayscale
image_to_grayscale(img, &opticflow->img_gray);
// Copy to previous image if not set
if (!opticflow->got_first_img) {
image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
opticflow->got_first_img = true;
}
// *************************************************************************************
// Corner detection
// *************************************************************************************
// FAST corner detection (TODO: non fixed threshold)
struct point_t *corners = fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
0, 0, &result->corner_cnt);
// Adaptive threshold
if (opticflow->fast9_adaptive) {
// Decrease and increase the threshold based on previous values
if (result->corner_cnt < 40 && opticflow->fast9_threshold > 5) {
opticflow->fast9_threshold--;
} else if (result->corner_cnt > 50 && opticflow->fast9_threshold < 60) {
opticflow->fast9_threshold++;
}
}
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_CORNERS
image_show_points(img, corners, result->corner_cnt);
#endif
// Check if we found some corners to track
if (result->corner_cnt < 1) {
free(corners);
image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
return;
}
// *************************************************************************************
// Corner Tracking
// *************************************************************************************
// Execute a Lucas Kanade optical flow
result->tracked_cnt = result->corner_cnt;
struct flow_t *vectors = opticFlowLK(&opticflow->img_gray, &opticflow->prev_img_gray, corners, &result->tracked_cnt,
opticflow->window_size / 2, opticflow->subpixel_factor, opticflow->max_iterations,
opticflow->threshold_vec, opticflow->max_track_corners, opticflow->pyramid_level);
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor);
#endif
// Estimate size divergence:
if (SIZE_DIV) {
n_samples = 100;
size_divergence = get_size_divergence(vectors, result->tracked_cnt, n_samples);
result->div_size = size_divergence;
} else {
result->div_size = 0.0f;
}
if (LINEAR_FIT) {
// Linear flow fit (normally derotation should be performed before):
error_threshold = 10.0f;
n_iterations_RANSAC = 20;
n_samples_RANSAC = 5;
success_fit = analyze_linear_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC,
n_samples_RANSAC, img->w, img->h, &fit_info);
if (!success_fit) {
fit_info.divergence = 0.0f;
fit_info.surface_roughness = 0.0f;
}
result->divergence = fit_info.divergence;
result->surface_roughness = fit_info.surface_roughness;
} else {
result->divergence = 0.0f;
result->surface_roughness = 0.0f;
}
// Get the median flow
qsort(vectors, result->tracked_cnt, sizeof(struct flow_t), cmp_flow);
if (result->tracked_cnt == 0) {
// We got no flow
result->flow_x = 0;
result->flow_y = 0;
} else if (result->tracked_cnt > 3) {
// Take the average of the 3 median points
result->flow_x = vectors[result->tracked_cnt / 2 - 1].flow_x;
result->flow_y = vectors[result->tracked_cnt / 2 - 1].flow_y;
result->flow_x += vectors[result->tracked_cnt / 2].flow_x;
result->flow_y += vectors[result->tracked_cnt / 2].flow_y;
result->flow_x += vectors[result->tracked_cnt / 2 + 1].flow_x;
result->flow_y += vectors[result->tracked_cnt / 2 + 1].flow_y;
result->flow_x /= 3;
result->flow_y /= 3;
} else {
// Take the median point
result->flow_x = vectors[result->tracked_cnt / 2].flow_x;
result->flow_y = vectors[result->tracked_cnt / 2].flow_y;
}
// Flow Derotation
float diff_flow_x = 0;
float diff_flow_y = 0;
/*// Flow Derotation TODO:
float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;*/
if (opticflow->derotation) {
diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;
}
result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor;
result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor;
opticflow->prev_phi = state->phi;
opticflow->prev_theta = state->theta;
// Velocity calculation
// 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.
float vel_x = result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FX;
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.
// 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
// Rotate velocities from camera frame coordinates to body coordinates.
// IMPORTANT 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;
// Determine quality of noise measurement for state filter
//TODO Experiment with multiple noise measurement models
if (result->tracked_cnt < 10) {
result->noise_measurement = (float)result->tracked_cnt / (float)opticflow->max_track_corners;
} else {
result->noise_measurement = 1.0;
}
// *************************************************************************************
// Next Loop Preparation
// *************************************************************************************
free(corners);
free(vectors);
image_switch(&opticflow->img_gray, &opticflow->prev_img_gray);
}
/**
* Run the optical flow 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 opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result)
{
// variables for size_divergence:
float size_divergence; int n_samples;
// variables for linear flow fit:
float error_threshold; int n_iterations_RANSAC, n_samples_RANSAC, success_fit; struct linear_flow_fit_info fit_info;
// Update FPS for information
result->fps = 1 / (timeval_diff(&opticflow->prev_timestamp, &img->ts) / 1000.);
memcpy(&opticflow->prev_timestamp, &img->ts, sizeof(struct timeval));
// Convert image to grayscale
image_to_grayscale(img, &opticflow->img_gray);
// Copy to previous image if not set
if (!opticflow->got_first_img) {
image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
opticflow->got_first_img = TRUE;
}
// *************************************************************************************
// Corner detection
// *************************************************************************************
// FAST corner detection (TODO: non fixed threshold)
struct point_t *corners = fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
20, 20, &result->corner_cnt);
// Adaptive threshold
if (opticflow->fast9_adaptive) {
// Decrease and increase the threshold based on previous values
if (result->corner_cnt < 40 && opticflow->fast9_threshold > 5) {
opticflow->fast9_threshold--;
} else if (result->corner_cnt > 50 && opticflow->fast9_threshold < 60) {
opticflow->fast9_threshold++;
}
}
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_CORNERS
image_show_points(img, corners, result->corner_cnt);
#endif
// Check if we found some corners to track
if (result->corner_cnt < 1) {
free(corners);
image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
return;
}
// *************************************************************************************
// Corner Tracking
// *************************************************************************************
// Execute a Lucas Kanade optical flow
result->tracked_cnt = result->corner_cnt;
struct flow_t *vectors = opticFlowLK(&opticflow->img_gray, &opticflow->prev_img_gray, corners, &result->tracked_cnt,
opticflow->window_size / 2, opticflow->subpixel_factor, opticflow->max_iterations,
opticflow->threshold_vec, opticflow->max_track_corners);
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor);
#endif
// Estimate size divergence:
if (SIZE_DIV) {
n_samples = 100;
size_divergence = get_size_divergence(vectors, result->tracked_cnt, n_samples);
result->div_size = size_divergence;
} else {
result->div_size = 0.0f;
}
if (LINEAR_FIT) {
// Linear flow fit (normally derotation should be performed before):
error_threshold = 10.0f;
n_iterations_RANSAC = 20;
n_samples_RANSAC = 5;
success_fit = analyze_linear_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC, n_samples_RANSAC, img->w, img->h, &fit_info);
if (!success_fit) {
fit_info.divergence = 0.0f;
fit_info.surface_roughness = 0.0f;
}
result->divergence = fit_info.divergence;
result->surface_roughness = fit_info.surface_roughness;
} else {
result->divergence = 0.0f;
result->surface_roughness = 0.0f;
}
// Get the median flow
qsort(vectors, result->tracked_cnt, sizeof(struct flow_t), cmp_flow);
if (result->tracked_cnt == 0) {
// We got no flow
result->flow_x = 0;
result->flow_y = 0;
} else if (result->tracked_cnt > 3) {
// Take the average of the 3 median points
result->flow_x = vectors[result->tracked_cnt / 2 - 1].flow_x;
result->flow_y = vectors[result->tracked_cnt / 2 - 1].flow_y;
result->flow_x += vectors[result->tracked_cnt / 2].flow_x;
result->flow_y += vectors[result->tracked_cnt / 2].flow_y;
result->flow_x += vectors[result->tracked_cnt / 2 + 1].flow_x;
result->flow_y += vectors[result->tracked_cnt / 2 + 1].flow_y;
result->flow_x /= 3;
result->flow_y /= 3;
} else {
// Take the median point
result->flow_x = vectors[result->tracked_cnt / 2].flow_x;
result->flow_y = vectors[result->tracked_cnt / 2].flow_y;
}
// Flow Derotation
float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;
result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor;
result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor;
opticflow->prev_phi = state->phi;
opticflow->prev_theta = state->theta;
// Velocity calculation
result->vel_x = -result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * img->w / OPTICFLOW_FX;
result->vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * img->h / OPTICFLOW_FY;
// *************************************************************************************
// Next Loop Preparation
// *************************************************************************************
free(corners);
free(vectors);
image_switch(&opticflow->img_gray, &opticflow->prev_img_gray);
}
/**
* Run the optical flow 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 opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result)
{
// Update FPS for information
result->fps = 1 / (timeval_diff(&opticflow->prev_timestamp, &img->ts) / 1000.);
memcpy(&opticflow->prev_timestamp, &img->ts, sizeof(struct timeval));
// Convert image to grayscale
image_to_grayscale(img, &opticflow->img_gray);
// Copy to previous image if not set
if (!opticflow->got_first_img) {
image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
opticflow->got_first_img = TRUE;
}
// *************************************************************************************
// Corner detection
// *************************************************************************************
// FAST corner detection (TODO: non fixed threashold)
struct point_t *corners = fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
20, 20, &result->corner_cnt);
// Adaptive threshold
if (opticflow->fast9_adaptive) {
// Decrease and increase the threshold based on previous values
if (result->corner_cnt < 40 && opticflow->fast9_threshold > 5) {
opticflow->fast9_threshold--;
} else if (result->corner_cnt > 50 && opticflow->fast9_threshold < 60) {
opticflow->fast9_threshold++;
}
}
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_CORNERS
image_show_points(img, corners, result->corner_cnt);
#endif
// Check if we found some corners to track
if (result->corner_cnt < 1) {
free(corners);
image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
return;
}
// *************************************************************************************
// Corner Tracking
// *************************************************************************************
// Execute a Lucas Kanade optical flow
result->tracked_cnt = result->corner_cnt;
struct flow_t *vectors = opticFlowLK(&opticflow->img_gray, &opticflow->prev_img_gray, corners, &result->tracked_cnt,
opticflow->window_size / 2, opticflow->subpixel_factor, opticflow->max_iterations,
opticflow->threshold_vec, opticflow->max_track_corners);
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor);
#endif
// Get the median flow
qsort(vectors, result->tracked_cnt, sizeof(struct flow_t), cmp_flow);
if (result->tracked_cnt == 0) {
// We got no flow
result->flow_x = 0;
result->flow_y = 0;
} else if (result->tracked_cnt > 3) {
// Take the average of the 3 median points
result->flow_x = vectors[result->tracked_cnt / 2 - 1].flow_x;
result->flow_y = vectors[result->tracked_cnt / 2 - 1].flow_y;
result->flow_x += vectors[result->tracked_cnt / 2].flow_x;
result->flow_y += vectors[result->tracked_cnt / 2].flow_y;
result->flow_x += vectors[result->tracked_cnt / 2 + 1].flow_x;
result->flow_y += vectors[result->tracked_cnt / 2 + 1].flow_y;
result->flow_x /= 3;
result->flow_y /= 3;
} else {
// Take the median point
result->flow_x = vectors[result->tracked_cnt / 2].flow_x;
result->flow_y = vectors[result->tracked_cnt / 2].flow_y;
}
// Flow Derotation
float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;
result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor;
result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor;
opticflow->prev_phi = state->phi;
opticflow->prev_theta = state->theta;
// Velocity calculation
result->vel_x = -result->flow_der_x * result->fps / opticflow->subpixel_factor * img->w / OPTICFLOW_FX;
result->vel_y = result->flow_der_y * result->fps / opticflow->subpixel_factor * img->h / OPTICFLOW_FY;
// *************************************************************************************
// Next Loop Preparation
// *************************************************************************************
free(corners);
free(vectors);
image_switch(&opticflow->img_gray, &opticflow->prev_img_gray);
}