From 66f3c2b2dae6c67f4e94d2c952f2cc36c30b8945 Mon Sep 17 00:00:00 2001
From: Mederic Fourmy <mederic.fourmy@gmail.com>
Date: Thu, 21 Jul 2022 16:51:33 +0200
Subject: [PATCH] [skip-ci] Add demo files for the apriltag vo system
---
launch/demo_vio.launch | 1 +
launch/demo_vio_apriltag.launch | 77 ++++++
launch/demo_vo.launch | 1 +
launch/demo_vo_aprilag.launch | 78 ++++++
package.xml | 1 +
rviz/d435i.rviz | 21 +-
rviz/d435i_apriltag.rviz | 246 ++++++++++++++++++
rviz/euroc.rviz | 8 +-
yaml/demo_vio_apriltag_d435i.yaml | 134 ++++++++++
yaml/demo_vio_d435i.yaml | 4 +-
yaml/demo_vio_euroc.yaml | 4 +-
yaml/demo_vo_apriltag_d435i.yaml | 103 ++++++++
yaml/demo_vo_d435i.yaml | 4 +-
yaml/demo_vo_euroc.yaml | 4 +-
yaml/processor_tracker_landmark_apriltag.yaml | 77 ++++++
yaml/solver.yaml | 2 +-
16 files changed, 739 insertions(+), 26 deletions(-)
create mode 100644 launch/demo_vio_apriltag.launch
create mode 100644 launch/demo_vo_aprilag.launch
create mode 100644 rviz/d435i_apriltag.rviz
create mode 100644 yaml/demo_vio_apriltag_d435i.yaml
create mode 100644 yaml/demo_vo_apriltag_d435i.yaml
create mode 100644 yaml/processor_tracker_landmark_apriltag.yaml
diff --git a/launch/demo_vio.launch b/launch/demo_vio.launch
index 52ca3d3..342d727 100644
--- a/launch/demo_vio.launch
+++ b/launch/demo_vio.launch
@@ -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" />
diff --git a/launch/demo_vio_apriltag.launch b/launch/demo_vio_apriltag.launch
new file mode 100644
index 0000000..52a14b6
--- /dev/null
+++ b/launch/demo_vio_apriltag.launch
@@ -0,0 +1,77 @@
+<!-- -->
+<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>
diff --git a/launch/demo_vo.launch b/launch/demo_vo.launch
index 017f57d..e02d931 100644
--- a/launch/demo_vo.launch
+++ b/launch/demo_vo.launch
@@ -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" />
diff --git a/launch/demo_vo_aprilag.launch b/launch/demo_vo_aprilag.launch
new file mode 100644
index 0000000..7324cdd
--- /dev/null
+++ b/launch/demo_vo_aprilag.launch
@@ -0,0 +1,78 @@
+<!-- -->
+<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>
diff --git a/package.xml b/package.xml
index 3e3a93b..4307e3f 100644
--- a/package.xml
+++ b/package.xml
@@ -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 -->
diff --git a/rviz/d435i.rviz b/rviz/d435i.rviz
index 0efb7f4..850179d 100644
--- a/rviz/d435i.rviz
+++ b/rviz/d435i.rviz
@@ -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:
diff --git a/rviz/d435i_apriltag.rviz b/rviz/d435i_apriltag.rviz
new file mode 100644
index 0000000..76f4c2d
--- /dev/null
+++ b/rviz/d435i_apriltag.rviz
@@ -0,0 +1,246 @@
+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
diff --git a/rviz/euroc.rviz b/rviz/euroc.rviz
index 0efb7f4..04c9409 100644
--- a/rviz/euroc.rviz
+++ b/rviz/euroc.rviz
@@ -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
diff --git a/yaml/demo_vio_apriltag_d435i.yaml b/yaml/demo_vio_apriltag_d435i.yaml
new file mode 100644
index 0000000..2d43c28
--- /dev/null
+++ b/yaml/demo_vio_apriltag_d435i.yaml
@@ -0,0 +1,134 @@
+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
diff --git a/yaml/demo_vio_d435i.yaml b/yaml/demo_vio_d435i.yaml
index eca7599..dea29e4 100644
--- a/yaml/demo_vio_d435i.yaml
+++ b/yaml/demo_vio_d435i.yaml
@@ -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"
diff --git a/yaml/demo_vio_euroc.yaml b/yaml/demo_vio_euroc.yaml
index db5f9e8..b9c55f8 100644
--- a/yaml/demo_vio_euroc.yaml
+++ b/yaml/demo_vio_euroc.yaml
@@ -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"
diff --git a/yaml/demo_vo_apriltag_d435i.yaml b/yaml/demo_vo_apriltag_d435i.yaml
new file mode 100644
index 0000000..44eeea7
--- /dev/null
+++ b/yaml/demo_vo_apriltag_d435i.yaml
@@ -0,0 +1,103 @@
+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
diff --git a/yaml/demo_vo_d435i.yaml b/yaml/demo_vo_d435i.yaml
index 0348d09..5afdc67 100644
--- a/yaml/demo_vo_d435i.yaml
+++ b/yaml/demo_vo_d435i.yaml
@@ -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"
diff --git a/yaml/demo_vo_euroc.yaml b/yaml/demo_vo_euroc.yaml
index 578a966..8e45d48 100644
--- a/yaml/demo_vo_euroc.yaml
+++ b/yaml/demo_vo_euroc.yaml
@@ -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"
diff --git a/yaml/processor_tracker_landmark_apriltag.yaml b/yaml/processor_tracker_landmark_apriltag.yaml
new file mode 100644
index 0000000..c26e0b1
--- /dev/null
+++ b/yaml/processor_tracker_landmark_apriltag.yaml
@@ -0,0 +1,77 @@
+# 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).
diff --git a/yaml/solver.yaml b/yaml/solver.yaml
index a3b1c83..4d500f7 100644
--- a/yaml/solver.yaml
+++ b/yaml/solver.yaml
@@ -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
--
GitLab