From adfdf9cd5d51e4d373735e7598a3fa077770bb07 Mon Sep 17 00:00:00 2001
From: Idril Geer <igeer@iri.upc.edu>
Date: Wed, 26 Jan 2022 12:25:48 +0100
Subject: [PATCH] yamls

---
 .gitignore                              |  1 +
 launch/imu2d_demo.launch                |  2 +-
 rviz/imu2d_demo.rviz                    |  3 +-
 yaml/csm.yaml                           | 80 ++++++++++++++-----------
 yaml/csm_inactive.yaml                  | 76 +++++++++++++----------
 yaml/csm_old.yaml                       | 35 +++++++++++
 yaml/imu2d_test1.yaml                   | 15 +----
 yaml/imu2d_test4.yaml                   | 31 +++++-----
 yaml/solver.yaml                        | 13 ++++
 yaml/test_imu_subscriber.yaml           | 26 ++++----
 yaml/test_laser_processor.yaml          |  3 +-
 yaml/test_laser_processor_inactive.yaml |  3 +-
 12 files changed, 176 insertions(+), 112 deletions(-)
 create mode 100644 .gitignore
 create mode 100644 yaml/csm_old.yaml
 create mode 100644 yaml/solver.yaml

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..85c6b68
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+bag/*
diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index 7d5a136..ce4520c 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -12,7 +12,7 @@
     <arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
     <!-- using "clock" option to use the simulated time. Rosbag launch as the first node  -->
 
-    <node pkg="rostopic" type="rostopic" name="" args='pub /helena/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
+    <node pkg="rostopic" type="rostopic" name="" args='pub /ana/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
 
     <node pkg="tf"
           type="static_transform_publisher"
diff --git a/rviz/imu2d_demo.rviz b/rviz/imu2d_demo.rviz
index 947506c..921254e 100644
--- a/rviz/imu2d_demo.rviz
+++ b/rviz/imu2d_demo.rviz
@@ -11,7 +11,6 @@ Panels:
         - /LaserScan1
         - /Landmarks1
         - /Factors1
-        - /Factors1/Status1
         - /Factors1/Namespaces1
         - /Trajectory1
         - /Trajectory1/Namespaces1
@@ -345,7 +344,7 @@ Visualization Manager:
       Enabled: true
       History Length: 1
       Name: Imu
-      Topic: /helena/sensors/bno055_imu/imu_remapped
+      Topic: /helena/sensors/imu/data
       Unreliable: false
       Value: true
   Enabled: true
diff --git a/yaml/csm.yaml b/yaml/csm.yaml
index 59f62f6..2dd8f86 100644
--- a/yaml/csm.yaml
+++ b/yaml/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/csm_inactive.yaml b/yaml/csm_inactive.yaml
index b99171d..b5336a9 100644
--- a/yaml/csm_inactive.yaml
+++ b/yaml/csm_inactive.yaml
@@ -1,35 +1,47 @@
-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
+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
 
 cov_factor:                          9999999999999999999999
diff --git a/yaml/csm_old.yaml b/yaml/csm_old.yaml
new file mode 100644
index 0000000..59f62f6
--- /dev/null
+++ b/yaml/csm_old.yaml
@@ -0,0 +1,35 @@
+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
diff --git a/yaml/imu2d_test1.yaml b/yaml/imu2d_test1.yaml
index b674e10..5491584 100644
--- a/yaml/imu2d_test1.yaml
+++ b/yaml/imu2d_test1.yaml
@@ -26,18 +26,8 @@ config:
     plugin: "core"
       
   solver:
-    max_num_iterations: 10
-    verbose: 0
-    period: 0
-    update_immediately: false
-    n_threads: 2
-    compute_cov: false
-    function_tolerance: 1e-6
-    gradient_tolerance: 1e-6
-    minimizer: "LEVENBERG_MARQUARDT"
-    use_nonmonotonic_steps: False
-    max_consecutive_nonmonotonic_steps: 10
-    
+    follow: "solver.yaml"
+
   sensors:
     -
       type: "SensorOdom2d"
@@ -135,4 +125,3 @@ config:
       p_free_th: 0.2
       p_obst_th: 0.9
       discard_max_range: true
-
diff --git a/yaml/imu2d_test4.yaml b/yaml/imu2d_test4.yaml
index e9dd597..a338b35 100644
--- a/yaml/imu2d_test4.yaml
+++ b/yaml/imu2d_test4.yaml
@@ -2,8 +2,12 @@ config:
   debug:
     profiling: true
     profiling_file: "~/wolf_demo_profiling_imu2d_test4.txt"
-    print_problem: false
+    print_problem: true
     print_period: 5
+    print_depth: 4
+    print_constr_by: false
+    print_metric: true
+    print_state_blocks: true
   problem:
     tree_manager:
       type: "none"
@@ -16,14 +20,13 @@ config:
         O: [0]
         V: [0,0]
       time_tolerance: 0.1
+    node_rate: 100 
+  map:
+    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:
     -
@@ -31,11 +34,11 @@ config:
       name: "scanner_front_left"
       plugin: "laser"
       follow: "test_laser_params.yaml"
-    -
-      type: "SensorImu2d"
-      name: "bno"
-      plugin: "imu"
-      follow: "test_imu_params.yaml" 
+        # -
+        #   type: "SensorImu2d"
+        #   name: "bno"
+        #   plugin: "imu"
+        #   follow: "test_imu_params.yaml" 
     -
       type: "SensorImu2d"
       name: "microstrain"
@@ -66,7 +69,7 @@ config:
     -
       package: "wolf_ros_laser"
       type: "SubscriberLaser2d"
-      topic: "/ana/sensors/scan"
+      topic: "/helena/sensors/scan"
       sensor_name: "scanner_front_left"
       load_params_from_msg: true
     -
@@ -76,7 +79,7 @@ config:
       #topic: "/ana/sensors/bno055_imu/imu"
       #sensor_name: "bno"
       #microstrain
-      topic: "/ana/imu/data"
+      topic: "/helena/sensors/imu/data"
       sensor_name: "microstrain"
       follow: "test_imu_subscriber.yaml"
 
diff --git a/yaml/solver.yaml b/yaml/solver.yaml
new file mode 100644
index 0000000..ef04fd4
--- /dev/null
+++ b/yaml/solver.yaml
@@ -0,0 +1,13 @@
+minimizer: LEVENBERG_MARQUARDT
+max_num_iterations: 20
+verbose: 0
+period: 0.0
+n_threads: 2
+interrupt_on_problem_change: false
+min_num_iterations: 5 #if update immediately
+compute_cov: false
+cov_enum: 3 # if compute_cov
+cov_period: 1 # if compute_cov
+function_tolerance: 1e-8
+gradient_tolerance: 1e-9
+use_nonmonotonic_steps: false
diff --git a/yaml/test_imu_subscriber.yaml b/yaml/test_imu_subscriber.yaml
index cd15bf7..8aa2403 100644
--- a/yaml/test_imu_subscriber.yaml
+++ b/yaml/test_imu_subscriber.yaml
@@ -1,20 +1,20 @@
-#bno:
-dt: 0.016
-imu_x_axis: 3
-imu_y_axis: -2
-imu_z_axis: 1
-cov_source: "sensor"
-in_degrees: true
-##microstrain:
-#dt: 0.01
-#imu_x_axis: 1
+##bno:
+#dt: 0.016
+#imu_x_axis: 3
 #imu_y_axis: -2
-#imu_z_axis: -3
+#imu_z_axis: 1
 #cov_source: "sensor"
-#in_degrees: false
+#in_degrees: true
+#microstrain:
+dt: 0.01
+imu_x_axis: 1
+imu_y_axis: -2
+imu_z_axis: -3
+cov_source: "sensor"
+in_degrees: false
 
 #all
 topic_enable: "/ana/sensors/bno055_imu/imu/topic_enable"
-static_init_duration: 7
+static_init_duration: 1
 lowpass_filter: true
 lowpass_cutoff_freq: 5
diff --git a/yaml/test_laser_processor.yaml b/yaml/test_laser_processor.yaml
index d2aacee..ae63e5c 100644
--- a/yaml/test_laser_processor.yaml
+++ b/yaml/test_laser_processor.yaml
@@ -7,7 +7,8 @@ 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: true
 state_priority: 10
diff --git a/yaml/test_laser_processor_inactive.yaml b/yaml/test_laser_processor_inactive.yaml
index a584d2c..38d7008 100644
--- a/yaml/test_laser_processor_inactive.yaml
+++ b/yaml/test_laser_processor_inactive.yaml
@@ -7,7 +7,8 @@ keyframe_vote:
   min_error: 999
   max_points: 0
 max_new_features: 15
-follow: "csm_inactive.yaml"
+icp:
+  follow: "csm_inactive.yaml"
 time_tolerance: 0.05 #Half the sample time
 state_getter: true
 state_priority: 10
-- 
GitLab