diff --git a/bag/analysys/compare.m b/bag/analysys/compare.m
index 420d416e60adb0844c2af34156d77af560cdc553..11bca203905fc13012481fef79169966bd3edb0e 100644
--- a/bag/analysys/compare.m
+++ b/bag/analysys/compare.m
@@ -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
diff --git a/bag/analysys/extractGT.m b/bag/analysys/extractGT.m
index a4754faa52e58e4f000d2153f7ae55b5e99c4bbb..2a5552dac9bc64fcaefc21483bfa1e0a476e9ab0 100644
--- a/bag/analysys/extractGT.m
+++ b/bag/analysys/extractGT.m
@@ -1,4 +1,4 @@
-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
diff --git a/bag/analysys/extractdata.m b/bag/analysys/extractdata.m
index 26fbb195176c744c947b884fae129baeb420c990..0a509aad9354e961cdfc3eccc4d19fe7386329e2 100644
--- a/bag/analysys/extractdata.m
+++ b/bag/analysys/extractdata.m
@@ -1,4 +1,4 @@
-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
diff --git a/bag/analysys/plotrmse.m b/bag/analysys/plotrmse.m
new file mode 100644
index 0000000000000000000000000000000000000000..7c9d64dc95d488f0e25053bdea1553a467234197
--- /dev/null
+++ b/bag/analysys/plotrmse.m
@@ -0,0 +1,51 @@
+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
diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index b141aaf5ebb2969e4fc881351aa063379f89306f..c58ef156f6f2f0f2ac5f25e8da77ae8ea5a2bd8c 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -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"
diff --git a/rviz/imu2d_demo_ana.rviz b/rviz/imu2d_demo_ana.rviz
index 37c624ac520ee25ef9a4f67c5a4446df25c2ceed..6a88cc005213a2ffbea119f1f0c0790a29d94f80 100644
--- a/rviz/imu2d_demo_ana.rviz
+++ b/rviz/imu2d_demo_ana.rviz
@@ -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