forked from ZacharyTaylor/Multi-Array-Calib
-
Notifications
You must be signed in to change notification settings - Fork 0
/
TestBasic.m
85 lines (65 loc) · 2.19 KB
/
TestBasic.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
% Basic template for tests
%% user set variables
%number of scans to use
scansTimeRange = 100;
%number of scans to combine in metric refine step
numScans = 20;
%samples
timeSamples = 10000;
metric = 'motion';
%% load sensor data
CalibPath(true);
%make sure to read in cameras last (due to issue with how I compensate for scale)
sensorData = LoadSensorData('Kitti','Vel','Cam1');
% %% fix timestamps
fprintf('Finding Timing Offsets\n');
[sensorData, offsets, varOffsets] = CorrectTimestamps(sensorData, timeSamples);
fprintf('Offsets:\n');
disp(offsets);
fprintf('Offset sd:\n');
disp(sqrt(varOffsets));
%% run calibration
%get random contiguous scans to use
sDataBase = RandTformTimes(sensorData, scansTimeRange);
%evenly sample data
sData = SampleData2(sDataBase);
%remove uninformative data
sData = RejectPoints(sData, 10, 0.00001);
%find rotation
fprintf('Finding Rotation\n');
rotVec = RoughR(sData);
rotVec = OptR(sData, rotVec);
rotVarL = ErrorEstCR(sData, rotVec);
rotVarM = max(rotVarL,ErrorEstCR2(sData, rotVec));
fprintf('Rotation:\n');
disp(rotVec);
fprintf('Rotation lower sd:\n');
disp(sqrt(rotVarL));
fprintf('Rotation mid sd:\n');
disp(sqrt(rotVarM));
%find camera transformation scale (only used for RoughT, OptT does its
%own smarter/better thing
fprintf('Finding Camera Scale\n');
sDataS = EasyScale(sData, rotVec, rotVarL,zeros(2,3),ones(2,3));
%show what we are dealing with
%PlotData(sDataS,rotVec);
fprintf('Finding Translation\n');
tranVec = RoughT(sDataS, rotVec);
tranVec = OptT(sData, tranVec, rotVec, rotVarM);
tranVarL = ErrorEstCT(sData, tranVec, rotVec, rotVarL);
tranVarM = max(tranVarL,ErrorEstCT2(sData, tranVec, rotVec, rotVarM));
fprintf('Translation:\n');
disp(tranVec);
fprintf('Translation lower sd:\n');
disp(sqrt(tranVarL));
fprintf('Translation mid sd:\n');
disp(sqrt(tranVarM));
%get grid of transforms
fprintf('Generating transformation grid\n');
[TGrid, vTGrid] = GenTformGrid(tranVec, rotVec, tranVarM, rotVarM);
%refine transforms using metrics
fprintf('Refining transformations\n');
[TGridR, vTGridR] = MetricRefine(TGrid, vTGrid, sDataBase, numScans, metric);
%correct for differences in grid
fprintf('Combining results\n');
[finalVec, finalVar] = OptGrid(TGridR, vTGridR);