Skip to content
Snippets Groups Projects
Commit 66f3c2b2 authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

[skip-ci] Add demo files for the apriltag vo system

parent fd967285
No related branches found
No related tags found
No related merge requests found
Pipeline #12220 skipped
Hide whitespace changes
Inline Side-by-side
Showing
with 739 additions and 26 deletions
launch/demo_vio.launch
+ 1
− 0
View file @ 66f3c2b2
......@@ -30,6 +30,7 @@
<param name="camera_yaml_path" value="$(find wolf_demo_visual_odometry)/yaml/camera_d435i.yaml" />
<!-- -->
<!-- Wolf autosetup file name -->
<arg name="param_file" default="demo_vio_$(arg setup_name).yaml" />
......
launch/demo_vio_apriltag.launch 0 → 100644
+ 77
− 0
View file @ 66f3c2b2
<!-- -->
<launch>
<!--USER ARGS-->
<arg name="rviz" default="true" />
<arg name="speed" default="1" />
<arg name="sec" default="0" />
<arg name="sim_time" default="true" />
<arg name="profiling" default="false" />
<arg name="gdb" default="false" />
<arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
<arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
<arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
<!-- Convention used by all files -->
<!-- D453I -->
<arg name="setup_name" default="d435i" />
<arg name="namespace_cam" value="camera/infra1" />
<param name="topic_image_raw_in" value="/camera/infra1/image_rect_raw" />
<param name="topic_image_info_out" value="/camera/infra1/camera_info" />
<param name="camera_yaml_path" value="$(find wolf_demo_visual_odometry)/yaml/camera_d435i.yaml" />
<!-- -->
<!-- Wolf autosetup file name -->
<arg name="param_file" default="demo_vio_apriltag_$(arg setup_name).yaml" />
<!--ROSBAG PLAY-->
<!--if the bag argument is not set, no bag is read here-->
<arg name="bag" default=""/>
<group if="$(eval arg('bag') != '')">
<param name="use_sim_time" value="$(arg sim_time)" />
<node pkg="rosbag"
type="play"
name="player"
required="true"
args="-r $(arg speed)
-s $(arg sec)
--clock
$(find wolf_demo_visual_odometry)/bag/$(arg bag).bag"/>
</group>
<!--VISUALIZATION-->
<group if="$(arg rviz)">
<node name="rviz"
pkg="rviz"
type="rviz"
args="-d $(find wolf_demo_visual_odometry)/rviz/$(arg setup_name)_apriltag.rviz" />
</group>
<!--Publish a CameraInfo topic reading at a standard wolf camera yaml file-->
<!-- <include file="$(find wolf_demo_visual_odometry)/launch/publish_camera_info.launch" /> -->
<node type="publisher_camera_info.py"
name="publisher_camera_info"
pkg="wolf_ros_vision"
>
</node>
<!--Undistort the image using the published camera info and raw images-->
<node type="image_proc"
name="image_proc"
pkg="image_proc"
ns="$(arg namespace_cam)" />
<!--WOLF-->
<node type="wolf_ros_node"
name="wolf_ros_node"
pkg="wolf_ros_node"
output="screen"
required="true"
launch-prefix="$(arg launch_pref)">
<param name="~yaml_file_path" value="$(find wolf_demo_visual_odometry)/yaml/$(arg param_file)" />
</node>
</launch>
launch/demo_vo.launch
+ 1
− 0
View file @ 66f3c2b2
......@@ -30,6 +30,7 @@
<param name="camera_yaml_path" value="$(find wolf_demo_visual_odometry)/yaml/camera_d435i.yaml" />
<!-- -->
<!-- Wolf autosetup file name -->
<arg name="param_file" default="demo_vo_$(arg setup_name).yaml" />
......
launch/demo_vo_aprilag.launch 0 → 100644
+ 78
− 0
View file @ 66f3c2b2
<!-- -->
<launch>
<!--USER ARGS-->
<arg name="rviz" default="true" />
<arg name="speed" default="1" />
<arg name="sec" default="0" />
<arg name="sim_time" default="true" />
<arg name="profiling" default="false" />
<arg name="gdb" default="true" />
<arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
<arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
<arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
<!-- Convention used by all files -->
<!-- D453I -->
<arg name="setup_name" default="d435i" />
<arg name="namespace_cam" value="camera/infra1" />
<param name="topic_image_raw_in" value="/camera/infra1/image_rect_raw" />
<param name="topic_image_info_out" value="/camera/infra1/camera_info" />
<param name="camera_yaml_path" value="$(find wolf_demo_visual_odometry)/yaml/camera_d435i.yaml" />
<!-- -->
<!-- Wolf autosetup file name -->
<arg name="param_file" default="demo_vo_apriltag_$(arg setup_name).yaml" />
<!--ROSBAG PLAY-->
<!--if the bag argument is not set, no bag is read here-->
<arg name="bag" default=""/>
<group if="$(eval arg('bag') != '')">
<param name="use_sim_time" value="$(arg sim_time)" />
<node pkg="rosbag"
type="play"
name="player"
required="true"
args="-r $(arg speed)
-s $(arg sec)
--clock
$(find wolf_demo_visual_odometry)/bag/$(arg bag).bag"/>
</group>
<!--VISUALIZATION-->
<group if="$(arg rviz)">
<node name="rviz"
pkg="rviz"
type="rviz"
args="-d $(find wolf_demo_visual_odometry)/rviz/$(arg setup_name)_apriltag.rviz" />
</group>
<!--Publish a CameraInfo topic reading at a standard wolf camera yaml file-->
<!-- <include file="$(find wolf_demo_visual_odometry)/launch/publish_camera_info.launch" /> -->
<node type="publisher_camera_info.py"
name="publisher_camera_info"
pkg="wolf_ros_vision"
>
</node>
<!--Undistort the image using the published camera info and raw images-->
<node type="image_proc"
name="image_proc"
pkg="image_proc"
ns="$(arg namespace_cam)" />
<!--WOLF-->
<node type="wolf_ros_node"
name="wolf_ros_node"
pkg="wolf_ros_node"
output="screen"
required="true"
launch-prefix="$(arg launch_pref)">
<param name="~yaml_file_path" value="$(find wolf_demo_visual_odometry)/yaml/$(arg param_file)" />
</node>
</launch>
package.xml
+ 1
− 0
View file @ 66f3c2b2
......@@ -9,6 +9,7 @@
<license>BSD2</license>
<build_depend>wolf_ros_vision</build_depend>
<build_depend>wolf_ros_apriltag</build_depend>
<buildtool_depend>catkin</buildtool_depend>
<!-- The export tag contains other, unspecified, tags -->
......
rviz/d435i.rviz
+ 8
− 13
View file @ 66f3c2b2
......@@ -6,7 +6,6 @@ Panels:
Expanded:
- /TF1/Frames1
- /Factors1
- /Factors1/Status1
- /Factors1/Namespaces1
- /Landmarks1/Namespaces1
- /Trajectory1/Namespaces1
......@@ -102,10 +101,7 @@ Visualization Manager:
Marker Topic: /wolf_ros_node/graph_factors
Name: Factors
Namespaces:
factors_VISUALODOMETRY PROC: true
factors_text_VISUALODOMETRY PROC: false
factors_text_unnamed_processor: true
factors_unnamed_processor: true
{}
Queue Size: 1
Value: true
- Class: rviz/MarkerArray
......@@ -113,8 +109,7 @@ Visualization Manager:
Marker Topic: /wolf_ros_node/graph_landmarks
Name: Landmarks
Namespaces:
landmarks: true
landmarks_text: true
{}
Queue Size: 1
Value: true
- Class: rviz/MarkerArray
......@@ -148,11 +143,11 @@ Visualization Manager:
Value: false
- Class: rviz/Image
Enabled: true
Image Topic: /debug_image
Image Topic: /image_vo_tracks
Max Value: 1
Median window: 5
Min Value: 0
Name: debug
Name: VO tracks
Normalize Range: true
Queue Size: 2
Transport Hint: raw
......@@ -160,7 +155,7 @@ Visualization Manager:
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /debug_image_preprocessor
Image Topic: /image_vo_tracks_preprocessor
Max Value: 1
Median window: 5
Min Value: 0
......@@ -169,7 +164,7 @@ Visualization Manager:
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
......@@ -226,7 +221,7 @@ Window Geometry:
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 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
QMainWindow State: 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
Selection:
collapsed: false
Time:
......@@ -235,7 +230,7 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
Width: 1920
Width: 960
X: 0
Y: 27
debug:
......
rviz/d435i_apriltag.rviz 0 → 100644
+ 246
− 0
View file @ 66f3c2b2
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /TF1/Frames1
- /Factors1
- /Factors1/Namespaces1
- /Landmarks1/Namespaces1
- /Trajectory1/Namespaces1
Splitter Ratio: 0.5
Tree Height: 192
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: VO tracks
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/Axes
Enabled: true
Length: 0.10000000149011612
Name: Axes
Radius: 0.009999999776482582
Reference Frame: base_footprint
Show Trail: true
Value: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
Marker Alpha: 1
Marker Scale: 0.5
Name: TF
Show Arrows: false
Show Axes: true
Show Names: false
Tree:
{}
Update Interval: 0
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /wolf_apriltag_vio/marker_array
Name: MarkerArray
Namespaces:
{}
Queue Size: 1
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /wolf_ros_node/graph_factors
Name: Factors
Namespaces:
factors_VISUALODOMETRY PROC: true
factors_text_VISUALODOMETRY PROC: false
Queue Size: 1
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /wolf_ros_node/graph_landmarks
Name: Landmarks
Namespaces:
landmarks: true
landmarks_text: true
Queue Size: 1
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /wolf_ros_node/graph_trajectory
Name: Trajectory
Namespaces:
frames: true
frames_text: true
Queue Size: 1
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: visualization_marker
Name: Ground truth
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz/Image
Enabled: false
Image Topic: /camera/infra1/image_rect_raw
Max Value: 1
Median window: 5
Min Value: 0
Name: image_raw
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: false
- Class: rviz/Image
Enabled: true
Image Topic: /image_vo_tracks
Max Value: 1
Median window: 5
Min Value: 0
Name: VO tracks
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /wolf_ros_node/apriltag_detections
Max Value: 1
Median window: 5
Min Value: 0
Name: AprilTag Detections
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Image
Enabled: false
Image Topic: /image_vo_tracks_preprocessor
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 3.074009656906128
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0.6342065930366516
Y: 0.34021610021591187
Z: 0.2876497805118561
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.05979748070240021
Target Frame: <Fixed Frame>
Yaw: 4.310366153717041
Saved: ~
Window Geometry:
AprilTag Detections:
collapsed: false
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 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
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
VO tracks:
collapsed: false
Views:
collapsed: false
Width: 1267
X: 0
Y: 27
image_raw:
collapsed: false
rviz/euroc.rviz
+ 4
− 4
View file @ 66f3c2b2
......@@ -148,11 +148,11 @@ Visualization Manager:
Value: false
- Class: rviz/Image
Enabled: true
Image Topic: /debug_image
Image Topic: /image_vo_tracks
Max Value: 1
Median window: 5
Min Value: 0
Name: debug
Name: VO tracks
Normalize Range: true
Queue Size: 2
Transport Hint: raw
......@@ -160,7 +160,7 @@ Visualization Manager:
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /debug_image_preprocessor
Image Topic: /image_vo_tracks_preprocessor
Max Value: 1
Median window: 5
Min Value: 0
......@@ -169,7 +169,7 @@ Visualization Manager:
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
......
yaml/demo_vio_apriltag_d435i.yaml 0 → 100644
+ 134
− 0
View file @ 66f3c2b2
config:
debug:
profiling: true
profiling_file: "~/wolf_demo_vio_profiling.txt"
print_problem: true
print_depth: 3 # only if print_problem
print_constr_by: true # only if print_problem
print_metric: true # only if print_problem
print_state_blocks: false # only if print_problem
print_period: 5 # only if print_problem
problem:
node_rate: 20
follow: "tree_manager_sliding.yaml"
frame_structure: "POV"
dimension: 3
prior:
mode: "initial_guess"
$state:
P: [0,0,0]
O: [1,0,0,0] # arbitrary
V: [0,0,0]
$sigma:
P: [0.00001, 0.00001, 0.00001]
O: [3, 3, 3]
V: [0.1, 0.1, 0.1]
time_tolerance: 0.01
solver:
follow: "solver.yaml"
sensors:
-
type: "SensorCamera"
name: "CAMERA"
plugin: "vision"
extrinsic:
pose: [-0.006, 0.005, 0.012, 0,0,0,1] # D435i left infra
# pose: [0,0,0, 0,0,0,1]
using_raw: false
follow: "camera_d435i.yaml"
-
type: "SensorImu"
name: "IMU"
plugin: "imu"
extrinsic:
pose: [0,0,0, 0,0,0,1]
follow: "imu_d435i.yaml"
processors:
-
type: "ProcessorVisualOdometry"
name: "VISUALODOMETRY PROC"
sensor_name: "CAMERA"
plugin: "vision"
follow: "processor_visual_odometry.yaml"
-
type: "ProcessorTrackerLandmarkApriltag"
name: "APRILTAG PROC"
sensor_name: "CAMERA"
plugin: "apriltag"
follow: "processor_tracker_landmark_apriltag.yaml"
-
type: "ProcessorImu"
name: "IMU PROC"
sensor_name: "IMU"
plugin: "imu"
follow: "processor_imu.yaml"
state_getter: true
state_priority: 1
ROS subscriber:
-
package: "wolf_ros_vision"
type: "SubscriberCamera"
topic: "/camera/infra1/image_rect_raw"
sensor_name: "CAMERA"
-
package: "wolf_ros_imu"
type: "SubscriberImu"
topic: "/camera/imu"
sensor_name: "IMU"
cov_source: "sensor"
in_degrees: false
ROS publisher:
-
type: "PublisherGraph"
topic: "graph"
package: "wolf_ros_node"
period: 0.2
viz_scale: 0.3
text_scale: 0.1
landmark_text_z_offset: 0.3
landmark_length: 0.5
frame_width: 0.01
frame_length: 0.1
frame_vel_scale: 1.0
factors_width: 0.01
factor_lmk_color: [0, 1, 1, 0.005]
-
type: "PublisherTf"
topic: " "
package: "wolf_ros_node"
period: 0.2
map_frame_id: "map"
odom_frame_id: "odom"
base_frame_id: "base_footprint"
publish_odom_tf: true
-
type: "PublisherVisionDebug"
processor_name: "VISUALODOMETRY PROC"
topic: "/image_vo_tracks"
topic_preprocessor: "/image_vo_tracks_preprocessor"
package: "wolf_ros_vision"
period: 0.05
follow: "publisher_vision_debug.yaml"
-
type: "PublisherImuBias"
sensor_name: "IMU"
processor_name: "IMU"
topic: "/imu_bias"
package: "wolf_ros_imu"
period: 0.05
-
type: "PublisherApriltagDebug"
processor_name: "APRILTAG PROC"
topic: "apriltag_detections"
package: "wolf_ros_apriltag"
period: 0.1
yaml/demo_vio_d435i.yaml
+ 2
− 2
View file @ 66f3c2b2
......@@ -107,8 +107,8 @@ config:
-
type: "PublisherVisionDebug"
processor_name: "VISUALODOMETRY PROC"
topic: "/debug_image"
topic_preprocessor: "/debug_image_preprocessor"
topic: "/image_vo_tracks"
topic_preprocessor: "/image_vo_tracks_preprocessor"
package: "wolf_ros_vision"
period: 0.05
follow: "publisher_vision_debug.yaml"
......
yaml/demo_vio_euroc.yaml
+ 2
− 2
View file @ 66f3c2b2
......@@ -111,8 +111,8 @@ config:
-
type: "PublisherVisionDebug"
processor_name: "VISUALODOMETRY PROC"
topic: "/debug_image"
topic_preprocessor: "/debug_image_preprocessor"
topic: "/image_vo_tracks"
topic_preprocessor: "/image_vo_tracks_preprocessor"
package: "wolf_ros_vision"
period: 0.05
follow: "publisher_vision_debug.yaml"
......
yaml/demo_vo_apriltag_d435i.yaml 0 → 100644
+ 103
− 0
View file @ 66f3c2b2
config:
debug:
profiling: true
profiling_file: "~/wolf_demo_vio_profiling.txt"
print_problem: true
print_depth: 3 # only if print_problem
print_constr_by: true # only if print_problem
print_metric: true # only if print_problem
print_state_blocks: false # only if print_problem
print_period: 5 # only if print_problem
problem:
node_rate: 20
follow: "tree_manager_sliding.yaml"
frame_structure: "PO"
dimension: 3
prior:
mode: "initial_guess"
$state:
P: [0,0,0]
O: [1,0,0,0] # arbitrary
$sigma:
P: [0.00001, 0.00001, 0.00001]
O: [3, 3, 3]
time_tolerance: 0.01
solver:
follow: "solver.yaml"
sensors:
-
type: "SensorCamera"
name: "CAMERA"
plugin: "vision"
extrinsic:
pose: [-0.006, 0.005, 0.012, 0,0,0,1] # D435i left infra
# pose: [0,0,0, 0,0,0,1]
using_raw: false
follow: "camera_d435i.yaml"
processors:
-
type: "ProcessorVisualOdometry"
name: "VISUALODOMETRY PROC"
sensor_name: "CAMERA"
plugin: "vision"
follow: "processor_visual_odometry.yaml"
-
type: "ProcessorTrackerLandmarkApriltag"
name: "APRILTAG PROC"
sensor_name: "CAMERA"
plugin: "apriltag"
follow: "processor_tracker_landmark_apriltag.yaml"
ROS subscriber:
-
package: "wolf_ros_vision"
type: "SubscriberCamera"
topic: "/camera/infra1/image_rect_raw"
sensor_name: "CAMERA"
ROS publisher:
# -
# type: "PublisherGraph"
# topic: "graph"
# package: "wolf_ros_node"
# period: 0.2
# viz_scale: 0.3
# text_scale: 0.1
# landmark_text_z_offset: 0.3
# landmark_length: 0.5
# frame_width: 0.01
# frame_length: 0.1
# frame_vel_scale: 1.0
# factors_width: 0.01
# factor_lmk_color: [0, 1, 1, 0.004]
# -
# type: "PublisherTf"
# topic: " "
# package: "wolf_ros_node"
# period: 0.2
# map_frame_id: "map"
# odom_frame_id: "odom"
# base_frame_id: "base_footprint"
# publish_odom_tf: true
-
type: "PublisherApriltagDebug"
processor_name: "APRILTAG PROC"
topic: "apriltag_detections"
package: "wolf_ros_apriltag"
period: 0.1
-
type: "PublisherVisionDebug"
processor_name: "VISUALODOMETRY PROC"
topic: "/image_vo_tracks"
topic_preprocessor: "/image_vo_tracks_preprocessor"
package: "wolf_ros_vision"
period: 0.05
follow: "publisher_vision_debug.yaml"
\ No newline at end of file
yaml/demo_vo_d435i.yaml
+ 2
− 2
View file @ 66f3c2b2
......@@ -83,8 +83,8 @@ config:
-
type: "PublisherVisionDebug"
processor_name: "VISUALODOMETRY PROC"
topic: "/debug_image"
topic_preprocessor: "/debug_image_preprocessor"
topic: "/image_vo_tracks"
topic_preprocessor: "/image_vo_tracks_preprocessor"
package: "wolf_ros_vision"
period: 0.05
follow: "publisher_vision_debug.yaml"
......
yaml/demo_vo_euroc.yaml
+ 2
− 2
View file @ 66f3c2b2
......@@ -104,8 +104,8 @@ config:
-
type: "PublisherVisionDebug"
processor_name: "VISUALODOMETRY PROC"
topic: "/debug_image"
topic_preprocessor: "/debug_image_preprocessor"
topic: "/image_vo_tracks"
topic_preprocessor: "/image_vo_tracks_preprocessor"
package: "wolf_ros_vision"
period: 0.05
follow: "publisher_vision_debug.yaml"
......
yaml/processor_tracker_landmark_apriltag.yaml 0 → 100644
+ 77
− 0
View file @ 66f3c2b2
# detector parameters:
quad_decimate: 0.0 # doing quad detection at lower resolution to speed things up (see end of file)
quad_sigma: 0.0 # gaussian blur good for noisy images, may be recommended with quad_decimate. Kernel size adapted (see end of this file)
nthreads: 1 # how many thread during tag detection (does not change much?)
debug: false # write some debugging images
refine_edges: true # better edge detection if quad_decimate > 1 (quite inexpensive, almost no diff)
ippe_min_ratio: 2 # quite arbitrary, always > 1 (to deactive, set at something < 1)
ippe_max_rep_error: 100 # to deactivate, set at something big (100)
tag_family: "tag36h11" # see in Wolf processor_tracker_landmark_apriltag.cpp for available families
tag_black_border: 1 # usually 1, 2 for Kalibr aprilgrid for ex
# tag_width_default: 0.165 # used if tag width not specified -- IRI $$
# tag_width_default: 0.2 # used if tag width not specified -- Simu $$
# tag_width_default: 0.055 # used if tag width not specified -- Calibration $$
# tag_width_default: 0.166 # used if tag width not specified -- LAAS (B181) $$
tag_width_default: 0.162 # used if tag width not specified -- LAAS, 04/07 $$
# specific tag widths if using tags of different sizes
$tag_widths:
-1: 0.0
# choose the type of factor used when building the problem
use_proj_factor: false
# used for covariance propagation
std_pix: 4 # pixel error $$
time_tolerance: 0.02
keyframe_vote:
voting_active: true
nb_vote_for_every_first: 1
min_time_span: 0.1 # s
max_time_span: 0.3 # s ; note: this must be higher (i.e. slower) than the solve period in solver.yaml
min_features_for_keyframe: 1
max_new_features: -1 # max number of features detected in detectNewFeatures() (-1: unlimited)
apply_loss_function: false
###############
# Apriltag settings
###############
# int nthreads;
# How many threads should be used?
# float quad_decimate;
# detection of quads can be done on a lower-resolution image,
# improving speed at a cost of pose accuracy and a slight
# decrease in detection rate. Decoding the binary payload is
# still done at full resolution. .
# ! Special optimized value of 1.5
# float quad_sigma;
# What Gaussian blur should be applied to the segmented image
# (used for quad detection?) Parameter is the standard deviation
# in pixels. Very noisy images benefit from non-zero values
# (e.g. 0.8).
# int refine_edges;
# When non-zero, the edges of the each quad are adjusted to "snap
# to" strong gradients nearby. This is useful when decimation is
# employed, as it can increase the quality of the initial quad
# estimate substantially. Generally recommended to be on (1).
# Very computationally inexpensive. Option is ignored if
# quad_decimate = 1.
# double decode_sharpening;
# How much sharpening should be done to decoded images? This
# can help decode small tags but may or may not help in odd
# lighting conditions or low light conditions.
# The default value is 0.25.
# int debug;
# When non-zero, write a variety of debugging images to the
# current working directory at various stages through the
# detection process. (Somewhat slow).
yaml/solver.yaml
+ 1
− 1
View file @ 66f3c2b2
......@@ -6,6 +6,6 @@ function_tolerance: 1e-8
gradient_tolerance: 1e-9
use_nonmonotonic_steps: false
period: 0.05
n_threads: 4
n_threads: 1
compute_cov: false
verbose: 0
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment