sustaining_gazes/matlab_version/fitting/NU_RLMS.m
2016-04-28 15:40:36 -04:00

314 lines
No EOL
9.3 KiB
Matlab

function [ final_global, final_local, final_lhood, landmark_lhoods ] = NU_RLMS( ...
init_global, init_local, PDM, patchResponses, visibilities,...
view, reliabilities, baseShape, OrigToRefTransform, rigid, ...
clmParams, gauss_resp)
%RLMS Summary of this function goes here
% Detailed explanation goes here
m = numel(PDM.E);
E = PDM.E;
V = PDM.V;
n = size(V, 1)/3;
current_local = init_local;
current_global = init_global;
pxWidth = size(patchResponses{1},1);
responseSize = size(patchResponses{1});
[iis,jjs] = meshgrid(1:pxWidth, 1:pxWidth);
% Grab all of the patch responses and convert them to single matrix
% representation for speed
patchResponsesFlat = reshape(cat(3,patchResponses{:}), responseSize(1)*responseSize(2), n)';
iisFlat = repmat(iis(:)', n, 1);
jjsFlat = repmat(jjs(:)', n, 1);
% An alternative formulation
reg_rigid = zeros(6,1);
if(rigid)
regularisations = reg_rigid;
regularisations = diag(regularisations);
else
regularisations = [reg_rigid; clmParams.regFactor ./ E]; % the above version, however, does not perform as well
regularisations = diag(regularisations);
end
% For generalised Tikhonov
if(clmParams.tikhonov_factor == 0)
P = eye(n*2);
else
% the worse the reliability the higher the variance of the
% prediction, so inverse variance correlate with reliability
P = clmParams.tikhonov_factor * diag(repmat(reliabilities',2,1));
end
for iter = 1:clmParams.num_RLMS_iter
% get the current estimates of x and y in image
currentShape = GetShapeOrtho(PDM.M, PDM.V, current_local, current_global);
currentShape = currentShape(:,1:2);
if(iter > 1)
% if the shape hasn't changed terminate
if(norm(currentShape - previousShape) < clmParams.fTol)
break;
end
end
previousShape = currentShape;
% calculate the appropriate Jacobians in 2D, even though the actual behaviour is in 3D, using small angle approximation and oriented shape
if(rigid)
J = CalcRigidJacobian(PDM.M, PDM.V, current_local, current_global);
else
J = CalcJacobian(PDM.M, PDM.V, current_local, current_global);
end
% as the mean shift is with reference to the point, we don't care about
% the translation
OrigToRefTransform.tdata.T(3,1:2) = 0;
OrigToRefTransform.tdata.Tinv(3,1:2) = 0;
% distance from center where the response was calculated around
% in reference frame of the patch
offsets = (currentShape - baseShape) * OrigToRefTransform.tdata.T(1:2,1:2)';
% perform the parallel version of the mean shift algorithm
dxs = offsets(:, 1) + (pxWidth-1)/2 + 1;
dys = offsets(:, 2) + (pxWidth-1)/2 + 1;
if(numel(gauss_resp) > 0)
meanShifts = meanShiftParallel_precalc(patchResponsesFlat, dxs, dys, iisFlat, jjsFlat, responseSize, gauss_resp.kd_precalc, gauss_resp.stepSize);
else
meanShifts = meanShiftParallel(patchResponsesFlat, clmParams.sigmaMeanShift, dxs, dys, iisFlat, jjsFlat, responseSize);
end
% invalidate illegal mean shifts
illegal_inds = find(~visibilities(view, :));
J(illegal_inds,:) = 0;
J(illegal_inds + n,:) = 0;
meanShifts(illegal_inds,:) = 0;
% Mean shift's here are calculate in the reference image frame,
% we want to move them back to actual image frame
meanShifts = meanShifts * OrigToRefTransform.tdata.Tinv(1:2,1:2)';
% put it into column format
meanShifts = meanShifts(:);
rigid_params = current_global - init_global;
rigid_params(2:4) = 0;
if(rigid)
params = rigid_params;
else
params = [rigid_params; current_local];
end
params_delta = (J'*P*J + regularisations) \ (J'*P*meanShifts - regularisations*params);
% update the reference
[current_local, current_global] = CalcReferenceUpdate(params_delta, current_local, current_global);
if(~rigid)
% clamp to the local parameters for valid expressions
current_local = ClampPDM(current_local, E);
end
end
if(nargout >= 4)
% get the current estimates of x and y in image
currentShape = GetShapeOrtho(PDM.M, PDM.V, current_local, current_global);
currentShape = currentShape(:,1:2);
% as the mean shift is with reference to the point, we don't care about
% the translation
OrigToRefTransform.tdata.T(3,1:2) = 0;
OrigToRefTransform.tdata.Tinv(3,1:2) = 0;
% distance from center where the response was calculated around
% in reference frame of the patch
offsets = (currentShape - baseShape) * OrigToRefTransform.tdata.T(1:2,1:2)';
% perform the parallel version of the mean shift algorithm
dxs = offsets(:, 1) + (pxWidth-1)/2 + 1;
dys = offsets(:, 2) + (pxWidth-1)/2 + 1;
landmark_lhoods = zeros(n,1);
prob = 0;
for i=1:n
if(visibilities(view, i))
dx = dxs(i);
dy = dys(i);
vxs = (-iis+dx).^2;
vys = (-jjs+dy).^2;
% Calculate the kde per patch
vs = patchResponses{i}.*exp(-0.5*(vxs + vys)/clmParams.sigmaMeanShift);
kde_est = sum(vs(:));
landmark_lhoods(i) = kde_est;
prob = prob + log(kde_est + 1e-8);
end
end
% Do not add the local parameter prior as it overpowers the
% log-likelihoods
final_lhood = prob / sum(visibilities(view, :));
% - LogPDMprior(current_local, E);
end
final_global = current_global;
final_local = current_local;
end
% This clamps the non-rigid parameters to stay within +- 3 standard
% deviations
function [non_rigid_params] = ClampPDM(non_rigid, E)
stds = sqrt(E);
non_rigid_params = non_rigid;
lower = non_rigid_params < -3 * stds;
non_rigid_params(lower) = -3*stds(lower);
higher = non_rigid_params > 3 * stds;
non_rigid_params(higher) = 3*stds(higher);
end
% This calculate the mean shift based on the kernel density response at dx,
% dy in the patch response, this can be used to find the mode
function [meanShifts] = meanShiftParallel(patchResponses, sigma, dxs, dys, iis, jjs, patchSize)
% Kernel density is
% K(x_i-x) = p(x_i)*exp(-0.5 * ||x_i-x||^2/sigma), so probability weighted
% distance from the center
% Mean shift is then m(x) = sum(K(x_i - dx)*x_i)/sum(K(x_i-dx))
% step_size = 0.1;
% gauss_resp_prec = precalc_kernel_densities(patchSize, sigma, step_size);
%
% %iis are row vectors of the locations of interest, for each patch
% nYs = numel(0:step_size:patchSize(2));
% % calculate the indices needed
%
% dxs2 = dxs - mod(dxs, step_size);
% dys2 = dys - mod(dys, step_size);
%
% xs = round(dxs2 * nYs * 1/step_size + (dys2 /step_size) +1);
%
% gauss_resp_c = gauss_resp_prec(xs,:);
% this part is doing (x_i - dx)^2
vxs = bsxfun(@plus, -iis, dxs);
vxs = vxs.^2;
% this part is doing (y_i - dy)^2
vys = bsxfun(@plus, -jjs, dys);
vys = vys.^2;
a = -0.5/(sigma.^2);
% this part is calculating K(x_i - x)
gauss_resp = exp(a*(vxs + vys));
vs = patchResponses.*gauss_resp;
% this part is calculating K(x_i - dx)*x_i
mxss = vs.*iis;
myss = vs.*jjs;
% this part is caluclating sum(K(x_i - dx)*x_i)
mxs = sum(mxss,2);
mys = sum(myss,2);
sumVs = sum(vs,2);
sumVs(sumVs == 0) = 1;
msx = mxs ./ sumVs - dxs;
msy = mys ./ sumVs - dys;
meanShifts = [msx, msy];
end
% This updates the parameters based on the updates from the RLMS
function [non_rigid, rigid] = CalcReferenceUpdate(params_delta, current_non_rigid, current_global)
rigid = zeros(6, 1);
% Same goes for scaling and translation parameters
rigid(1) = current_global(1) + params_delta(1);
rigid(5) = current_global(5) + params_delta(5);
rigid(6) = current_global(6) + params_delta(6);
% for rotation however, we want to make sure that the rotation matrix
% approximation we have
% R' = [1, -wz, wy
% wz, 1, -wx
% -wy, wx, 1]
% is a legal rotation matrix, and then we combine it with current
% rotation (through matrix multiplication) to acquire the new rotation
R = Euler2Rot(current_global(2:4));
wx = params_delta(2);
wy = params_delta(3);
wz = params_delta(4);
R_delta = [1, -wz, wy;
wz, 1, -wx;
-wy, wx, 1];
% Make sure R_delta is orthonormal
R_delta = OrthonormaliseRotation(R_delta);
% Combine rotations
R_final = R * R_delta;
% Extract euler angle
euler = Rot2Euler(R_final);
rigid(2:4) = euler;
if(length(params_delta) > 6)
% non-rigid parameters can just be added together
non_rigid = params_delta(7:end) + current_non_rigid;
else
non_rigid = current_non_rigid;
end
end
function R_ortho = OrthonormaliseRotation(R)
% U * V' is basically what we want, as it's guaranteed to be
% orthonormal
[U, ~, V] = svd(R);
% We also want to make sure no reflection happened
% get the orthogonal matrix from the initial rotation matrix
X = U*V';
% This makes sure that the handedness is preserved and no reflection happened
% by making sure the determinant is 1 and not -1
W = eye(3);
W(3,3) = det(X);
R_ortho = U*W*V';
end
function [prob] = LogPDMprior(params, E)
cov = diag(E);
prob = 0.5 * params'*inv(cov)*params;
end