open source pkg v1
This commit is contained in:
Vijay Yadev
@@ -0,0 +1,9 @@
|
||||
function [RotFull] = AddOrthRow(RotSmall)
|
||||
|
||||
% We can work out these values from the small version of the rotation matrix Rx * Ry * Rz (if you plug in values you can work it out, just slightly tedious)
|
||||
RotFull = zeros(3,3);
|
||||
RotFull(1:2, :) = RotSmall;
|
||||
RotFull(3,1) = RotSmall(1, 2) * RotSmall(2, 3) - RotSmall(1, 3) * RotSmall(2, 2);
|
||||
RotFull(3,2) = RotSmall(1, 3) * RotSmall(2, 1) - RotSmall(1, 1) * RotSmall(2, 3);
|
||||
RotFull(3,3) = RotSmall(1, 1) * RotSmall(2, 2) - RotSmall(1, 2) * RotSmall(2, 1);
|
||||
|
||||
@@ -0,0 +1,22 @@
|
||||
function [ R, T ] = AlignShapesKabsch ( alignFrom, alignTo )
|
||||
%ALIGN3DSHAPES Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
dims = size(alignFrom, 2);
|
||||
|
||||
alignFromMean = alignFrom - repmat(mean(alignFrom), size(alignFrom,1),1);
|
||||
alignToMean = alignTo - repmat(mean(alignTo), size(alignTo,1),1);
|
||||
|
||||
[U, ~, V] = svd( alignFromMean' * alignToMean);
|
||||
|
||||
% make sure no reflection is there
|
||||
d = sign(det(V*U'));
|
||||
corr = eye(dims);
|
||||
corr(end,end) = d;
|
||||
|
||||
R = V*corr*U';
|
||||
|
||||
T = mean(alignTo) - (R * mean(alignFrom)')';
|
||||
T = T';
|
||||
end
|
||||
|
||||
@@ -0,0 +1,30 @@
|
||||
function [ A, T, error, alignedShape, s ] = AlignShapesWithScale( alignFrom, alignTo )
|
||||
%ALIGNSHAPESWITHSCALE Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
numPoints = size(alignFrom,1);
|
||||
|
||||
meanFrom = mean(alignFrom);
|
||||
meanTo = mean(alignTo);
|
||||
|
||||
alignFromMeanNormed = bsxfun(@minus, alignFrom, meanFrom);
|
||||
alignToMeanNormed = bsxfun(@minus, alignTo, meanTo);
|
||||
|
||||
% scale now
|
||||
sFrom = sqrt(sum(alignFromMeanNormed(:).^2)/numPoints);
|
||||
sTo = sqrt(sum(alignToMeanNormed(:).^2)/numPoints);
|
||||
|
||||
s = sTo / sFrom;
|
||||
|
||||
alignFromMeanNormed = alignFromMeanNormed/sFrom;
|
||||
alignToMeanNormed = alignToMeanNormed/sTo;
|
||||
|
||||
[R, t] = AlignShapesKabsch(alignFromMeanNormed, alignToMeanNormed);
|
||||
|
||||
A = s * R;
|
||||
aligned = (A * alignFrom')';
|
||||
T = mean(alignTo - aligned);
|
||||
alignedShape = bsxfun(@plus, aligned, T);
|
||||
error = mean(sum((alignedShape - alignTo).^2,2));
|
||||
|
||||
end
|
||||
@@ -0,0 +1,11 @@
|
||||
function [Rot] = AxisAngle2Rot(axisAngle)
|
||||
|
||||
theta = norm(axisAngle, 2);
|
||||
|
||||
nx = axisAngle / theta;
|
||||
|
||||
nx = [ 0 -nx(3) nx(2);
|
||||
nx(3) 0 -nx(1);
|
||||
-nx(2) nx(1) 0 ];
|
||||
|
||||
Rot = eye(3) + sin(theta) * nx + (1-cos(theta))*nx^2;
|
||||
@@ -0,0 +1,11 @@
|
||||
function [Rot] = Euler2Rot(euler)
|
||||
|
||||
rx = euler(1);
|
||||
ry = euler(2);
|
||||
rz = euler(3);
|
||||
|
||||
Rx = [1 0 0; 0 cos(rx) -sin(rx); 0 sin(rx) cos(rx)];
|
||||
Ry = [cos(ry) 0 sin(ry); 0 1 0; -sin(ry) 0 cos(ry)];
|
||||
Rz = [cos(rz) -sin(rz) 0; sin(rz) cos(rz) 0; 0 0 1];
|
||||
|
||||
Rot = Rx * Ry * Rz;
|
||||
@@ -0,0 +1,7 @@
|
||||
function [shape3D] = GetShape3D(M, V, p)
|
||||
|
||||
shape3D = M + V * p;
|
||||
|
||||
shape3D = reshape(shape3D, numel(shape3D) / 3, 3);
|
||||
|
||||
end
|
||||
@@ -0,0 +1,20 @@
|
||||
function [shape2D] = GetShapeOrtho(M, V, p, global_params)
|
||||
|
||||
% M - mean shape vector
|
||||
% V - eigenvectors
|
||||
% p - parameters of non-rigid shape
|
||||
% V_exp
|
||||
% p_exp
|
||||
% global_params includes scale, euler rotation, translation,
|
||||
% R - rotation matrix
|
||||
% T - translation vector (tx, ty)
|
||||
|
||||
R = Euler2Rot(global_params(2:4));
|
||||
T = [global_params(5:6); 0];
|
||||
a = global_params(1);
|
||||
|
||||
shape3D = GetShape3D(M, V, p);
|
||||
|
||||
shape2D = bsxfun(@plus, a * R*shape3D', T);
|
||||
shape2D = shape2D';
|
||||
end
|
||||
@@ -0,0 +1,103 @@
|
||||
function [normX, normY, meanShape, Transform] = ProcrustesAnalysis(x, y, options)
|
||||
|
||||
% Translate all elements to origin and scale to 1
|
||||
normX = zeros(size(x));
|
||||
normY = zeros(size(y));
|
||||
|
||||
for i = 1:size(x,1)
|
||||
|
||||
offsetX = mean(x(i,:));
|
||||
offsetY = mean(y(i,:));
|
||||
|
||||
Transform.offsetX(i) = offsetX;
|
||||
Transform.offsetY(i) = offsetY;
|
||||
|
||||
normX(i,:) = x(i,:) - offsetX;
|
||||
normY(i,:) = y(i,:) - offsetY;
|
||||
|
||||
% Get the Frobenius norm, to scale the shapes to unit size
|
||||
scale = norm([normX(i,:) normY(i,:)], 'fro');
|
||||
|
||||
Transform.scale(i) = scale;
|
||||
|
||||
normX(i,:) = normX(i,:)/scale;
|
||||
normY(i,:) = normY(i,:)/scale;
|
||||
|
||||
end
|
||||
|
||||
% Rotate elements untill all of them have the same orientation
|
||||
|
||||
% the initial estimate of rotation would be the first element
|
||||
% if change is less than 1% stop (shouldn't take more than 2 steps)
|
||||
change = 0.1;
|
||||
|
||||
meanShape = [ normX(1,:); normY(1,:) ]';
|
||||
|
||||
Transform.Rotation = zeros(size(x,1),1);
|
||||
|
||||
for i = 1:30
|
||||
|
||||
% align all of the shapes to the mean shape
|
||||
|
||||
% remember all orientations to get the mean one
|
||||
orientations = zeros(size(normX,1),1);
|
||||
|
||||
for j = 1:size(x,1)
|
||||
|
||||
% do SVD of mean * X'
|
||||
currentShape = [ normX(j,:); normY(j,:) ]';
|
||||
[U, ~, V] = svd( meanShape' * currentShape);
|
||||
rot = V*U';
|
||||
|
||||
if(asin(rot(2,1)) > 0)
|
||||
orientations(j) = real(acos(rot(1,1)));
|
||||
else
|
||||
orientations(j) = real(-acos(rot(1,1)));
|
||||
end
|
||||
|
||||
Transform.Rotation(j) = Transform.Rotation(j) + orientations(j);
|
||||
|
||||
currentShape = currentShape * rot;
|
||||
|
||||
normX(j,:) = currentShape(:,1)';
|
||||
normY(j,:) = currentShape(:,2)';
|
||||
|
||||
end
|
||||
|
||||
% recalculate the mean shape;
|
||||
oldMean = meanShape;
|
||||
meanShape = [mean(normX); mean(normY)]';
|
||||
|
||||
% rotate the mean shape to mean rotation
|
||||
meanOrientation = mean(orientations);
|
||||
|
||||
% Do this only the first time
|
||||
if(i==1)
|
||||
|
||||
rotM = [ cos(-meanOrientation) -sin(-meanOrientation); sin(-meanOrientation) cos(-meanOrientation) ];
|
||||
meanShape = meanShape * rotM;
|
||||
end
|
||||
% scale mean shape to unit
|
||||
meanScale = norm(meanShape, 'fro');
|
||||
meanShape = meanShape*(1/meanScale);
|
||||
|
||||
% find frobenious norm
|
||||
diff = norm(oldMean - meanShape, 'fro');
|
||||
|
||||
if(diff/norm(oldMean,'fro') < change)
|
||||
break;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
% transform to tangent space to preserve linearities
|
||||
|
||||
% get the scaling factors for each shape
|
||||
if(options.TangentSpaceTransform)
|
||||
scaling = [ normX normY ] * [ meanShape(:,1)' meanShape(:,2)']';
|
||||
for i=1:size(x,1)
|
||||
normX(i,:) = normX(i,:) * (1 / scaling(i));
|
||||
normY(i,:) = normY(i,:) * (1 / scaling(i));
|
||||
end
|
||||
end
|
||||
|
||||
@@ -0,0 +1,139 @@
|
||||
function [ normX, normY, normZ, meanShape, Transform ] = ProcrustesAnalysis3D( x, y, z, tangentSpace, meanShape )
|
||||
%PROCRUSTESANALYSIS3D Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
meanProvided = false;
|
||||
|
||||
if(nargin > 4)
|
||||
meanProvided = true;
|
||||
end
|
||||
|
||||
% Translate all elements to origin
|
||||
normX = zeros(size(x));
|
||||
normY = zeros(size(y));
|
||||
normZ = zeros(size(z));
|
||||
|
||||
for i = 1:size(x,1)
|
||||
|
||||
offsetX = mean(x(i,:));
|
||||
offsetY = mean(y(i,:));
|
||||
offsetZ = mean(z(i,:));
|
||||
|
||||
Transform.offsetX(i) = offsetX;
|
||||
Transform.offsetY(i) = offsetY;
|
||||
Transform.offsetZ(i) = offsetZ;
|
||||
|
||||
normX(i,:) = x(i,:) - offsetX;
|
||||
normY(i,:) = y(i,:) - offsetY;
|
||||
normZ(i,:) = z(i,:) - offsetZ;
|
||||
|
||||
end
|
||||
|
||||
% Rotate elements untill all of them have the same orientation
|
||||
|
||||
% the initial estimate of rotation would be the first element
|
||||
% if change is less than 1% stop (shouldn't take more than 2 steps)
|
||||
change = 0.1;
|
||||
|
||||
if(~meanProvided)
|
||||
meanShape = [ mean(normX); mean(normY); mean(normZ) ]';
|
||||
end
|
||||
% scale all the shapes to mean shape
|
||||
|
||||
% Get the Frobenius norm, to scale the shapes to mean size (still want to
|
||||
% retain mm)
|
||||
meanScale = norm(meanShape, 'fro');
|
||||
|
||||
for i = 1:size(x,1)
|
||||
|
||||
scale = norm([normX(i,:) normY(i,:) normZ(i,:)], 'fro')/meanScale;
|
||||
|
||||
normX(i,:) = normX(i,:)/scale;
|
||||
normY(i,:) = normY(i,:)/scale;
|
||||
normZ(i,:) = normZ(i,:)/scale;
|
||||
|
||||
end
|
||||
|
||||
Transform.RotationX = zeros(size(x,1),1);
|
||||
Transform.RotationY = zeros(size(x,1),1);
|
||||
Transform.RotationZ = zeros(size(x,1),1);
|
||||
|
||||
for i = 1:30
|
||||
|
||||
% align all of the shapes to the mean shape
|
||||
|
||||
% remember all orientations to get the mean one (in euler angle form, pitch, yaw roll)
|
||||
orientationsX = zeros(size(normX,1),1);
|
||||
orientationsY = zeros(size(normX,1),1);
|
||||
orientationsZ = zeros(size(normX,1),1);
|
||||
|
||||
for j = 1:size(x,1)
|
||||
|
||||
currentShape = [normX(j,:); normY(j,:); normZ(j,:)]';
|
||||
% we want to align the current shape to the mean one
|
||||
[ R, T ] = AlignShapesKabsch(currentShape, meanShape);
|
||||
|
||||
eulers = Rot2Euler(R);
|
||||
|
||||
orientationsX(j) = eulers(1);
|
||||
orientationsY(j) = eulers(2);
|
||||
orientationsZ(j) = eulers(3);
|
||||
|
||||
Transform.RotationX(j) = eulers(1);
|
||||
Transform.RotationY(j) = eulers(2);
|
||||
Transform.RotationZ(j) = eulers(3);
|
||||
|
||||
currentShape = R * currentShape';
|
||||
|
||||
normX(j,:) = currentShape(1,:);
|
||||
normY(j,:) = currentShape(2,:);
|
||||
normZ(j,:) = currentShape(3,:);
|
||||
|
||||
end
|
||||
|
||||
% recalculate the mean shape
|
||||
% if(~meanProvided)
|
||||
oldMean = meanShape;
|
||||
meanShape = [mean(normX); mean(normY); mean(normZ)]';
|
||||
meanScale = norm(meanShape, 'fro');
|
||||
% end
|
||||
|
||||
for j = 1:size(x,1)
|
||||
|
||||
scale = norm([normX(j,:) normY(j,:) normZ(j,:)], 'fro')/meanScale;
|
||||
|
||||
normX(j,:) = normX(j,:)/scale;
|
||||
normY(j,:) = normY(j,:)/scale;
|
||||
normZ(j,:) = normZ(j,:)/scale;
|
||||
|
||||
end
|
||||
|
||||
if(i==1 && ~meanProvided)
|
||||
|
||||
% rotate the mean shape to mean rotation
|
||||
meanOrientationX = mean(orientationsX);
|
||||
meanOrientationY = mean(orientationsY);
|
||||
meanOrientationZ = mean(orientationsZ);
|
||||
|
||||
R = Euler2Rot([meanOrientationX, meanOrientationY, meanOrientationZ]);
|
||||
meanShape = (R * meanShape')';
|
||||
end
|
||||
|
||||
% find frobenious norm
|
||||
diff = norm(oldMean - meanShape, 'fro');
|
||||
|
||||
if(diff/norm(oldMean,'fro') < change)
|
||||
break;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
% transform to tangent space to preserve linearities
|
||||
|
||||
% get the scaling factors for each shape
|
||||
if(tangentSpace)
|
||||
[ normX, normY, normZ] = TangentSpaceTransform(normX, normY, normZ, meanShape);
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
function [ axisAngle ] = Rot2AxisAngle( Rot )
|
||||
%ROT2AXISANGLE Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
theta = acos((trace(Rot) - 1) / 2);
|
||||
|
||||
vec = 1.0/(2*sin(theta));
|
||||
vec = vec * [Rot(3,2) - Rot(2,3), Rot(1,3) - Rot(3,1), Rot(2,1) - Rot(1,2)];
|
||||
axisAngle = vec * theta;
|
||||
end
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
function [euler] = Rot2Euler(R)
|
||||
|
||||
q0 = sqrt( 1 + R(1,1) + R(2,2) + R(3,3) ) / 2;
|
||||
if(q0 ~= 0)
|
||||
q1 = (R(3,2) - R(2,3)) / (4*q0) ;
|
||||
q2 = (R(1,3) - R(3,1)) / (4*q0) ;
|
||||
q3 = (R(2,1) - R(1,2)) / (4*q0) ;
|
||||
|
||||
yaw = asin(2*(q0*q2 + q1*q3));
|
||||
pitch= atan2(2*(q0*q1-q2*q3), q0*q0-q1*q1-q2*q2+q3*q3);
|
||||
roll = atan2(2*(q0*q3-q1*q2), q0*q0+q1*q1-q2*q2-q3*q3);
|
||||
|
||||
euler = [pitch, yaw, roll];
|
||||
else
|
||||
euler = [0, 0, 0];
|
||||
end
|
||||
@@ -0,0 +1,17 @@
|
||||
function [ transformedX, transformedY, transformedZ ] = TangentSpaceTransform( x, y, z, meanShape )
|
||||
%TANGENTSPACETRANSFORM Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
scaling = [ x y z] * [ meanShape(:,1)' meanShape(:,2)' meanShape(:,3)']';
|
||||
for i=1:size(x,1)
|
||||
x(i,:) = x(i,:) * (1 / scaling(i));
|
||||
y(i,:) = y(i,:) * (1 / scaling(i));
|
||||
z(i,:) = z(i,:) * (1 / scaling(i));
|
||||
end
|
||||
|
||||
transformedX = x * mean(scaling);
|
||||
transformedY = y * mean(scaling);
|
||||
transformedZ = z * mean(scaling);
|
||||
|
||||
end
|
||||
|
||||
@@ -0,0 +1,373 @@
|
||||
function [ a, R, T, T3D, params, error, shapeOrtho ] = fit_PDM_ortho_proj_to_2D( M, E, V, shape2D, f, cx, cy)
|
||||
%FITPDMTO2DSHAPE Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
params = zeros(size(E));
|
||||
|
||||
hidden = false;
|
||||
|
||||
% if some of the points are unavailable modify M, V, and shape2D (can
|
||||
% later infer the actual shape from this)
|
||||
if(sum(shape2D(:)==0) > 0)
|
||||
|
||||
hidden = true;
|
||||
% which indices to remove
|
||||
inds_to_rem = shape2D(:,1) == 0 | shape2D(:,2) == 0;
|
||||
|
||||
shape2D = shape2D(~inds_to_rem,:);
|
||||
|
||||
inds_to_rem = repmat(inds_to_rem, 3, 1);
|
||||
|
||||
M_old = M;
|
||||
V_old = V;
|
||||
|
||||
M = M(~inds_to_rem);
|
||||
V = V(~inds_to_rem,:);
|
||||
|
||||
end
|
||||
|
||||
num_points = numel(M) / 3;
|
||||
|
||||
m = reshape(M, num_points, 3)';
|
||||
width_model = max(m(1,:)) - min(m(1,:));
|
||||
height_model = max(m(2,:)) - min(m(2,:));
|
||||
|
||||
bounding_box = [min(shape2D(:,1)), min(shape2D(:,2)),...
|
||||
max(shape2D(:,1)), max(shape2D(:,2))];
|
||||
|
||||
a = (((bounding_box(3) - bounding_box(1)) / width_model) + ((bounding_box(4) - bounding_box(2))/ height_model)) / 2;
|
||||
|
||||
tx = (bounding_box(3) + bounding_box(1))/2;
|
||||
ty = (bounding_box(4) + bounding_box(2))/2;
|
||||
|
||||
% correct it so that the bounding box is just around the minimum
|
||||
% and maximum point in the initialised face
|
||||
tx = tx - a*(min(m(1,:)) + max(m(1,:)))/2;
|
||||
ty = ty - a*(min(m(2,:)) + max(m(2,:)))/2;
|
||||
|
||||
% To deal with really extreme cases of roll
|
||||
M3D = cat(2, M(1:end/3), M(end/3+1:2*end/3), zeros(numel(M(1:end/3)),1));
|
||||
shape3D = cat(2, shape2D, zeros(numel(M(1:end/3)),1));
|
||||
[ A, T, error, alignedShape, s ] = AlignShapesWithScale(M3D, shape3D);
|
||||
R = A/s;
|
||||
% R = eye(3);
|
||||
|
||||
T = [tx; ty];
|
||||
|
||||
currShape = getShapeOrtho(M, V, params, R, T, a);
|
||||
|
||||
currError = getRMSerror(currShape, shape2D);
|
||||
|
||||
reg_rigid = zeros(6,1);
|
||||
regFactor = 20;
|
||||
regularisations = [reg_rigid; regFactor ./ E]; % the above version, however, does not perform as well
|
||||
regularisations = diag(regularisations)*diag(regularisations);
|
||||
|
||||
red_in_a_row = 0;
|
||||
|
||||
for i=1:1000
|
||||
|
||||
shape3D = M + V * params;
|
||||
shape3D = reshape(shape3D, numel(shape3D) / 3, 3);
|
||||
|
||||
% Now find the current residual error
|
||||
currShape = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
|
||||
currShape = currShape';
|
||||
|
||||
error_res = shape2D - currShape;
|
||||
|
||||
eul = Rot2Euler(R);
|
||||
|
||||
p_global = [a; eul'; T];
|
||||
|
||||
% get the Jacobians
|
||||
J = CalcJacobian(M, V, params, p_global);
|
||||
|
||||
% RLMS style update
|
||||
p_delta = (J'*J + regularisations) \ (J'*error_res(:) - regularisations*[p_global;params]);
|
||||
|
||||
% Take smaller steps
|
||||
p_delta = 0.25 * p_delta;
|
||||
|
||||
[params, p_global] = CalcReferenceUpdate(p_delta, params, p_global);
|
||||
|
||||
% Make sure the params do not get out of hand
|
||||
params = ClampPDM(params, E);
|
||||
|
||||
a = p_global(1);
|
||||
R = Euler2Rot(p_global(2:4));
|
||||
T = p_global(5:6);
|
||||
|
||||
shape3D = M + V * params;
|
||||
shape3D = reshape(shape3D, numel(shape3D) / 3, 3);
|
||||
currShape = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
|
||||
currShape = currShape';
|
||||
|
||||
error = getRMSerror(currShape, shape2D);
|
||||
|
||||
if(0.999 * currError < error)
|
||||
red_in_a_row = red_in_a_row + 1;
|
||||
if(red_in_a_row == 5)
|
||||
break;
|
||||
end
|
||||
end
|
||||
|
||||
currError = error;
|
||||
|
||||
end
|
||||
|
||||
if(hidden)
|
||||
shapeOrtho = getShapeOrtho(M_old, V_old, params, R, T, a);
|
||||
else
|
||||
shapeOrtho = currShape;
|
||||
end
|
||||
if(nargin == 7)
|
||||
|
||||
Zavg = f / a;
|
||||
Xavg = (T(1) - cx) / a;
|
||||
Yavg = (T(2) - cy) / a;
|
||||
|
||||
T3D = [Xavg;Yavg;Zavg];
|
||||
else
|
||||
T3D = [0;0;0];
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
function [shape2D] = getShapeOrtho(M, V, p, R, T, a)
|
||||
|
||||
% M - mean shape vector
|
||||
% V - eigenvectors
|
||||
% p - parameters of non-rigid shape
|
||||
% R - rotation matrix
|
||||
% T - translation vector (tx, ty)
|
||||
shape3D = getShape3D(M, V, p);
|
||||
shape2D = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
|
||||
shape2D = shape2D';
|
||||
end
|
||||
|
||||
function [shape2D] = getShapeOrthoFull(M, V, p, R, T, a)
|
||||
|
||||
% M - mean shape vector
|
||||
% V - eigenvectors
|
||||
% p - parameters of non-rigid shape
|
||||
% R - rotation matrix
|
||||
% T - translation vector (tx, ty)
|
||||
T = [T; 0];
|
||||
shape3D = getShape3D(M, V, p);
|
||||
shape2D = a * R*shape3D' + repmat(T, 1, numel(M)/3);
|
||||
shape2D = shape2D';
|
||||
end
|
||||
|
||||
function [shape3D] = getShape3D(M, V, params)
|
||||
|
||||
shape3D = M + V * params;
|
||||
shape3D = reshape(shape3D, numel(shape3D) / 3, 3);
|
||||
|
||||
end
|
||||
|
||||
function [error] = getRMSerror(shape2Dv1, shape2Dv2)
|
||||
|
||||
error = sqrt(mean(reshape(shape2Dv1 - shape2Dv2, numel(shape2Dv1), 1).^2));
|
||||
|
||||
end
|
||||
|
||||
% This calculates the combined rigid with non-rigid Jacobian
|
||||
function J = CalcJacobian(M, V, p, p_global)
|
||||
|
||||
n = size(M, 1)/3;
|
||||
|
||||
non_rigid_modes = size(V,2);
|
||||
|
||||
J = zeros(n*2, 6 + non_rigid_modes);
|
||||
|
||||
|
||||
% now the layour is
|
||||
% ---------- Rigid part -------------------|----Non rigid part--------|
|
||||
% dx_1/ds, dx_1/dr1, ... dx_1/dtx, dx_1/dty dx_1/dp_1 ... dx_1/dp_m
|
||||
% dx_2/ds, dx_2/dr1, ... dx_2/dtx, dx_2/dty dx_2/dp_1 ... dx_2/dp_m
|
||||
% ...
|
||||
% dx_n/ds, dx_n/dr1, ... dx_n/dtx, dx_n/dty dx_n/dp_1 ... dx_n/dp_m
|
||||
% dy_1/ds, dy_1/dr1, ... dy_1/dtx, dy_1/dty dy_1/dp_1 ... dy_1/dp_m
|
||||
% ...
|
||||
% dy_n/ds, dy_n/dr1, ... dy_n/dtx, dy_n/dty dy_n/dp_1 ... dy_n/dp_m
|
||||
|
||||
% getting the rigid part
|
||||
J(:,1:6) = CalcRigidJacobian(M, V, p, p_global);
|
||||
|
||||
% constructing the non-rigid part
|
||||
R = Euler2Rot(p_global(2:4));
|
||||
s = p_global(1);
|
||||
|
||||
% 'rotate' and 'scale' the principal components
|
||||
|
||||
% First reshape to 3D
|
||||
V_X = V(1:n,:);
|
||||
V_Y = V(n+1:2*n,:);
|
||||
V_Z = V(2*n+1:end,:);
|
||||
|
||||
J_x_non_rigid = s*(R(1,1)*V_X + R(1,2)*V_Y + R(1,3)*V_Z);
|
||||
J_y_non_rigid = s*(R(2,1)*V_X + R(2,2)*V_Y + R(2,3)*V_Z);
|
||||
|
||||
J(1:n, 7:end) = J_x_non_rigid;
|
||||
J(n+1:end, 7:end) = J_y_non_rigid;
|
||||
|
||||
end
|
||||
|
||||
function J = CalcRigidJacobian(M, V, p, p_global)
|
||||
|
||||
n = size(M, 1)/3;
|
||||
|
||||
% Get the current 3D shape (not affected by global transform, as this
|
||||
% is how the Jacobian was derived (for derivation please see
|
||||
% ../derivations/orthoJacobian
|
||||
shape3D = GetShape3D(M, V, p);
|
||||
|
||||
% Get the rotation matrix corresponding to current global orientation
|
||||
R = Euler2Rot(p_global(2:4));
|
||||
s = p_global(1);
|
||||
|
||||
% Rigid Jacobian is laid out as follows
|
||||
% dx_1/ds, dx_1/dr1, dx_1/dr2, dx_1/dr3, dx_1/dtx, dx_1/dty
|
||||
% dx_2/ds, dx_2/dr1, dx_2/dr2, dx_2/dr3, dx_2/dtx, dx_2/dty
|
||||
% ...
|
||||
% dx_n/ds, dx_n/dr1, dx_n/dr2, dx_n/dr3, dx_n/dtx, dx_n/dty
|
||||
% dy_1/ds, dy_1/dr1, dy_1/dr2, dy_1/dr3, dy_1/dtx, dy_1/dty
|
||||
% ...
|
||||
% dy_n/ds, dy_n/dr1, dy_n/dr2, dy_n/dr3, dy_n/dtx, dy_n/dty
|
||||
|
||||
J = zeros(n*2, 6);
|
||||
|
||||
% dx/ds = X * r11 + Y * r12 + Z * r13
|
||||
% dx/dr1 = s*(r13 * Y - r12 * Z)
|
||||
% dx/dr2 = -s*(r13 * X - r11 * Z)
|
||||
% dx/dr3 = s*(r12 * X - r11 * Y)
|
||||
% dx/dtx = 1
|
||||
% dx/dty = 0
|
||||
|
||||
% dy/ds = X * r21 + Y * r22 + Z * r23
|
||||
% dy/dr1 = s * (r23 * Y - r22 * Z)
|
||||
% dy/dr2 = -s * (r23 * X - r21 * Z)
|
||||
% dy/dr3 = s * (r22 * X - r21 * Y)
|
||||
% dy/dtx = 0
|
||||
% dy/dty = 1
|
||||
|
||||
% set the Jacobian for x's
|
||||
|
||||
% with respect to scaling factor
|
||||
J(1:n,1) = shape3D * R(1,:)';
|
||||
|
||||
% with respect to angular rotation around x, y, and z axes
|
||||
|
||||
% Change of x with respect to change in axis angle rotation
|
||||
dxdR = [ 0, R(1,3), -R(1,2);
|
||||
-R(1,3), 0, R(1,1);
|
||||
R(1,2), -R(1,1), 0];
|
||||
|
||||
J(1:n,2:4) = s*(dxdR * shape3D')';
|
||||
|
||||
% with respect to translation
|
||||
J(1:n,5) = 1;
|
||||
J(1:n,6) = 0;
|
||||
|
||||
% set the Jacobian for y's
|
||||
|
||||
% with respect to scaling factor
|
||||
J(n+1:end,1) = shape3D * R(2,:)';
|
||||
|
||||
% with respect to angular rotation around x, y, and z axes
|
||||
|
||||
% Change of y with respect to change in axis angle rotation
|
||||
dydR = [ 0, R(2,3), -R(2,2);
|
||||
-R(2,3), 0, R(2,1);
|
||||
R(2,2), -R(2,1), 0];
|
||||
|
||||
J(n+1:end,2:4) = s*(dydR * shape3D')';
|
||||
|
||||
% with respect to translation
|
||||
J(n+1:end,5) = 0;
|
||||
J(n+1:end,6) = 1;
|
||||
|
||||
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 = double(OrthonormaliseRotation(R_delta));
|
||||
|
||||
% Combine rotations
|
||||
R_final = R * R_delta;
|
||||
|
||||
% Extract euler angle
|
||||
euler = real(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
|
||||
|
||||
% 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
|
||||
@@ -0,0 +1,16 @@
|
||||
% for easier readibility write them row by row
|
||||
function writeMatrix(fileID, M, type)
|
||||
|
||||
fprintf(fileID, '%d\r\n', size(M,1));
|
||||
fprintf(fileID, '%d\r\n', size(M,2));
|
||||
fprintf(fileID, '%d\r\n', type);
|
||||
|
||||
for i=1:size(M,1)
|
||||
if(type == 4 || type == 0)
|
||||
fprintf(fileID, '%d ', M(i,:));
|
||||
else
|
||||
fprintf(fileID, '%.9f ', M(i,:));
|
||||
end
|
||||
fprintf(fileID, '\r\n');
|
||||
end
|
||||
end
|
||||
Reference in New Issue
Block a user