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
|
function [position_error,rotation_error] = evaluateTransformationError(T_gt,T)
temp = size(size(T));
numberSolutions = 1;
if temp(1,2) == 3
temp2 = size(T);
numberSolutions = temp2(1,3);
end
if numberSolutions == 1
position_error = norm(T_gt(:,4)-T(:,4));
rotation_error = norm(rodrigues(T_gt(:,1:3)'*T(:,1:3)));
else
position_errors = zeros(1,numberSolutions);
rotation_errors = zeros(1,numberSolutions);
index = 0;
for i=1:numberSolutions
%Check if there is any Nan
if ~isnan(T(1,1,i))
index = index + 1;
position_errors(1,index) = norm(T_gt(:,4)-T(:,4,i));
rotation_errors(1,index) = norm(rodrigues(T_gt(:,1:3)'*T(:,1:3,i)));
end
end
%find the smallest error (we are the most "nice" to algorithms returning multiple solutions,
%and do the disambiguation by hand)
[~,minIndex] = min(position_errors(1,1:index));
position_error = position_errors(1,minIndex);
rotation_error = rotation_errors(1,minIndex);
end
end
|
