wip

bag/.gitignore

 *.bag
 CSV*/
+csv/
 recordings/
 !analysys/
 !.gittignore

bag/generate_csvs.bash

+#!/bin/bash
+
+SPEED=0.5
+EXPERIMENTS=('inside' \
+             'outside1' \
+             'outside2' \
+             'outside3' \
+             'outside4' \
+             'outside5' \
+             'outside6')
+BAGS=('ana_lab_2' \
+      'data_2021-07-27-11-31-13' \
+      'data_2021-07-27-11-34-18' \
+      'data_2021-07-27-11-36-31' \
+      'data_2021-07-27-11-39-04' \
+      'data_2021-07-29-13-17-57' \
+      'data_2021-07-29-13-20-21')
+INDEXS=(1)
+ROBOTS=('ana'\
+        'helena' \
+        'helena' \
+        'helena' \
+        'helena' \
+        'helena' \
+        'helena')
+
+#There are three methods:
+#   Starting with E ==> experiment in 2D: odom_icp + imu2d
+#   Starting with T ==> experiment in 3D: odom_icp(3D) + imu3d
+#   Starting with S ==> experiment in 2D: odom_icp + imu2d sloped
+METHODS=("E" "T" "S") 
+
+#make dirs
+mkdir -pv csv
+mkdir -pv recordings
+
+# all experiments will be executed
+for i in "${INDEXS[@]}"; do
+  echo "Experiment ${i}: ${EXPERIMENTS[i]}..."
+  for method in "${METHODS[@]}"; do
+    echo "Method $method..."
+
+    #The roslaunch file takes several useful parameters. We can lower speed if we see that the processor isn't working at real time. We disable rviz as we don't need to see anything.
+    roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=${BAGS[i]} test:=$method speed:=$SPEED robot:=${ROBOTS[i]} record_name:=${EXPERIMENTS[i]} rviz:=false
+
+    #Generate CSVs
+    rostopic echo -b recordings/${EXPERIMENTS[i]}_${method}.bag /wolf_ros_node/odom_icp -p > csv/${EXPERIMENTS[i]}_odom_icp_${method}.csv
+    rostopic echo -b recordings/${EXPERIMENTS[i]}_${method}.bag /imu_micro_pose_from_origin -p > csv/${EXPERIMENTS[i]}_odom_imu_${method}.csv
+    rostopic echo -b recordings/${EXPERIMENTS[i]}_${method}.bag /imu_micro_bias_accel -p > csv/${EXPERIMENTS[i]}_bias_accel_${method}.csv
+    rostopic echo -b recordings/${EXPERIMENTS[i]}_${method}.bag /imu_micro_bias_gyro -p > csv/${EXPERIMENTS[i]}_bias_gyro_${method}.csv
+
+    #Move profiling file
+    mv ~/profiling/wolf_demo_profiling_${method}.txt recordings/profiling_${EXPERIMENTS[i]}_${method}.txt
+  done
+done
\ No newline at end of file

launch/imu2d_analysys.launch

@@ -6,9 +6,10 @@
     <arg name="sec" default="0" />
     <arg name="profiling" default="false" />
     <arg name="gdb" default="false" />
-    <arg name="test" default="1" />
+    <arg name="test" default="E" />
     <arg name="bag" default="test_no_map" />
-    <arg name="suffix" default="" />
+    <arg name="record_suffix" default="" />
+    <arg name="record_name" default="experiment" />
     <arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
     <arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
     <arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
@@ -22,7 +23,7 @@
       <node pkg="rosbag" 
             type="record" 
             name="recorder" 
-            args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag 
+            args='record -O $(find wolf_demo_imu2d)/bag/recordings/$(arg record_name)_$(arg test)$(arg record_suffix).bag 
                   "/wolf_ros_node/odom_icp"
                   "/imu_micro_bias_accel"
                   "/imu_micro_bias_gyro"
@@ -87,6 +88,6 @@
           required="true"
           output="screen"
           launch-prefix="$(arg launch_pref)">
-    <param name="~yaml_file_path" value="$(find wolf_demo_imu2d)/yaml/trajectory_analysys/$(arg test)" />
+    <param name="~yaml_file_path" value="$(find wolf_demo_imu2d)/yaml/trajectory_analysys/$(arg test).yaml" />
     </node>
 </launch>

rviz/imu2d_demo_ana.rviz

@@ -11,7 +11,7 @@ Panels:
         - /Factors1/Namespaces1
         - /Trajectory1/Namespaces1
       Splitter Ratio: 0.708737850189209
-    Tree Height: 839
+    Tree Height: 689
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -147,9 +147,9 @@ Visualization Manager:
       Marker Topic: /wolf_ros_node/graph_factors
       Name: Factors
       Namespaces:
-        factors_processorimumicro: true
+        factors_processorimu2dmicro: true
         factors_processorodomicp: true
-        factors_text_processorimumicro: false
+        factors_text_processorimu2dmicro: true
         factors_text_processorodomicp: false
         factors_text_unnamed_processor: false
         factors_unnamed_processor: true
@@ -264,9 +264,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.2547972500324249
+      Pitch: 1.354796290397644
       Target Frame: map
-      Yaw: 4.588164329528809
+      Yaw: 4.453161239624023
     Saved:
       - Angle: 0
         Class: rviz/TopDownOrtho
@@ -313,10 +313,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1136
+  Height: 986
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000205000003d2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007420000003efc0100000002fb0000000800540069006d0065010000000000000742000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000537000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002050000033cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000033c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006560000003efc0100000002fb0000000800540069006d0065010000000000000656000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000044b0000033c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -325,6 +325,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1858
-  X: 62
+  Width: 1622
+  X: 58
   Y: 27

yaml/trajectory_analysys/E.yaml

 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_E-5_0.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_E.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
@@ -14,11 +14,15 @@ config:
     frame_structure: "POV"
     dimension: 2
     prior:
-      mode: "fix"
+      mode: "factor"
       $state:
         P: [0,0]
         O: [0]
         V: [0,0]
+      $sigma:
+        P: [0.01, 0.01]
+        O: [1]
+        V: [0.01, 0.01]
       time_tolerance: 0.1
     node_rate: 100
       
@@ -120,13 +124,13 @@ config:
     #   topic: "trajectory"
     #   period: 1
     #   frame_id: "map"
-    -
-      package: "wolf_ros_node"
-      type: "PublisherPose"
-      topic: "pose"
-      extrinsics: false
-      period: 0.1
-      frame_id: "map"
+    # -
+    #   package: "wolf_ros_node"
+    #   type: "PublisherPose"
+    #   topic: "pose"
+    #   extrinsics: false
+    #   period: 0.1
+    #   frame_id: "map"
     # -
     #   package: "wolf_ros_laser"
     #   type: "PublisherLaserMap"

yaml/trajectory_analysys/G.yaml

 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_G-5_0.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_G.txt"
     print_problem: false
     print_period: 2
     print_depth: 4

yaml/trajectory_analysys/S.yaml

 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_S-5_0.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_S.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
@@ -14,20 +14,20 @@ config:
     frame_structure: "POV"
     dimension: 2
     prior:
-      mode: "fix"
+      mode: "factor"
       $state:
         P: [0,0]
         O: [0]
         V: [0,0]
+      $sigma:
+        P: [0.01, 0.01]
+        O: [1]
+        V: [0.01, 0.01]
       time_tolerance: 0.1
     node_rate: 100
-  map:
-    type: "MapBase"
-    plugin: "core"
       
   solver:
     follow: "parameters/solver.yaml"
-
     
   sensors:
     -
@@ -35,16 +35,18 @@ config:
       name: "scanner_front_left"
       plugin: "laser"
       follow: "parameters/test_laser_params.yaml"
-    -
-      type: "SensorImu2d"
-      name: "bno"
-      plugin: "imu"
-      follow: "parameters/test_imu_params_bno.yaml" 
+    # -
+    #   type: "SensorImu2d"
+    #   name: "bno"
+    #   plugin: "imu"
+    #   orthogonal_gravity: true
+    #   follow: "parameters/test_imu_params_bno.yaml" 
     -
       type: "SensorImu2d"
       name: "microstrain"
       plugin: "imu"
-      follow: "parameters/test_imu_params_microstrain_sloped.yaml" 
+      orthogonal_gravity: false
+      follow: "parameters/test_imu_params_microstrain.yaml" 
 
   processors:
     -
@@ -53,31 +55,12 @@ config:
       sensor_name: "scanner_front_left"
       plugin: "laser"
       follow: "parameters/test_laser_processor.yaml"
-        #-
-        #  type: "ProcessorLoopClosureIcp"
-        #  name: "processorloopclosureicp"
-        #  sensor_name: "scanner_front_left"
-        #  plugin: "laser"
-        #  time_tolerance: 0.1
-        #  apply_loss_function: true
-        #  keyframe_vote:
-        #    voting_active: false
-        #  recent_frames_ignored: 10
-        #  frames_ignored_after_loop: 0
-        #  max_error_threshold: 0.02
-        #  min_points_percent: 40
-        #  max_loops: 1
-        #  max_candidates: 5
-        #  max_attempts: 5
-        #  candidate_generation: "random" # 'random' or 'tree'
-        #  icp:
-        #    follow: "parameters/csm.yaml"
-    -
-      type: "ProcessorImu2d"
-      name: "processorimu2dbno"
-      sensor_name: "bno"
-      plugin: "imu"
-      follow: "parameters/test_imu_processor_bno.yaml"
+    # -
+    #   type: "ProcessorImu2d"
+    #   name: "processorimu2dbno"
+    #   sensor_name: "bno"
+    #   plugin: "imu"
+    #   follow: "parameters/test_imu_processor_bno.yaml"
     -
       type: "ProcessorImu2d"
       name: "processorimu2dmicro"
@@ -118,19 +101,19 @@ config:
       topic: "graph"
       period: 1
       viz_overlapped_factors: true
-    -
-      package: "wolf_ros_node"
-      type: "PublisherTrajectory"
-      topic: "trajectory"
-      period: 1
-      frame_id: "map"
-    -
-      package: "wolf_ros_node"
-      type: "PublisherPose"
-      topic: "pose"
-      extrinsics: false
-      period: 0.01
-      frame_id: "map"
+    # -
+    #   package: "wolf_ros_node"
+    #   type: "PublisherTrajectory"
+    #   topic: "trajectory"
+    #   period: 1
+    #   frame_id: "map"
+    # -
+    #   package: "wolf_ros_node"
+    #   type: "PublisherPose"
+    #   topic: "pose"
+    #   extrinsics: false
+    #   period: 0.1
+    #   frame_id: "map"
     -
       package: "wolf_ros_laser"
       type: "PublisherLaserMap"
@@ -146,4 +129,10 @@ config:
       p_free_th: 0.2
       p_obst_th: 0.9
       discard_max_range: true
+    -
+      package: "wolf_ros_laser"
+      type: "PublisherOdomIcp"
+      topic: "odom_icp"
+      period: 0.1
+      processor_name: "processorodomicp"
 

yaml/trajectory_analysys/T.yaml

 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_T-5_0.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_T.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
@@ -47,7 +47,7 @@ config:
       name: "microstrain"
       plugin: "imu"
       extrinsic:
-        pose: [0,0,0,0,0,0,1]  
+        pose: [0,0,0,0,0,0,1]
       follow: "parameters/test_imu_params_microstrain.yaml" 
 
   processors:
@@ -109,13 +109,13 @@ config:
     #   topic: "trajectory"
     #   period: 1
     #   frame_id: "map"
-    -
-      package: "wolf_ros_node"
-      type: "PublisherPose"
-      topic: "pose"
-      extrinsics: false
-      period: 0.1
-      frame_id: "map"
+    # -
+    #   package: "wolf_ros_node"
+    #   type: "PublisherPose"
+    #   topic: "pose"
+    #   extrinsics: false
+    #   period: 0.1
+    #   frame_id: "map"
     # -
     #   package: "wolf_ros_laser"
     #   type: "PublisherLaserMap"

yaml/trajectory_analysys/parameters/csm.yaml

@@ -42,4 +42,4 @@ sigma:                        0.2   # Noise of the scan
 
 # COVARIANCE
 do_compute_covariance:        true
-cov_factor:                   5     # Factor multiplying the cov output of csm
+cov_factor:                   1     # Factor multiplying the cov output of csm

yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml

-#microstrain
-extrinsic:
-  pose: [0,0,0]    
-a_noise: 0.05 #0.9
-w_noise: 0.01 #0.01
-ab_initial_stdev: 0.5     # m/s2    - initial bias
-wb_initial_stdev: 0.1     # rad/sec - initial bias
-ab_rate_stdev: 0.00001    # m/s2/sqrt(s)
-wb_rate_stdev: 0.00001    # rad/s/sqrt(s)
-orthogonal_gravity: false

yaml/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml

@@ -9,5 +9,5 @@ in_degrees: false
 #all
 topic_enable: "/imu_micro/enable"
 static_init_duration: 0
-lowpass_filter: false
+lowpass_filter: true
 lowpass_cutoff_freq: 5