/**
* 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)
{
if (opticflow->method == 0) {
calc_fast9_lukas_kanade(opticflow, state, img, result);
} else {
if (opticflow->method == 1) {
calc_edgeflow_tot(opticflow, state, img, result);
} else {}
}
}
/**
* 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)
{
// A switch counter that checks in the loop if the current method is similar,
// to the previous (for reinitializing structs)
static int8_t switch_counter = -1;
if (switch_counter != opticflow->method) {
opticflow->just_switched_method = true;
switch_counter = opticflow->method;
} else {
opticflow->just_switched_method = false;
}
// Switch between methods (0 = fast9/lukas-kanade, 1 = EdgeFlow)
if (opticflow->method == 0) {
calc_fast9_lukas_kanade(opticflow, state, img, result);
} else if (opticflow->method == 1) {
calc_edgeflow_tot(opticflow, state, img, result);
}
/* Rotate velocities from camera frame coordinates to body coordinates for control
* IMPORTANT!!! This frame to body orientation should be the case for the Parrot
* ARdrone and Bebop, however this can be different for other quadcopters
* ALWAYS double check!
*/
result->vel_body_x = result->vel_y;
result->vel_body_y = - result->vel_x;
// KALMAN filter on velocity
float measurement_noise[2] = {result->noise_measurement, 1.0f};
static bool reinitialize_kalman = true;
static uint8_t wait_filter_counter =
0; // When starting up the opticalflow module, or switching between methods, wait for a bit to prevent bias
if (opticflow->kalman_filter == true) {
if (opticflow->just_switched_method == true) {
wait_filter_counter = 0;
reinitialize_kalman = true;
}
if (wait_filter_counter > 50) {
// Get accelerometer values rotated to body axis
// TODO: use acceleration from the state ?
struct FloatVect3 accel_imu_f;
ACCELS_FLOAT_OF_BFP(accel_imu_f, state->accel_imu_meas);
struct FloatVect3 accel_meas_body;
float_quat_vmult(&accel_meas_body, &state->imu_to_body_quat, &accel_imu_f);
float acceleration_measurement[2];
acceleration_measurement[0] = accel_meas_body.x;
acceleration_measurement[1] = accel_meas_body.y;
kalman_filter_opticflow_velocity(&result->vel_body_x, &result->vel_body_y, acceleration_measurement, result->fps,
measurement_noise, opticflow->kalman_filter_process_noise, reinitialize_kalman);
if (reinitialize_kalman) {
reinitialize_kalman = false;
}
} else {
wait_filter_counter++;
}
} else {
reinitialize_kalman = true;
}
}
