forked from cookem4/T265MatlabAnalysis
-
Notifications
You must be signed in to change notification settings - Fork 0
/
ErrorPlotter.m
139 lines (122 loc) · 3.84 KB
/
ErrorPlotter.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
%This script aligns the T265 data and motion capture data in time to plot
%and analyze the errors associated with the T265
M1 = csvread("C:\Users\mitch\Documents\ResearchSummer19\FacingSideways\ZAxisNvidia.csv");
M2 = csvread("C:\Users\mitch\Documents\ResearchSummer19\FacingSideways\ZAxisMotivFormatted.csv");
dataT = M1(:,7:9);
dataM = M2(:,1:3);
dataM = dataM(1:50*120,:);
%The two arrays are now of corresponding sizes
%Can bin dataT in bins of 5 as it is 200Hz and dataM in bins of 3 as it is
%120Hz
length(dataM)
length(dataT)
avgBinT = zeros(length(dataT)/5, 3);
avgBinM = zeros(length(dataM)/3, 3);
k = 1;
newInd = 0;
%Binning of 5
for i=1:length(dataT)
if(newInd == 4)
newInd = 0;
k = k+1;
else
newInd = newInd + 1;
avgBinT(k,1) = avgBinT(k,1) + dataT(i,1);
avgBinT(k,2) = avgBinT(k,2) + dataT(i,2);
avgBinT(k,3) = avgBinT(k,3) + dataT(i,3);
end
end
k = 1;
newInd = 0;
%Binning of 3
for i=1:length(dataM)
if(newInd == 2)
newInd = 0;
k = k+1;
else
newInd = newInd + 1;
avgBinM(k,1) = avgBinM(k,1) + dataM(i,1);
avgBinM(k,2) = avgBinM(k,2) + dataM(i,2);
avgBinM(k,3) = avgBinM(k,3) + dataM(i,3);
end
end
%Now divide bins by bin sizes
for i=1:length(avgBinM)
avgBinM(i,1) = avgBinM(i,1)/3;
avgBinM(i,2) = avgBinM(i,2)/3;
avgBinM(i,3) = avgBinM(i,3)/3;
avgBinT(i,1) = avgBinT(i,1)/5;
avgBinT(i,2) = avgBinT(i,2)/5;
avgBinT(i,3) = avgBinT(i,3)/5;
end
errorsVect = zeros(size(avgBinT));
for i=1:length(avgBinM)
errorsVect(i,1) = abs(avgBinM(i,1) - avgBinT(i,1));
errorsVect(i,2) = abs(avgBinM(i,2) - avgBinT(i,2));
errorsVect(i,3) = abs(avgBinM(i,3) - avgBinT(i,3));
end
errors = zeros(length(avgBinT),1);
for i=1:length(avgBinM);
errors(i) = sqrt(errorsVect(i,1)^2 + errorsVect(i,2)^2 + errorsVect(i,3).^2);
end
%histogram(errors, round(sqrt(length(errors))));
hold on
xlabel("Error (m)");
ylabel("Number of Data Points");
title("RealSense T235 Error");
maximum = max(errors)
minimum = min(errors)
sigma = std(errors)
middle = median(errors)
average = mean(errors)
%If the error is large and is in the same direction as the motion of the
%device then it is likely due to the two tests starting at different times
xCount = 0;
yCount = 0;
zCount = 0;
for i=1:length(errorsVect)
if errorsVect(i,1) > average
xCount = xCount + 1;
elseif errorsVect(i,2) > average
yCount = yCount + 1;
elseif errorsVect(i,3) > average
zCount = zCount + 1;
end
end
%For non temporal alignment can look at the errors that aren't in the
%direction of motion to get a good idea of how much error is actually
%present
%Used to find the path of motion based on the errors
xFlag = 0;
yFlag = 0;
zFlag = 0;
if xCount > yCount && xCount > zCount
xFlag = 1;
elseif yCount > xCount && yCount > zCount
yFlag = 1;
elseif zCount > yCount && zCount > xCount
zFlag = 1;
end
correctedAvgError = zeros(length(avgBinT),1);
for i=1:length(errorsVect)
if xFlag == 1
correctedAvgError(i,1) = sqrt(errorsVect(i,2)^2 + errorsVect(i,3)^2);
elseif yFlag == 1
correctedAvgError(i,1) = sqrt(errorsVect(i,1)^2 + errorsVect(i,3)^2);
elseif zFlag == 1
correctedAvgError(i,1) = sqrt(errorsVect(i,1)^2 + errorsVect(i,2)^2);
end
end
histogram(correctedAvgError, round(sqrt(length(correctedAvgError))));
hold on
xlabel("Error (m)");
ylabel("Number of Data Points");
title("RealSense T235 Error");
maximum = max(correctedAvgError)
minimum = min(correctedAvgError)
sigma = std(correctedAvgError)
middle = median(correctedAvgError)
average = mean(correctedAvgError)
line([average average], [0 120], 'color', 'r', 'LineWidth', 2)
line([average-sigma average-sigma], [0 120], 'color', 'g', 'LineStyle', '--', 'LineWidth', 2)
line([average+sigma average+sigma], [0 120], 'color','g', 'LineStyle', '--', 'LineWidth', 2)