15 files changed, 422 insertions, 0 deletions

diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w.m b/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w.m
new file mode 100755
index 0000000..372b4b6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w.m
@@ -0,0 +1,25 @@
+function W = construct_w(centers,Ds,img_center,indexes,frames)
+%
+%  function W = construct_w(centers,Ds,img_center,indexes,frames)
+%   optional: frames
+%  
+
+
+points = length(indexes);
+if (nargin == 4),
+ frames = 0.5*size(centers,2);
+end
+
+W = zeros(2*frames,points);
+
+center_x = img_center(1);
+center_y = img_center(2);
+
+for j=1:frames,
+  % x is centers(:,2*j-1)
+  % y is centers(:,2*j)
+  % d is Ds(:,2*j-1)
+  W(j,:) = (centers(indexes,2*j-1) -center_x)'./Ds(indexes,2*j-1)';
+  W(j+frames,:) = (centers(indexes,2*j) -center_y)'./Ds(indexes,2*j-1)'; 
+ % W(j+2*frames,:) = ones(1,points)./Ds(indexes,2*j-1)';
+end
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w2.m b/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w2.m
new file mode 100755
index 0000000..b2939b7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w2.m
@@ -0,0 +1,25 @@
+function W = construct_w2(centers,Ds,img_center,indexes,frames)
+%
+%  function W = construct_w2(centers,Ds,img_center,indexes,frames)
+%   optional: frames
+%  
+
+
+points = length(indexes);
+if (nargin == 4),
+ frames = 0.5*size(centers,2);
+end
+
+W = zeros(3*frames,points);
+
+center_x = img_center(1);
+center_y = img_center(2);
+
+for j=1:frames,
+  % x is centers(:,2*j-1)
+  % y is centers(:,2*j)
+  % d is Ds(:,2*j-1)
+  W(j,:) = (centers(indexes,2*j-1) -center_x)'./Ds(indexes,2*j-1)';
+  W(j+frames,:) = (centers(indexes,2*j) -center_y)'./Ds(indexes,2*j-1)'; 
+  W(j+2*frames,:) = ones(1,points)./Ds(indexes,2*j-1)';
+end
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/factor.m b/SD-VBS/common/toolbox/toolbox_basic/fact/factor.m
new file mode 100755
index 0000000..635c29a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/factor.m
@@ -0,0 +1,50 @@
+%
+% Usage:
+%
+%    [R,t,S] = factor(W)
+%
+% Function to factor a matrix of input data (W) into the camera
+% rotation matrix (R), translation (t), and the shape matrix (S).
+% Three-dimensional version. Failure of normalization results in
+% empty R and S.
+
+function [R,t,S] = factor(W)
+
+pts = size(W,2);
+t = W*ones(pts,1)/pts;
+W = W - t*ones(1,pts);
+
+% Use SVD to factor W. 
+   [a,b,c] = svd(W,0);
+ 
+smallb = b(1:3,1:3);    % Since W is rank 3, b has only three meaningful values
+sqrtb = sqrt(smallb);
+Rhat = a(:,1:3) * sqrtb;
+Shat = sqrtb * c(:,1:3)';
+
+G = findG(Rhat);
+
+if size(G,1) == 0,
+R = [];
+S = [];
+else
+  R = Rhat*G;
+  S = inv(G)*Shat;
+
+  % rotation matrix that aligns the reference frame with the first camera
+  F = size(R,1)/2;
+  R1 = R(1,:);
+  R1 = R1/norm(R1);
+  R2 = R(F+1,:);
+  R2 = R2/norm(R2);
+  R3 = cross(R1,R2);
+  R3 = R3/norm(R3);
+  P = [R1; R2; R3];
+  P = P';
+
+  R = R*P;
+  S = inv(P)*S;
+end
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test.m b/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test.m
new file mode 100755
index 0000000..12ceb95
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test.m
@@ -0,0 +1,52 @@
+%
+% Usage:
+%
+%    [R,t,S] = factor(W)
+%
+% Function to factor a matrix of input data (W) into the camera
+% rotation matrix (R), translation (t), and the shape matrix (S).
+% Three-dimensional version. Failure of normalization results in
+% empty R and S.
+
+function [R,t,S,C,b] = factor(W)
+
+pts = size(W,2);
+t = W*ones(pts,1)/pts;
+W = W - t*ones(1,pts);
+
+% Use SVD to factor W. 
+   [a,b,c] = svd(W,0);
+
+figure(3);plot(diag(b))
+ 
+smallb = b(1:3,1:3);    % Since W is rank 3, b has only three meaningful values
+sqrtb = sqrt(smallb);
+Rhat = a(:,1:3) * sqrtb;
+Shat = sqrtb * c(:,1:3)';
+
+[G,C] = findg1(Rhat);
+
+if size(G,1) == 0,
+R = [];
+S = [];
+else
+  R = Rhat*G;
+  S = inv(G)*Shat;
+
+  % rotation matrix that aligns the reference frame with the first camera
+  F = size(R,1)/2;
+  R1 = R(1,:);
+  R1 = R1/norm(R1);
+  R2 = R(F+1,:);
+  R2 = R2/norm(R2);
+  R3 = cross(R1,R2);
+  R3 = R3/norm(R3);
+  P = [R1; R2; R3];
+  P = P';
+
+  R = R*P;
+  S = inv(P)*S;
+end
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test2.m b/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test2.m
new file mode 100755
index 0000000..3520122
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test2.m
@@ -0,0 +1,52 @@
+%
+% Usage:
+%
+%    [R,t,S] = factor(W)
+%
+% Function to factor a matrix of input data (W) into the camera
+% rotation matrix (R), translation (t), and the shape matrix (S).
+% Three-dimensional version. Failure of normalization results in
+% empty R and S.
+
+function [R,t,S,C,b] = factor(W)
+
+pts = size(W,2);
+t = W*ones(pts,1)/pts;
+W = W - t*ones(1,pts);
+
+% Use SVD to factor W. 
+   [a,b,c] = svd(W,0);
+
+figure(3);plot(diag(b))
+ 
+smallb = b(1:3,1:3);    % Since W is rank 3, b has only three meaningful values
+sqrtb = sqrt(smallb);
+Rhat = a(:,1:3) * sqrtb;
+Shat = sqrtb * c(:,1:3)';
+
+[G,C] = findg2(Rhat);
+
+if size(G,1) == 0,
+R = [];
+S = [];
+else
+  R = Rhat*G;
+  S = inv(G)*Shat;
+
+  % rotation matrix that aligns the reference frame with the first camera
+  F = size(R,1)/2;
+  R1 = R(1,:);
+  R1 = R1/norm(R1);
+  R2 = R(F+1,:);
+  R2 = R2/norm(R2);
+  R3 = cross(R1,R2);
+  R3 = R3/norm(R3);
+  P = [R1; R2; R3];
+  P = P';
+
+  R = R*P;
+  S = inv(P)*S;
+end
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/factorizaion.tar b/SD-VBS/common/toolbox/toolbox_basic/fact/factorizaion.tar
new file mode 100755
index 0000000..133cdff
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/factorizaion.tar
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/findG.m b/SD-VBS/common/toolbox/toolbox_basic/fact/findG.m
new file mode 100755
index 0000000..9a6bd73
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/findG.m
@@ -0,0 +1,48 @@
+function G = 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)),
+  G = [];
+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
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/findg1.m b/SD-VBS/common/toolbox/toolbox_basic/fact/findg1.m
new file mode 100755
index 0000000..f14ecc6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/findg1.m
@@ -0,0 +1,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
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
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/hotel.mat b/SD-VBS/common/toolbox/toolbox_basic/fact/hotel.mat
new file mode 100755
index 0000000..61ea6c8
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/hotel.mat
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/show_3dpoints.m b/SD-VBS/common/toolbox/toolbox_basic/fact/show_3dpoints.m
new file mode 100755
index 0000000..b6edfd5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/show_3dpoints.m
@@ -0,0 +1,22 @@
+function show_3dpoints(S)
+
+
+for j=1:size(S,2),
+  x = S(1,j);
+  y = S(2,j);
+  z = S(3,j);
+  plot3(x,y,z,'*');
+  hold on;
+  plot3([x,0],[y,0],[z,0],'r');
+%  plot3([x,x],[y,y],[z,0],'r');
+%  plot3([x,0],[y,y],[z,z],'r'); plot3([x,x],[y,0],[z,z],'r');
+  text(x,y,z,int2str(j))
+%  plot3(x,y,0,'co');
+end
+
+grid on
+xlabel('x');
+ylabel('y');
+zlabel('z');
+
+hold off
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/show_S.m b/SD-VBS/common/toolbox/toolbox_basic/fact/show_S.m
new file mode 100755
index 0000000..5828696
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/show_S.m
@@ -0,0 +1,17 @@
+function show_S(S,fig)
+
+if (nargin == 1),
+  figure(1);
+else
+  figure(fig);
+end
+
+num_points = size(S,2);
+
+subplot(1,2,1); plot(S(1,:),S(3,:),'cx'); axis('equal');axis('square');hold on
+subplot(1,2,2); plot(S(2,:),S(3,:),'cx'); axis('equal');axis('square');hold on
+
+for j=1:num_points,
+ subplot(1,2,1);text(S(1,j),S(3,j),int2str(j));hold off
+ subplot(1,2,2);text(S(2,j),S(3,j),int2str(j));hold off
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/show_t.m b/SD-VBS/common/toolbox/toolbox_basic/fact/show_t.m
new file mode 100755
index 0000000..b475c76
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/show_t.m
@@ -0,0 +1,10 @@
+function show_t(t)
+
+frames = 0.5*length(t);
+
+ts = reshape(t,frames,2);
+
+plot(ts(:,1),ts(:,2));
+hold on
+plot(ts(:,1),ts(:,2),'rx');
+hold off;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/show_t3.m b/SD-VBS/common/toolbox/toolbox_basic/fact/show_t3.m
new file mode 100755
index 0000000..2766061
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/show_t3.m
@@ -0,0 +1,10 @@
+function show_t(t)
+
+frames = length(t)/3;
+
+ts = reshape(t,frames,3);
+
+plot3(ts(:,1),ts(:,2),ts(:,3));
+hold on
+plot3(ts(:,1),ts(:,2),ts(:,3),'rx');
+hold off;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/zt.m b/SD-VBS/common/toolbox/toolbox_basic/fact/zt.m
new file mode 100755
index 0000000..3f88d21
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/zt.m
@@ -0,0 +1,6 @@
+% the z' operator described in the paper (returns a row vector)
+
+function v = zt(a, b)
+
+v = [   a(1)*b(1), a(1)*b(2)+a(2)*b(1), a(1)*b(3)+a(3)*b(1), ...
+        a(2)*b(2), a(2)*b(3)+a(3)*b(2), a(3)*b(3) ];