update scripts

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);
 

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);

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

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

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

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

yaml/trajectory_analysys/E.yaml

 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

yaml/trajectory_analysys/G.yaml

 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

yaml/trajectory_analysys/S.yaml

 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

yaml/trajectory_analysys/T.yaml

 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

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

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

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

yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml

 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