work on matlab scripts

bag/analysys/compare.asv

-clc; clear; clearvars; close all;
-
-CSVFolder = '../CSV/transp/';
-if ~isfolder(CSVFolder)
-    errorMessage = sprintf('Error: The following folder does not exist:\n%s\nPlease specify a new folder.', myFolder);
-    uiwait(warndlg(errorMessage));
-    myFolder = uigetdir(); % Ask for a new one.
-    if myFolder == 0
-         % User clicked Cancel
-         return;
-    end
-end
-Gvec = [];
-Evec = [];
-myFiles = dir(fullfile(CSVFolder,'*.csv')); %gets all csv files in struct
-for k = 1:length(myFiles)
-    baseFileName = myFiles(k).name;
-    fullFileName = fullfile(CSVFolder, baseFileName);
-    fprintf('Now reading file %s\n', baseFileName);
-    Table = readtable(fullFileName,'PreserveVariableNames',true);
-    if baseFileName(1) == 'G'
-        [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(Table);
-        Gvec = [Gvec, {baseFileName, Gx, Gy, Gz, Gang, Gt, Gts}'];
-    elseif baseFileName(1) == 'E'
-        [Ex, Ey, Ez, Eang, Et, Ets] = extractdata(Table);
-        Evec = [Evec, {baseFileName, Ex, Ey, Ez, Eang, Et, Ets}'];
-    end
-
-%     
-%     t10 = linspace(0,min(Gt(end),gt10(end))-1,1e6);
-%     ts0 = resample(Gts, t10);
-%     ts1 = resample(gts10, t10);
-%     ploterrs(ts0, ts1)
-%     
-%     GC20 = readtable('trajectory_ana_lab_2_exp_20_transp.csv','PreserveVariableNames',true);
-%     [gx20, gy20, gz20, gth20, gt20, gts20] = extractdata(GC20);
-%     
-%     t20 = linspace(0,min(Gt(end),gt20(end))-1,1e6);
-%     ts0 = resample(Gts, t20);
-%     ts2 = resample(gts20, t20);
-%     ploterrs(ts0, ts2)
-end
-
-[Edata, Gdata] = matchaxes(Gvec{7,1}, Evec{7,1}, 1, 2);

bag/analysys/compare.m

@@ -40,6 +40,11 @@ for k = 1:length(myFiles)
 %     ts2 = resample(gts20, t20);
 %     ploterrs(ts0, ts2)
 end
-
-[Edata, Gdata] = matchaxes(Gvec{7,1}, Evec{7,1}, 2, 3);
-error = vecnorm(Edata(end,:)-Gdata(end,:))
\ No newline at end of file
+ERRORS=[];
+min_len = 99999999999999999999999999999999999999999999999999999999999999999999999999999999999999999;
+for i = 1:size(Gvec,2)
+    [perrors, angerrors] = geterror(Gvec{7,i}, Evec{7,i});
+    Initial_RMSE = [sqrt(mean(perrors.^2)), sqrt(mean(angerrors.^2))];
+    ERRORS = [ERRORS, {strcat(Gvec{1,i},' ', Evec{1,i}), Initial_RMSE, perrors, angerrors}']
+    min_len = min(min_len, length(perrors));
+end
\ No newline at end of file

bag/analysys/extractGT.asv

-function [Gx, Gy, Gz, Gth, Gt, Gts] = extractGT(GT)
-    t0 = GT(6,2).Var2;
-    GT(1,2).Var2 = (GT(1,2).Var2 - t0)*1e-9;
-    GT(3,2).Var2 = (GT(3,2).Var2 - t0)*1e-9;
-    texpr = 'field.poses\d{1,4}.header.stamp';
-    xexpr = 'field.poses\d{1,4}.pose.position.x';
-    yexpr = 'field.poses\d{1,4}.pose.position.y';
-    zexpr = 'field.poses\d{1,4}.pose.position.z';
-    thexpr = 'field.poses\d{1,4}.pose.orientation.x';
-
-    Gt = [];
-    Gx = [];
-    Gy = [];
-    Gz = [];
-    Gth = [];
-    for i=1:height(GT)
-        if (size(regexp(GT(i,1).Var1{1,1}, texpr)) > 0)
-            GT(i,2).Var2 = (GT(i, 2).Var2 - t0)*1e-9;
-            Gt = [Gt, GT(i,2).Var2];
-        end
-        if (size(regexp(GT(i,1).Var1{1,1}, xexpr)) > 0)
-            Gx = [Gx, GT(i,2).Var2];
-        end
-        if (size(regexp(GT(i,1).Var1{1,1}, yexpr)) > 0)
-            Gy = [Gy, GT(i,2).Var2];
-        end
-        if (size(regexp(GT(i,1).Var1{1,1}, zexpr)) > 0)
-            Gz = [Gz, GT(i,2).Var2];
-        end
-        if (size(regexp(GT(i,1).Var1{1,1}, thexpr)) > 0)
-            [r1 r2 r3] = quat2angle([GT(i+3,2).Var2 GT(i,2).Var2 GT(i+1,2).Var2 GT(i+2,2).Var2]);
-            Gth = [Gth, r1];
-        end
-    end
-    
-    Gts = timeseries([Gx' Gy' Gz' Gth'], Gt, 'name', 'groundtruth');
-    Gts.TimeInfo.Units='seconds';
-    
-end
\ No newline at end of file

bag/analysys/geterror.m

+function [Poserror, Angerror] = geterror(GT, TS)
+    npairs = length(GT.Time)-1;
+
+    Poserror = zeros(1, npairs);
+    Angerror = zeros(1, npairs);
+    for i = 1:npairs
+        [pe, ae] = matchaxes2(GT, TS, i, i+1);
+        Poserror(i) = pe;
+        Angerror(i) = ae;
+    end
+
+end
\ No newline at end of file

bag/analysys/matchaxes.asv

-function matchaxes(GT, TS, t0, t1)
-    [GTpos TSpos] = synchronize(GT.pos, TS.pos, 'Intersection');
-    [GTang TSang] = synchronize(GT.ang, TS.ang, 'Union');
-    GT = tscollection({GTpos GTang});
-    TS = tscollection({TSpos TSang});
-    Gpos = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).pos.Data;
-    Tpos = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).pos.Data;
-    Tt = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).Time;
-    Gt = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).Time;
-    Grot = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).ang.Data;
-    Trot = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).ang.Data;
-
-    rotangles = Grot(1,:)-Trot(1,:)
-    rotmatrix = eul2rotm(rotangles)
-    Tpos_at_origin = (Tpos - Tpos(1,:));
-    Tpos_rotated_at_origin = Tpos_at_origin*rotmatrix;
-    Tpos_at_Gpos_origin = Tpos_rotated_at_origin + Gpos(1,:);
-
-    figure
-    plot(Gt, Gpos)
-    hold on
-    plot(Tt, Tpos_at_Gpos_origin)
-
-end
\ No newline at end of file

bag/analysys/matchaxes.m

 function [Tpos_at_Gpos_origin, Gdata] = matchaxes(GT, TS, t0, t1)
+    %Get Positions of timeseries and synchronize them
     Gpos = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).pos;
     Tpos = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).pos;
     [Gpos, Tpos] = synchronize(Gpos, Tpos, 'Union', 'KeepOriginalTimes',true);
 
+    %Get Angles of axes of timeseries and synchronize them
     Grot = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).ang;
     Trot = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).ang;
     [Grot, Trot] = synchronize(Grot, Trot, 'Union', 'KeepOriginalTimes',true');
 
     Tt = Tpos.Time;
     Gt = Gpos.Time;
-
-
+    
+    %Get Rotation matrix such that the initial position's axes coincide
+    %with GT
     rotangles = Grot.Data(1,:)-Trot.Data(1,:);
     rotmatrix = eul2rotm(rotangles);
+
+    %Set Axes at origin to apply rotation
     Tpos_at_origin = (Tpos.Data - Tpos.Data(1,:));
     Tpos_rotated_at_origin = Tpos_at_origin*rotmatrix;
 
+    %Set rotated points so that the initial position is at origin of GT
+    %initial position
     Tpos_at_Gpos_origin = Tpos_rotated_at_origin + Gpos.Data(1,:);
     Gdata = Gpos.Data;
 

bag/analysys/matchaxes2.m

+function [epos, eang] = matchaxes2(GT, TS, t0, t1)
+    %Get Positions of timeseries and synchronize them
+    Gpos = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).pos;
+    Tpos = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).pos;
+    [Gpos, Tpos] = synchronize(Gpos, Tpos, 'Union', 'KeepOriginalTimes',true);
+
+    %Get Angles of axes of timeseries and synchronize them
+    Grot = getsampleusingtime(GT, GT.Time(t0), GT.Time(t1)).ang;
+    Trot = getsampleusingtime(TS, GT.Time(t0), GT.Time(t1)).ang;
+    [Grot, Trot] = synchronize(Grot, Trot, 'Union', 'KeepOriginalTimes',true');
+
+    Tt = Tpos.Time;
+    Gt = Gpos.Time;
+    
+    %Get quaternions to work
+    Gq0 = eul2quat(Grot.Data(1,:));
+    Gq1 = eul2quat(Grot.Data(end,:));
+    Tq0 = eul2quat(Trot.Data(1,:));
+    Tq1 = eul2quat(Trot.Data(end,:));
+
+    %Position vectors of end frame in initial frame
+    dpG = quatrotate(quatinv(Gq0), (Gpos.Data(end,:) - Gpos.Data(1,:)));
+    dpT = quatrotate(quatinv(Tq0), (Tpos.Data(end,:) - Tpos.Data(1,:)));
+
+    %Orientation quaternions of end frame in initial frame
+    dqG = quatmultiply(Gq0,quatconj(Gq1));
+    dqT = quatmultiply(Tq0,quatconj(Tq1));
+
+    %Errors
+    epos = norm(dpT-dpG);
+    eang = norm(quat2eul(quatmultiply(quatinv(dqT),dqG)));
+
+end
\ No newline at end of file

yaml/trajectory_analysys/E-0_1.yaml

+config:
+  debug:
+    profiling: true
+    profiling_file: "~/profiling/wolf_demo_profiling_E-0_1.txt"
+    print_problem: false
+    print_period: 2
+    print_depth: 4
+    print_constr_by: false
+    print_metric: true
+    print_state_blocks: true
+  problem:
+    tree_manager:
+      type: "none"
+    frame_structure: "POV"
+    dimension: 2
+    prior:
+      mode: "fix"
+      $state:
+        P: [0,0]
+        O: [0]
+        V: [0,0]
+      time_tolerance: 0.1
+    node_rate: 100
+  map:
+    type: "MapBase"
+    plugin: "core"
+      
+  solver:
+    follow: "parameters/solver.yaml"
+
+    
+  sensors:
+    -
+      type: "SensorLaser2d"
+      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: "microstrain"
+      plugin: "imu"
+      follow: "parameters/test_imu_params_microstrain.yaml" 
+
+  processors:
+    -
+      type: "ProcessorOdomIcp"
+      name: "processorodomicp"
+      sensor_name: "scanner_front_left"
+      plugin: "laser"
+      keyframe_vote:
+        voting_active: true
+        min_features_for_keyframe: 10
+        min_dist: 999
+        min_angle: 999
+        min_time: 0.1
+        min_error: 999
+        max_points: 0
+      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: "processorimu2dmicro"
+      sensor_name: "microstrain"
+      plugin: "imu"
+      follow: "parameters/test_imu_processor_microstrain.yaml"
+      
+  ROS subscriber:
+    -
+      package: "wolf_ros_laser"
+      type: "SubscriberLaser2d"
+      topic: "/ana/sensors/scan"
+      sensor_name: "scanner_front_left"
+      load_params_from_msg: true
+    #-
+    #  package: "wolf_ros_imu"
+    #  type: "SubscriberImuEnableable"
+    #  topic: "/ana/sensors/bno055_imu/imumal"
+    #  sensor_name: "bno"
+    #  follow: "parameters/test_imu_subscriber_bno.yaml"
+    -
+      package: "wolf_ros_imu"
+      type: "SubscriberImuEnableable"
+      topic: "/ana/imu/data"
+      sensor_name: "microstrain"
+      follow: "parameters/test_imu_subscriber_microstrain.yaml"
+
+  ROS publisher:
+    -
+      package: "wolf_ros_node"
+      type: "PublisherTf"
+      topic: " "
+      period: 0.1
+      follow: "parameters/test_publisher_ros_node.yaml"
+    -
+      package: "wolf_ros_node"
+      type: "PublisherGraph"
+      topic: "graph"
+      period: 0.1
+      viz_overlapped_factors: true
+    -
+      package: "wolf_ros_node"
+      type: "PublisherTrajectory"
+      topic: "trajectory"
+      period: 0.1
+      frame_id: "map"
+    -
+      package: "wolf_ros_node"
+      type: "PublisherPose"
+      topic: "pose"
+      extrinsics: false
+      period: 0.005
+      frame_id: "map"
+    -
+      package: "wolf_ros_laser"
+      type: "PublisherLaserMap"
+      topic: "map"
+      period: 1
+      map_frame_id: "map"
+      update_dist_th: 0.05
+      update_angle_th: 0.05
+      max_n_cells: 1000000
+      grid_size: 0.1
+      p_free: 0.3
+      p_obst: 0.8
+      p_free_th: 0.2
+      p_obst_th: 0.9
+      discard_max_range: true
+

yaml/trajectory_analysys/hold/G-0_1.yaml → yaml/trajectory_analysys/G-0_1.yaml

@@ -90,13 +90,13 @@ config:
       package: "wolf_ros_node"
       type: "PublisherGraph"
       topic: "graph"
-      period: 1
+      period: 0.1
       viz_overlapped_factors: true
     -
       package: "wolf_ros_node"
       type: "PublisherTrajectory"
       topic: "trajectory"
-      period: 1
+      period: 0.1
       frame_id: "map"
     -
       package: "wolf_ros_laser"

yaml/trajectory_analysys/G-0_5.yaml

+config:
+  debug:
+    profiling: true
+    profiling_file: "~/profiling/wolf_demo_profiling_G-0_5.txt"
+    print_problem: false
+    print_period: 2
+    print_depth: 4
+    print_constr_by: false
+    print_metric: true
+    print_state_blocks: true
+  problem:
+    tree_manager:
+      type: "none"
+    frame_structure: "POV"
+    dimension: 2
+    prior:
+      mode: "fix"
+      $state:
+        P: [0,0]
+        O: [0]
+        V: [0,0]
+      time_tolerance: 0.1
+    node_rate: 100
+  map:
+    type: "MapBase"
+    plugin: "core"
+      
+  solver:
+    follow: "parameters/solver.yaml"
+
+  sensors:
+    -
+      type: "SensorLaser2d"
+      name: "scanner_front_left"
+      plugin: "laser"
+      follow: "parameters/test_laser_params.yaml"
+
+  processors:
+    -
+      type: "ProcessorOdomIcp"
+      name: "processorodomicp"
+      sensor_name: "scanner_front_left"
+      plugin: "laser"
+      keyframe_vote:
+        voting_active: true
+        min_features_for_keyframe: 10
+        min_dist: 999
+        min_angle: 999
+        min_time: 0.5
+        min_error: 999
+        max_points: 0
+      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: 3
+        #  frames_ignored_after_loop: 0
+        #  max_error_threshold: 0.02
+        #  min_points_percent: 40
+        #  max_loops: 3
+        #  max_candidates: 5
+        #  max_attempts: 15
+        #  candidate_generation: "random" # 'random' or 'tree'
+        #  icp:
+        #    follow: "parameters/csm.yaml"
+
+      
+  ROS subscriber:
+    -
+      package: "wolf_ros_laser"
+      type: "SubscriberLaser2d"
+      topic: "/ana/sensors/scan"
+      sensor_name: "scanner_front_left"
+      load_params_from_msg: true
+
+  ROS publisher:
+    -
+      package: "wolf_ros_node"
+      type: "PublisherTf"
+      topic: " "
+      period: 0.1
+      follow: "parameters/test_publisher_ros_node.yaml"
+    -
+      package: "wolf_ros_node"
+      type: "PublisherGraph"
+      topic: "graph"
+      period: 1
+      viz_overlapped_factors: true
+    -
+      package: "wolf_ros_node"
+      type: "PublisherTrajectory"
+      topic: "trajectory"
+      period: 0.5
+      frame_id: "map"
+    -
+      package: "wolf_ros_laser"
+      type: "PublisherLaserMap"
+      topic: "map"
+      period: 1
+      map_frame_id: "map"
+      update_dist_th: 0.05
+      update_angle_th: 0.05
+      max_n_cells: 1000000
+      grid_size: 0.1
+      p_free: 0.3
+      p_obst: 0.8
+      p_free_th: 0.2
+      p_obst_th: 0.9
+      discard_max_range: true