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
|
function [G,C] = find3G(Rhat)
% number of frames
F = size(Rhat,1)/2;
% Build matrix Q such that Q * v = [1,...,1,0,...,0] where v is a six
% element vector containg all six distinct elements of the Matrix C
clear Q
for f = 1:F,
g = f + F;
h = g + F;
Q(f,:) = zt(Rhat(f,:), Rhat(f,:));
Q(g,:) = zt(Rhat(g,:), Rhat(g,:));
Q(h,:) = zt(Rhat(f,:), Rhat(g,:));
end
% Solve for v
rhs = [ones(2*F,1); zeros(F,1)];
v = Q \ rhs;
% C is a symmetric 3x3 matrix such that C = G * transpose(G)
C(1,1) = v(1);
C(1,2) = v(2);
C(1,3) = v(3);
C(2,2) = v(4);
C(2,3) = v(5);
C(3,3) = v(6);
C(2,1) = C(1,2);
C(3,1) = C(1,3);
C(3,2) = C(2,3);
e = eig(C);
disp(e)
if (any(e<= 0)),
C = C -2*min(e)*eye(3);
G = sqrtm(C);
else
G = sqrtm(C);
end
%neg = 0;
%if e(1) <= 0, neg = 1; end
%if e(2) <= 0, neg = 1; end
%if e(3) <= 0, neg = 1; end
%if neg == 1, G = [];
%else G = sqrtm(C);
%end
|
