From 4d47c0dae63a315bd0b39ca0c8d4ea7f96cb4940 Mon Sep 17 00:00:00 2001
From: Idril Geer <igeer@iri.upc.edu>
Date: Wed, 16 Feb 2022 13:08:54 +0100
Subject: [PATCH] update scripts

---
 bag/analysys/compare.m                        | 12 ++++++++--
 bag/analysys/extractdata.m                    |  4 ++--
 bag/generate_trajectory_csv_ana_lab_2.bash    | 10 +++++++--
 launch/imu2d_analysys.launch                  |  2 +-
 launch/imu2d_demo.launch                      |  2 +-
 rviz/imu2d_demo_ana.rviz                      | 22 ++++++++-----------
 yaml/trajectory_analysys/E.yaml               |  2 +-
 yaml/trajectory_analysys/G.yaml               |  2 +-
 yaml/trajectory_analysys/S.yaml               |  2 +-
 yaml/trajectory_analysys/T.yaml               |  2 +-
 .../test_imu_params_microstrain.yaml          |  2 +-
 .../test_imu_params_microstrain_sloped.yaml   |  2 +-
 .../parameters/test_laser_processor.yaml      |  2 +-
 .../parameters/test_publisher_ros_node.yaml   | 10 ++++-----
 14 files changed, 43 insertions(+), 33 deletions(-)

diff --git a/bag/analysys/compare.m b/bag/analysys/compare.m
index f810641..532e2da 100644
--- a/bag/analysys/compare.m
+++ b/bag/analysys/compare.m
@@ -2,7 +2,7 @@ clc; clear; clearvars; close all;
 %time at which to start counting data
 tstart=0;
 %Number of frames to cut
-N=8;
+N=0;
 
 %Folder where CSV files are saved
 CSVFolder = '../CSV/transp/';
@@ -21,6 +21,7 @@ end
 Gvec = [];
 Evec = [];
 Tvec = [];
+Svec = [];
 myFiles = dir(fullfile(CSVFolder,'*.csv')); %gets all csv files in folder
 for k = 1:length(myFiles)
     baseFileName = myFiles(k).name;
@@ -36,6 +37,9 @@ for k = 1:length(myFiles)
     elseif baseFileName(1) == 'T'
         [Tx, Ty, Tz, Tang, Tt, Tts] = extractdata(Table, tstart);
         Tvec = [Tvec, {baseFileName, Tx, Ty, Tz, Tang, Tt, Tts}'];
+    elseif baseFileName(1) == 'S'
+        [Sx, Sy, Sz, Sang, St, Sts] = extractdata(Table, tstart);
+        Svec = [Svec, {baseFileName, Sx, Sy, Sz, Sang, St, Sts}'];
     end
 end
 
@@ -46,7 +50,11 @@ min_len3d = 99999999999999999999999999999999999999999999999999999999999999999999
 for i = 1:size(Evec,2)
     %Get position and angular error vectors for exepriments comparing them
     %to GroundTruth
-    [perrors2d, angerrors2d, tstart2] = geterror(Gvec{7,i}, Evec{7,i}, N);
+    [perrors2d, angerrors2d, ~] = geterror(Gvec{7,i}, Evec{7,i}, N);
+    [perrors3d, angerrors3d, tstart2] = geterror(Gvec{7,i}, Tvec{7,i}, N);
+    if ~isempty(Svec)
+        
+    end
     [perrors3d, angerrors3d, tstart2] = geterror(Gvec{7,i}, Tvec{7,i}, N);
     tstart = max(tstart, tstart2);
 
diff --git a/bag/analysys/extractdata.m b/bag/analysys/extractdata.m
index 802975b..2db920a 100644
--- a/bag/analysys/extractdata.m
+++ b/bag/analysys/extractdata.m
@@ -9,8 +9,8 @@ function [gx, gy, gz, gang, gt, gts] = extractdata(GC, tstart)
     [R1, R2, R3] = quat2angle([GC{:,11} GC{:,8} GC{:,9} GC{:,10}]); %Convert quaternions to euler angles
     gang = [R1'; R2'; R3'];
     gt = GC{:,3}';
-    gt = gt-t0;
-    gt = gt*1e-9;
+    %gt = gt-t0;
+    %gt = gt*1e-9;
 
     %We cut all elements with a timestamp lower than tstart
     n = sum(gt<=tstart);
diff --git a/bag/generate_trajectory_csv_ana_lab_2.bash b/bag/generate_trajectory_csv_ana_lab_2.bash
index 947c410..0f3e393 100644
--- a/bag/generate_trajectory_csv_ana_lab_2.bash
+++ b/bag/generate_trajectory_csv_ana_lab_2.bash
@@ -12,9 +12,10 @@ fi
 #   Starting with G ==> groundtruth, we extract the trajectory
 #   Starting with E ==> experiment, we extract the poses
 #   Starting with T ==> experiment in 3D, we extract the poses
+#   Starting with S ==> experiment in 2D with slope, we extract the poses
 
 # this array lists the timespans in which the KeyFrames will be created. For each timespan the three experiments will be executed
-timespanarray=("0.1" "0.2" "0.3" "0.5" "0.7" "1.0" "2.0" "5.0" "10.0") 
+timespanarray=("1.0" "2.0" "3.0" "4.0" "5.0" "6.0" "7.0" "8.0" "9.0" "10.0") 
 for time_span in "${timespanarray[@]}"
 do
   for yamlfile in $(find ../yaml/trajectory_analysys -maxdepth 1 -type f -printf "%f\n")
@@ -31,7 +32,7 @@ do
     rm recordings/trajectory_recording$yamlname.bag
 
     #The roslaunch file takes several useful parameters. We can lower speed if we see that the processor isn't working at real time. The suffix allows us to record different bags every loop. The bag file has to be previously filtered. We disable rviz as we don't need to see anything.
-    roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=$BAG\_filtered_notf test:=$yamlfile speed:=0.05 suffix:=$yamlname rviz:=false
+    roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=$BAG\_filtered_notf test:=$yamlfile speed:=1 suffix:=$yamlname rviz:=false
     #The following ifs distinguish between the 3 types of yaml files
     if [[ ${yamlname:0:1} == 'G' ]]
     then
@@ -54,6 +55,11 @@ do
       rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
       cp CSV/$yamlname.csv CSV/transp/$yamlname.csv
     elif [[ ${yamlname:0:1} == 'T' ]]
+    then
+      rm CSV/$yamlname.csv
+      rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
+      cp CSV/$yamlname.csv CSV/transp/$yamlname.csv
+    elif [[ ${yamlname:0:1} == 'S' ]]
     then
       rm CSV/$yamlname.csv
       rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
diff --git a/launch/imu2d_analysys.launch b/launch/imu2d_analysys.launch
index 238359a..0e9b2ff 100644
--- a/launch/imu2d_analysys.launch
+++ b/launch/imu2d_analysys.launch
@@ -22,7 +22,7 @@
       <node pkg="rosbag" 
             type="record" 
             name="recorder" 
-            args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
+        args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov" "/imu_micro_bias_accel" "/imu_micro_bias_gyro"'/>
     </group>
 
     <!-- TF -->
diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index c58ef15..1b53a92 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -22,7 +22,7 @@
       <node pkg="rosbag" 
             type="record" 
             name="recorder" 
-            args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
+            args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov" "/imu_micro_bias_accel" "/imu_micro_bias_gyro"'/>
     </group>
 
     <!-- TF -->
diff --git a/rviz/imu2d_demo_ana.rviz b/rviz/imu2d_demo_ana.rviz
index 6a88cc0..7caf7f1 100644
--- a/rviz/imu2d_demo_ana.rviz
+++ b/rviz/imu2d_demo_ana.rviz
@@ -17,7 +17,7 @@ Panels:
         - /PointCloud21
         - /Imu1
       Splitter Ratio: 0.708737850189209
-    Tree Height: 728
+    Tree Height: 725
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -36,7 +36,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: LaserScan
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -182,13 +182,9 @@ Visualization Manager:
       Marker Topic: /wolf_ros_node/graph_factors
       Name: Factors
       Namespaces:
-        factors_processorimu2dmicro: true
-        factors_processorloopclosureicp: true
-        factors_processorodom2d: true
+        factors_processorimumicro: true
         factors_processorodomicp: true
-        factors_text_processorimu2dmicro: false
-        factors_text_processorloopclosureicp: false
-        factors_text_processorodom2d: false
+        factors_text_processorimumicro: false
         factors_text_processorodomicp: false
         factors_text_unnamed_processor: false
         factors_unnamed_processor: true
@@ -352,10 +348,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1025
+  Height: 1022
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000020500000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005320000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000020500000360fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000360000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006900000003efc0100000002fb0000000800540069006d0065010000000000000690000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004850000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -364,6 +360,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1853
-  X: 67
-  Y: 27
+  Width: 1680
+  X: 1920
+  Y: 0
diff --git a/yaml/trajectory_analysys/E.yaml b/yaml/trajectory_analysys/E.yaml
index a993e15..32b5cb6 100644
--- a/yaml/trajectory_analysys/E.yaml
+++ b/yaml/trajectory_analysys/E.yaml
@@ -1,7 +1,7 @@
 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_E-1_0_outside_1.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_E-5_0.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
diff --git a/yaml/trajectory_analysys/G.yaml b/yaml/trajectory_analysys/G.yaml
index 8172adf..dcc3d60 100644
--- a/yaml/trajectory_analysys/G.yaml
+++ b/yaml/trajectory_analysys/G.yaml
@@ -1,7 +1,7 @@
 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_G-10_0.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_G-5_0.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
diff --git a/yaml/trajectory_analysys/S.yaml b/yaml/trajectory_analysys/S.yaml
index 4683ca6..e4d07cc 100644
--- a/yaml/trajectory_analysys/S.yaml
+++ b/yaml/trajectory_analysys/S.yaml
@@ -1,7 +1,7 @@
 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_E-10_0.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_S-5_0.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
diff --git a/yaml/trajectory_analysys/T.yaml b/yaml/trajectory_analysys/T.yaml
index 629e3a9..eddf381 100644
--- a/yaml/trajectory_analysys/T.yaml
+++ b/yaml/trajectory_analysys/T.yaml
@@ -1,7 +1,7 @@
 config:
   debug:
     profiling: true
-    profiling_file: "~/profiling/wolf_demo_profiling_T-1_0_outside_1.txt"
+    profiling_file: "~/profiling/wolf_demo_profiling_T-5_0.txt"
     print_problem: false
     print_period: 2
     print_depth: 4
diff --git a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
index 51ced29..433952d 100644
--- a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
+++ b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
@@ -7,4 +7,4 @@ 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
+orthogonal_gravity: true
diff --git a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml
index 433952d..51ced29 100644
--- a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml
+++ b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml
@@ -7,4 +7,4 @@ 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: true
+orthogonal_gravity: false
diff --git a/yaml/trajectory_analysys/parameters/test_laser_processor.yaml b/yaml/trajectory_analysys/parameters/test_laser_processor.yaml
index 3aa7b0b..6c34e38 100644
--- a/yaml/trajectory_analysys/parameters/test_laser_processor.yaml
+++ b/yaml/trajectory_analysys/parameters/test_laser_processor.yaml
@@ -3,7 +3,7 @@ keyframe_vote:
   min_features_for_keyframe: 10
   min_dist: 999
   min_angle: 999
-  min_time: 1.0
+  min_time: 5.0
   min_error: 999
   max_points: 0
 max_new_features: 15
diff --git a/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml b/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml
index 492c369..30c8768 100644
--- a/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml
+++ b/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml
@@ -1,6 +1,6 @@
 map_frame_id: "map"
-  #odom_frame_id: "ana/odom"
-  #base_frame_id: "ana/base_footprint"
-odom_frame_id: "helena/odom"
-base_frame_id: "helena/base_footprint"
-publish_odom_tf: true
+odom_frame_id: "ana/odom"
+base_frame_id: "ana/base_footprint"
+#odom_frame_id: "helena/odom"
+#base_frame_id: "helena/base_footprint"
+publish_odom_tf: false
-- 
GitLab