Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 02:38:07 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Merge pull request #1143 from paparazzi/opticflow

Browse Source 
[vision] Opticflow rewrite: 
-less grabbing threads
-faster grabbing
-static linking issues fixed properly
-documentation
-rewritten code
This commit is contained in:
Christophe De Wagter
2015-04-09 11:47:38 +02:00
parent 4f5806a392 dea8152f26
commit ea58643e91
49 changed files with 5570 additions and 9624 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/Makefile.arm-linux
+3 -3
View File
@@ -39,12 +39,12 @@ else
FLOAT_ABI ?= -mfloat-abi=softfp -mfpu=vfp
endif
ARCH_FLAGS ?= -mtune=cortex-a8 -march=armv7-a
ARCH_CFLAGS ?= -mtune=cortex-a8 -march=armv7-a
# add ARM specifc flags to CFLAGS, LDFLAGS
CFLAGS += $(FLOAT_ABI) $(ARCH_FLAGS)
CFLAGS += $(FLOAT_ABI) $(ARCH_CFLAGS)
LDFLAGS += $(FLOAT_ABI)
CXXFLAGS += $(FLOAT_ABI) $(ARCH_FLAGS)
CXXFLAGS += $(FLOAT_ABI) $(ARCH_CFLAGS)
# include the common linux Makefile (common CFLAGS, actual targets)
include $(PAPARAZZI_SRC)/conf/Makefile.linux
conf/boards/ardrone2_raw.makefile
+4
View File
@@ -42,6 +42,10 @@ $(TARGET).CFLAGS +=-DARDRONE2_RAW
# handle linux signals by hand
$(TARGET).CFLAGS += -DUSE_LINUX_SIGNAL
# Link static (Done for GLIBC)
$(TARGET).CFLAGS += -DLINUX_LINK_STATIC
$(TARGET).LDFLAGS += -static
# -----------------------------------------------------------------------
# default LED configuration
conf/boards/bebop.makefile
+4
View File
@@ -34,6 +34,10 @@ $(TARGET).CFLAGS += -DUSE_LINUX_SIGNAL
# Compile the video specific parts
$(TARGET).srcs += $(SRC_BOARD)/video.c
# Link static (Done for GLIBC)
$(TARGET).CFLAGS += -DLINUX_LINK_STATIC
$(TARGET).LDFLAGS += -static
# -----------------------------------------------------------------------
# default LED configuration
conf/messages.xml
+14 -23
View File
@@ -1972,31 +1972,22 @@
<field name="t" type="uint32"/>
</message>
<message name="OF_HOVER" id="228">
<field name="FPS" type="float"/>
<field name="dx" type="float"/>
<field name="dy" type="float"/>
<field name="dx_trans" type="float"/>
<field name="dy_trans" type="float"/>
<field name="diff_roll" type="float"/>
<field name="diff_pitch" type="float"/>
<field name="velx" type="float"/>
<field name="vely" type="float"/>
<field name="velx_Ned" type="float"/>
<field name="vely_Ned" type="float"/>
<field name="z_sonar" type="float"/>
<field name="count" type="int32"/>
</message>
<message name="VISION_STABILIZATION" id="229">
<field name="velx" type="float"/>
<field name="vely" type="float"/>
<field name="velx_i" type="float"/>
<field name="vely_i" type="float"/>
<field name="cmd_phi" type="int32"/>
<field name="cmd_theta" type="int32"/>
<message name="OPTIC_FLOW_EST" id="228">
<field name="fps" type="float"/>
<field name="corner_cnt" type="uint16"/>
<field name="tracked_cnt" type="uint16"/>
<field name="flow_x" type="int16" unit="subpixels"/>
<field name="flow_y" type="int16" unit="subpixels"/>
<field name="flow_der_x" type="int16" unit="subpixels"/>
<field name="flow_der_y" type="int16" unit="subpixels"/>
<field name="vel_x" type="float" unit="cm/s"/>
<field name="vel_y" type="float" unit="cm/s"/>
<field name="cmd_phi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
<field name="cmd_theta" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
</message>
<!--229 is free -->
<message name="AHRS_ARDRONE2" id="230">
<field name="state" type="uint32" />
<field name="control_state" type="uint32" values="DEFAULT|INIT|LANDING|FLYING|HOVERING|TEST|TRANS_TAKEOFF|TRANS_GOTOFIX|TRANS_LANDING|TRANS_LOOPING"/>
conf/modules/cv_opticflow.xml
+101 -44
View File
@@ -3,38 +3,78 @@
<module name="cv_opticflow" dir="computer_vision">
<doc>
<description>
Compute Optic Flow from Ardrone2 Bottom Camera
Computes Pitch- and rollrate corrected optic flow from downward looking
ARDrone2 camera looking at a textured floor.
Hovers the drone based on optical flow made for Linux video Devices.
Computes Pitch- and roll attide from downward looking camera looking at a textured floor.
- Sonar is required.
- Controller can hold position
</description>
<!-- Satbilization parameters and gains -->
<section name="VISION" prefix="VISION_">
<define name="HOVER" value="FALSE" description="TRUE/FALSE active or not"/>
<define name="PHI_PGAIN" value="500" description="optic flow pgain"/>
<define name="PHI_IGAIN" value="10" description="optic flow igain"/>
<define name="THETA_PGAIN" value="500" description="optic flow pgain"/>
<define name="THETA_IGAIN" value="10" description="optic flow igain"/>
<define name="DESIRED_VX" value="0" description="feedforward optic flow vx"/>
<define name="DESIRED_VY" value="0" description="feedforward optic flow vy"/>
<define name="PHI_PGAIN" value="400" description="Optic flow proportional gain on the roll velocity error"/>
<define name="PHI_IGAIN" value="20" description="Optic flow integrated gain on the summed roll velocity error"/>
<define name="THETA_PGAIN" value="400" description="Optic flow proportional gain on the pitch velocity error"/>
<define name="THETA_IGAIN" value="20" description="Optic flow integrated gain on the summed pitch velocity error"/>
<define name="DESIRED_VX" value="0" description="The desired velocity in the body frame x direction"/>
<define name="DESIRED_VY" value="0" description="The desired velocity in the body frame y direction"/>
</section>
<!-- Optical flow calculation parameters -->
<section name="OPTICFLOW" prefix="OPTICFLOW_">
<define name="AGL_ID" value="ABI_SENDER_ID" description="ABI sender id for AGL message (sonar measurement) (default: ABI_BROADCAST)"/>
<!-- Video device parameters -->
<define name="DEVICE" value="/dev/video2" description="The V4L2 camera device that is used for the calculations"/>
<define name="DEVICE_SIZE" value="320,240" description="The V4L2 camera device width and height"/>
<define name="DEVICE_BUFFERS" value="15" description="Amount of V4L2 video buffers"/>
<define name="SUBDEV" description="The V4L2 subdevice to initialize before the main device"/>
<!-- Camera parameters -->
<define name="FOV_W" value="0.89360857702" description="The field of view width of the bottom camera (Defaults are from an ARDrone 2)"/>
<define name="FOV_H" value="0.67020643276" description="The field of view height of the bottom camera (Defaults are from an ARDrone 2)"/>
<define name="FX" value="343.1211" description="Field in the x direction of the camera (Defaults are from an ARDrone 2)"/>
<define name="FY" value="348.5053" description="Field in the y direction of the camera (Defaults are from an ARDrone 2)"/>
<!-- Lucas Kanade optical flow calculation parameters -->
<define name="MAX_TRACK_CORNERS" value="25" description="The maximum amount of corners the Lucas Kanade algorithm is tracking between two frames"/>
<define name="WINDOW_SIZE" value="10" description="Window size used in Lucas Kanade algorithm"/>
<define name="SUBPIXEL_FACTOR" value="10" description="Amount of subpixels per pixel, used for more precise (subpixel) calculations of the flow"/>
<define name="MAX_ITERATIONS" value="10" description="Maximum number of iterations the Lucas Kanade algorithm should take"/>
<define name="THRESHOLD_VEC" value="2" description="TThreshold in subpixels when the iterations of Lucas Kanade should stop"/>
<!-- FAST9 corner detection parameters -->
<define name="FAST9_ADAPTIVE" value="TRUE" description="Whether we should use and adapative FAST9 crner detection threshold"/>
<define name="FAST9_THRESHOLD" value="20" description="FAST9 default threshold"/>
<define name="FAST9_MIN_DISTANCE" value="10" description="The amount of pixels between corners that should be detected"/>
</section>
<define name="DOWNLINK_VIDEO" value="FALSE" description="Also stream video: warning: this makes the optic flow slow: DEBUGGING only" />
<define name="OPTICFLOW_AGL_ID" value="ABI_SENDER_ID" description="ABI sender id for AGL message (sonar measurement) (default: ABI_BROADCAST)"/>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="Vision Loop">
<dl_setting var="activate_opticflow_hover" min="0" step="1" max="1" module="computer_vision/opticflow/hover_stabilization" shortname="hover" param="VISION_HOVER" values="FALSE|TRUE"/>
<dl_setting var="vision_phi_pgain" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
<dl_setting var="vision_phi_igain" min="0" step="1" max="1000" shortname="ki_v_phi" param="VISION_PHI_IGAIN"/>
<dl_setting var="vision_theta_pgain" min="0" step="1" max="10000" shortname="kp_v_theta" param="VISION_THETA_PGAIN"/>
<dl_setting var="vision_theta_igain" min="0" step="1" max="1000" shortname="ki_v_theta" param="VISION_THETA_IGAIN"/>
<dl_setting var="vision_desired_vx" min="-5" step="0.01" max="5" shortname="desired_vx" param="VISION_DESIRED_VX"/>
<dl_setting var="vision_desired_vy" min="-5" step="0.01" max="5" shortname="desired_vy" param="VISION_DESIRED_VY"/>
</dl_settings> </dl_settings>
<dl_settings NAME="Vision stabilization">
<!-- Satabilization loop parameters and gains -->
<dl_settings name="vision_stab">
<dl_setting var="opticflow_stab.phi_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
<dl_setting var="opticflow_stab.phi_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_phi" param="VISION_PHI_IGAIN"/>
<dl_setting var="opticflow_stab.theta_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_theta" param="VISION_THETA_PGAIN"/>
<dl_setting var="opticflow_stab.theta_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_theta" param="VISION_THETA_IGAIN"/>
<dl_setting var="opticflow_stab.desired_vx" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vx" param="VISION_DESIRED_VX"/>
<dl_setting var="opticflow_stab.desired_vy" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vy" param="VISION_DESIRED_VY"/>
</dl_settings>
<!-- Optical flow calculations parameters for Lucas Kanade and FAST9 -->
<dl_settings name="vision_calc">
<dl_setting var="opticflow.max_track_corners" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="max_trck_corners" param="OPTICFLOW_MAX_TRACK_CORNERS"/>
<dl_setting var="opticflow.window_size" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="window_size" param="OPTICFLOW_WINDOW_SIZE"/>
<dl_setting var="opticflow.subpixel_factor" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="subpixel_factor" param="OPTICFLOW_SUBPIXEL_FACTOR"/>
<dl_setting var="opticflow.max_iterations" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="max_iterations" param="OPTICFLOW_MAX_ITERATIONS"/>
<dl_setting var="opticflow.threshold_vec" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="threshold_vec" param="OPTICFLOW_THRESHOLD_VEC"/>
<dl_setting var="opticflow.fast9_adaptive" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="TRUE|FALSE" shortname="fast9_adaptive" param="OPTICFLOW_FAST9_ADAPTIVE"/>
<dl_setting var="opticflow.fast9_threshold" module="computer_vision/opticflow_module" min="0" step="1" max="255" shortname="fast9_threshold" param="OPTICFLOW_FAST9_THRESHOLD"/>
<dl_setting var="opticflow.fast9_min_distance" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="fast9_min_distance" param="OPTICFLOW_FAST9_MIN_DISTANCE"/>
</dl_settings>
</dl_settings>
</settings>
<header>
@@ -42,31 +82,48 @@
</header>
<init fun="opticflow_module_init()"/>
<periodic fun="opticflow_module_run()" start="opticflow_module_start()" stop="opticflow_module_stop()" autorun="TRUE"/>
<makefile target="ap">
<define name="ARDRONE_VIDEO_PORT" value="2002" />
<file name="opticflow_module.c"/>
<file name="opticflow_thread.c" dir="modules/computer_vision/opticflow"/>
<file name="visual_estimator.c" dir="modules/computer_vision/opticflow"/>
<file name="hover_stabilization.c" dir="modules/computer_vision/opticflow"/>
<file name="optic_flow_int.c" dir="modules/computer_vision/cv/opticflow"/>
<file name="fastRosten.c" dir="modules/computer_vision/cv/opticflow/fast9"/>
<file name="trig.c" dir="modules/computer_vision/cv"/>
<file name="framerate.c" dir="modules/computer_vision/cv"/>
<file name="jpeg.c" dir="modules/computer_vision/cv/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/cv/encoding"/>
<file name="socket.c" dir="modules/computer_vision/lib/udp"/>
<makefile target="ap">
<!-- Include the needed Computer Vision files -->
<define name="modules/computer_vision" type="include"/>
<file name="image.c" dir="modules/computer_vision/lib/vision"/>
<file name="jpeg.c" dir="modules/computer_vision/lib/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/lib/encoding"/>
<file name="v4l2.c" dir="modules/computer_vision/lib/v4l"/>
<define name="modules/computer_vision/cv" type="include"/>
<define name="modules/computer_vision/lib" type="include"/>
<define name="pthread" type="raw"/>
<define name="__USE_GNU"/>
<flag name="LDFLAGS" value="pthread"/>
<flag name="LDFLAGS" value="lrt"/>
<flag name="LDFLAGS" value="static"/>
<!-- The optical flow module (calculator+stabilization) -->
<file name="opticflow_module.c"/>
<file name="opticflow_calculator.c" dir="modules/computer_vision/opticflow"/>
<file name="stabilization_opticflow.c" dir="modules/computer_vision/opticflow"/>
<!-- Main vision calculations -->
<file name="fast_rosten.c" dir="modules/computer_vision/lib/vision"/>
<file name="lucas_kanade.c" dir="modules/computer_vision/lib/vision"/>
<raw>
VIEWVIDEO_DEV ?= UDP1
VIEWVIDEO_HOST ?= $(MODEM_HOST)
VIEWVIDEO_PORT_OUT ?= 5000
VIEWVIDEO_PORT_IN ?= 4999
VIEWVIDEO_BROADCAST ?= $(MODEM_BROADCAST)
VIEWVIDEO_DEV_LOWER = $(shell echo $(VIEWVIDEO_DEV) | tr A-Z a-z)
VIEWVID_G_CFLAGS = -DVIEWVIDEO_HOST=\"$(VIEWVIDEO_HOST)\" -DVIEWVIDEO_PORT_OUT=$(VIEWVIDEO_PORT_OUT)
VIEWVID_CFLAGS = -DUSE_$(VIEWVIDEO_DEV) -DVIEWVIDEO_DEV=$(VIEWVIDEO_DEV_LOWER)
VIEWVID_CFLAGS += -D$(VIEWVIDEO_DEV)_PORT_OUT=$(VIEWVIDEO_PORT_OUT) -D$(VIEWVIDEO_DEV)_PORT_IN=$(VIEWVIDEO_PORT_IN)
VIEWVID_CFLAGS += -D$(VIEWVIDEO_DEV)_BROADCAST=$(VIEWVIDEO_BROADCAST) -D$(VIEWVIDEO_DEV)_HOST=\"$(VIEWVIDEO_HOST)\"
ifeq ($(VIEWVIDEO_USE_NC),)
ap.CFLAGS += $(VIEWVID_G_CFLAGS) $(VIEWVID_CFLAGS)
else
ap.CFLAGS += $(VIEWVID_G_CFLAGS) -DVIEWVIDEO_USE_NC
endif
ap.CFLAGS += -DGUIDANCE_V_MODE_MODULE_SETTING=GUIDANCE_V_MODE_HOVER
ap.CFLAGS += -DGUIDANCE_H_MODE_MODULE_SETTING=GUIDANCE_H_MODE_MODULE
</raw>
</makefile>
<makefile target="nps">
<file name="viewvideo_nps.c"/>
</makefile>
conf/modules/video_rtp_stream.xml
+3 -2
View File
@@ -37,8 +37,9 @@
<!-- Include the needed Computer Vision files -->
<define name="modules/computer_vision" type="include"/>
<file name="jpeg.c" dir="modules/computer_vision/cv/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/cv/encoding"/>
<file name="image.c" dir="modules/computer_vision/lib/vision"/>
<file name="jpeg.c" dir="modules/computer_vision/lib/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/lib/encoding"/>
<file name="v4l2.c" dir="modules/computer_vision/lib/v4l"/>
<!-- Define the network connection to send images over -->
conf/telemetry/default_ardrone.xml
+1
View File
@@ -21,6 +21,7 @@
<message name="UART_ERRORS" period="3.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="OPTIC_FLOW_EST" period="0.25"/>
</mode>
<mode name="ppm">