Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Image undistortion for fisheye lenses #2308

Merged
merged 5 commits into from
Aug 2, 2018
Merged

Image undistortion for fisheye lenses #2308

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
1fb1fa6
Added an undistortion file that has the supporting functions for undi…
guidoAI Jul 31, 2018
fe50645
Added undistort image module - now compiling
guidoAI Jul 31, 2018
bedcf5b
inverted the process, first copying the distorted image, and then fil…
guidoAI Aug 1, 2018
1ecfd39
Added settings to the GCS
guidoAI Aug 1, 2018
cdf55d6
Added settings for the camera calibration matrix and removed unnecess…
guidoAI Aug 1, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
248 changes: 248 additions & 0 deletions conf/airframes/tudelft/bebop2_undistort_front.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,248 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">

<airframe name="bebop2_optitrack_visionfront">

<firmware name="rotorcraft">
<target name="ap" board="bebop2"/>

<module name="telemetry" type="transparent_udp"/>
<module name="radio_control" type="datalink"/>
<module name="motor_mixing"/>
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<!-- <define name="AHRS_USE_GPS_HEADING" value="TRUE"/> -->
</module>
<define name="USE_SONAR" value="0"/>
<module name="ins" type="extended"/>

<define name="MT9F002_OUTPUT_HEIGHT" value="640" />
<define name="MT9F002_OUTPUT_WIDTH" value="640" />
<define name="MT9F002_INITIAL_OFFSET_X" value="0.15" />
<define name="MT9F002_INITIAL_OFFSET_Y" value="0.0" />
<define name="MT9F002_TARGET_EXPOSURE" value="30" />
<define name="MT9F002_GAIN_GREEN1" value="4"/>
<define name="MT9F002_GAIN_GREEN2" value="4"/>
<define name="MT9F002_GAIN_RED" value="5"/>
<define name="MT9F002_GAIN_BLUE" value="5"/>
<define name="MT9F002_OUTPUT_SCALER" value="0.25"/>
<define name="MT9F002_X_ODD_INC_VAL" value="1"/>
<define name="MT9F002_Y_ODD_INC_VAL" value="1"/>
</firmware>

<modules main_freq="512">
<module name="geo_mag"/>
<module name="air_data"/>
<module name="send_imu_mag_current"/>
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
</module>

<module name="video_thread"/>

<module name="video_capture">
<define name="VIDEO_CAPTURE_CAMERA" value="front_camera"/>
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images/"/>
</module>

<module name="pose_history">
<define name="POSE_HISTORY_SIZE" value="128" />
</module>

<module name="cv_undistort_image">
<define name="UNDISTORT_MIN_X_NORMALIZED" value="-2.0"/>
<define name="UNDISTORT_MAX_X_NORMALIZED" value="2.0"/>
<define name="UNDISTORT_DHANE_K" value="1.25"/>
<define name="UNDISTORT_CAMERA" value="front_camera"/>
</module>

<module name="video_rtp_stream">
<define name="VIEWVIDEO_CAMERA" value="front_camera"/>
<!-- <define name="VIEWVIDEO_CAMERA2" value="bottom_camera"/> -->
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="90"/>
</module>

<!--<module name="bebop_ae_awb"/>-->
</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="6000"/>
</commands>

<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="2500" neutral="2500" max="12000"/>
<servo name="TOP_RIGHT" no="1" min="2500" neutral="2500" max="12000"/>
<servo name="BOTTOM_RIGHT" no="2" min="2500" neutral="2500" max="12000"/>
<servo name="BOTTOM_LEFT" no="3" min="2500" neutral="2500" max="12000"/>
</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"/>
<define name="REVERSE" value="TRUE"/>
<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="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" value="TRUE"/>
</section>

<!-- Magnetometer still needs to be calibrated -->
<section name="IMU" prefix="IMU_">
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="0"/>
<define name="MAG_X_SENS" value="7.28514789391" integer="16"/>
<define name="MAG_Y_SENS" value="7.33022132691" integer="16"/>
<define name="MAG_Z_SENS" value="7.57102035692" integer="16"/>
</section>

<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>
<!-- Use GPS heading instead of magneto -->
<define name="USE_GPS_HEADING" value="1"/>
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</section>

<section name="INS" prefix="INS_">
<!--<define name="SONAR_MAX_RANGE" value="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/> -->
<!-- Use GPS altitude measurments and set the R gain -->
<define name="USE_GPS_ALT" value="1"/>
<define name="VFF_R_GPS" value="0.01"/>
</section>


<section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoint limits for attitude stabilization rc flight -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="300" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
</section>

<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
<!-- attitude reference generation model -->
<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.094"/>
<define name="G1_Q" value="0.094"/>
<define name="G1_R" value="0.0025"/>
<define name="G2_R" value="0.36"/>

<!-- Here it is assumed that your removed the damping from your bebop2!
The dampers do not really damp, but cause oscillation. By removing/
fixing them, the bebop2 will fly much better-->
<define name="FILTER_ROLL_RATE" value="FALSE"/>
<define name="FILTER_PITCH_RATE" value="FALSE"/>
<define name="FILTER_YAW_RATE" value="FALSE"/>

<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="600.0"/>
<define name="REF_ERR_Q" value="600.0"/>
<define name="REF_ERR_R" value="600.0"/>
<define name="REF_RATE_P" value="28.0"/>
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.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="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>

<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="350"/>
<define name="HOVER_KD" value="85"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="REF_MAX_SPEED" value="2" unit="m/s"/>
<define name="MAX_BANK" value="32" unit="deg"/>
<define name="PGAIN" value="220"/>
<define name="DGAIN" value="160"/>
<define name="IGAIN" value="15"/>
</section>

<section name="NAVIGATION" prefix="NAV_">
<define name="CLIMB_VSPEED" value="1.0"/>
<define name="DESCEND_VSPEED" value="-1.0"/>
</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_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
</section>

<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
<!-- <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/> -->
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>

<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.9" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="11.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
11 changes: 11 additions & 0 deletions conf/airframes/tudelft/guido_conf.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,17 @@
settings_modules="modules/optical_flow_landing.xml modules/video_capture.xml modules/cv_opticflow.xml modules/air_data.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="blue"
/>
<aircraft
name="Bebop2_undistort_front"
ac_id="5"
airframe="airframes/tudelft/bebop2_undistort_front.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_guido_optitrack.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/video_rtp_stream.xml modules/cv_undistort_image.xml modules/video_capture.xml modules/air_data.xml modules/geo_mag.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="blue"
/>
<aircraft
name="Bebop_opticflow"
ac_id="14"
Expand Down
48 changes: 48 additions & 0 deletions conf/modules/cv_undistort_image.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
<!DOCTYPE module SYSTEM "module.dtd">

<module name="cv_undistort_image" dir="computer_vision">
<doc>
<description>ColorFilter</description>
<define name="UNDISTORT_MIN_X_NORMALIZED" value="-2.0" description="Minimal normalized x-coordinate to be used for the undistortion"/>
<define name="UNDISTORT_MAX_X_NORMALIZED" value="2.0" description="Maximal normalized x-coordinate to be used for the undistortion"/>
<define name="UNDISTORT_DHANE_K" value="1.25" description="The single parameter of the Dhane invertible (un)distortion model."/>
<define name="UNDISTORT_FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
<define name="UNDISTORT_CAMERA" value="bottom_camera|front_camera" description="The V4L2 camera device that is used for the calculations"/>
<define name="UNDISTORT_CENTER_RATIO" value="1.0" description="If smaller than 1 only generate pixels for the center_ratio times the min_x to max_x interval. This makes undistortion quicker, but for a smaller FOV."/>
<define name="UNDISTORT_FOCAL_X" value="189.69" description="Focal length x-axis in pixels."/>
<define name="UNDISTORT_FOCAL_Y" value="188.60" description="Focal length y-axis in pixels."/>
<define name="UNDISTORT_CENTER_X" value="165.04" description="Image x-coordinate of the camera principal axis."/>
<define name="UNDISTORT_CENTER_Y" value="118.44" description="Image y-coordinate of the camera principal axis."/>

</doc>

<settings>
<dl_settings>
<dl_settings NAME="Undistort">
<dl_setting var="min_x_normalized" min="-4.0" step="0.1" max="0.0" shortname="min_x_n" param="UNDISTORT_MIN_X_NORMALIZED"/>
<dl_setting var="max_x_normalized" min="0.1" step="0.1" max="4.0" shortname="max_x_n" param="UNDISTORT_MAX_X_NORMALIZED"/>
<dl_setting var="dhane_k" min="1.0" step="0.01" max="2.5" shortname="dhane_k" param="UNDISTORT_DHANE_K"/>
<dl_setting var="center_ratio" min="0.05" step="0.01" max="1.0" shortname="center_ratio" param="UNDISTORT_CENTER_RATIO"/>
<dl_setting var="focal_x" min="0.0" step="0.05" max="1024.0" shortname="focal_x" param="UNDISTORT_FOCAL_X"/>
<dl_setting var="center_x" min="0.0" step="0.05" max="1024.0" shortname="center_x" param="UNDISTORT_CENTER_X"/>
<dl_setting var="focal_y" min="0.0" step="0.05" max="1024.0" shortname="focal_y" param="UNDISTORT_FOCAL_Y"/>
<dl_setting var="center_y" min="0.0" step="0.05" max="1024.0" shortname="center_y" param="UNDISTORT_CENTER_Y"/>

</dl_settings>
</dl_settings>
</settings>

<depends>video_thread</depends>

<header>
<file name="undistort_image.h"/>
</header>

<init fun="undistort_image_init()"/>
<makefile target="ap|nps">
<file name="undistort_image.c"/>
<file name="image.c" dir="modules/computer_vision/lib/vision"/>
<file name="undistortion.c" dir="modules/computer_vision/lib/vision"/>
</makefile>
</module>

Loading