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.