1 files changed, 56 insertions, 0 deletions

diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/findg2.m b/SD-VBS/common/toolbox/toolbox_basic/fact/findg2.m
new file mode 100755
index 0000000..5a84b86
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/findg2.m
@@ -0,0 +1,56 @@
+function [G,C] = find3G(Rhat)
+
+% number of frames
+F = size(Rhat,1)/3;
+
+% 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;
+  j = h + F;
+  k = j + F;
+  l = k + F;
+  Q(f,:) = zt(Rhat(f,:), Rhat(f,:));
+  Q(g,:) = zt(Rhat(g,:), Rhat(g,:));
+  Q(h,:) = zt(Rhat(h,:), Rhat(h,:));
+  Q(j,:) = zt(Rhat(f,:), Rhat(g,:));
+  Q(k,:) = zt(Rhat(f,:), Rhat(h,:));
+  Q(l,:) = zt(Rhat(g,:), Rhat(h,:));
+end
+
+% Solve for v
+rhs = [ones(3*F,1); zeros(3*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