From 1d0e03bb6adf387cee1ccba8ebe802d1b3537370 Mon Sep 17 00:00:00 2001
From: Mederic Fourmy <mederic.fourmy@gmail.com>
Date: Wed, 14 Sep 2022 12:41:02 +0200
Subject: [PATCH] [skip-ci] Adapt to misc changes in plugins + tuning
---
yaml/demo_vio_apriltag_d435i.yaml | 55 +++++++++----------
yaml/demo_vio_d435i.yaml | 25 +++------
yaml/demo_vio_euroc.yaml | 19 ++-----
yaml/imu_d435i.yaml | 15 ++---
yaml/imu_euroc.yaml | 2 +
yaml/processor_imu.yaml | 6 +-
yaml/processor_tracker_landmark_apriltag.yaml | 27 +++++----
yaml/processor_visual_odometry.yaml | 2 +-
yaml/solver.yaml | 2 +-
9 files changed, 74 insertions(+), 79 deletions(-)
diff --git a/yaml/demo_vio_apriltag_d435i.yaml b/yaml/demo_vio_apriltag_d435i.yaml
index c0ae8fe..aef3405 100644
--- a/yaml/demo_vio_apriltag_d435i.yaml
+++ b/yaml/demo_vio_apriltag_d435i.yaml
@@ -2,8 +2,8 @@ config:
debug:
profiling: true
- profiling_file: "~/wolf_demo_vio_profiling.txt"
- print_problem: true
+ profiling_file: "~/wolf_demo_vo_apriltag_profiling.txt"
+ print_problem: false
print_depth: 3 # only if print_problem
print_constr_by: true # only if print_problem
print_metric: true # only if print_problem
@@ -11,7 +11,7 @@ config:
print_period: 5 # only if print_problem
problem:
- node_rate: 20
+ node_rate: 200
follow: "tree_manager_sliding.yaml"
frame_structure: "POV"
dimension: 3
@@ -19,13 +19,13 @@ config:
mode: "initial_guess"
$state:
P: [0,0,0]
- O: [1,0,0,0] #Â arbitrary
+ O: [1,0,0,0] #Â arbitrary -> modified by IMU init
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
+ P: [42, 42, 42] # arbitrary because initial_guess
+ O: [42, 42, 42]
+ V: [42, 42, 42]
+ time_tolerance: 0.005
solver:
follow: "solver.yaml"
@@ -37,7 +37,14 @@ config:
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: "SensorCamera"
+ name: "CAMERA APRIL"
+ plugin: "vision"
+ extrinsic:
+ pose: [-0.006, 0.005, 0.012, 0,0,0,1] # D435i left infra
using_raw: false
follow: "camera_d435i.yaml"
-
@@ -47,7 +54,7 @@ config:
extrinsic:
pose: [0,0,0, 0,0,0,1]
follow: "imu_d435i.yaml"
-
+
processors:
-
type: "ProcessorVisualOdometry"
@@ -58,7 +65,7 @@ config:
-
type: "ProcessorTrackerLandmarkApriltag"
name: "APRILTAG PROC"
- sensor_name: "CAMERA"
+ sensor_name: "CAMERA APRIL"
plugin: "apriltag"
follow: "processor_tracker_landmark_apriltag.yaml"
-
@@ -67,9 +74,6 @@ config:
sensor_name: "IMU"
plugin: "imu"
follow: "processor_imu.yaml"
- state_getter: true
- state_priority: 1
-
ROS subscriber:
-
@@ -77,6 +81,11 @@ 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"
-
package: "wolf_ros_imu"
type: "SubscriberImu"
@@ -99,8 +108,7 @@ config:
frame_length: 0.1
frame_vel_scale: 1.0
factors_width: 0.01
- factor_lmk_color: [0, 1, 1, 0.005]
-
+ factor_lmk_color: [0, 1, 1, 0.004]
-
type: "PublisherTf"
topic: " "
@@ -110,27 +118,18 @@ config:
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.1
+ 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
+ period: 0.05
+
diff --git a/yaml/demo_vio_d435i.yaml b/yaml/demo_vio_d435i.yaml
index e341ca8..881487f 100644
--- a/yaml/demo_vio_d435i.yaml
+++ b/yaml/demo_vio_d435i.yaml
@@ -11,7 +11,7 @@ config:
print_period: 5 # only if print_problem
problem:
- node_rate: 20
+ node_rate: 200
follow: "tree_manager_sliding.yaml"
frame_structure: "POV"
dimension: 3
@@ -19,13 +19,13 @@ config:
mode: "initial_guess"
$state:
P: [0,0,0]
- O: [1,0,0,0] #Â arbitrary
+ O: [1,0,0,0] #Â arbitrary -> modified by IMU init
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
+ P: [42, 42, 42] # arbitrary because initial_guess
+ O: [42, 42, 42]
+ V: [42, 42, 42]
+ time_tolerance: 0.005
solver:
follow: "solver.yaml"
@@ -92,8 +92,7 @@ config:
frame_length: 0.1
frame_vel_scale: 1.0
factors_width: 0.01
- factor_lmk_color: [0, 1, 1, 0.005]
-
+ factor_lmk_color: [0, 1, 1, 0.004]
-
type: "PublisherTf"
topic: " "
@@ -103,7 +102,6 @@ config:
odom_frame_id: "odom"
base_frame_id: "base_footprint"
publish_odom_tf: true
-
-
type: "PublisherVisionDebug"
processor_name: "VISUALODOMETRY PROC"
@@ -111,11 +109,4 @@ config:
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
+ follow: "publisher_vision_debug.yaml"
\ No newline at end of file
diff --git a/yaml/demo_vio_euroc.yaml b/yaml/demo_vio_euroc.yaml
index b765e88..80cba3b 100644
--- a/yaml/demo_vio_euroc.yaml
+++ b/yaml/demo_vio_euroc.yaml
@@ -11,7 +11,7 @@ config:
print_period: 1 # only if print_problem
problem:
- node_rate: 20
+ node_rate: 200
follow: "tree_manager_sliding.yaml"
frame_structure: "POV"
dimension: 3
@@ -19,13 +19,13 @@ config:
mode: "initial_guess"
$state:
P: [0,0,0]
- O: [1,0,0,0] #Â arbitrary
+ O: [1,0,0,0] #Â arbitrary -> modified by IMU init
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
+ P: [42, 42, 42] # arbitrary because initial_guess
+ O: [42, 42, 42]
+ V: [42, 42, 42]
+ time_tolerance: 0.005
solver:
follow: "solver.yaml"
@@ -116,10 +116,3 @@ config:
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/imu_d435i.yaml b/yaml/imu_d435i.yaml
index 45c454b..94822b1 100644
--- a/yaml/imu_d435i.yaml
+++ b/yaml/imu_d435i.yaml
@@ -1,7 +1,8 @@
-# IMU intrinsics:
-a_noise: 0.03 # standard deviation of Acceleration noise (same for all the axis) in m/s2
-w_noise: 0.05 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
-ab_initial_stdev: 0.1 # m/s2 - initial bias
-wb_initial_stdev: 0.1 # rad/sec - initial bias
-ab_rate_stdev: 0.001 # m/s2/sqrt(s)
-wb_rate_stdev: 0.001 # rad/s/sqrt(s)
+a_noise: 0.02 # standard deviation of Acceleration noise (same for all the axis) in m/s2
+w_noise: 0.03 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+ab_initial_stdev: 0.00042 # m/s2 - initial bias
+wb_initial_stdev: 0.00042 # rad/sec - initial bias
+ab_rate_stdev: 0.0001 # m/s2/sqrt(s)
+wb_rate_stdev: 0.0001 # rad/s/sqrt(s)
+
+dynamic_imu_bias: false
diff --git a/yaml/imu_euroc.yaml b/yaml/imu_euroc.yaml
index 45c454b..07fbe54 100644
--- a/yaml/imu_euroc.yaml
+++ b/yaml/imu_euroc.yaml
@@ -5,3 +5,5 @@ ab_initial_stdev: 0.1 # m/s2 - initial bias
wb_initial_stdev: 0.1 # rad/sec - initial bias
ab_rate_stdev: 0.001 # m/s2/sqrt(s)
wb_rate_stdev: 0.001 # rad/s/sqrt(s)
+
+dynamic_imu_bias: false
diff --git a/yaml/processor_imu.yaml b/yaml/processor_imu.yaml
index 18567f3..2bfb2c1 100644
--- a/yaml/processor_imu.yaml
+++ b/yaml/processor_imu.yaml
@@ -1,6 +1,8 @@
time_tolerance: 0.0025 # Time tolerance for joining KFs (half of 5ms or 200Hz)
unmeasured_perturbation_std: 0.000001
apply_loss_function: false
+state_getter: true
+state_priority: 1
keyframe_vote:
voting_active: false
@@ -10,7 +12,7 @@ keyframe_vote:
angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
bootstrap:
- enable: false
+ enable: true
method: "G" # methods: "G", "STATIC" or "V0_G"
- averaging_length: 0.10 # seconds
+ averaging_length: 0.02 # seconds
keyframe_provider_processor_name: "processor_other_name" # Not yet implemented
\ No newline at end of file
diff --git a/yaml/processor_tracker_landmark_apriltag.yaml b/yaml/processor_tracker_landmark_apriltag.yaml
index c26e0b1..b046d5d 100644
--- a/yaml/processor_tracker_landmark_apriltag.yaml
+++ b/yaml/processor_tracker_landmark_apriltag.yaml
@@ -1,10 +1,23 @@
+# ProcessorBase
+time_tolerance: 0.02
+apply_loss_function: false
+
+# ProcessorTracker
+max_new_features: -1 # max number of features detected in detectNewFeatures() (-1: unlimited)
+
+# MotionProvider
+state_getter: true
+state_priority: 2
+
# detector parameters:
quad_decimate: 0.0 # doing quad detection at lower resolution to speed things up (see end of file)
+# quad_decimate: 1.5 # 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?)
+nthreads: 8 # how many thread during tag detection
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_min_ratio: 10 # 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
@@ -13,20 +26,18 @@ tag_black_border: 1 # usually 1, 2 for Kalibr aprilgrid for ex
# 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 $$
+tag_width_default: 0.168 # used if tag width not specified -- LAAS, 07/22 $$
# 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
+use_proj_factor: true
# used for covariance propagation
std_pix: 4 # pixel error $$
-time_tolerance: 0.02
-
keyframe_vote:
voting_active: true
nb_vote_for_every_first: 1
@@ -34,10 +45,6 @@ keyframe_vote:
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
###############
diff --git a/yaml/processor_visual_odometry.yaml b/yaml/processor_visual_odometry.yaml
index 6fae676..ca61662 100644
--- a/yaml/processor_visual_odometry.yaml
+++ b/yaml/processor_visual_odometry.yaml
@@ -85,4 +85,4 @@ 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
+ min_pixel_dist: 4.0
diff --git a/yaml/solver.yaml b/yaml/solver.yaml
index 4d500f7..a3b1c83 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: 1
+n_threads: 4
compute_cov: false
verbose: 0
\ No newline at end of file
--
GitLab