-
Notifications
You must be signed in to change notification settings - Fork 12
/
LieGroupOptimize.m
119 lines (102 loc) · 5.03 KB
/
LieGroupOptimize.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
clear, clc; % DO NOT add 'clear' here
if exist('pc','var') && exist('data', 'var') && exist('mat_files', 'var')
% if exist('pc','var')
disp("Data have been loaded, it may take a while to reload them.")
prompt = "Are you sure you want to reload them again? [Y] [N]";
urs_ans = input(prompt, 's');
if contains(urs_ans, 'N','IgnoreCase', true)
disp('Reload datasets cancelled')
return
else
clear
disp("ALL varialbes cleared! ")
disp("Keep reloading datasets...")
end
end
% Single variable called point_cloud
% opts.path = "/home/brucebot/workspace/griztag/src/matlab/matlab/slider/intrinsic_latest/data/";
% opts.path = "./data/";
opts.path = "..\intrinsic_lidar_calibration\intrinsic_calibration_mat\";
opts.load_all = 1;
opts.datatype = "Experiment";
opts.show_results = 0;
opt_formulation = ["Lie","BaseLine1","BaseLine3"]; % Lie or Spherical
opts.method = 1; % "Lie","BaseLine1","BaseLine3"
opts.iterative = 0;
opts.num_beams = 32;
opts.num_scans = 1;
opts.num_iters = 10; % user defined iterations
% path = "/home/chenxif/Documents/me590/Calibration/IntrinsicCalibration/extracted_tags/";
disp("Loading names of data sets...")
if ~opts.load_all
mat_files = struct('file_name', {opts.path+'velodyne_points-Intrinsic-LargeTag--2019-11-21-22-04.mat';...
opts.path+'velodyne_points-Intrinsic-SmallTag--2019-11-21-22-00.mat';...
opts.path+'velodyne_points-Intrinsic4-SmallTag--2019-11-22-22-54.mat';...
opts.path+'velodyne_points-Intrinsic5-LargeTag--2019-11-22-23-02.mat';...
opts.path+'velodyne_points-Intrinsic5-SmallTag--2019-11-22-23-00.mat';...
opts.path+'velodyne_points-Intrinsic-further-LargeTag--2019-11-22-23-05.mat';...
opts.path+'velodyne_points-Intrinsic-further-SmallTag--2019-11-22-23-09.mat';...
opts.path+'velodyne_points-Intrinsic-further2-LargeTag--2019-11-22-23-15.mat';...
opts.path+'velodyne_points-Intrinsic-further2-SmallTag--2019-11-22-23-17.mat';...
opts.path+'velodyne_points-upper1-SmallTag--2019-12-05-20-13.mat';...
opts.path+'velodyne_points-upper2-SmallTag--2019-12-05-20-16.mat';...
opts.path+'velodyne_points-upper3-SmallTag--2019-12-05-20-19.mat';...
opts.path+'velodyne_points-upper4-SmallTag--2019-12-05-20-22.mat';...
opts.path+'velodyne_points-upper5-SmallTag--2019-12-05-20-23.mat';...
opts.path+'velodyne_points-upper6-SmallTag--2019-12-05-20-26.mat';...
opts.path+'velodyne_points-upper7-SmallTag--2019-12-05-20-29.mat';...
opts.path+'velodyne_points-upper8-SmallTag--2019-12-05-20-29.mat'});
for file = 1:length(mat_files)
mat_files(file).tag_size = identifyTagSizeFromAName(mat_files(file).file_name);
end
else
mat_files = loadFilesFromAFolder(opts.path, '*Tag*.mat');
end
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).payload_points = getPayload(pc(t).point_cloud, iter, 1);
data(t).tag_size = mat_files(t).tag_size;
end
end
disp("Done loading data!")
%%
opts.iterative = 0;
opts.method = 1; % ["Lie","BaseLine1","BaeLine3"]
opts.num_iters = 10;
opts.save_path = ".\results\scan1\training_data\Lie\";
disp('Computing planes...')
opt.corners.rpy_init = [45 2 3];
opt.corners.T_init = [2, 0, 0];
opt.corners.H_init = eye(4);
opt.corners.method = "Constraint Customize"; %% will add more later on
opt.corners.UseCentroid = 1;
plane = cell(1,num_targets);
for t = 1:num_targets
X = [];
for j = 1: opts.num_beams
X = [X, data(t).payload_points];
end
[plane{t}, ~] = estimateNormal(opt.corners, X(1:3, :), 1.5);
end
data_split_with_ring_cartesian = cell(1,num_targets);
disp("Parsing data...")
for t = 1:num_targets
data_split_with_ring_cartesian{t} = splitPointsBasedOnRing(data(t).payload_points, opts.num_beams, opts.datatype);
end
%%
distance_original = point2PlaneDistance(data_split_with_ring_cartesian, plane, opts.num_beams, num_targets);
delta(opts.num_beams) = struct();
for ring = 1: opts.num_beams
delta(ring).Affine = GaussNewtonSolver(data_split_with_ring_cartesian, plane, ring);
end
data_split_with_ring_cartesian = updateDataRaw(opts.num_beams, num_targets, data_split_with_ring_cartesian, delta, 1, opt_formulation(opts.method));
distance = point2PlaneDistance(data_split_with_ring_cartesian, plane, opts.num_beams, num_targets);
plotCalibratedResults(num_targets, plane, data_split_with_ring_cartesian, data);