-
Notifications
You must be signed in to change notification settings - Fork 7
/
main_compare_extrinsic.m
87 lines (87 loc) · 4.13 KB
/
main_compare_extrinsic.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
close all
clear
clc
%% Parameter Setup
filename_x_LI = "./results/2021-11-10/bag1/x_LI.mat"; % LiDAR to INS Extrinsic
filename_x_CI = "./results/2021-11-10/bag1/x_CI.mat"; % Camera to INS Extrinsic
filename_x_LC = "./results/2021-11-10/bag1/x_LC.mat"; % LiDAR to Camera Extrinsic
filename_x_LI_FCPE = "./results/2021-11-10/bag1/x_LI_FCPE.mat"; % LiDAR to INS Extrinsic
filename_x_CI_FCPE = "./results/2021-11-10/bag1/x_CI_FCPE.mat"; % Camera to INS Extrinsic
filename_x_LC_FCPE = "./results/2021-11-10/bag1/x_LC_FCPE.mat"; % LiDAR to Camera Extrinsic
filename_x_LI_FCPE_update = "./results/2021-11-10/bag1/x_LI_FCPE_update.mat"; % LiDAR to INS Extrinsic
filename_x_CI_FCPE_update = "./results/2021-11-10/bag1/x_CI_FCPE_update.mat"; % Camera to INS Extrinsic
filename_x_LC_FCPE_update = "./results/2021-11-10/bag1/x_LC_FCPE_update.mat"; % LiDAR to Camera Extrinsic
%% Load Extrinsic
data_LI = load(filename_x_LI, '-ascii');
data_CI = load(filename_x_CI, '-ascii');
data_LC = load(filename_x_LC, '-ascii');
data_LI_FCPE = load(filename_x_LI_FCPE, '-ascii');
data_CI_FCPE = load(filename_x_CI_FCPE, '-ascii');
data_LC_FCPE = load(filename_x_LC_FCPE, '-ascii');
data_LI_FCPE_update = load(filename_x_LI_FCPE_update, '-ascii');
data_CI_FCPE_update = load(filename_x_CI_FCPE_update, '-ascii');
data_LC_FCPE_update = load(filename_x_LC_FCPE_update, '-ascii');
%%% Hand-Eye Calibration
t_LI = data_LI(1, 1 : 3);
q_LI = data_LI(1, 4 : 7);
T_LI = quat2tform(q_LI);
T_LI(1 : 3, 4) = t_LI';
t_CI = data_CI(1, 1 : 3);
q_CI = data_CI(1, 4 : 7);
scale_CI = data_CI(1, 8);
T_CI = quat2tform(q_LI);
T_CI(1 : 3, 4) = t_CI';
t_LC = data_LC(1, 1 : 3);
q_LC = data_LC(1, 4 : 7);
scale_LC = data_LC(1, 8);
T_LC = quat2tform(q_LC);
T_LC(1 : 3, 4) = t_LC';
%%% Round 1
t_LI_FCPE = data_LI_FCPE(1, 1 : 3);
q_LI_FCPE = data_LI_FCPE(1, 4 : 7);
T_LI_FCPE = quat2tform(q_LI_FCPE);
T_LI_FCPE(1 : 3, 4) = t_LI_FCPE';
t_CI_FCPE = data_CI_FCPE(1, 1 : 3);
q_CI_FCPE = data_CI_FCPE(1, 4 : 7);
scale_CI_FCPE = data_CI_FCPE(1, 8);
T_CI_FCPE = quat2tform(q_LI_FCPE);
T_CI_FCPE(1 : 3, 4) = t_CI_FCPE';
t_LC_FCPE = data_LC_FCPE(1, 1 : 3);
q_LC_FCPE = data_LC_FCPE(1, 4 : 7);
scale_LC_FCPE = data_LC_FCPE(1, 8);
T_LC_FCPE = quat2tform(q_LC_FCPE);
T_LC_FCPE(1 : 3, 4) = t_LC_FCPE';
%%% Round 2
t_LI_FCPE_update = data_LI_FCPE_update(1, 1 : 3);
q_LI_FCPE_update = data_LI_FCPE_update(1, 4 : 7);
T_LI_FCPE_update = quat2tform(q_LI_FCPE_update);
T_LI_FCPE_update(1 : 3, 4) = t_LI_FCPE_update';
t_CI_FCPE_update = data_CI_FCPE_update(1, 1 : 3);
q_CI_FCPE_update = data_CI_FCPE_update(1, 4 : 7);
scale_CI_FCPE_update = data_CI_FCPE_update(1, 8);
T_CI_FCPE_update = quat2tform(q_LI_FCPE_update);
T_CI_FCPE_update(1 : 3, 4) = t_CI_FCPE_update';
t_LC_FCPE_update = data_LC_FCPE_update(1, 1 : 3);
q_LC_FCPE_update = data_LC_FCPE_update(1, 4 : 7);
scale_LC_FCPE_update = data_LC_FCPE_update(1, 8);
T_LC_FCPE_update = quat2tform(q_LC_FCPE_update);
T_LC_FCPE_update(1 : 3, 4) = t_LC_FCPE_update';
%% Display Results
consistence_HE = [norm(T_LC * T_CI - T_LI, 2), (scale_LC - scale_CI)];
consistence_FCPE = [norm(T_LC_FCPE * T_CI_FCPE - T_LI_FCPE, 2), (scale_LC_FCPE - scale_CI_FCPE)];
consistence_update = [norm(T_LC_FCPE_update * T_CI_FCPE_update - T_LI_FCPE_update, 2), (scale_LC_FCPE_update - scale_CI_FCPE_update)];
fprintf("Hand-Eye Calibration Consistence:\t\t%.6f\t%.6f\n", consistence_HE)
fprintf("Round 1 Joint Calibration Consistence:\t%.6f\t%.6f\n", consistence_FCPE)
fprintf("Round 2 Joint Calibration Consistence:\t%.6f\t%.6f\n", consistence_update)
fprintf("t_LI:\t\t\t\t%.6f\t%.6f\t%.6f\n", t_LI)
fprintf("t_LI_FCPE:\t\t\t%.6f\t%.6f\t%.6f\n", t_LI_FCPE)
fprintf("t_LI_FCPE_update:\t%.6f\t%.6f\t%.6f\n", t_LI_FCPE_update)
fprintf("t_CI:\t\t\t\t%.6f\t%.6f\t%.6f\n", t_CI)
fprintf("t_CI_FCPE:\t\t\t%.6f\t%.6f\t%.6f\n", t_CI_FCPE)
fprintf("t_CI_FCPE_update:\t%.6f\t%.6f\t%.6f\n", t_CI_FCPE_update)
fprintf("t_LC:\t\t\t\t%.6f\t%.6f\t%.6f\n", t_LC)
fprintf("t_LC_FCPE:\t\t\t%.6f\t%.6f\t%.6f\n", t_LC_FCPE)
fprintf("t_LC_FCPE_update:\t%.6f\t%.6f\t%.6f\n", t_LC_FCPE_update)
fprintf("q_LC:\t\t\t\t%.4f\t%.4f\t%.4f\t%.4f\t\n", q_LC)
fprintf("q_LC_FCPE:\t\t\t%.4f\t%.4f\t%.4f\t%.4f\t\n", q_LC_FCPE)
fprintf("q_LC_FCPE_update:\t%.4f\t%.4f\t%.4f\t%.4f\t\n", q_LC_FCPE_update)