-
Notifications
You must be signed in to change notification settings - Fork 12
/
generateTable1withThickness.m
126 lines (105 loc) · 4.31 KB
/
generateTable1withThickness.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
%% parameters of user setting %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% opts.path: directory of saved mat files
% opts.load_all(0/1):
% 0: only load the files specified in mat_files structure
% 1: load all the files from the opts.path
% opts.datatype: 0: experiment data
% 1: simulation data
% opts.show_results(0/1): plot the calibrated results on figure
clear, clc
mat_path = 'F:\Calibration\matlab_utils\general';
opts.path = "F:\Calibration\intrinsic_lidar_calibration\intrinsic_calibration_mat\robotics_building\tetrahedron4\";
opts.load_all = 1;
opt_datatype = ["Experiment", "Simulation"];
opts.datatype = 1;
opts.show_results = 0;
opts.estimatePlaneMean = 0;
opts.calibrateMean = 0;
opts.fixXY = 0;
opt_noise_type = ["3_param", "6_param", "sim3"];
opts.noise_type = 3;
opt_formulation = ["Lie","BaseLine1","BaseLine2","BaseLine3"]; % Lie group, 3 params, 6 params, 8 params
opts.iterative = 0;
opts.num_beams = 32;
opts.num_scans = 1;
opts.num_iters = 2; % user defined iterations
opts.threshold = 0;
opts.save_path = ".\results\tetrahedron4\Lie\";
addpath(mat_path);
%% Load datasets
clc; % DO NOT add 'clear' here
% Single variable called point_cloud
% path = "/home/chenxif/Documents/me590/Calibration/IntrinsicCalibration/extracted_tags/";
disp("Loading names of data sets...")
mat_files = loadFilesFromAFolder(opts.path, '*.mat');
num_targets = length(mat_files);
disp("Loading point cloud from .mat files")
pc = struct('point_cloud', cell(1,num_targets));
for t = 1:num_targets
pc(t).point_cloud = loadPointCloud(mat_files(t).file_name);
end
disp("Pre-processing payload points...")
data = struct('point_cloud', cell(1,num_targets), 'tag_size', cell(1,num_targets));% XYZIR
for t = 1:num_targets
for iter = 1: opts.num_scans
data(t).scan(iter).payload_points = getPayload(pc(t).point_cloud, iter);
data(t).tag_size = mat_files(t).tag_size;
end
end
v_path = "F:\Calibration\intrinsic_lidar_calibration\intrinsic_calibration_mat\robotics_building\validation\t4\";
validation_mat_files = loadFilesFromAFolder(v_path, '*.mat');
v_num_targets = length(validation_mat_files);
v_pc = struct('point_cloud', cell(1,v_num_targets));
for t = 1:v_num_targets
v_pc(t).point_cloud = loadPointCloud(validation_mat_files(t).file_name);
end
v_data = struct('point_cloud', cell(1,v_num_targets), ...
'tag_size', cell(1,v_num_targets));% XYZIR
for t = 1:v_num_targets
for iter = 1: opts.num_scans
v_data(t).scan(iter).payload_points = getPayload(v_pc(t).point_cloud, iter);
v_data(t).tag_size = validation_mat_files(t).tag_size;
end
end
disp("Done loading data!")
%% Optimize intrinsic parameters
table = zeros(9,1);
level = [0,1,2,3,4,5,6,7];
for i = 1:length(level) % different noise level(0-7), our method encounter problems when noise level = 2,3,5,6
opts.noise_level = level(i);
for j = 1:3 % 3 calibration method : "Lie","BaseLine1","BaseLine2",
opts.method = j;
[calib_param, plane_original, plane, distance_original, distance, ...
original_points, updated_points] = intrinsicCalibration(...
opts, data, num_targets);
if ~exist(opts.save_path, 'dir')
mkdir(opts.save_path)
end
final_parameters = computeCalibParameters(calib_param, opts.num_beams, opts.method);
%% Show graphical results
if opts.show_results
disp("Now plotting....")
plotCalibratedResults(num_targets, opts.num_scans, ...
plane_original, plane, original_points, updated_points);
disp("Done plotting!")
end
%% Validation
% validation_data = parseValidationbag(v_path, "*.bag", 1, opts.num_scans);
[validation_results] = validation(v_data, final_parameters, opts);
table(1,i) = mean([validation_results.mean_original]);
table(5,i) = mean([validation_results.thickness_original]);
if j == 2
table(2,i) = mean([validation_results.mean_calibrated]);
table(6,i) = mean([validation_results.thickness_calibrated]);
elseif j ==3
table(3,i) = mean([validation_results.mean_calibrated]);
table(7,i) = mean([validation_results.thickness_calibrated]);
elseif j ==1
table(4,i) = mean([validation_results.mean_calibrated]);
table(8,i) = mean([validation_results.thickness_calibrated]);
end
end
end
table
disp("All processes has finished")