-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdebug.m
41 lines (28 loc) · 1.02 KB
/
debug.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
%debug
clear
clc
baseTrace = [0,0,0; %blue
1,0,0;
2,0,0]
optimizeTrace = [0,0,0;
1,1,0;
2,2,0]
figure(1) % show original tracks
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
optAngleZ = optimizeZ(baseTrace,optimizeTrace,0.001)
optimizeTrace = rotateObject(optimizeTrace,0,0,optAngleZ )
figure(2) % show rotated tracks tracks
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
optAngleY = optimizeY(baseTrace,optimizeTrace,0.001)
optimizeTrace = rotateObject(optimizeTrace,0,optAngleY,0 )
figure(3) % show rotated tracks tracks
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