open source pkg v1

pkg/OpenFace/model_training/pdm_generation/nrsfm-em/rigidfac.m

@@ -0,0 +1,39 @@
+function [R, Tr, S] = rigidfac(P, MD)
+% [R, Tr, S] = rigidfac(P, MD)
+%
+% Computes rank 3 factorization: P = R*S + Tr
+
+Pnew = P;
+
+[T, J] = size(Pnew); T = T/2;
+
+
+if sum(MD(:)) > 0, % if there is missing data, then it uses an iterative solution to get a rough initialization for the missing points
+   [i,j] = find(MD==1);
+   ind = sub2ind(size(P), [i; i+T], [j; j]);
+   numIter = 10;
+else
+   numIter = 1;
+   ind = [];
+end
+
+r = 3;
+for iter=1:numIter,
+   Tr = Pnew*ones(J,1)/J;
+   Pnew_c = Pnew - Tr*ones(1,J);
+   
+   [a,b,c] = svd(Pnew_c,0);   
+   
+   smallb = b(1:r,1:r);
+   sqrtb = sqrt(smallb);
+   Rhat = a(:,1:r) * sqrtb;
+   Shat = sqrtb * c(:,1:r)';
+   
+   P_approx = Rhat*Shat + Tr*ones(1,J);
+   
+   Pnew(ind) = P_approx(ind);   
+end
+
+G = findG(Rhat); 
+R = Rhat*G;
+S = inv(G)*Shat;