mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
Merge pull request #2048 from ETitus/EdgeFlow_fixes
Edge flow fixes. Thx to all involved for all the hard work done and testing validation . There will be some others working with the OF fixed solution, so more validation tests by others soon in the renewed Optitrack. Issues can be filed if still something is amiss.
This commit is contained in:
OpenUAS
GitHub
@@ -0,0 +1,224 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<airframe name="ardrone2_opticflow">
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="ardrone2"/>
|
||||
|
||||
<!--target name="nps" board="pc">
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
</target-->
|
||||
|
||||
<define name="USE_SONAR" value="TRUE"/>
|
||||
|
||||
<!-- Subsystem section -->
|
||||
<module name="telemetry" type="transparent_udp"/>
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="motor_mixing"/>
|
||||
<module name="actuators" type="ardrone2"/>
|
||||
<module name="imu" type="ardrone2"/>
|
||||
<module name="gps" type="ublox"/>
|
||||
<module name="stabilization" type="indi_simple"/>
|
||||
<module name="ahrs" type="int_cmpl_quat">
|
||||
<configure name="USE_MAGNETOMETER" value="TRUE"/>
|
||||
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
|
||||
</module>
|
||||
<module name="ins" type="extended"/>
|
||||
</firmware>
|
||||
|
||||
<modules main_freq="512">
|
||||
<module name="bat_voltage_ardrone2"/>
|
||||
<module name="gps" type="ubx_ucenter"/>
|
||||
<module name="send_imu_mag_current"/>
|
||||
<module name="logger_file"/>
|
||||
|
||||
<module name="video_thread"/>
|
||||
|
||||
<module name="pose_history"/>
|
||||
<module name="cv_opticflow">
|
||||
<define name="OPTICFLOW_CAMERA" value="bottom_camera"/>
|
||||
</module>
|
||||
|
||||
<module name="video_capture">
|
||||
<define name="VIDEO_CAPTURE_CAMERA" value="bottom_camera"/>
|
||||
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
|
||||
</module>
|
||||
|
||||
<!--module name="opticflow_hover"/-->
|
||||
</modules>
|
||||
|
||||
<commands>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="3000"/>
|
||||
</commands>
|
||||
|
||||
<servos driver="Default">
|
||||
<servo name="TOP_LEFT" no="0" min="0" neutral="1" max="500"/>
|
||||
<servo name="TOP_RIGHT" no="1" min="0" neutral="1" max="500"/>
|
||||
<servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500"/>
|
||||
<servo name="BOTTOM_LEFT" no="3" min="0" neutral="1" max="500"/>
|
||||
</servos>
|
||||
|
||||
<section name="MIXING" prefix="MOTOR_MIXING_">
|
||||
<define name="TRIM_ROLL" value="0"/>
|
||||
<define name="TRIM_PITCH" value="0"/>
|
||||
<define name="TRIM_YAW" value="0"/>
|
||||
|
||||
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
|
||||
<define name="TYPE" value="QUAD_X"/>
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
|
||||
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
|
||||
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
|
||||
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
|
||||
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="VISION" prefix="VISION_">
|
||||
<define name="HOVER" value="FALSE"/>
|
||||
<define name="PHI_PGAIN" value="1500"/>
|
||||
<define name="PHI_IGAIN" value="800"/>
|
||||
<define name="THETA_PGAIN" value="1500"/>
|
||||
<define name="THETA_IGAIN" value="800"/>
|
||||
<define name="DESIRED_VX" value="0"/>
|
||||
<define name="DESIRED_VY" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<!-- Accelero -->
|
||||
<define name="ACCEL_X_NEUTRAL" value="2048"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="2048"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="2048"/>
|
||||
|
||||
<!-- Magneto calibration -->
|
||||
<define name="MAG_X_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="-180"/>
|
||||
<define name="MAG_X_SENS" value="16." integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="16." integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="16." integer="16"/>
|
||||
|
||||
<!-- Magneto current calibration -->
|
||||
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
|
||||
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
|
||||
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<!-- local magnetic field -->
|
||||
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<!-- Delft -->
|
||||
<define name="H_X" value="0.3892503"/>
|
||||
<define name="H_Y" value="0.0017972"/>
|
||||
<define name="H_Z" value="0.9211303"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="SONAR_MAX_RANGE" value="2.2"/>
|
||||
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_PHI" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_R" value="600" unit="deg/s"/>
|
||||
<define name="DEADBAND_A" value="0"/>
|
||||
<define name="DEADBAND_E" value="0"/>
|
||||
<define name="DEADBAND_R" value="250"/>
|
||||
|
||||
<!-- reference -->
|
||||
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.9"/>
|
||||
<define name="REF_MAX_P" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.9"/>
|
||||
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.9"/>
|
||||
<define name="REF_MAX_R" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<define name="G1_P" value="0.032"/>
|
||||
<define name="G1_Q" value="0.025"/>
|
||||
<define name="G1_R" value="0.0032"/>
|
||||
<define name="G2_R" value="0.16"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="380.0"/>
|
||||
<define name="REF_ERR_Q" value="380.0"/>
|
||||
<define name="REF_ERR_R" value="250.0"/>
|
||||
<define name="REF_RATE_P" value="21.6"/>
|
||||
<define name="REF_RATE_Q" value="21.6"/>
|
||||
<define name="REF_RATE_R" value="21.0"/>
|
||||
|
||||
<!-- second order filter parameters -->
|
||||
<define name="FILT_CUTOFF" value="3.2"/>
|
||||
<define name="FILT_CUTOFF_R" value="3.2"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN_P" value="0.06"/>
|
||||
<define name="ACT_DYN_Q" value="0.06"/>
|
||||
<define name="ACT_DYN_R" value="0.06"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="TRUE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="283"/>
|
||||
<define name="HOVER_KD" value="82"/>
|
||||
<define name="HOVER_KI" value="13"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<!-- Good weather -->
|
||||
<define name="MAX_BANK" value="20" unit="deg"/>
|
||||
<!-- Bad weather -->
|
||||
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
|
||||
<define name="PGAIN" value="79"/>
|
||||
<define name="DGAIN" value="100"/>
|
||||
<define name="IGAIN" value="30"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="simple_ardrone2" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_ardrone2.h" type="string"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
|
||||
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_HOVER_Z_HOLD"/>
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
</section>
|
||||
</airframe>
|
||||
@@ -31,6 +31,7 @@
|
||||
<define name="WINDOW_SIZE" value="10" description="Window size used for block matching (pixels)"/>
|
||||
<define name="SEARCH_DISTANCE" value="10" description="Maximum search distance for blockmatching (pixels)"/>
|
||||
<define name="SUBPIXEL_FACTOR" value="10" description="Amount of subpixels per pixel, used for more precise (subpixel) calculations of the flow"/>
|
||||
<define name="RESOLUTION_FACTOR" value="1000" description="The resolution factor needed to calculate the divergence without floats"/>
|
||||
<define name="DEROTATION" value="1" description="Derotation either turned on or off (depended on gyroscope measurements)"/>
|
||||
<define name="MEDIAN_FILTER" value="0" description="A median filter on the resulting velocities to be turned on or off (last 5 measurements)"/>
|
||||
<define name="KALMAN_FILTER" value="1" description="A kalman filter on the resulting velocities to be turned on or off (fused with accelerometers)"/>
|
||||
@@ -58,6 +59,7 @@
|
||||
<dl_setting var="opticflow.window_size" module="computer_vision/opticflow_module" min="0" step="1" max="20" shortname="window_size" param="OPTICFLOW_WINDOW_SIZE"/>
|
||||
<dl_setting var="opticflow.search_distance" module="computer_vision/opticflow_module" min="0" step="1" max="50" shortname="search_distance" param="SEARCH_DISTANCE"/>
|
||||
<dl_setting var="opticflow.subpixel_factor" module="computer_vision/opticflow_module" min="0" step="10" max="1000" shortname="subpixel_factor" param="OPTICFLOW_SUBPIXEL_FACTOR"/>
|
||||
<dl_setting var="opticflow.resolution_factor" module="computer_vision/opticflow_module" min="10" step="10" max="10000" shortname="resolution_factor" param="OPTICFLOW_RESOLUTION_FACTOR"/>
|
||||
<dl_setting var="opticflow.derotation" min="0" step="1" max="1" module="computer_vision/opticflow_module" values="OFF|ON" shortname="derotation" param="OPTICFLOW_DEROTATION"/>
|
||||
<dl_setting var="opticflow.median_filter" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="median_filter" param="OPTICFLOW_MEDIAN_FILTER"/>
|
||||
<dl_setting var="opticflow.kalman_filter" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="kalman_filter" param="OPTICFLOW_KALMAN_FILTER"/>
|
||||
|
||||
@@ -121,10 +121,10 @@ void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
|
||||
for (y = 0; y < image_height; y++) {
|
||||
sobel_sum = 0;
|
||||
|
||||
for (c = -1; c <= 1; c++) {
|
||||
idx = interlace * (image_width * y + (x + c)); // 2 for interlace
|
||||
for (c = -1; c <= 1; c+=2) {
|
||||
idx = interlace * (image_width * y + (x + c));
|
||||
|
||||
sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx + 1];
|
||||
sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx];
|
||||
}
|
||||
sobel_sum = abs(sobel_sum);
|
||||
if (sobel_sum > edge_threshold) {
|
||||
@@ -140,10 +140,10 @@ void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
|
||||
for (x = 0; x < image_width; x++) {
|
||||
sobel_sum = 0;
|
||||
|
||||
for (c = -1; c <= 1; c++) {
|
||||
idx = interlace * (image_width * (y + c) + x); // 2 for interlace
|
||||
for (c = -1; c <= 1; c+=2) {
|
||||
idx = interlace * (image_width * (y + c) + x);
|
||||
|
||||
sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx + 1];
|
||||
sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx];
|
||||
}
|
||||
sobel_sum = abs(sobel_sum);
|
||||
if (sobel_sum > edge_threshold) {
|
||||
@@ -159,7 +159,7 @@ void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[],
|
||||
* Calculate_displacement calculates the displacement between two histograms
|
||||
* @param[in] *edge_histogram The edge histogram from the current frame_step
|
||||
* @param[in] *edge_histogram_prev The edge histogram from the previous frame_step
|
||||
* @param[in] *displacement array with pixel displacement of the sequential edge histograms
|
||||
* @param[out] *displacement array with pixel displacement of the sequential edge histograms
|
||||
* @param[in] size Indicating the size of the displacement array
|
||||
* @param[in] window Indicating the search window size
|
||||
* @param[in] disp_range Indicating the maximum disparity range for the block matching
|
||||
@@ -196,10 +196,8 @@ void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogra
|
||||
if (border[0] >= border[1] || abs(der_shift) >= 10) {
|
||||
SHIFT_TOO_FAR = 1;
|
||||
}
|
||||
{
|
||||
// TODO: replace with arm offset subtract
|
||||
for (x = border[0]; x < border[1]; x++) {
|
||||
displacement[x] = 0;
|
||||
if (!SHIFT_TOO_FAR) {
|
||||
for (c = -D; c <= D; c++) {
|
||||
SAD_temp[c + D] = 0;
|
||||
@@ -211,8 +209,6 @@ void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogra
|
||||
} else {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -271,7 +267,7 @@ void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_
|
||||
// compute fixed sums
|
||||
int32_t xend = size_int - border_int - 1;
|
||||
sumX = xend * (xend + 1) / 2 - border_int * (border_int + 1) / 2 + border_int;
|
||||
sumX2 = xend * (xend + 1) * (2 * xend + 1) / 6;
|
||||
sumX2 = xend * (xend + 1) * (2 * xend + 1) / 6 - border_int * (border_int + 1) * (2 * border_int + 1) / 6 + border_int*border_int;
|
||||
xMean = (size_int - 1) / 2;
|
||||
count = size_int - 2 * border_int;
|
||||
|
||||
|
||||
@@ -353,6 +353,7 @@ void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t
|
||||
* This outputs a subpixel window image in grayscale
|
||||
* Currently only works with Grayscale images as input but could be upgraded to
|
||||
* also support YUV422 images.
|
||||
* You can and should only ask a subpixel window of a center point that is w/2 pixels away from the edges
|
||||
* @param[in] *input Input image (grayscale only)
|
||||
* @param[out] *output Window output (width and height is used to calculate the window size)
|
||||
* @param[in] *center Center point in subpixel coordinates
|
||||
@@ -369,18 +370,18 @@ void image_subpixel_window(struct image_t *input, struct image_t *output, struct
|
||||
// Calculate the window size
|
||||
uint16_t half_window = output->w / 2;
|
||||
|
||||
uint32_t subpixel_w = input->w * subpixel_factor;
|
||||
uint32_t subpixel_h = input->h * subpixel_factor;
|
||||
uint32_t subpixel_w = (input->w -2) * subpixel_factor;
|
||||
uint32_t subpixel_h = (input->h -2) * subpixel_factor;
|
||||
|
||||
// Go through the whole window size in normal coordinates
|
||||
for (uint16_t i = 0; i < output->w; i++) {
|
||||
for (uint16_t j = 0; j < output->h; j++) {
|
||||
// Calculate the subpixel coordinate
|
||||
uint32_t x = center->x + border_size * subpixel_factor + (i - half_window) * subpixel_factor ;
|
||||
uint32_t y = center->y + border_size * subpixel_factor + (j - half_window) * subpixel_factor ;
|
||||
uint32_t x = center->x + border_size * subpixel_factor + (i - half_window) * subpixel_factor;
|
||||
uint32_t y = center->y + border_size * subpixel_factor + (j - half_window) * subpixel_factor;
|
||||
|
||||
BoundUpper(x, subpixel_w - 1);
|
||||
BoundUpper(y, subpixel_h - 1);
|
||||
BoundUpper(x, subpixel_w);
|
||||
BoundUpper(y, subpixel_h);
|
||||
|
||||
// Calculate the original pixel coordinate
|
||||
uint16_t orig_x = x / subpixel_factor;
|
||||
|
||||
@@ -331,8 +331,9 @@ struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img
|
||||
vectors[new_p].pos.y + vectors[new_p].flow_y
|
||||
};
|
||||
// If the pixel is outside ROI, do not track it
|
||||
if (new_point.x / subpixel_factor < half_window_size || (old_img->w - new_point.x / subpixel_factor) < half_window_size
|
||||
|| new_point.y / subpixel_factor < half_window_size || (old_img->h - new_point.y / subpixel_factor) < half_window_size) {
|
||||
if (new_point.x / subpixel_factor < half_window_size || (old_img->w - new_point.x / subpixel_factor) <= half_window_size
|
||||
|| new_point.y / subpixel_factor < half_window_size || (old_img->h - new_point.y / subpixel_factor) <= half_window_size
|
||||
|| new_point.x / subpixel_factor > old_img->w || new_point.y / subpixel_factor > old_img->h) {
|
||||
tracked = FALSE;
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -95,13 +95,18 @@ PRINT_CONFIG_VAR(OPTICFLOW_WINDOW_SIZE)
|
||||
#ifndef OPTICFLOW_SEARCH_DISTANCE
|
||||
#define OPTICFLOW_SEARCH_DISTANCE 20
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_MAX_SEARCH_DISTANCE)
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_SEARCH_DISTANCE)
|
||||
|
||||
#ifndef OPTICFLOW_SUBPIXEL_FACTOR
|
||||
#define OPTICFLOW_SUBPIXEL_FACTOR 10
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_SUBPIXEL_FACTOR)
|
||||
|
||||
#ifndef OPTICFLOW_RESOLUTION_FACTOR
|
||||
#define OPTICFLOW_RESOLUTION_FACTOR 100
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_RESOLUTION_FACTOR)
|
||||
|
||||
#ifndef OPTICFLOW_MAX_ITERATIONS
|
||||
#define OPTICFLOW_MAX_ITERATIONS 10
|
||||
#endif
|
||||
@@ -194,23 +199,9 @@ static int cmp_flow(const void *a, const void *b);
|
||||
/**
|
||||
* Initialize the opticflow calculator
|
||||
* @param[out] *opticflow The new optical flow calculator
|
||||
* @param[in] *w The image width
|
||||
* @param[in] *h The image height
|
||||
*/
|
||||
void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
|
||||
void opticflow_calc_init(struct opticflow_t *opticflow)
|
||||
{
|
||||
|
||||
init_median_filter(&vel_x_filt);
|
||||
init_median_filter(&vel_y_filt);
|
||||
|
||||
/* Create the image buffers */
|
||||
image_create(&opticflow->img_gray, w, h, IMAGE_GRAYSCALE);
|
||||
image_create(&opticflow->prev_img_gray, w, h, IMAGE_GRAYSCALE);
|
||||
|
||||
/* Set the previous values */
|
||||
opticflow->got_first_img = false;
|
||||
FLOAT_RATES_ZERO(opticflow->prev_rates);
|
||||
|
||||
/* Set the default values */
|
||||
opticflow->method = OPTICFLOW_METHOD; //0 = LK_fast9, 1 = Edgeflow
|
||||
opticflow->window_size = OPTICFLOW_WINDOW_SIZE;
|
||||
@@ -221,6 +212,7 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
|
||||
|
||||
opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
|
||||
opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR;
|
||||
opticflow->resolution_factor = OPTICFLOW_RESOLUTION_FACTOR;
|
||||
opticflow->max_iterations = OPTICFLOW_MAX_ITERATIONS;
|
||||
opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC;
|
||||
opticflow->pyramid_level = OPTICFLOW_PYRAMID_LEVEL;
|
||||
@@ -234,7 +226,6 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
|
||||
opticflow->fast9_padding = OPTICFLOW_FAST9_PADDING;
|
||||
opticflow->fast9_rsize = 512;
|
||||
opticflow->fast9_ret_corners = malloc(sizeof(struct point_t) * opticflow->fast9_rsize);
|
||||
|
||||
}
|
||||
/**
|
||||
* Run the optical flow with fast9 and lukaskanade on a new image frame
|
||||
@@ -247,7 +238,17 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
|
||||
struct opticflow_result_t *result)
|
||||
{
|
||||
if (opticflow->just_switched_method) {
|
||||
opticflow_calc_init(opticflow, img->w, img->h);
|
||||
// Create the image buffers
|
||||
image_create(&opticflow->img_gray, img->w, img->h, IMAGE_GRAYSCALE);
|
||||
image_create(&opticflow->prev_img_gray, img->w, img->h, IMAGE_GRAYSCALE);
|
||||
|
||||
// Set the previous values
|
||||
opticflow->got_first_img = false;
|
||||
FLOAT_RATES_ZERO(opticflow->prev_rates);
|
||||
|
||||
// Init median filters with zeros
|
||||
init_median_filter(&vel_x_filt);
|
||||
init_median_filter(&vel_y_filt);
|
||||
}
|
||||
|
||||
// variables for size_divergence:
|
||||
@@ -471,7 +472,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
|
||||
window_size = MAX_WINDOW_SIZE;
|
||||
}
|
||||
|
||||
uint16_t RES = opticflow->subpixel_factor;
|
||||
uint16_t RES = opticflow->resolution_factor;
|
||||
|
||||
//......................Calculating EdgeFlow..................... //
|
||||
|
||||
@@ -526,7 +527,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
|
||||
|
||||
/* Save Resulting flow in results
|
||||
* Warning: The flow detected here is different in sign
|
||||
* and size, therefore this will be multiplied with
|
||||
* and size, therefore this will be divided with
|
||||
* the same subpixel factor and -1 to make it on par with
|
||||
* the LK algorithm of t opticalflow_calculator.c
|
||||
* */
|
||||
@@ -536,12 +537,15 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
|
||||
result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0];
|
||||
result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1];
|
||||
|
||||
result->flow_x = (int16_t)edgeflow.flow_x / RES;
|
||||
result->flow_y = (int16_t)edgeflow.flow_y / RES;
|
||||
|
||||
//Fill up the results optic flow to be on par with LK_fast9
|
||||
result->flow_der_x = result->flow_x;
|
||||
result->flow_der_y = result->flow_y;
|
||||
result->corner_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
|
||||
result->tracked_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
|
||||
result->divergence = (float)edgeflow.flow_x / RES;
|
||||
result->divergence = (float)edgeflow.div_x / RES;
|
||||
result->div_size = 0.0f;
|
||||
result->noise_measurement = 0.0f;
|
||||
result->surface_roughness = 0.0f;
|
||||
@@ -584,6 +588,10 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
|
||||
#endif
|
||||
// Increment and wrap current time frame
|
||||
current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON;
|
||||
|
||||
// Free malloc'd variables
|
||||
free(displacement.x);
|
||||
free(displacement.y);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -57,6 +57,7 @@ struct opticflow_t {
|
||||
float derotation_correction_factor_y; ///< Correction factor for derotation in Y axis, determined from a fit from the gyros and flow rotation. (wrong FOV, camera not in center)
|
||||
|
||||
uint16_t subpixel_factor; ///< The amount of subpixels per pixel
|
||||
uint16_t resolution_factor; ///< The resolution in EdgeFlow to determine the Divergence
|
||||
uint8_t max_iterations; ///< The maximum amount of iterations the Lucas Kanade algorithm should do
|
||||
uint8_t threshold_vec; ///< The threshold in x, y subpixels which the algorithm should stop
|
||||
uint8_t pyramid_level; ///< Number of pyramid levels used in Lucas Kanade algorithm (0 == no pyramids used)
|
||||
@@ -72,7 +73,7 @@ struct opticflow_t {
|
||||
};
|
||||
|
||||
|
||||
void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h);
|
||||
void opticflow_calc_init(struct opticflow_t *opticflow);
|
||||
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
|
||||
struct opticflow_result_t *result);
|
||||
|
||||
|
||||
@@ -124,6 +124,7 @@ void opticflow_module_init(void)
|
||||
|
||||
// Initialize the opticflow calculation
|
||||
opticflow_got_result = false;
|
||||
opticflow_calc_init(&opticflow);
|
||||
|
||||
cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user