diff --git a/launch/demo_vo_aprilag.launch b/launch/demo_vo_aprilag.launch
index e465cf962ab1191de9ef927952fe92df70771613..8a237945c05dca389b9f1fa9cf1261add576ea69 100644
--- a/launch/demo_vo_aprilag.launch
+++ b/launch/demo_vo_aprilag.launch
@@ -1,7 +1,7 @@
<!-- -->
<launch>
<!--USER ARGS-->
- <arg name="rviz" default="false" />
+ <arg name="rviz" default="true" />
<arg name="speed" default="1" />
<arg name="sec" default="0" />
<arg name="sim_time" default="true" />
diff --git a/rviz/d435i.rviz b/rviz/d435i.rviz
index 850179ddc9f803fdd80f1cea304ea3ad64c090ab..b67f53ad7e0345febdaf17267fc41488b63e779b 100644
--- a/rviz/d435i.rviz
+++ b/rviz/d435i.rviz
@@ -143,7 +143,7 @@ Visualization Manager:
Value: false
- Class: rviz/Image
Enabled: true
- Image Topic: /image_vo_tracks
+ Image Topic: /wolf_ros_node/image_vo_tracks
Max Value: 1
Median window: 5
Min Value: 0
@@ -155,7 +155,7 @@ Visualization Manager:
Value: true
- Class: rviz/Image
Enabled: true
- Image Topic: /image_vo_tracks_preprocessor
+ Image Topic: /wolf_ros_node/image_vo_tracks_preprocessor
Max Value: 1
Median window: 5
Min Value: 0
diff --git a/rviz/d435i_apriltag.rviz b/rviz/d435i_apriltag.rviz
index 76f4c2dce2e43cec617569e7dc3806aae59e6436..1e533f5fbe8ffa95c0cc7c715beebb2be16d60b8 100644
--- a/rviz/d435i_apriltag.rviz
+++ b/rviz/d435i_apriltag.rviz
@@ -29,7 +29,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: VO tracks
+ SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
@@ -69,6 +69,12 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: false
+ base_footprint:
+ Value: true
+ map:
+ Value: true
+ odom:
+ Value: true
Marker Alpha: 1
Marker Scale: 0.5
Name: TF
@@ -76,7 +82,10 @@ Visualization Manager:
Show Axes: true
Show Names: false
Tree:
- {}
+ map:
+ odom:
+ base_footprint:
+ {}
Update Interval: 0
Value: true
- Class: rviz/MarkerArray
@@ -136,7 +145,7 @@ Visualization Manager:
Value: false
- Class: rviz/Image
Enabled: true
- Image Topic: /image_vo_tracks
+ Image Topic: /wolf_ros_node/image_vo_tracks
Max Value: 1
Median window: 5
Min Value: 0
@@ -160,7 +169,7 @@ Visualization Manager:
Value: true
- Class: rviz/Image
Enabled: false
- Image Topic: /image_vo_tracks_preprocessor
+ Image Topic: /wolf_ros_node/image_vo_tracks_preprocessor
Max Value: 1
Median window: 5
Min Value: 0
diff --git a/rviz/euroc.rviz b/rviz/euroc.rviz
index 04c940921ab9179c343dac89b4a5e56b33ca8280..cf7f2e03abc37bc0a3b8816767e39092a916c8cd 100644
--- a/rviz/euroc.rviz
+++ b/rviz/euroc.rviz
@@ -148,7 +148,7 @@ Visualization Manager:
Value: false
- Class: rviz/Image
Enabled: true
- Image Topic: /image_vo_tracks
+ Image Topic: /wolf_ros_node/image_vo_tracks
Max Value: 1
Median window: 5
Min Value: 0
@@ -160,7 +160,7 @@ Visualization Manager:
Value: true
- Class: rviz/Image
Enabled: true
- Image Topic: /image_vo_tracks_preprocessor
+ Image Topic: /wolf_ros_node/image_vo_tracks_preprocessor
Max Value: 1
Median window: 5
Min Value: 0
diff --git a/yaml/demo_vio_apriltag_d435i.yaml b/yaml/demo_vio_apriltag_d435i.yaml
index 2d43c28c4c1b3d2ac7b4619bb48b5cb2cdb8865c..c0ae8fee71df66fec7d2c94347f6832cb5811fd9 100644
--- a/yaml/demo_vio_apriltag_d435i.yaml
+++ b/yaml/demo_vio_apriltag_d435i.yaml
@@ -114,19 +114,21 @@ config:
-
type: "PublisherVisionDebug"
processor_name: "VISUALODOMETRY PROC"
- topic: "/image_vo_tracks"
- topic_preprocessor: "/image_vo_tracks_preprocessor"
+ topic: "image_vo_tracks"
+ topic_preprocessor: "image_vo_tracks_preprocessor"
package: "wolf_ros_vision"
- period: 0.05
+ period: 0.1
follow: "publisher_vision_debug.yaml"
- -
- type: "PublisherImuBias"
- sensor_name: "IMU"
- processor_name: "IMU"
- topic: "/imu_bias"
- package: "wolf_ros_imu"
- period: 0.05
+
+ # -
+ # 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"
diff --git a/yaml/demo_vio_d435i.yaml b/yaml/demo_vio_d435i.yaml
index dea29e415e532f5f54666cf34c9d5c1aeaef8b5f..e341ca816bb7d6b146892c011d5b005e803f5608 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: "/image_vo_tracks"
- topic_preprocessor: "/image_vo_tracks_preprocessor"
+ topic: "image_vo_tracks"
+ topic_preprocessor: "image_vo_tracks_preprocessor"
package: "wolf_ros_vision"
period: 0.05
follow: "publisher_vision_debug.yaml"
@@ -116,6 +116,6 @@ config:
type: "PublisherImuBias"
sensor_name: "IMU"
processor_name: "IMU"
- topic: "/imu_bias"
+ topic: "imu_bias"
package: "wolf_ros_imu"
period: 0.05
diff --git a/yaml/demo_vio_euroc.yaml b/yaml/demo_vio_euroc.yaml
index b9c55f8d7d41dd82ebff41f9f90cb80ded7d5def..b765e884fb9b0dad08d5fd133f88fff135ce56f8 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: "/image_vo_tracks"
- topic_preprocessor: "/image_vo_tracks_preprocessor"
+ topic: "image_vo_tracks"
+ topic_preprocessor: "image_vo_tracks_preprocessor"
package: "wolf_ros_vision"
period: 0.05
follow: "publisher_vision_debug.yaml"
@@ -120,6 +120,6 @@ config:
type: "PublisherImuBias"
sensor_name: "IMU"
processor_name: "IMU"
- topic: "/imu_bias"
+ topic: "imu_bias"
package: "wolf_ros_imu"
period: 0.05
diff --git a/yaml/demo_vo_apriltag_d435i.yaml b/yaml/demo_vo_apriltag_d435i.yaml
index d494662a02fe5fd5b4442c6bb5d62e763874651f..3de7d83b61aedc6dac201e4e735b2ca85fc84392 100644
--- a/yaml/demo_vo_apriltag_d435i.yaml
+++ b/yaml/demo_vo_apriltag_d435i.yaml
@@ -2,7 +2,7 @@ config:
debug:
profiling: true
- profiling_file: "~/wolf_demo_vio_profiling.txt"
+ profiling_file: "~/wolf_demo_vo_apriltag_profiling.txt"
print_problem: true
print_depth: 3 # only if print_problem
print_constr_by: true # only if print_problem
@@ -38,6 +38,15 @@ config:
# pose: [0,0,0, 0,0,0,1]
using_raw: false
follow: "camera_d435i.yaml"
+ -
+ type: "SensorCamera"
+ name: "CAMERA APRIL"
+ 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:
-
@@ -46,10 +55,11 @@ config:
sensor_name: "CAMERA"
plugin: "vision"
follow: "processor_visual_odometry.yaml"
+
-
type: "ProcessorTrackerLandmarkApriltag"
name: "APRILTAG PROC"
- sensor_name: "CAMERA"
+ sensor_name: "CAMERA APRIL"
plugin: "apriltag"
follow: "processor_tracker_landmark_apriltag.yaml"
@@ -59,46 +69,52 @@ config:
type: "SubscriberCamera"
topic: "/camera/infra1/image_rect_raw"
sensor_name: "CAMERA"
+
+ -
+ package: "wolf_ros_vision"
+ type: "SubscriberCamera"
+ topic: "/camera/infra1/image_rect_raw"
+ sensor_name: "CAMERA APRIL"
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: "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: "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: 100
- # follow: "publisher_vision_debug.yaml"
+ -
+ type: "PublisherVisionDebug"
+ processor_name: "VISUALODOMETRY PROC"
+ topic: "image_vo_tracks"
+ topic_preprocessor: "image_vo_tracks_preprocessor"
+ package: "wolf_ros_vision"
+ period: 0.1
+ follow: "publisher_vision_debug.yaml"
-
type: "PublisherApriltagDebug"
processor_name: "APRILTAG PROC"
topic: "apriltag_detections"
package: "wolf_ros_apriltag"
- period: 100
+ period: 0.1
diff --git a/yaml/demo_vo_d435i.yaml b/yaml/demo_vo_d435i.yaml
index 5afdc673ebc11f579060fcea0eb6c59b2d6ba34b..ee48cd9dceb26526fd09725c4fbc76ccbef5b64c 100644
--- a/yaml/demo_vo_d435i.yaml
+++ b/yaml/demo_vo_d435i.yaml
@@ -16,7 +16,7 @@ config:
frame_structure: "PO"
dimension: 3
prior:
- mode: "initial_guess"
+ mode: "factor"
$state:
P: [0,0,0]
O: [1,0,0,0] #Â arbitrary
@@ -83,8 +83,8 @@ config:
-
type: "PublisherVisionDebug"
processor_name: "VISUALODOMETRY PROC"
- topic: "/image_vo_tracks"
- topic_preprocessor: "/image_vo_tracks_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 8e45d480bcb82a174d06d83c1306593654d97276..95b1398825311ddf98e80c769376b6199296dcdb 100644
--- a/yaml/demo_vo_euroc.yaml
+++ b/yaml/demo_vo_euroc.yaml
@@ -19,10 +19,7 @@ config:
mode: "factor"
$state:
P: [0,0,0]
- # P: [0,0,0.5]
- O: [0,0,0,1]
- # O: [0, 0.20791169, 0, 0.9781476] # x,y,z,w
-# O: [0, -0.78770522698, 0, -0.61605233169] # -105 deg along Y axes
+ O: [0,0,0,1] # arbitrary
$sigma:
P: [0.00001, 0.00001, 0.00001]
O: [0.01, 0.01, 0.01]
@@ -64,16 +61,6 @@ config:
type: "SubscriberCamera"
topic: "/cam0/image_rect"
sensor_name: "CAMERA"
- -
- package: "wolf_ros_imu"
- type: "SubscriberImu"
- topic: "/imu0"
- sensor_name: "IMU"
- # imu_x_axis: 3 # See VI-sensor axes in https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets#platform_and_sensors ???
- # imu_y_axis: -2
- # imu_z_axis: 1
- cov_source: "sensor"
- in_degrees: false
ROS publisher:
-
@@ -104,15 +91,8 @@ config:
-
type: "PublisherVisionDebug"
processor_name: "VISUALODOMETRY PROC"
- topic: "/image_vo_tracks"
- topic_preprocessor: "/image_vo_tracks_preprocessor"
+ 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
diff --git a/yaml/processor_visual_odometry.yaml b/yaml/processor_visual_odometry.yaml
index 8fe26c1b51919d1d3b6bc85dca7d4f37be23f8a4..6fae6763ec665abb98874ed83e44a023fc8c4c47 100644
--- a/yaml/processor_visual_odometry.yaml
+++ b/yaml/processor_visual_odometry.yaml
@@ -3,6 +3,9 @@
####################################
time_tolerance: 0.05
+state_getter: false
+state_priority: 0
+
keyframe_vote:
voting_active: true
# Trigger a new keyframe creation as well as detection of new keypoints in last frame
@@ -77,5 +80,9 @@ ransac:
# standard deviation of the pixel reprojection factor
std_pix: 0.5
-# before creating a landmark, wait until the track is old enough
-min_track_length_for_landmark: 10
+# Rules to create a new landmark from a track
+lmk_creation:
+ # wait until the track is old enough
+ min_track_length_for_landmark: 10
+ # enough pixel distance (does not make a big difference)
+ min_pixel_dist: 10.0