/** * 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; } }