diff --git a/launch/imu2d_gravity_demo.launch b/launch/imu2d_gravity_demo.launch
index 848352beb6a453e1d56ff36cb7fbb0c753f1cc27..ad52a4e6336e17a27b7242b0e5ad2e18e9da9676 100644
--- a/launch/imu2d_gravity_demo.launch
+++ b/launch/imu2d_gravity_demo.launch
@@ -37,7 +37,7 @@
<node pkg="rosbag"
type="play"
name="player"
- args="-s $(arg sec) -r $(arg speed) -k --clock $(find wolf_demo_imu2d)/bag/Sloped/$(arg bag).bag"/>
+ args="-s $(arg sec) -r $(arg speed) -k --clock $(find wolf_demo_imu2d)/bag/$(arg bag).bag"/>
<node type="rviz"
name="rviz"
diff --git a/rviz/imu2d_demo.rviz b/rviz/imu2d_demo.rviz
index 921254e521956da2cbd5c00ed0a0425943f34de3..2de424efb9ccad0b5ad0d1e4a26f0fb3bd33e140 100644
--- a/rviz/imu2d_demo.rviz
+++ b/rviz/imu2d_demo.rviz
@@ -249,7 +249,7 @@ Visualization Manager:
Color: 250; 84; 255
Color Transformer: FlatColor
Decay Time: 100
- Enabled: true
+ Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
@@ -264,7 +264,7 @@ Visualization Manager:
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
- Value: true
+ Value: false
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /wolf_ros_node/graph_landmarks
@@ -278,10 +278,14 @@ Visualization Manager:
Marker Topic: /wolf_ros_node/graph_factors
Name: Factors
Namespaces:
+ factors_processorimu2dmicro: true
factors_processorodom2d: true
factors_processorodomicp: true
+ factors_text_processorimu2dmicro: false
factors_text_processorodom2d: false
factors_text_processorodomicp: false
+ factors_text_unnamed_processor: false
+ factors_unnamed_processor: true
Queue Size: 100
Value: true
- Class: rviz/MarkerArray
@@ -289,8 +293,8 @@ Visualization Manager:
Marker Topic: /wolf_ros_node/graph_trajectory
Name: Trajectory
Namespaces:
- frames: true
- frames_text: true
+ frames: false
+ frames_text: false
Queue Size: 100
Value: true
- Class: rviz/Axes
@@ -382,9 +386,9 @@ Visualization Manager:
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: 0.7309823632240295
- Y: -0.0005876626819372177
- Z: 1.5242977142333984
+ X: 5.9762163162231445
+ Y: 1.321905493736267
+ Z: -1.1534262895584106
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
diff --git a/yaml/Gravity/csm.yaml b/yaml/Gravity/csm.yaml
index 59f62f628e15cb484f40d3b0ce50b404dfdd317d..2dd8f8680123cbf977c7bd7defcb0c65dd1d33f5 100644
--- a/yaml/Gravity/csm.yaml
+++ b/yaml/Gravity/csm.yaml
@@ -1,35 +1,45 @@
-max_iterations: 50
-max_correspondence_dist: 10
-use_corr_tricks: 1
-outliers_maxPerc: 0.9
-use_point_to_line_distance: 1
-outliers_adaptive_order: 0.7
-outliers_adaptive_mult: 1.5
-do_compute_covariance: 1
-
-max_angular_correction_deg: 10
-max_linear_correction: 1
-epsilon_xy: 0
-epsilon_theta: 0
-sigma: 0.2
-restart: 0
-restart_threshold_mean_error: 0
-restart_dt: 0
-restart_dtheta: 0
-clustering_threshold: 0
-orientation_neighbourhood: 0
-do_alpha_test: 0
-do_alpha_test_thresholdDeg: 0
-do_visibility_test: 0
-outliers_remove_doubles: 0
-debug_verify_tricks: 0
-gpm_theta_bin_size_deg: 0
-gpm_extend_range_deg: 0
-gpm_interval: 0
-min_reading: 0.023
-max_reading: 60
-use_ml_weights: 0
-use_sigma_weights: 0
-
-
-cov_factor: 1
+verbose: false # prints debug messages
+
+# ALGORITHM OPTIONS
+use_point_to_line_distance: true # use PlICP (true) or use vanilla ICP (false).
+max_angular_correction_deg: 180 # Maximum angular displacement between scans (deg)
+max_linear_correction: 10 # Maximum translation between scans (m)
+
+max_correspondence_dist: 0.5 # Maximum distance for a correspondence to be valid
+use_corr_tricks: true # Use smart tricks for finding correspondences. Only influences speed; not convergence.
+debug_verify_tricks: false # Checks that find_correspondences_tricks give the right answer
+
+# STOP CRITERIA
+max_iterations: 50 # maximum iterations
+epsilon_xy: 1e-4 # distance change
+epsilon_theta: 1e-3 # angle change
+
+# RESTART
+restart: false # Restart algorithm
+restart_threshold_mean_error: 0 # Threshold for restarting
+restart_dt: 0 # Displacement for restarting
+restart_dtheta: 0 # Displacement for restarting
+
+# DISCARDING POINTS/CORRESPONDENCES
+min_reading: 0.023 # discard rays outside of this interval
+max_reading: 60 # discard rays outside of this interval
+outliers_maxPerc: 0.9 # Percentage of correspondences to consider: if 0.9, always discard the top 10% of correspondences with more error
+outliers_adaptive_order: 0.7 # Parameters describing a simple adaptive algorithm for discarding
+outliers_adaptive_mult: 1.5 # Parameters describing a simple adaptive algorithm for discarding
+outliers_remove_doubles: false # Do not allow two different correspondences to share a point
+do_visibility_test: false # If initial guess, visibility test can discard points that are not visible
+do_alpha_test: false # Discard correspondences based on the angles
+do_alpha_test_thresholdDeg: 10 #
+
+# POINT ORIENTATION
+clustering_threshold: 0.5 # Max-distance clustering for point orientation
+orientation_neighbourhood: 4 # Number of neighbour rays used to estimate the orientation
+
+# WEIGHTS
+use_ml_weights: false # weight the impact of each correspondence. This works fabolously if the first scan has no noise.
+use_sigma_weights: false # If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2
+sigma: 0.2 # Noise of the scan
+
+# COVARIANCE
+do_compute_covariance: true
+cov_factor: 5 # Factor multiplying the cov output of csm
diff --git a/yaml/Gravity/imu2d_gravity_test1.yaml b/yaml/Gravity/imu2d_gravity_test1.yaml
index b6509469d423dc8ecf4e97f8fc9216c81250927d..88af1787910a2c1537bc31ceb8ca8320b1b5b16e 100644
--- a/yaml/Gravity/imu2d_gravity_test1.yaml
+++ b/yaml/Gravity/imu2d_gravity_test1.yaml
@@ -1,9 +1,13 @@
config:
debug:
profiling: true
- profiling_file: "~/wolf_demo_profiling_imu2d_test1.txt"
+ profiling_file: "~/wolf_demo_profiling_imu2d_test1_gravity.txt"
print_problem: false
- print_period: 5
+ print_period: 2
+ print_depth: 4
+ print_constr_by: false
+ print_metric: true
+ print_state_blocks: true
problem:
tree_manager:
type: "none"
@@ -16,18 +20,13 @@ config:
O: [0]
V: [0,0]
time_tolerance: 0.1
+ node_rate: 100
map:
- type: "MapGrid2dGravity"
- plugin: "imu"
- resolution: 4
+ type: "MapBase"
+ plugin: "core"
solver:
- max_num_iterations: 100
- verbose: 0
- period: 0
- update_immediately: false
- n_threads: 2
- compute_cov: false
+ follow: "../solver.yaml"
sensors:
-
diff --git a/yaml/Gravity/test_imu_processor.yaml b/yaml/Gravity/test_imu_processor.yaml
index 3ea39004a54e1051b0d791ec931992eaa5a499af..6e0ef2737d4ff6289e6d2d189d87508ee7b0ac60 100644
--- a/yaml/Gravity/test_imu_processor.yaml
+++ b/yaml/Gravity/test_imu_processor.yaml
@@ -10,3 +10,4 @@ keyframe_vote:
state_getter: true
state_priority: 1
apply_loss_function: false
+use_gravity_grid: false
diff --git a/yaml/Gravity/test_imu_subscriber.yaml b/yaml/Gravity/test_imu_subscriber.yaml
index 0a77d7c3e92e6afd7babda98e5cb615db9b4bfd1..01519126b65d517fd2e32ab31ef7229622c9c6ce 100644
--- a/yaml/Gravity/test_imu_subscriber.yaml
+++ b/yaml/Gravity/test_imu_subscriber.yaml
@@ -15,6 +15,6 @@ in_degrees: false
#all
topic_enable: "/helena/sensors/bno055_imu/imu/topic_enable"
-static_init_duration: 5
+static_init_duration: 1
lowpass_filter: true
lowpass_cutoff_freq: 5
diff --git a/yaml/Gravity/test_laser_processor.yaml b/yaml/Gravity/test_laser_processor.yaml
index 06847e9355446b14a201ee058fd0c5617f027897..ae63e5c449a9c38b02edfae415b5aab12d67e98b 100644
--- a/yaml/Gravity/test_laser_processor.yaml
+++ b/yaml/Gravity/test_laser_processor.yaml
@@ -7,9 +7,10 @@ keyframe_vote:
min_error: 999
max_points: 0
max_new_features: 15
-follow: "csm.yaml"
+icp:
+ follow: "csm.yaml"
time_tolerance: 0.05 #Half the sample time
-state_getter: false
+state_getter: true
state_priority: 10
apply_loss_function: false
initial_guess: "state"
diff --git a/yaml/test_imu_params.yaml b/yaml/test_imu_params.yaml
index 70f8799fa9290a55fc126a629ce3f728f10c1cde..9a08c32fa9b924502494ba43f614840a368e10f5 100644
--- a/yaml/test_imu_params.yaml
+++ b/yaml/test_imu_params.yaml
@@ -17,4 +17,4 @@ ab_initial_stdev: 0.5 # m/s2 - initial bias
wb_initial_stdev: 0.1 # rad/sec - initial bias
ab_rate_stdev: 0.0001 # m/s2/sqrt(s)
wb_rate_stdev: 0.0001 # rad/s/sqrt(s)
-orthogonal_gravity: true
+orthogonal_gravity: false