Skip to content
Snippets Groups Projects
Commit 3a4838ee authored by Idril-Tadzio Geer Cousté's avatar Idril-Tadzio Geer Cousté
Browse files

updated analysys scripts

parent 53b0f83f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
bag/analysys/compare.m
+ 9
− 8
View file @ 3a4838ee
......@@ -24,13 +24,13 @@ for k = 1:length(myFiles)
fprintf('Now reading file %s\n', baseFileName);
Table = readtable(fullFileName,'PreserveVariableNames',true);
if baseFileName(1) == 'G'
[Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(Table);
[Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(Table, 0);
Gvec = [Gvec, {baseFileName, Gx, Gy, Gz, Gang, Gt, Gts}'];
elseif baseFileName(1) == 'E'
[Ex, Ey, Ez, Eang, Et, Ets] = extractdata(Table);
[Ex, Ey, Ez, Eang, Et, Ets] = extractdata(Table, 0);
Evec = [Evec, {baseFileName, Ex, Ey, Ez, Eang, Et, Ets}'];
elseif baseFileName(1) == 'T'
[Tx, Ty, Tz, Tang, Tt, Tts] = extractdata(Table);
[Tx, Ty, Tz, Tang, Tt, Tts] = extractdata(Table, 0);
Tvec = [Tvec, {baseFileName, Tx, Ty, Tz, Tang, Tt, Tts}'];
end
end
......@@ -56,25 +56,25 @@ for i = 1:size(Evec,2)
%the following for
figure('visible','off');
plot(linspace(0,366, size(ERRORS{3,i},2)), ERRORS{3,i})
title(strcat('2D Position error for ', ERRORS{1,i}))
title(strcat('2D Position error for timespan', str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.')))
xlabel('seconds')
ylabel('meters')
figure('visible','off');
plot(linspace(0,366, size(ERRORS{4,i},2)), ERRORS{4,i})
title(strcat('2D Angular error for ', ERRORS{1,i}))
title(strcat('2D Angular error for timespan', str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.')))
xlabel('seconds')
ylabel('radians')
figure('visible','off');
plot(linspace(0,366, size(ERRORS{6,i},2)), ERRORS{6,i})
title(strcat('3D Position error for ', ERRORS{1,i}))
title(strcat('3D Position error for timespan ', str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.')))
xlabel('seconds')
ylabel('meters')
figure('visible','off');
plot(linspace(0,366, size(ERRORS{7,i},2)), ERRORS{7,i})
title(strcat('3D Angular error for ', ERRORS{1,i}))
title(strcat('3D Angular error for timespan ', str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.')))
xlabel('seconds')
ylabel('radians')
end
......@@ -118,4 +118,5 @@ for i = 1:size(ERRORS,2)
Final_RMSE3d = [sqrt(mean(vecp3d.^2)), sqrt(mean(veca3d.^2))];
ERRORS{8,i} = Final_RMSE2d;
ERRORS{9,i} = Final_RMSE3d;
end
\ No newline at end of file
end
plotrmse(ERRORS)
\ No newline at end of file
bag/analysys/extractGT.m
+ 8
− 6
View file @ 3a4838ee
function [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(GT)
function [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(GT, t0)
%Given a Table object extracts the necessary data. Intended for the
%GroundTruth, with message being a Trajectory
t0 = GT(6,2).Var2;
......@@ -19,18 +19,20 @@ function [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(GT)
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];
if GT(i,2).Var2 >= t0
Gt = [Gt, GT(i,2).Var2];
end
end
if (size(regexp(GT(i,1).Var1{1,1}, xexpr)) > 0)
if (GT(i,2).Var2 >= t0) && (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)
if (GT(i,2).Var2 >= t0) && (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)
if (GT(i,2).Var2 >= t0) && (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)
if (GT(i,2).Var2 >= t0) && (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]);
Gang = [Gang, [r1 r2 r3]'];
end
......
bag/analysys/extractdata.m
+ 4
− 1
View file @ 3a4838ee
function [gx, gy, gz, gang, gt, gts] = extractdata(GC)
function [gx, gy, gz, gang, gt, gts] = extractdata(GC, t0)
%Given a Table object extracts the necessary data. Intended for the 2d and
%3d experiments
......@@ -11,6 +11,9 @@ function [gx, gy, gz, gang, gt, gts] = extractdata(GC)
gt = GC{:,3}';
gt = gt-t0;
gt = gt*1e-9;
n = length(gt>=t0);
%remove duplicates. Due to the high sampling rate of the PosePublisher
%some timestamps will have been saved multiple times, this cuts them
......
bag/analysys/plotrmse.m 0 → 100644
+ 51
− 0
View file @ 3a4838ee
function plotrmse(ERRORS)
errvec = zeros(5,size(ERRORS,2));
for i = 1:size(ERRORS,2)
%Get timespan through regular expression of the string
timespan = str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.'));
errvec(:,i) = [ERRORS{8,i}(1); ERRORS{8,i}(2); ERRORS{9,i}(1); ERRORS{9,i}(2); timespan];
end
tsp2d = timeseries(errvec(1,:), errvec(5,:), 'Name', '2d Position error');
tsa2d = timeseries(errvec(2,:), errvec(5,:), 'Name', '2d Angular error');
tsp3d = timeseries(errvec(3,:), errvec(5,:), 'Name', '3d Position error');
tsa3d = timeseries(errvec(4,:), errvec(5,:), 'Name', '3d Angular error');
figure
plot(tsp2d.Time, tsp2d.Data(1,:))
hold on
plot(tsp3d.Time, tsp3d.Data(1,:))
hold off
title('Position RMSE')
xlabel('Timespan (s)')
ylabel('m')
legend('2D','3D')
figure
plot(tsa2d.Time, tsa2d.Data(1,:))
hold on
plot(tsa3d.Time, tsa3d.Data(1,:))
hold off
title('Angular RMSE')
xlabel('Timespan (s)')
ylabel('rad')
legend('2D','3D')
figure
plot(tsp2d.Time, tsp2d.Data(1,:)./sqrt(tsp2d.Time)')
hold on
plot(tsp3d.Time, tsp3d.Data(1,:)./sqrt(tsp3d.Time)')
hold off
title('Position RMSE')
xlabel('Timespan (s)')
ylabel('m/sqrt(timespan)')
legend('2D','3D')
figure
plot(tsa2d.Time, tsa2d.Data(1,:)./sqrt(tsa2d.Time)')
hold on
plot(tsa3d.Time, tsa3d.Data(1,:)./sqrt(tsa3d.Time)')
hold off
title('Angular RMSE')
xlabel('Timespan (s)')
ylabel('rad/sqrt(timespan)')
legend('2D','3D')
end
\ No newline at end of file
launch/imu2d_demo.launch
+ 4
− 4
View file @ 3a4838ee
......@@ -27,20 +27,20 @@
<!-- TF -->
<group>
<!-- <node pkg="tf"
<node pkg="tf"
type="static_transform_publisher"
name="static_tf"
args="0 0 0 0 0 0 /base_footprint /base_link 100"/>
args="0 0 0 0 0 0 /$(arg robot)/base_footprint /$(arg robot)/base_link 100"/>
<node pkg="tf"
type="static_transform_publisher"
name="static_tf2"
args="0 0 0 0 0 0 /base_link /imu_bno055 100"/>
args="0 0 0 0 0 0 /$(arg robot)/base_link /$(arg robot)/imu_bno055 100"/>
<node pkg="tf"
type="static_transform_publisher"
name="static_tf3"
args="0 0 0 3.14159265 0 0 /base_link /velodyne 100"/> -->
args="0 0 0 3.14159265 0 0 /$(arg robot)/base_link /$(arg robot)/velodyne 100"/>
<node pkg="tf"
type="static_transform_publisher"
......
rviz/imu2d_demo_ana.rviz
+ 24
− 163
View file @ 3a4838ee
......@@ -5,7 +5,6 @@ Panels:
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1/Status1
- /TF1/Frames1
- /TF1/Tree1
- /Odometry1/Shape1
......@@ -16,8 +15,9 @@ Panels:
- /Trajectory1/Namespaces1
- /Axes1
- /PointCloud21
- /Imu1
Splitter Ratio: 0.708737850189209
Tree Height: 839
Tree Height: 728
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
......@@ -36,7 +36,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource: LaserScan
SyncSource: PointCloud2
Preferences:
PromptSaveOnExit: true
Toolbars:
......@@ -67,119 +67,24 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: false
ana/ana_box:
Value: false
ana/ana_realsense_support:
Value: false
ana/base_footprint:
Value: false
ana/base_footprint_robot:
Value: false
ana/base_link:
Value: true
ana/camera_aligned_depth_to_color_frame:
Value: false
ana/camera_aligned_depth_to_infra1_frame:
Value: false
ana/camera_bottom_screw_frame:
Value: false
ana/camera_color_frame:
Value: false
ana/camera_color_optical_frame:
Value: false
ana/camera_depth_frame:
Value: false
ana/camera_depth_optical_frame:
Value: false
ana/camera_infra1_frame:
Value: false
ana/camera_infra1_optical_frame:
Value: false
ana/camera_infra2_frame:
Value: false
ana/camera_infra2_optical_frame:
Value: false
ana/camera_link:
Value: false
ana/camera_pan_tilt_bottom_screw_frame:
Value: false
ana/camera_pan_tilt_color_frame:
Value: false
ana/camera_pan_tilt_color_optical_frame:
Value: false
ana/camera_pan_tilt_depth_frame:
Value: false
ana/camera_pan_tilt_depth_optical_frame:
Value: false
ana/camera_pan_tilt_infra1_frame:
Value: false
ana/camera_pan_tilt_infra1_optical_frame:
Value: false
ana/camera_pan_tilt_infra2_frame:
Value: false
ana/camera_pan_tilt_infra2_optical_frame:
Value: false
ana/camera_pan_tilt_link:
Value: false
ana/front_left_axle:
Value: false
ana/front_left_hub:
Value: false
ana/front_left_wheel:
Value: false
ana/front_right_axle:
Value: false
ana/front_right_hub:
Value: false
ana/front_right_wheel:
Value: false
ana/front_sonar:
Value: false
ana/imu_bno055:
Value: false
ana/imu_bno055_base:
Value: false
ana/imu_microstrain:
Value: false
ana/odom:
Value: true
ana/pan_frame:
Value: false
ana/realsense_support_pan_tilt:
Value: false
ana/rear_left_axle:
Value: false
ana/rear_left_hub:
Value: false
ana/rear_left_wheel:
Value: false
ana/rear_right_axle:
Value: false
ana/rear_right_hub:
Value: false
ana/rear_right_wheel:
Value: false
ana/rear_sonar:
Value: false
ana/top_plate:
Value: false
ana/velodyne:
Value: false
ana/velodyne_base:
Value: false
camera_accel_frame:
Value: false
camera_accel_optical_frame:
Value: false
camera_gyro_frame:
Value: false
camera_gyro_optical_frame:
Value: false
imu:
Value: false
map:
Value: true
Marker Alpha: 1
Marker Scale: 5
Name: TF
Show Arrows: false
......@@ -190,65 +95,14 @@ Visualization Manager:
ana/odom:
ana/base_footprint:
ana/base_link:
ana/front_left_axle:
ana/front_left_hub:
ana/front_left_wheel:
{}
ana/front_right_axle:
ana/front_right_hub:
ana/front_right_wheel:
{}
ana/front_sonar:
ana/imu_bno055:
{}
ana/imu_microstrain:
{}
ana/rear_left_axle:
ana/rear_left_hub:
ana/rear_left_wheel:
{}
ana/rear_right_axle:
ana/rear_right_hub:
ana/rear_right_wheel:
{}
ana/rear_sonar:
ana/velodyne:
{}
ana/top_plate:
ana/ana_box:
ana/pan_frame:
{}
ana/ana_realsense_support:
ana/camera_bottom_screw_frame:
ana/camera_link:
ana/camera_aligned_depth_to_color_frame:
{}
ana/camera_aligned_depth_to_infra1_frame:
{}
ana/camera_color_frame:
ana/camera_color_optical_frame:
{}
ana/camera_depth_frame:
ana/camera_depth_optical_frame:
{}
ana/camera_infra1_frame:
ana/camera_infra1_optical_frame:
{}
ana/camera_infra2_frame:
ana/camera_infra2_optical_frame:
{}
camera_accel_frame:
camera_accel_optical_frame:
{}
camera_gyro_frame:
camera_gyro_optical_frame:
{}
ana/velodyne_base:
ana/velodyne:
{}
imu:
{}
ana/imu_bno055_base:
ana/imu_bno055:
{}
ana/base_footprint_robot:
{}
Update Interval: 0
......@@ -274,7 +128,6 @@ Visualization Manager:
Keep: 300
Name: Odometry
Position Tolerance: 0.10000000149011612
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 1
......@@ -299,7 +152,7 @@ Visualization Manager:
Class: rviz/LaserScan
Color: 250; 84; 255
Color Transformer: FlatColor
Decay Time: 10
Decay Time: 5
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
......@@ -329,8 +182,16 @@ Visualization Manager:
Marker Topic: /wolf_ros_node/graph_factors
Name: Factors
Namespaces:
factors_processorimu2dmicro: true
factors_processorloopclosureicp: true
factors_processorodom2d: true
factors_processorodomicp: true
factors_text_processorimu2dmicro: false
factors_text_processorloopclosureicp: 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
......@@ -341,14 +202,12 @@ Visualization Manager:
{}
Queue Size: 100
Value: false
- Alpha: 1
Class: rviz/Axes
- Class: rviz/Axes
Enabled: true
Length: 1
Name: Axes
Radius: 0.10000000149011612
Reference Frame: ana/base_link
Show Trail: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
......@@ -394,8 +253,7 @@ Visualization Manager:
Enabled: true
History Length: 1
Name: Imu
Queue Size: 10
Topic: /imu_micro
Topic: /imu_micro_remapped
Unreliable: false
Value: true
Enabled: true
......@@ -432,7 +290,6 @@ Visualization Manager:
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 2.3574845790863037
Y: -1.3090972900390625
......@@ -444,6 +301,7 @@ Visualization Manager:
Near Clip Distance: 0.009999999776482582
Pitch: 0.5097973346710205
Target Frame: map
Value: Orbit (rviz)
Yaw: 4.688166618347168
Saved:
- Angle: 0
......@@ -458,6 +316,7 @@ Visualization Manager:
Near Clip Distance: 0.009999999776482582
Scale: 64.65961456298828
Target Frame: map
Value: TopDownOrtho (rviz)
X: 8.825014114379883
Y: -0.17566636204719543
- Angle: 0
......@@ -472,6 +331,7 @@ Visualization Manager:
Near Clip Distance: 0.009999999776482582
Scale: 50.07240295410156
Target Frame: map
Value: TopDownOrtho (rviz)
X: 1.947830080986023
Y: 3.9687037467956543
- Angle: -1.5700000524520874
......@@ -486,15 +346,16 @@ Visualization Manager:
Near Clip Distance: 0.009999999776482582
Scale: 42.187442779541016
Target Frame: map
Value: TopDownOrtho (rviz)
X: 1.9746785163879395
Y: 4.48727560043335
Window Geometry:
Displays:
collapsed: false
Height: 1136
Height: 1025
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd000000040000000000000205000003d2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007420000003efc0100000002fb0000000800540069006d0065010000000000000742000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000537000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd00000004000000000000020500000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005320000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
......@@ -503,6 +364,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: true
Width: 1858
X: 62
Width: 1853
X: 67
Y: 27
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment