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

[computer vision] Opticflow multiple cameras (#2618)

Browse Source 
* cv.c changes to support multiple cameras, compiles but untested
* opticflow module runs on both cameras
* modified airframe to showcase module running with two cameras
* updated OPTICFLOW ABI message to send camera id info
* now possible to subscribe to the ABI message from a specific camera
* Added camera_id field to conform with new telemetry message
* Updated pprzlink
This commit is contained in:
Matteo Barbera
2021-01-30 18:30:12 +01:00
committed by GitHub
parent 409ac5efcb
commit c6c33572db
32 changed files with 589 additions and 265 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/bebop_opticflow_indoor.xml
+15 -6
View File
@@ -29,23 +29,32 @@
<module name="bebop_cam"/>
<module name="cv_opticflow">
<define name="OPTICFLOW_CAMERA" value="bottom_camera"/>
<define name="MAX_HORIZON" value="10"/>
<define name="OPTICFLOW_MAX_TRACK_CORNERS" value="10"/>
<define name="OPTICFLOW_SUBPIXEL_FACTOR" value = "200"/> <!-- due to high framerate of the bebop bottom camera, a high subpixel factor is needed-->
<define name="OPTICFLOW_CORNER_METHOD" value="0"/>
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
<define name="MAX_TRACK_CORNERS" value="10"/>
<define name="OPTICFLOW_MAX_TRACK_CORNERS" value="10"/>
<define name="OPTICFLOW_FEATURE_MANAGEMENT" value="0"/>
<define name="OPTICFLOW_FPS" value="5"/>
<define name="OPTICFLOW_SHOW_FLOW" value="1"/>
<define name="OPTICFLOW_CAMERA2" value="front_camera"/>
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2" value="0.8"/> <!--Obtained from a linefit-->
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2" value="0.85"/> <!--Obtained from a linefit-->
<define name="OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2" value="10"/>
<define name="OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2" value="0"/>
<define name="OPTICFLOW_FPS_CAMERA2" value="5"/>
<define name="OPTICFLOW_SHOW_FLOW_CAMERA2" value="1"/>
</module>
<module name="video_capture">
<define name="VIDEO_CAPTURE_CAMERA" value="bottom_camera"/>
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
<define name="VIDEO_CAPTURE_FPS" value="5"/>
</module>
<module name="video_rtp_stream">
<define name="VIEWVIDEO_CAMERA" value="bottom_camera"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
<define name="VIEWVIDEO_CAMERA2" value="front_camera"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="2"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="40"/>
</module>
</firmware>
conf/modules/cv_opticflow.xml
+110 -30
View File
@@ -6,6 +6,7 @@
Compute optical flow.
Made for Linux video Devices.
Computes x and y velocity using optical flow and distance to ground (using sonar).
Cannot define CAMERA2 without first defining CAMERA.
</description>
<!-- Optical flow calculation parameters -->
@@ -39,7 +40,7 @@
<!-- 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="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"/>
<define name="THRESHOLD_VEC" value="2" description="Threshold in subpixels when the iterations of Lucas Kanade should stop"/>
<define name="CORNER_METHOD" value="1" description="Method used to look for corners, exhaustive FAST (0) or ACT-FAST (1)."/>
@@ -57,51 +58,130 @@
<define name="ACTFAST_MIN_GRADIENT" value="10" description="Threshold that decides when there is sufficient texture for edge following"/>
<define name="ACTFAST_GRADIENT_METHOD" value="1" description="Whether to use a simple (0) or Sobel (1) filter"/>
<!-- =========== CAMERA 2 SETTINGS ============ -->
<!-- Video device parameters for camera 2 -->
<define name="CAMERA2" value="bottom_camera|front_camera" description="The V4L2 camera device that is used for the calculations"/>
<!-- Camera 2 parameters -->
<define name="DEROTATION_CORRECTION_FACTOR_X_CAMERA2" value="1.0" description="Correction factor for derotation (in x direction), estimated from a fit between the gyro's rates and the resulting flow (caused by the camera not being exactly in the middle (Defaults are from an ARDrone 2)"/>
<define name="DEROTATION_CORRECTION_FACTOR_Y_CAMERA2" value="1.0" description="Correction factor for derotation (in y direction), estimated from a fit between the gyro's rates and the resulting flow (caused by the camera not being exactly in the middle (Defaults are from an ARDrone 2)"/>
<configure name="BODY_TO_CAM_PHI_CAMERA2" value="0" description="Rotation from body frame to camera frame around x axis"/>
<configure name="BODY_TO_CAM_THETA_CAMERA2" value="0" description="Rotation from body frame to camera frame around y axis"/>
<configure name="BODY_TO_CAM_PSI_CAMERA2" value="0" description="Rotation from body frame to camera frame around z axis"/>
<!-- General optical flow calculation parameters for camera 2-->
<define name="METHOD_CAMERA2" value="0" description="Method used to calculate optical flow"/>
<define name="WINDOW_SIZE_CAMERA2" value="10" description="Window size used for block matching (pixels)"/>
<define name="SEARCH_DISTANCE_CAMERA2" value="10" description="Maximum search distance for blockmatching (pixels)"/>
<define name="SUBPIXEL_FACTOR_CAMERA2" value="10" description="Amount of subpixels per pixel, used for more precise (subpixel) calculations of the flow"/>
<define name="RESOLUTION_FACTOR_CAMERA2" value="1000" description="The resolution factor needed to calculate the divergence without floats"/>
<define name="DEROTATION_CAMERA2" value="1" description="Derotation either turned on or off (depended on gyroscope measurements)"/>
<define name="MEDIAN_FILTER_CAMERA2" value="0" description="A median filter on the resulting velocities to be turned on or off (last 5 measurements)"/>
<define name="FEATURE_MANAGEMENT_CAMERA2" value="1" description="Whether to keep already tracked corners in memory for the next frame or re-detect new ones every time"/>
<define name="FPS_CAMERA2" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
<define name="TRACK_BACK_CAMERA2" value="TRUE" description="Whether flow vectors are tracked back to the previous image and only kept if they end up close to the original point."/>
<define name="SHOW_FLOW_CAMERA2" value="TRUE" description="Whether to draw the flow vectors in the image."/>
<!-- Lucas Kanade optical flow calculation parameters -->
<define name="MAX_TRACK_CORNERS_CAMERA2" value="25" description="The maximum amount of corners the Lucas Kanade algorithm is tracking between two frames"/>
<define name="MAX_ITERATIONS_CAMERA2" value="10" description="Maximum number of iterations the Lucas Kanade algorithm should take"/>
<define name="THRESHOLD_VEC_CAMERA2" value="2" description="Threshold in subpixels when the iterations of Lucas Kanade should stop"/>
<define name="CORNER_METHOD_CAMERA2" value="1" description="Method used to look for corners, exhaustive FAST (0) or ACT-FAST (1)."/>
<!-- FAST9 corner detection parameters -->
<define name="FAST9_ADAPTIVE_CAMERA2" value="TRUE" description="Whether we should use and adapative FAST9 crner detection threshold"/>
<define name="FAST9_THRESHOLD_CAMERA2" value="20" description="FAST9 default threshold"/>
<define name="FAST9_MIN_DISTANCE_CAMERA2" value="10" description="The amount of pixels between corners that should be detected"/>
<define name="FAST9_PADDING_CAMERA2" value="20" description="The outer border in which no corners will be searched"/>
<define name="FAST9_REGION_DETECT_CAMERA2" value="1" description="Whether to detect fast9 corners in regions of interest or the whole image (only works with feature management)"/>
<define name="FAST9_NUM_REGIONS_CAMERA2" value="9" description="The number of regions of interest to split the image into"/>
<!-- ACT-FAST parameters -->
<define name="ACTFAST_LONG_STEP_CAMERA2" value="10" description="Step size to take when there is no texture"/>
<define name="ACTFAST_SHORT_STEP_CAMERA2" value="2" description="Step size to take when there is an edge to be followed"/>
<define name="ACTFAST_MIN_GRADIENT_CAMERA2" value="10" description="Threshold that decides when there is sufficient texture for edge following"/>
<define name="ACTFAST_GRADIENT_METHOD_CAMERA2" value="1" description="Whether to use a simple (0) or Sobel (1) filter"/>
</section>
</doc>
<settings>
<dl_settings NAME="Vision stabilization">
<dl_settings name="Vision stabilization">
<!-- Optical flow calculations parameters -->
<dl_settings name="vision_calc">
<dl_setting var="opticflow.method" min="0" step="1" max="1" module="computer_vision/opticflow_module" shortname="method" values="LK_Fast9|EdgeFlow" param="METHOD"/>
<dl_setting var="opticflow.corner_method" min="0" step="1" max="1" module="computer_vision/opticflow_module" shortname="corner_method" values="exhaustive-FAST|ACT-FAST" param="CORNER_METHOD"/>
<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.feature_management" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="feature_management" param="OPTICFLOW_FEATURE_MANAGEMENT"/>
<dl_setting var="opticflow.track_back" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="FALSE|TRUE" shortname="track_back" param="OPTICFLOW_TRACK_BACK"/>
<dl_setting var="opticflow.show_flow" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="FALSE|TRUE" shortname="show_flow" param="OPTICFLOW_SHOW_FLOW"/>
<dl_settings name="vision_calc camera1">
<dl_setting var="opticflow[0].method" min="0" step="1" max="1" module="computer_vision/opticflow_module" shortname="method" values="LK_Fast9|EdgeFlow" param="METHOD"/>
<dl_setting var="opticflow[0].corner_method" min="0" step="1" max="1" module="computer_vision/opticflow_module" shortname="corner_method" values="exhaustive-FAST|ACT-FAST" param="CORNER_METHOD"/>
<dl_setting var="opticflow[0].window_size" module="computer_vision/opticflow_module" min="0" step="1" max="20" shortname="window_size" param="OPTICFLOW_WINDOW_SIZE"/>
<dl_setting var="opticflow[0].search_distance" module="computer_vision/opticflow_module" min="0" step="1" max="50" shortname="search_distance" param="SEARCH_DISTANCE"/>
<dl_setting var="opticflow[0].subpixel_factor" module="computer_vision/opticflow_module" min="0" step="10" max="1000" shortname="subpixel_factor" param="OPTICFLOW_SUBPIXEL_FACTOR"/>
<dl_setting var="opticflow[0].resolution_factor" module="computer_vision/opticflow_module" min="10" step="10" max="10000" shortname="resolution_factor" param="OPTICFLOW_RESOLUTION_FACTOR"/>
<dl_setting var="opticflow[0].derotation" min="0" step="1" max="1" module="computer_vision/opticflow_module" values="OFF|ON" shortname="derotation" param="OPTICFLOW_DEROTATION"/>
<dl_setting var="opticflow[0].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[0].feature_management" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="feature_management" param="OPTICFLOW_FEATURE_MANAGEMENT"/>
<dl_setting var="opticflow[0].track_back" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="FALSE|TRUE" shortname="track_back" param="OPTICFLOW_TRACK_BACK"/>
<dl_setting var="opticflow[0].show_flow" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="FALSE|TRUE" shortname="show_flow" param="OPTICFLOW_SHOW_FLOW"/>
<!-- Specifically for Lucas Kanade and FAST9 -->
<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.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="FALSE|TRUE" 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_setting var="opticflow.fast9_padding" module="computer_vision/opticflow_module" min="0" step="1" max="50" shortname="fast9_padding" param="OPTICFLOW_FAST9_PADDING"/>
<dl_setting var="opticflow.fast9_region_detect" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="fast9_region_detect" param="OPTICFLOW_FAST9_REGION_DETECT"/>
<dl_setting var="opticflow.fast9_num_regions" module="computer_vision/opticflow_module" min="1" step="1" max="25" shortname="fast9_num_regions" param="OPTICFLOW_FAST9_NUM_REGIONS"/>
<dl_setting var="opticflow[0].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[0].max_iterations" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="max_iterations" param="OPTICFLOW_MAX_ITERATIONS"/>
<dl_setting var="opticflow[0].threshold_vec" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="threshold_vec" param="OPTICFLOW_THRESHOLD_VEC"/>
<dl_setting var="opticflow[0].fast9_adaptive" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="FALSE|TRUE" shortname="fast9_adaptive" param="OPTICFLOW_FAST9_ADAPTIVE"/>
<dl_setting var="opticflow[0].fast9_threshold" module="computer_vision/opticflow_module" min="0" step="1" max="255" shortname="fast9_threshold" param="OPTICFLOW_FAST9_THRESHOLD"/>
<dl_setting var="opticflow[0].fast9_min_distance" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="fast9_min_distance" param="OPTICFLOW_FAST9_MIN_DISTANCE"/>
<dl_setting var="opticflow[0].fast9_padding" module="computer_vision/opticflow_module" min="0" step="1" max="50" shortname="fast9_padding" param="OPTICFLOW_FAST9_PADDING"/>
<dl_setting var="opticflow[0].fast9_region_detect" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="fast9_region_detect" param="OPTICFLOW_FAST9_REGION_DETECT"/>
<dl_setting var="opticflow[0].fast9_num_regions" module="computer_vision/opticflow_module" min="1" step="1" max="25" shortname="fast9_num_regions" param="OPTICFLOW_FAST9_NUM_REGIONS"/>
<!-- ACT-FAST settings -->
<dl_setting var="opticflow.actfast_long_step" module="computer_vision/opticflow_module" min="1" step="1" max="100" shortname="actfast_long_step" param="OPTICFLOW_ACTFAST_LONG_STEP"/>
<dl_setting var="opticflow.actfast_short_step" module="computer_vision/opticflow_module" min="1" step="1" max="10" shortname="actfast_short_step" param="OPTICFLOW_ACTFAST_SHORT_STEP"/>
<dl_setting var="opticflow.actfast_min_gradient" module="computer_vision/opticflow_module" min="1" step="1" max="255" shortname="actfast_min_gradient" param="OPTICFLOW_ACTFAST_MIN_GRADIENT"/>
<dl_setting var="opticflow.actfast_gradient_method" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="SIMPLE|SOBEL" shortname="actfast_gradient_method" param="OPTICFLOW_ACTFAST_GRADIENT_METHOD"/>
<dl_setting var="opticflow[0].actfast_long_step" module="computer_vision/opticflow_module" min="1" step="1" max="100" shortname="actfast_long_step" param="OPTICFLOW_ACTFAST_LONG_STEP"/>
<dl_setting var="opticflow[0].actfast_short_step" module="computer_vision/opticflow_module" min="1" step="1" max="10" shortname="actfast_short_step" param="OPTICFLOW_ACTFAST_SHORT_STEP"/>
<dl_setting var="opticflow[0].actfast_min_gradient" module="computer_vision/opticflow_module" min="1" step="1" max="255" shortname="actfast_min_gradient" param="OPTICFLOW_ACTFAST_MIN_GRADIENT"/>
<dl_setting var="opticflow[0].actfast_gradient_method" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="SIMPLE|SOBEL" shortname="actfast_gradient_method" param="OPTICFLOW_ACTFAST_GRADIENT_METHOD"/>
<!-- Changes pyramid level of lucas kanade optical flow. -->
<dl_setting var="opticflow.pyramid_level" module="computer_vision/opticflow_module" min="0" step="1" max="10" shortname="pyramid_level" param="OPTICFLOW_PYRAMID_LEVEL"/>
<dl_setting var="opticflow[0].pyramid_level" module="computer_vision/opticflow_module" min="0" step="1" max="10" shortname="pyramid_level" param="OPTICFLOW_PYRAMID_LEVEL"/>
</dl_settings>
<dl_settings name="vision_calc camera2">
<dl_setting var="opticflow[1].method" min="0" step="1" max="1" module="computer_vision/opticflow_module" shortname="method" values="LK_Fast9|EdgeFlow" param="METHOD_CAMERA2"/>
<dl_setting var="opticflow[1].corner_method" min="0" step="1" max="1" module="computer_vision/opticflow_module" shortname="corner_method" values="exhaustive-FAST|ACT-FAST" param="CORNER_METHOD_CAMERA2"/>
<dl_setting var="opticflow[1].window_size" module="computer_vision/opticflow_module" min="0" step="1" max="20" shortname="window_size" param="OPTICFLOW_WINDOW_SIZE_CAMERA2"/>
<dl_setting var="opticflow[1].search_distance" module="computer_vision/opticflow_module" min="0" step="1" max="50" shortname="search_distance" param="SEARCH_DISTANCE_CAMERA2"/>
<dl_setting var="opticflow[1].subpixel_factor" module="computer_vision/opticflow_module" min="0" step="10" max="1000" shortname="subpixel_factor" param="OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2"/>
<dl_setting var="opticflow[1].resolution_factor" module="computer_vision/opticflow_module" min="10" step="10" max="10000" shortname="resolution_factor" param="OPTICFLOW_RESOLUTION_FACTOR_CAMERA2"/>
<dl_setting var="opticflow[1].derotation" min="0" step="1" max="1" module="computer_vision/opticflow_module" values="OFF|ON" shortname="derotation" param="OPTICFLOW_DEROTATION_CAMERA2"/>
<dl_setting var="opticflow[1].median_filter" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="median_filter" param="OPTICFLOW_MEDIAN_FILTER_CAMERA2"/>
<dl_setting var="opticflow[1].feature_management" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="feature_management" param="OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2"/>
<dl_setting var="opticflow[1].track_back" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="FALSE|TRUE" shortname="track_back" param="OPTICFLOW_TRACK_BACK_CAMERA2"/>
<dl_setting var="opticflow[1].show_flow" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="FALSE|TRUE" shortname="show_flow" param="OPTICFLOW_SHOW_FLOW_CAMERA2"/>
<!-- Specifically for Lucas Kanade and FAST9 -->
<dl_setting var="opticflow[1].max_track_corners" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="max_trck_corners" param="OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2"/>
<dl_setting var="opticflow[1].max_iterations" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="max_iterations" param="OPTICFLOW_MAX_ITERATIONS_CAMERA2"/>
<dl_setting var="opticflow[1].threshold_vec" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="threshold_vec" param="OPTICFLOW_THRESHOLD_VEC_CAMERA2"/>
<dl_setting var="opticflow[1].fast9_adaptive" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="FALSE|TRUE" shortname="fast9_adaptive" param="OPTICFLOW_FAST9_ADAPTIVE_CAMERA2"/>
<dl_setting var="opticflow[1].fast9_threshold" module="computer_vision/opticflow_module" min="0" step="1" max="255" shortname="fast9_threshold" param="OPTICFLOW_FAST9_THRESHOLD_CAMERA2"/>
<dl_setting var="opticflow[1].fast9_min_distance" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="fast9_min_distance" param="OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2"/>
<dl_setting var="opticflow[1].fast9_padding" module="computer_vision/opticflow_module" min="0" step="1" max="50" shortname="fast9_padding" param="OPTICFLOW_FAST9_PADDING_CAMERA2"/>
<dl_setting var="opticflow[1].fast9_region_detect" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="OFF|ON" shortname="fast9_region_detect" param="OPTICFLOW_FAST9_REGION_DETECT_CAMERA2"/>
<dl_setting var="opticflow[1].fast9_num_regions" module="computer_vision/opticflow_module" min="1" step="1" max="25" shortname="fast9_num_regions" param="OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2"/>
<!-- ACT-FAST settings -->
<dl_setting var="opticflow[1].actfast_long_step" module="computer_vision/opticflow_module" min="1" step="1" max="100" shortname="actfast_long_step" param="OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2"/>
<dl_setting var="opticflow[1].actfast_short_step" module="computer_vision/opticflow_module" min="1" step="1" max="10" shortname="actfast_short_step" param="OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2"/>
<dl_setting var="opticflow[1].actfast_min_gradient" module="computer_vision/opticflow_module" min="1" step="1" max="255" shortname="actfast_min_gradient" param="OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2"/>
<dl_setting var="opticflow[1].actfast_gradient_method" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="SIMPLE|SOBEL" shortname="actfast_gradient_method" param="OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2"/>
<!-- Changes pyramid level of lucas kanade optical flow. -->
<dl_setting var="opticflow[1].pyramid_level" module="computer_vision/opticflow_module" min="0" step="1" max="10" shortname="pyramid_level" param="OPTICFLOW_PYRAMID_LEVEL_CAMERA2"/>
</dl_settings>
</dl_settings>
</settings>
<depends>video_thread</depends>
<autoload name="agl_dist"/>
<autoload name="video_thread"/>
<autoload name="pose_history"/>
<header>
conf/userconf/tudelft/conf.xml
+1 -1
View File
@@ -530,7 +530,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotorcraft_cyberzoo_no_gps.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/video_rtp_stream.xml modules/video_capture.xml modules/cv_opticflow.xml modules/bebop_cam.xml modules/ins_hff_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml modules/video_rtp_stream.xml"
gui_color="blue"
release="398071ca8f7c8dc24becbe76bc5628b6ea604248"
/>
sw/airborne/modules/computer_vision/bebop_ae_awb.c
+2 -2
View File
@@ -146,7 +146,7 @@ PRINT_CONFIG_VAR(BEBOP_AWB_MIN_GREY_PIXELS)
struct ae_setting_t ae_set;
struct awb_setting_t awb_set;
static struct image_t *update_ae_awb(struct image_t *img)
static struct image_t *update_ae_awb(struct image_t *img, uint8_t camera_id)
{
static struct isp_yuv_stats_t yuv_stats;
@@ -352,5 +352,5 @@ void bebop_ae_awb_init(void)
awb_set.gain_scheduling_tolerance = BEBOP_AWB_GAIN_SCHEDULING_TOLERANCE;
awb_set.gain_scheduling_step = BEBOP_AWB_GAIN_SCHEDULING_STEP;
cv_add_to_device_async(&front_camera, update_ae_awb, BEBOP_AE_AWB_NICE, 0);
cv_add_to_device_async(&front_camera, update_ae_awb, BEBOP_AE_AWB_NICE, 0, 0);
}
sw/airborne/modules/computer_vision/colorfilter.c
+2 -2
View File
@@ -56,7 +56,7 @@ volatile int color_count = 0;
#include "subsystems/abi.h"
// Function
static struct image_t *colorfilter_func(struct image_t *img)
static struct image_t *colorfilter_func(struct image_t *img, uint8_t camera_id)
{
// Filter
color_count = image_yuv422_colorfilt(img, img,
@@ -81,5 +81,5 @@ static struct image_t *colorfilter_func(struct image_t *img)
void colorfilter_init(void)
{
cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func, COLORFILTER_FPS);
cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func, COLORFILTER_FPS, 0);
}
sw/airborne/modules/computer_vision/cv.c
+6 -5
View File
@@ -43,7 +43,7 @@ static inline uint32_t timeval_diff(struct timeval *A, struct timeval *B)
}
struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps)
struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id)
{
// Create a new video listener
struct video_listener *new_listener = malloc(sizeof(struct video_listener));
@@ -54,6 +54,7 @@ struct video_listener *cv_add_to_device(struct video_config_t *device, cv_functi
new_listener->next = NULL;
new_listener->async = NULL;
new_listener->maximum_fps = fps;
new_listener->id = id;
// Initialise the device that we want our function to use
add_video_device(device);
@@ -80,10 +81,10 @@ struct video_listener *cv_add_to_device(struct video_config_t *device, cv_functi
struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level,
uint16_t fps)
uint16_t fps, uint8_t id)
{
// Create a normal listener
struct video_listener *listener = cv_add_to_device(device, func, fps);
struct video_listener *listener = cv_add_to_device(device, func, fps, id);
// Add asynchronous structure to override default synchronous behavior
listener->async = malloc(sizeof(struct cv_async));
@@ -163,7 +164,7 @@ void *cv_async_thread(void *args)
}
// Execute vision function from this thread
listener->func(&async->img_copy);
listener->func(&async->img_copy, listener->id);
// Mark image as processed
async->img_processed = true;
@@ -198,7 +199,7 @@ void cv_run_device(struct video_config_t *device, struct image_t *img)
}
} else {
// Execute the cvFunction and catch result
result = listener->func(img);
result = listener->func(img, listener->id);
// If result gives an image pointer, use it in the next stage
if (result != NULL) {
sw/airborne/modules/computer_vision/cv.h
+4 -3
View File
@@ -36,7 +36,7 @@
#include BOARD_CONFIG
typedef struct image_t *(*cv_function)(struct image_t *img);
typedef struct image_t *(*cv_function)(struct image_t *img, uint8_t camera_id);
struct cv_async {
pthread_t thread_id;
@@ -53,6 +53,7 @@ struct video_listener {
struct cv_async *async;
struct timeval ts;
cv_function func;
uint8_t id;
// Can be set by user
uint16_t maximum_fps;
@@ -61,9 +62,9 @@ struct video_listener {
extern bool add_video_device(struct video_config_t *device);
extern struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps);
extern struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id);
extern struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level,
uint16_t fps);
uint16_t fps, uint8_t id);
extern void cv_run_device(struct video_config_t *device, struct image_t *img);
sw/airborne/modules/computer_vision/cv_blob_locator.c
+9 -9
View File
@@ -58,8 +58,8 @@ volatile bool marker_enabled = false;
volatile bool window_enabled = false;
// Computer vision thread
struct image_t *cv_marker_func(struct image_t *img);
struct image_t *cv_marker_func(struct image_t *img)
struct image_t *cv_marker_func(struct image_t *img, uint8_t camera_id);
struct image_t *cv_marker_func(struct image_t *img, uint8_t camera_id)
{
if (!marker_enabled) {
@@ -80,8 +80,8 @@ struct image_t *cv_marker_func(struct image_t *img)
// Computer vision thread
struct image_t *cv_window_func(struct image_t *img);
struct image_t *cv_window_func(struct image_t *img)
struct image_t *cv_window_func(struct image_t *img, uint8_t camera_id);
struct image_t *cv_window_func(struct image_t *img, uint8_t camera_id)
{
if (!window_enabled) {
@@ -127,8 +127,8 @@ struct image_t *cv_window_func(struct image_t *img)
}
struct image_t *cv_blob_locator_func(struct image_t *img);
struct image_t *cv_blob_locator_func(struct image_t *img)
struct image_t *cv_blob_locator_func(struct image_t *img, uint8_t camera_id);
struct image_t *cv_blob_locator_func(struct image_t *img, uint8_t camera_id)
{
if (!blob_enabled) {
@@ -249,9 +249,9 @@ void cv_blob_locator_init(void)
georeference_init();
cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func, BLOB_LOCATOR_FPS);
cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func, BLOB_LOCATOR_FPS);
cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func, BLOB_LOCATOR_FPS);
cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func, BLOB_LOCATOR_FPS, 0);
cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func, BLOB_LOCATOR_FPS, 0);
cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func, BLOB_LOCATOR_FPS, 0);
}
void cv_blob_locator_periodic(void)
sw/airborne/modules/computer_vision/cv_detect_color_object.c
+6 -6
View File
@@ -139,14 +139,14 @@ static struct image_t *object_detector(struct image_t *img, uint8_t filter)
return img;
}
struct image_t *object_detector1(struct image_t *img);
struct image_t *object_detector1(struct image_t *img)
struct image_t *object_detector1(struct image_t *img, uint8_t camera_id);
struct image_t *object_detector1(struct image_t *img, uint8_t camera_id)
{
return object_detector(img, 1);
}
struct image_t *object_detector2(struct image_t *img);
struct image_t *object_detector2(struct image_t *img)
struct image_t *object_detector2(struct image_t *img, uint8_t camera_id);
struct image_t *object_detector2(struct image_t *img, uint8_t camera_id)
{
return object_detector(img, 2);
}
@@ -168,7 +168,7 @@ void color_object_detector_init(void)
cod_draw1 = COLOR_OBJECT_DETECTOR_DRAW1;
#endif
cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA1, object_detector1, COLOR_OBJECT_DETECTOR_FPS1);
cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA1, object_detector1, COLOR_OBJECT_DETECTOR_FPS1, 0);
#endif
#ifdef COLOR_OBJECT_DETECTOR_CAMERA2
@@ -184,7 +184,7 @@ void color_object_detector_init(void)
cod_draw2 = COLOR_OBJECT_DETECTOR_DRAW2;
#endif
cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA2, object_detector2, COLOR_OBJECT_DETECTOR_FPS2);
cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA2, object_detector2, COLOR_OBJECT_DETECTOR_FPS2, 1);
#endif
}
sw/airborne/modules/computer_vision/cv_opencvdemo.c
+3 -3
View File
@@ -33,8 +33,8 @@
PRINT_CONFIG_VAR(OPENCVDEMO_FPS)
// Function
struct image_t *opencv_func(struct image_t *img);
struct image_t *opencv_func(struct image_t *img)
struct image_t *opencv_func(struct image_t *img, uint8_t camera_id);
struct image_t *opencv_func(struct image_t *img, uint8_t camera_id)
{
if (img->type == IMAGE_YUV422) {
@@ -49,6 +49,6 @@ struct image_t *opencv_func(struct image_t *img)
void opencvdemo_init(void)
{
cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func, OPENCVDEMO_FPS);
cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func, OPENCVDEMO_FPS, 0);
}
sw/airborne/modules/computer_vision/detect_contour.c
+3 -3
View File
@@ -33,8 +33,8 @@
PRINT_CONFIG_VAR(DETECT_CONTOUR_FPS)
// Function
struct image_t *contour_func(struct image_t *img);
struct image_t *contour_func(struct image_t *img)
struct image_t *contour_func(struct image_t *img, uint8_t camera_id);
struct image_t *contour_func(struct image_t *img, uint8_t camera_id)
{
if (img->type == IMAGE_YUV422) {
@@ -46,7 +46,7 @@ struct image_t *contour_func(struct image_t *img)
void detect_contour_init(void)
{
cv_add_to_device(&DETECT_CONTOUR_CAMERA, contour_func, DETECT_CONTOUR_FPS);
cv_add_to_device(&DETECT_CONTOUR_CAMERA, contour_func, DETECT_CONTOUR_FPS, 0);
// in the mavlab, bright
cont_thres.lower_y = 16; cont_thres.lower_u = 135; cont_thres.lower_v = 80;
cont_thres.upper_y = 100; cont_thres.upper_u = 175; cont_thres.upper_v = 165;
sw/airborne/modules/computer_vision/detect_gate.c
+2 -2
View File
@@ -131,7 +131,7 @@ struct vision_relative_position_struct {
// Function
static struct image_t *detect_gate_func(struct image_t *img)
static struct image_t *detect_gate_func(struct image_t *img, uint8_t camera_id)
{
// detect the gate and draw it in the image:
if (just_filtering) {
@@ -354,7 +354,7 @@ void detect_gate_init(void)
detect_gate_y = 0;
detect_gate_z = 0;
cv_add_to_device(&DETECT_GATE_CAMERA, detect_gate_func, DETECT_GATE_FPS);
cv_add_to_device(&DETECT_GATE_CAMERA, detect_gate_func, DETECT_GATE_FPS, 0);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VISION_POSITION_ESTIMATE, send_detect_gate_visual_position);
}
sw/airborne/modules/computer_vision/detect_window.c
+2 -2
View File
@@ -39,13 +39,13 @@ PRINT_CONFIG_VAR(DETECT_WINDOW_FPS)
void detect_window_init(void)
{
#ifdef DETECT_WINDOW_CAMERA
cv_add_to_device(&DETECT_WINDOW_CAMERA, detect_window, DETECT_WINDOW_FPS);
cv_add_to_device(&DETECT_WINDOW_CAMERA, detect_window, DETECT_WINDOW_FPS, 0);
#else
#warning "DETECT_WINDOW_CAMERA not defined, CV callback not added to device"
#endif
}
struct image_t *detect_window(struct image_t *img)
struct image_t *detect_window(struct image_t *img, uint8_t camera_id)
{
uint16_t coordinate[2];
sw/airborne/modules/computer_vision/detect_window.h
+1 -1
View File
@@ -35,7 +35,7 @@
#include "inttypes.h"
extern void detect_window_init(void);
extern struct image_t* detect_window(struct image_t *img);
extern struct image_t* detect_window(struct image_t *img, uint8_t camera_id);
uint16_t detect_window_sizes(uint8_t *in, uint32_t image_width, uint32_t image_height, uint16_t *coordinate,
uint32_t *integral_image, uint8_t MODE);
sw/airborne/modules/computer_vision/lib/vision/act_fast.c
+36 -30
View File
@@ -22,16 +22,21 @@ All rights reserved.
* de Croon, G.C.H.E. "ACT-FAST: efficiently finding corners by actively exploring images.", in submission.
*
*/
#include "fast_rosten.h"
#include "act_fast.h"
#include "math.h"
#include "image.h"
#include "../../opticflow/opticflow_calculator.h"
// ACT-FAST agents arrays
// equal to the maximal number of corners defined by fast9_rsize in opticflow_calculator.c
// TODO Currently hardcoded to two cameras
#define MAX_AGENTS FAST9_MAX_CORNERS
struct agent_t agents[MAX_AGENTS];
#ifndef OPTICFLOW_CAMERA2
struct agent_t agents[1][MAX_AGENTS];
#else
struct agent_t agents[2][MAX_AGENTS];
#endif
/**
* Do an ACT-FAST corner detection.
@@ -46,8 +51,9 @@ struct agent_t agents[MAX_AGENTS];
* @param[in] min_gradient The minimum gradient, in order to determine when to take a long or short step
* @param[in] gradient_method: 0 = simple {-1, 0, 1}, 1 = Sobel {-1,0,1,-2,0,2,-1,0,1}
*/
void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners,
uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient, int gradient_method)
void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners,
uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient,
int gradient_method, int camera_id)
{
/*
@@ -56,7 +62,7 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
* 2) loop over the agents, moving and checking for corners
*/
// ensure that n_agents is never bigger than MAX_AGENTS
// ensure that n_agents is never bigger than max_agents
n_agents = (n_agents < MAX_AGENTS) ? n_agents : MAX_AGENTS;
// min_gradient should be bigger than 0:
min_gradient = (min_gradient == 0) ? 1 : min_gradient;
@@ -84,7 +90,7 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
py = ((float)(rand() % 10000) + 1) / 10000.0f;
pnorm = sqrtf(px * px + py * py);
struct agent_t ag = { (border + c * step_size_x), (border + r * step_size_y), 1, px / pnorm, py / pnorm};
agents[a] = ag;
agents[camera_id][a] = ag;
a++;
if (a == n_agents) { break; }
}
@@ -105,41 +111,41 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
// loop over the agents
for (a = 0; a < n_agents; a++) {
// only do something if the agent is active:
if (agents[a].active) {
if (agents[camera_id][a].active) {
// check if this position is a corner:
uint16_t x = (uint16_t) agents[a].x;
uint16_t y = (uint16_t) agents[a].y;
uint16_t x = (uint16_t) agents[camera_id][a].x;
uint16_t y = (uint16_t) agents[camera_id][a].y;
if (fast9_detect_pixel(img, fast_threshold, x, y)) {
// we arrived at a corner, yeah!!!
agents[a].active = 0;
agents[camera_id][a].active = 0;
continue;
} else {
// make a step:
struct point_t loc = { .x = agents[a].x, .y = agents[a].y};
struct point_t loc = { .x = agents[camera_id][a].x, .y = agents[camera_id][a].y};
image_gradient_pixel(img, &loc, gradient_method, &dx, &dy);
int gradient = (abs(dx) + abs(dy)) / 2;
if (abs(gradient) >= min_gradient) {
// determine the angle and make a step in that direction:
float norm_factor = sqrtf((float)(dx * dx + dy * dy));
agents[a].x += (dy / norm_factor) * short_step;
agents[a].y += (dx / norm_factor) * short_step;
agents[camera_id][a].x += (dy / norm_factor) * short_step;
agents[camera_id][a].y += (dx / norm_factor) * short_step;
} else {
// make a step in the preferred direction:
agents[a].x += agents[a].preferred_dir_x * long_step;
agents[a].y += agents[a].preferred_dir_y * long_step;
agents[camera_id][a].x += agents[camera_id][a].preferred_dir_x * long_step;
agents[camera_id][a].y += agents[camera_id][a].preferred_dir_y * long_step;
}
}
// let the agent move over the image in a toroid world:
if (agents[a].x > img->w - border) {
agents[a].x = border;
} else if (agents[a].x < border) {
agents[a].x = img->w - border;
if (agents[camera_id][a].x > img->w - border) {
agents[camera_id][a].x = border;
} else if (agents[camera_id][a].x < border) {
agents[camera_id][a].x = img->w - border;
}
if (agents[a].y > img->h - border) {
agents[a].y = border;
} else if (agents[a].y < border) {
agents[a].y = img->h - border;
if (agents[camera_id][a].y > img->h - border) {
agents[camera_id][a].y = border;
} else if (agents[camera_id][a].y < border) {
agents[camera_id][a].y = img->h - border;
}
}
}
@@ -150,20 +156,20 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
for (a = 0; a < n_agents; a++) {
// for active agents do a last check on the new position:
if (agents[a].active) {
if (agents[camera_id][a].active) {
// check if the last step brought the agent to a corner:
uint16_t x = (uint16_t) agents[a].x;
uint16_t y = (uint16_t) agents[a].y;
uint16_t x = (uint16_t) agents[camera_id][a].x;
uint16_t y = (uint16_t) agents[camera_id][a].y;
if (fast9_detect_pixel(img, fast_threshold, x, y)) {
// we arrived at a corner, yeah!!!
agents[a].active = 0;
agents[camera_id][a].active = 0;
}
}
// if inactive, the agent is a corner:
if (!agents[a].active) {
(*ret_corners)[(*num_corners)].x = (uint32_t) agents[a].x;
(*ret_corners)[(*num_corners)].y = (uint32_t) agents[a].y;
if (!agents[camera_id][a].active) {
(*ret_corners)[(*num_corners)].x = (uint32_t) agents[camera_id][a].x;
(*ret_corners)[(*num_corners)].y = (uint32_t) agents[camera_id][a].y;
(*num_corners)++;
}
}
sw/airborne/modules/computer_vision/lib/vision/act_fast.h
+2 -1
View File
@@ -37,6 +37,7 @@ struct agent_t {
#include "lib/vision/image.h"
void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners,
uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient, int gradient_method);
uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient,
int gradient_method, int camera_id);
#endif
sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
+1
View File
@@ -52,6 +52,7 @@ struct opticflow_result_t {
float surface_roughness; ///< Surface roughness as determined with a linear optical flow fit
float divergence; ///< Divergence as determined with a linear flow fit
uint8_t camera_id; ///< Camera id as passed to cv_add_to_device
float noise_measurement; ///< noise of measurement, for state filter
};
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+270 -87
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+3 -2
View File
@@ -79,12 +79,13 @@ struct opticflow_t {
int actfast_min_gradient; ///< Threshold that decides when there is sufficient texture for edge following
int actfast_gradient_method; ///< Whether to use a simple or Sobel filter
const struct video_config_t *camera;
uint8_t id;
};
#define FAST9_MAX_CORNERS 512
extern void opticflow_calc_init(struct opticflow_t *opticflow);
extern void opticflow_calc_init(struct opticflow_t opticflow[]);
extern bool opticflow_calc_frame(struct opticflow_t *opticflow, struct image_t *img,
struct opticflow_result_t *result);
sw/airborne/modules/computer_vision/opticflow_module.c
+65 -42
View File
@@ -48,17 +48,28 @@ PRINT_CONFIG_VAR(OPTICFLOW_AGL_ID)
#ifndef OPTICFLOW_FPS
#define OPTICFLOW_FPS 0 ///< Default FPS (zero means run at camera fps)
#endif
#ifndef OPTICFLOW_FPS_CAMERA2
#define OPTICFLOW_FPS_CAMERA2 0 ///< Default FPS (zero means run at camera fps)
#endif
PRINT_CONFIG_VAR(OPTICFLOW_FPS)
PRINT_CONFIG_VAR(OPTICFLOW_FPS_CAMERA2)
#ifdef OPTICFLOW_CAMERA2
#define ACTIVE_CAMERAS 2
#else
#define ACTIVE_CAMERAS 1
#endif
/* The main opticflow variables */
struct opticflow_t opticflow; ///< Opticflow calculations
static struct opticflow_result_t opticflow_result; ///< The opticflow result
struct opticflow_t opticflow[ACTIVE_CAMERAS]; ///< Opticflow calculations
static struct opticflow_result_t opticflow_result[ACTIVE_CAMERAS]; ///< The opticflow result
static bool opticflow_got_result; ///< When we have an optical flow calculation
static pthread_mutex_t opticflow_mutex; ///< Mutex lock fo thread safety
static bool opticflow_got_result[ACTIVE_CAMERAS]; ///< When we have an optical flow calculation
static pthread_mutex_t opticflow_mutex; ///< Mutex lock fo thread safety
/* Static functions */
struct image_t *opticflow_module_calc(struct image_t *img); ///< The main optical flow calculation thread
struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id); ///< The main optical flow calculation thread
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -70,15 +81,19 @@ struct image_t *opticflow_module_calc(struct image_t *img); ///< The main op
static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev)
{
pthread_mutex_lock(&opticflow_mutex);
if (opticflow_result.noise_measurement < 0.8) {
pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID,
&opticflow_result.fps, &opticflow_result.corner_cnt,
&opticflow_result.tracked_cnt, &opticflow_result.flow_x,
&opticflow_result.flow_y, &opticflow_result.flow_der_x,
&opticflow_result.flow_der_y, &opticflow_result.vel_body.x,
&opticflow_result.vel_body.y, &opticflow_result.vel_body.z,
&opticflow_result.div_size, &opticflow_result.surface_roughness,
&opticflow_result.divergence); // TODO: no noise measurement here...
for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
if (opticflow_result[idx_camera].noise_measurement < 0.8) {
pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID,
&opticflow_result[idx_camera].fps, &opticflow_result[idx_camera].corner_cnt,
&opticflow_result[idx_camera].tracked_cnt, &opticflow_result[idx_camera].flow_x,
&opticflow_result[idx_camera].flow_y, &opticflow_result[idx_camera].flow_der_x,
&opticflow_result[idx_camera].flow_der_y, &opticflow_result[idx_camera].vel_body.x,
&opticflow_result[idx_camera].vel_body.y, &opticflow_result[idx_camera].vel_body.z,
&opticflow_result[idx_camera].div_size,
&opticflow_result[idx_camera].surface_roughness,
&opticflow_result[idx_camera].divergence,
&opticflow_result[idx_camera].camera_id); // TODO: no noise measurement here...
}
}
pthread_mutex_unlock(&opticflow_mutex);
}
@@ -90,10 +105,15 @@ static void opticflow_telem_send(struct transport_tx *trans, struct link_device
void opticflow_module_init(void)
{
// Initialize the opticflow calculation
opticflow_got_result = false;
opticflow_calc_init(&opticflow);
for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
opticflow_got_result[idx_camera] = false;
}
opticflow_calc_init(opticflow);
cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc, OPTICFLOW_FPS);
cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc, OPTICFLOW_FPS, 0);
#ifdef OPTICFLOW_CAMERA2
cv_add_to_device(&OPTICFLOW_CAMERA2, opticflow_module_calc, OPTICFLOW_FPS_CAMERA2, 1);
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send);
@@ -109,27 +129,29 @@ void opticflow_module_run(void)
{
pthread_mutex_lock(&opticflow_mutex);
// Update the stabilization loops on the current calculation
if (opticflow_got_result) {
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_ID, now_ts,
opticflow_result.flow_x,
opticflow_result.flow_y,
opticflow_result.flow_der_x,
opticflow_result.flow_der_y,
opticflow_result.noise_measurement,
opticflow_result.div_size);
//TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt
if (opticflow_result.noise_measurement < 0.8) {
AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID, now_ts,
opticflow_result.vel_body.x,
opticflow_result.vel_body.y,
0.0f, //opticflow_result.vel_body.z,
opticflow_result.noise_measurement,
opticflow_result.noise_measurement,
-1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity.
);
for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) {
if (opticflow_got_result[idx_camera]) {
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_ID + idx_camera, now_ts,
opticflow_result[idx_camera].flow_x,
opticflow_result[idx_camera].flow_y,
opticflow_result[idx_camera].flow_der_x,
opticflow_result[idx_camera].flow_der_y,
opticflow_result[idx_camera].noise_measurement,
opticflow_result[idx_camera].div_size);
//TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt
if (opticflow_result[idx_camera].noise_measurement < 0.8) {
AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID + idx_camera, now_ts,
opticflow_result[idx_camera].vel_body.x,
opticflow_result[idx_camera].vel_body.y,
0.0f, //opticflow_result.vel_body.z,
opticflow_result[idx_camera].noise_measurement,
opticflow_result[idx_camera].noise_measurement,
-1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity.
);
}
opticflow_got_result[idx_camera] = false;
}
opticflow_got_result = false;
}
pthread_mutex_unlock(&opticflow_mutex);
}
@@ -139,9 +161,10 @@ void opticflow_module_run(void)
* This thread passes the images trough the optical flow
* calculator
* @param[in] *img The image_t structure of the captured image
* @param[in] camera_id The camera index id
* @return *img The processed image structure
*/
struct image_t *opticflow_module_calc(struct image_t *img)
struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id)
{
// Copy the state
// TODO : put accelerometer values at pose of img timestamp
@@ -150,12 +173,12 @@ struct image_t *opticflow_module_calc(struct image_t *img)
img->eulers = pose.eulers;
// Do the optical flow calculation
static struct opticflow_result_t temp_result; // static so that the number of corners is kept between frames
if(opticflow_calc_frame(&opticflow, img, &temp_result)){
static struct opticflow_result_t temp_result[ACTIVE_CAMERAS]; // static so that the number of corners is kept between frames
if(opticflow_calc_frame(&opticflow[camera_id], img, &temp_result[camera_id])){
// Copy the result if finished
pthread_mutex_lock(&opticflow_mutex);
opticflow_result = temp_result;
opticflow_got_result = true;
opticflow_result[camera_id] = temp_result[camera_id];
opticflow_got_result[camera_id] = true;
pthread_mutex_unlock(&opticflow_mutex);
}
return img;
sw/airborne/modules/computer_vision/opticflow_module.h
+1 -1
View File
@@ -31,7 +31,7 @@
#include "opticflow/opticflow_calculator.h"
// Needed for settings
extern struct opticflow_t opticflow;
extern struct opticflow_t opticflow[];
// Module functions
extern void opticflow_module_init(void);
sw/airborne/modules/computer_vision/qrcode/qr_code.c
+2 -2
View File
@@ -42,7 +42,7 @@ PRINT_CONFIG_VAR(QRCODE_FPS)
void qrcode_init(void)
{
// Add qrscan to the list of image processing tasks in video_thread
cv_add_to_device(&QRCODE_CAMERA, qrscan, QRCODE_FPS);
cv_add_to_device(&QRCODE_CAMERA, qrscan, QRCODE_FPS, 0);
}
// Telemetry
@@ -51,7 +51,7 @@ void qrcode_init(void)
zbar_image_scanner_t *scanner = 0;
struct image_t *qrscan(struct image_t *img)
struct image_t *qrscan(struct image_t *img, uint8_t camera_id)
{
int i, j;
sw/airborne/modules/computer_vision/qrcode/qr_code.h
+1 -1
View File
@@ -35,7 +35,7 @@
extern bool drawRectangleAroundQRCode;
extern void qrcode_init(void);
extern struct image_t *qrscan(struct image_t *img);
extern struct image_t *qrscan(struct image_t *img, uint8_t camera_id);
#endif
sw/airborne/modules/computer_vision/undistort_image.c
+2 -2
View File
@@ -43,7 +43,7 @@ static float K[9] = {0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f};
// Function
static struct image_t *undistort_image_func(struct image_t *img)
static struct image_t *undistort_image_func(struct image_t *img, uint8_t camera_id)
{
// TODO: These commands could actually only be run when the parameters or image size are changed
float normalized_step = (max_x_normalized - min_x_normalized) / img->w;
@@ -118,5 +118,5 @@ void undistort_image_init(void)
min_x_normalized = UNDISTORT_MIN_X_NORMALIZED;
max_x_normalized = UNDISTORT_MAX_X_NORMALIZED;
center_ratio = UNDISTORT_CENTER_RATIO;
listener = cv_add_to_device(&UNDISTORT_CAMERA, undistort_image_func, UNDISTORT_FPS);
listener = cv_add_to_device(&UNDISTORT_CAMERA, undistort_image_func, UNDISTORT_FPS), 0;
}
sw/airborne/modules/computer_vision/video_capture.c
+3 -3
View File
@@ -61,7 +61,7 @@ int video_capture_index = 0;
static char save_dir[256];
// Forward function declarations
struct image_t *video_capture_func(struct image_t *img);
struct image_t *video_capture_func(struct image_t *img, uint8_t camera_id);
void video_capture_save(struct image_t *img);
@@ -80,11 +80,11 @@ void video_capture_init(void)
// This prevents empty folders if nothing is actually recorded.
// Add function to computer vision pipeline
cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func, VIDEO_CAPTURE_FPS);
cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func, VIDEO_CAPTURE_FPS, 0);
}
struct image_t *video_capture_func(struct image_t *img)
struct image_t *video_capture_func(struct image_t *img, uint8_t camera_id)
{
// If take_shot bool is set, save the image
if (video_capture_take_shot || video_capture_record_video) {
sw/airborne/modules/computer_vision/video_usb_logger.c
+2 -2
View File
@@ -111,7 +111,7 @@ static void save_shot_on_disk(struct image_t *img, struct image_t *img_jpeg)
}
static struct image_t *log_image(struct image_t *img)
static struct image_t *log_image(struct image_t *img, uint8_t camera_id)
{
if (!created_jpeg) {
@@ -148,7 +148,7 @@ void video_usb_logger_start(void)
}
// Subscribe to a camera
cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image, VIDEO_USB_LOGGER_FPS);
cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image, VIDEO_USB_LOGGER_FPS, 0);
}
/** Stop the logger an nicely close the file */
sw/airborne/modules/computer_vision/viewvideo.c
+4 -4
View File
@@ -191,7 +191,7 @@ static struct image_t *viewvideo_function(struct UdpSocket *viewvideo_socket, st
}
#ifdef VIEWVIDEO_CAMERA
static struct image_t *viewvideo_function1(struct image_t *img)
static struct image_t *viewvideo_function1(struct image_t *img, uint8_t camera_id)
{
static uint16_t rtp_packet_nr = 0;
static uint32_t rtp_frame_time = 0;
@@ -202,7 +202,7 @@ static struct image_t *viewvideo_function1(struct image_t *img)
#endif
#ifdef VIEWVIDEO_CAMERA2
static struct image_t *viewvideo_function2(struct image_t *img)
static struct image_t *viewvideo_function2(struct image_t *img, uint8_t camera_id)
{
static uint16_t rtp_packet_nr = 0;
static uint32_t rtp_frame_time = 0;
@@ -263,13 +263,13 @@ void viewvideo_init(void)
#ifdef VIEWVIDEO_CAMERA
cv_add_to_device_async(&VIEWVIDEO_CAMERA, viewvideo_function1,
VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS);
VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS, 0);
fprintf(stderr, "[viewvideo] Added asynchronous video streamer listener for CAMERA1 at %u FPS \n", VIEWVIDEO_FPS);
#endif
#ifdef VIEWVIDEO_CAMERA2
cv_add_to_device_async(&VIEWVIDEO_CAMERA2, viewvideo_function2,
VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS);
VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS, 1);
fprintf(stderr, "[viewvideo] Added asynchronous video streamer listener for CAMERA2 at %u FPS \n", VIEWVIDEO_FPS);
#endif
}
sw/airborne/modules/pano_unwrap/pano_unwrap.c
+2 -2
View File
@@ -300,7 +300,7 @@ static void unwrap_LUT(struct image_t *img_raw, struct image_t *img)
}
}
static struct image_t *camera_cb(struct image_t *img)
static struct image_t *camera_cb(struct image_t *img, uint8_t camera_id)
{
set_output_image_size();
update_LUT(img);
@@ -314,6 +314,6 @@ void pano_unwrap_init()
{
image_create(&pano_unwrapped_image, 0, 0, IMAGE_YUV422);
set_output_image_size();
cv_add_to_device(&PANO_UNWRAP_CAMERA, camera_cb, PANO_UNWRAP_FPS);
cv_add_to_device(&PANO_UNWRAP_CAMERA, camera_cb, PANO_UNWRAP_FPS, 0);
}
sw/airborne/modules/sensors/opticflow_pmw3901.c
+3 -1
View File
@@ -143,6 +143,7 @@ static void opticflow_pmw3901_publish(int16_t delta_x, int16_t delta_y, uint32_t
#if SENSOR_SYNC_SEND_OPTICFLOW_PMW3901
float dummy_f = 0.f;
uint16_t dummy_u16 = 0;
uint8_t dummy_u8 = 0;
float fps = 1.f / dt;
DOWNLINK_SEND_OPTIC_FLOW_EST(DefaultChannel, DefaultDevice,
&fps, /* fps */
@@ -157,7 +158,8 @@ static void opticflow_pmw3901_publish(int16_t delta_x, int16_t delta_y, uint32_t
&dummy_f, /* vel_z */
&dummy_f, /* div_size */
&dummy_f, /* surface_roughness */
&dummy_f /* divergence */
&dummy_f, /* divergence */
&dummy_u8 /* camera_id */
);
#endif
}
sw/airborne/modules/wedgebug/wedgebug.c
+6 -6
View File
@@ -299,9 +299,9 @@ uint8_t getMedian(uint8_t *a, uint32_t n);
//Core
static struct image_t *copy_left_img_func(struct image_t
*img); // Function 1: Copies left image into a buffer (buf_left)
*img, uint8_t camera_id); // Function 1: Copies left image into a buffer (buf_left)
static struct image_t *copy_right_img_func(struct image_t
*img); // Function 2: Copies left image into a buffer (buf_right)
*img, uint8_t camera_id); // Function 2: Copies left image into a buffer (buf_right)
void UYVYs_interlacing_V(struct image_t *YY, struct image_t *left,
struct image_t *right); // Function 3: Copies gray pixel values of left and right UYVY images into merged YY image
void UYVYs_interlacing_H(struct image_t *merged, struct image_t *left, struct image_t *right);
@@ -540,7 +540,7 @@ uint16_t getMedian16bit(uint16_t *a, uint32_t n)
// Core:
// Function 1
static struct image_t *copy_left_img_func(struct image_t *img)
static struct image_t *copy_left_img_func(struct image_t *img, uint8_t camera_id)
{
image_copy(img, &img_left);
//show_image_data(img);
@@ -550,7 +550,7 @@ static struct image_t *copy_left_img_func(struct image_t *img)
// Function 2
static struct image_t *copy_right_img_func(struct image_t *img)
static struct image_t *copy_right_img_func(struct image_t *img, uint8_t camera_id)
{
image_copy(img, &img_right);
//show_image_data(img);
@@ -1655,8 +1655,8 @@ void wedgebug_init()
// Adding callback functions
cv_add_to_device(&WEDGEBUG_CAMERA_LEFT, copy_left_img_func, WEDGEBUG_CAMERA_LEFT_FPS);
cv_add_to_device(&WEDGEBUG_CAMERA_RIGHT, copy_right_img_func, WEDGEBUG_CAMERA_RIGHT_FPS);
cv_add_to_device(&WEDGEBUG_CAMERA_LEFT, copy_left_img_func, WEDGEBUG_CAMERA_LEFT_FPS, 0);
cv_add_to_device(&WEDGEBUG_CAMERA_RIGHT, copy_right_img_func, WEDGEBUG_CAMERA_RIGHT_FPS, 1);
//Initialization of constant rotation matrices and transition vectors for frame to frame transformations
sw/airborne/subsystems/abi_sender_ids.h
+19 -3
View File
@@ -355,8 +355,16 @@
#define FLOW_OPTICFLOW_ID 1
#endif
#ifndef FLOW_OPTICFLOW_CAM1_ID
#define FLOW_OPTICFLOW_CAM1_ID 1
#endif
#ifndef FLOW_OPTICFLOW_CAM2_ID
#define FLOW_OPTICFLOW_CAM2_ID 2
#endif
#ifndef FLOW_OPTICFLOW_PMW3901_ID
#define FLOW_OPTICFLOW_PMW3901_ID 2
#define FLOW_OPTICFLOW_PMW3901_ID 3
#endif
/*
@@ -374,12 +382,20 @@
#define VEL_OPTICFLOW_ID 3
#endif
#ifndef VEL_OPTICFLOW_CAM1_ID
#define VEL_OPTICFLOW_CAM1_ID 3
#endif
#ifndef VEL_OPTICFLOW_CAM2_ID
#define VEL_OPTICFLOW_CAM2_ID 4
#endif
#ifndef VEL_STEREOCAM_ID
#define VEL_STEREOCAM_ID 4
#define VEL_STEREOCAM_ID 5
#endif
#ifndef VEL_OPTICFLOW_PMW3901_ID
#define VEL_OPTICFLOW_PMW3901_ID 5
#define VEL_OPTICFLOW_PMW3901_ID 6
#endif
/*
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: f8e9822347...46c4c210ac