update yamls

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"

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

yaml/Gravity/csm.yaml

-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

yaml/Gravity/imu2d_gravity_test1.yaml

 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:
     -

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

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

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" 

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