-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain.m
79 lines (54 loc) · 2.07 KB
/
main.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
%MAIN
clear
clc
load('base_track.mat')
load('optimize_track.mat')
load('map.mat')
baseTrace = baseTrace(:,1:3);
optimizeTrace = optimizeTrace(:,1:3);
Xstep = 0.01;
Ystep = 0.01;
Zstep = 0.01;
figure(1) % show original tracks
title('ORIGINAL GNSS AND LIDAR TRACKS')
hold on
pcshow(baseTrace,[0,0,1],'MarkerSize' ,50) %gnss track track (blue)
pcshow(optimizeTrace(:,1:3),[0,0,0],'MarkerSize' ,50) %lidar track track (black)
hold off
optAngleZ = optimizeZ(baseTrace,optimizeTrace,Zstep);
optAngleZ = wrapTo2Pi(optAngleZ);
optimizeTrace = rotateObject(optimizeTrace,0,0,optAngleZ );
figure(2) % show rotated tracks tracks
title('AFTER Z AXIS OPTIMIZATION')
hold on
pcshow(baseTrace,[0,0,1],'MarkerSize' ,50) %gnss track track (blue)
pcshow(optimizeTrace(:,1:3),[0,0,0],'MarkerSize' ,50) %lidar track track (black)
hold off
optAngleY = optimizeY(baseTrace,optimizeTrace,Ystep);
optAngleY = wrapTo2Pi(optAngleY);
optimizeTrace = rotateObject(optimizeTrace,0,optAngleY,0 );
figure(3) % show rotated tracks tracks
title('AFTER Z and Y AXIS OPTIMIZATION')
hold on
pcshow(baseTrace,[0,0,1],'MarkerSize' ,50) %gnss track track (blue)
pcshow(optimizeTrace(:,1:3),[0,0,0],'MarkerSize' ,10) %lidar track track (black)
hold off
optAngleX = optimizeX(baseTrace,optimizeTrace,Xstep);
optAngleX = wrapTo2Pi(optAngleX);
optimizeTrace = rotateObject(optimizeTrace,optAngleX,0,0 );
figure(4) % show rotated tracks tracks
title('AFTER Z and Y and X AXIS OPTIMIZATION')
hold on
pcshow(baseTrace,[0,0,1],'MarkerSize' ,10) %gnss track track (blue)
pcshow(optimizeTrace(:,1:3),[0,0,0],'MarkerSize' ,10) %lidar track track (black)
hold off
figure(5) % Rotate map and plot everything
hold on
title('Projection on rotated map')
map = rotateObject(map,0,optAngleY,optAngleZ);
pcshow(map)
pcshow(baseTrace,[0,0,1],'MarkerSize' ,10) %gnss track track (blue)
pcshow(optimizeTrace(:,1:3),[0,0,0],'MarkerSize' ,10) %lidar track track (black)
hold off
%display total rotation angle for optimization
optimal_rotation=[optAngleX,optAngleY,optAngleZ]