-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathUntitled.m
159 lines (96 loc) · 3.18 KB
/
Untitled.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
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
%optimize main
clc
clear
load('base_track.mat')
load('optimize_track.mat')
load('map.mat')
%rotational steps
thetaX = 0.01;
thetaY = 0.01;
thetaZ = 0.01;
% Number of iterations
% calculate number of iterations
iterationsZ = int16(2*pi/thetaZ);
iterationsZ = iterationsZ*1.2;
iterationsY = int16(2*pi/thetaY);
iterationsY = iterationsY*1.2;
iterationsX = int16(2*pi/thetaX);
iterationsX = iterationsX*1.2;
% Maximum step
%maxAngleStep = 0.003; % RAD
baseTrace = baseTrace(1:1500,1:3);
optimizeTrace = optimizeTrace(1:1500,1:3);
%%%%%%%%%%% OPTIMIZE Z
% Calculate mean distances manually for first time
loss(1) = calculateMeanDistance(baseTrace,optimizeTrace)
% initialize rotation angle for the first time
thetaRot = thetaZ
totalRotation(3) =0;
% Main loop Z axis optimisation
for i=1:iterationsZ-1
% rotate for given angle
optimizeTrace = rotateObject(optimizeTrace,0,0,thetaRot);
%calculate mean distances
loss(i+1) = calculateMeanDistance(baseTrace,optimizeTrace);
%
dLoss(i) = loss(i+1) - loss(i);
totalRotation(3) = totalRotation(3)+thetaRot;
end %end of main loop
% X axis for plotting. X step is equal to step
X = linspace(0,totalRotation(3),iterationsZ);
[m,i] = min(loss)
optimum_angleZ = i*thetaZ
figure(1)
plot(X(1:size(loss,2)),loss)
%Optimize Y%%%%%%%%%%%%%%%%%%%%%%%%%%
loss = [];
dLoss = [];
% Calculate mean distances manually for first time
loss(1) = calculateMeanDistance(baseTrace,optimizeTrace)
% initialize rotation angle for the first time
thetaRot = thetaY
totalRotation(2) =0;
for i=1:iterationsY-1
% rotate for given angle
optimizeTrace = rotateObject(optimizeTrace,0,thetaRot,0);
%calculate mean distances
loss(i+1) = calculateMeanDistance(baseTrace,optimizeTrace);
%
dLoss(i) = loss(i+1) - loss(i);
totalRotation(2) = totalRotation(2)+thetaRot;
end %end of main loop
% X axis for plotting. X step is equal to step
X = linspace(0,totalRotation(2),iterationsY);
[m,i] = min(loss)
optimum_angleY = i*thetaY
figure(2)
plot(X(1:size(loss,2)),loss)
%%%%%%%%%%%% OPTIMIZE X
%Optimize Y%%%%%%%%%%%%%%%%%%%%%%%%%%
loss = [];
dLoss = [];
% Calculate mean distances manually for first time
loss(1) = calculateMeanDistance(baseTrace,optimizeTrace)
% initialize rotation angle for the first time
thetaRot = thetaX
totalRotation(1) =0;
for i=1:iterationsX-1
% rotate for given angle
optimizeTrace = rotateObject(optimizeTrace,thetaRot,0,0);
%calculate mean distances
loss(i+1) = calculateMeanDistance(baseTrace,optimizeTrace);
%
dLoss(i) = loss(i+1) - loss(i);
totalRotation(1) = totalRotation(1)+thetaRot;
end %end of main loop
% X axis for plotting. X step is equal to step
X = linspace(0,totalRotation(1),iterationsX);
[m,i] = min(loss)
optimum_angleX = i*thetaX
figure(3)
plot(X(1:size(loss,2)),loss)
% Output optimum angles
optimumAngle(1) = optimum_angleX;
optimumAngle(2) = optimum_angleY;
optimumAngle(3) = optimum_angleZ;
optimumAngle = wrapTo2Pi(optimumAngle)