-
Notifications
You must be signed in to change notification settings - Fork 38
/
evaluate_poses_comparison.m
189 lines (167 loc) · 6.43 KB
/
evaluate_poses_comparison.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
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
function evaluate_poses_comparison
opt = globals();
% your results
resdir = '/var/Projects/PoseRNN/output/lov/lov_keyframe/vgg16_fcn_color_single_frame_2d_lov_iter_80000/mat';
% read class names
fid = fopen('classes.txt', 'r');
C = textscan(fid, '%s');
object_names = C{1};
fclose(fid);
% load model points
num_objects = numel(object_names);
models = cell(num_objects, 1);
for i = 1:num_objects
filename = fullfile(opt.root, 'models', object_names{i}, 'points.xyz');
disp(filename);
models{i} = load(filename);
end
% load the keyframe indexes
fid = fopen('keyframe.txt', 'r');
C = textscan(fid, '%s');
keyframes = C{1};
fclose(fid);
% save results
distances_sys = zeros(100000, 2);
distances_non = zeros(100000, 2);
errors_rotation = zeros(100000, 2);
errors_translation = zeros(100000, 2);
results_seq_id = zeros(100000, 1);
results_frame_id = zeros(100000, 1);
results_object_id = zeros(100000, 1);
results_cls_id = zeros(100000, 1);
% for each image
count = 0;
for i = 1:numel(keyframes)
% parse keyframe name
name = keyframes{i};
pos = strfind(name, '/');
seq_id = str2double(name(1:pos-1));
frame_id = str2double(name(pos+1:end));
% load PoseCNN result
filename = sprintf('results_PoseCNN/%04d.mat', i - 1);
result = load(filename);
% load 3D coordinate regression result
filename = sprintf('%s/%04d.mat', resdir, i - 1);
result_new = load(filename);
% load gt poses
filename = fullfile(opt.root, 'data', sprintf('%04d/%06d-meta.mat', seq_id, frame_id));
disp(filename);
gt = load(filename);
% for each gt poses
for j = 1:numel(gt.cls_indexes)
count = count + 1;
cls_index = gt.cls_indexes(j);
RT_gt = gt.poses(:, :, j);
results_seq_id(count) = seq_id;
results_frame_id(count) = frame_id;
results_object_id(count) = j;
results_cls_id(count) = cls_index;
% network result
roi_index = find(result.rois(:, 2) == cls_index);
if isempty(roi_index) == 0
RT = zeros(3, 4);
% pose from network
RT(1:3, 1:3) = quat2rotm(result.poses(roi_index, 1:4));
RT(:, 4) = result.poses(roi_index, 5:7);
distances_sys(count, 1) = adi(RT, RT_gt, models{cls_index}');
distances_non(count, 1) = add(RT, RT_gt, models{cls_index}');
errors_rotation(count, 1) = re(RT(1:3, 1:3), RT_gt(1:3, 1:3));
errors_translation(count, 1) = te(RT(:, 4), RT_gt(:, 4));
else
distances_sys(count, 1) = inf;
distances_non(count, 1) = inf;
errors_rotation(count, 1) = inf;
errors_translation(count, 1) = inf;
end
% your result
roi_index = find(result_new.rois(:, 2) == cls_index);
if isempty(roi_index) == 0
RT = zeros(3, 4);
RT(1:3, 1:3) = quat2rotm(result_new.poses(roi_index, 1:4));
RT(:, 4) = result_new.poses(roi_index, 5:7);
distances_sys(count, 2) = adi(RT, RT_gt, models{cls_index}');
distances_non(count, 2) = add(RT, RT_gt, models{cls_index}');
errors_rotation(count, 2) = re(RT(1:3, 1:3), RT_gt(1:3, 1:3));
errors_translation(count, 2) = te(RT(:, 4), RT_gt(:, 4));
else
distances_sys(count, 2) = inf;
distances_non(count, 2) = inf;
errors_rotation(count, 2) = inf;
errors_translation(count, 2) = inf;
end
end
end
distances_sys = distances_sys(1:count, :);
distances_non = distances_non(1:count, :);
errors_rotation = errors_rotation(1:count, :);
errors_translation = errors_translation(1:count, :);
results_seq_id = results_seq_id(1:count);
results_frame_id = results_frame_id(1:count);
results_object_id = results_object_id(1:count, :);
results_cls_id = results_cls_id(1:count, :);
save('results_comparison.mat', 'distances_sys', 'distances_non', 'errors_rotation', 'errors_translation',...
'results_seq_id', 'results_frame_id', 'results_object_id', 'results_cls_id');
function pts_new = transform_pts_Rt(pts, RT)
% """
% Applies a rigid transformation to 3D points.
%
% :param pts: nx3 ndarray with 3D points.
% :param R: 3x3 rotation matrix.
% :param t: 3x1 translation vector.
% :return: nx3 ndarray with transformed 3D points.
% """
n = size(pts, 2);
pts_new = RT * [pts; ones(1, n)];
function error = add(RT_est, RT_gt, pts)
% """
% Average Distance of Model Points for objects with no indistinguishable views
% - by Hinterstoisser et al. (ACCV 2012).
%
% :param R_est, t_est: Estimated pose (3x3 rot. matrix and 3x1 trans. vector).
% :param R_gt, t_gt: GT pose (3x3 rot. matrix and 3x1 trans. vector).
% :param model: Object model given by a dictionary where item 'pts'
% is nx3 ndarray with 3D model points.
% :return: Error of pose_est w.r.t. pose_gt.
% """
pts_est = transform_pts_Rt(pts, RT_est);
pts_gt = transform_pts_Rt(pts, RT_gt);
diff = pts_est - pts_gt;
error = mean(sqrt(sum(diff.^2, 1)));
function error = adi(RT_est, RT_gt, pts)
% """
% Average Distance of Model Points for objects with indistinguishable views
% - by Hinterstoisser et al. (ACCV 2012).
%
% :param R_est, t_est: Estimated pose (3x3 rot. matrix and 3x1 trans. vector).
% :param R_gt, t_gt: GT pose (3x3 rot. matrix and 3x1 trans. vector).
% :param model: Object model given by a dictionary where item 'pts'
% is nx3 ndarray with 3D model points.
% :return: Error of pose_est w.r.t. pose_gt.
% """
pts_est = transform_pts_Rt(pts, RT_est);
pts_gt = transform_pts_Rt(pts, RT_gt);
% Calculate distances to the nearest neighbors from pts_gt to pts_est
MdlKDT = KDTreeSearcher(pts_est');
[~, D] = knnsearch(MdlKDT, pts_gt');
error = mean(D);
function error = re(R_est, R_gt)
% """
% Rotational Error.
%
% :param R_est: Rotational element of the estimated pose (3x1 vector).
% :param R_gt: Rotational element of the ground truth pose (3x1 vector).
% :return: Error of t_est w.r.t. t_gt.
% """
error_cos = 0.5 * (trace(R_est * inv(R_gt)) - 1.0);
error_cos = min(1.0, max(-1.0, error_cos));
error = acos(error_cos);
error = 180.0 * error / pi;
function error = te(t_est, t_gt)
% """
% Translational Error.
%
% :param t_est: Translation element of the estimated pose (3x1 vector).
% :param t_gt: Translation element of the ground truth pose (3x1 vector).
% :return: Error of t_est w.r.t. t_gt.
% """
error = norm(t_gt - t_est);