理解AFLW資料集提供的頭部姿態角度。

close all;

close all;
dbpath = '../data/';
dbfile = 'aflw.sqlite';

model3d = createMeanFace3DModel(fullfile(dbpath,dbfile));

% get one face_id
mksqlite('open',fullfile(dbpath,dbfile));
fidQuery = 'SELECT face_id FROM Faces LIMIT 100';
res = mksqlite(fidQuery);
mksqlite('close'
 
);
face_id = res(3).face_id;
% or alternatively select one here
%face_id = 39341;

facedata = getFaceDataFromSQLite([dbpath dbfile],face_id);

%
headpose = facedata.pose;
landmark = facedata.pts;
imgpath = facedata.image.filepath;
disp(['headpose: ']);
disp(headpose);

feature2dNames = fieldnames(facedata.
 
pts);
num2dPts = size(feature2dNames,1);

pts2d = zeros(num2dPts,2);
pts3d = zeros(num2dPts,3);

for i=1:num2dPts
    fn = feature2dNames{i};
    if isfield(model3d,fn)
        pts3d(i,:) = model3d.(fn);
        pts2d(i,:) = facedata.pts.(fn);
    else
        fprintf('Error: "%s" is not a feature name in the 3D model\n'
 
,fn);
    end
end

% show 3d model
%show3DModel(model3d);

camera = struct();
camera.center = [facedata.image.width/2 facedata.image.height/2];
camera.viewPlaneDistance = 1.5 *facedata.image.width;

% web site: http://legacydirs.umiacs.umd.edu/~daniel/Site_2/Code.html

[rot1,trans1] = modernPosit(pts2d,pts3d,camera.viewPlaneDistance,camera.center);
[rot2,trans2] = classicPosit(pts2d,pts3d,camera.viewPlaneDistance,camera.center);
% call modernPosit in this function
[rot,trans] = calculateTransformation(camera,pts2d,pts3d,facedata.pose,true);

%%%%% version1: matlab function the default order: ZYX %%%%%
% roll, yaw , pitch ?
ruler = rotm2eul(rot);
ruler_angle1 = [ruler(3) ruler(2) ruler(1)];

%%%%%%%%%%%% version2: %%%%%%%%%%%%%%%%%%
% pitch, yaw, roll
ruler_angle2 = RotMat2Euler(rot);
%%%%%%%%%%%%%%%%%%%  version3 %%%%%%%%%%%%%%
% refer to: [從旋轉矩陣計算尤拉角] deep studio
ruler_angle3 = rotationMatrix2eulerAngles(rot);

%%%%%%%%%%%%%%%%%%%%%%%%%  version4 %%%%%%%%%%%%%%%%%%%%%%
% refer to: https://stackoverflow.com/questions/15022630/how-to-calculate-the-angle-from-rotation-matrix
thetax = atan2(rot(3,2),rot(3,3)); % pitch
thetay = atan2(-rot(3,1), sqrt(rot(3,2) * rot(3,2) + rot(3,3) * rot(3,3)));% yaw
thetaz = atan2(rot(2,1),rot(1,1));% roll
ruler_angle4 = [thetax thetay thetaz];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% rotate model points
rotPts = (rot*pts3d' + repmat(trans,[1 num2dPts]))';

% project model points
projPts = [rotPts(:,1)./rotPts(:,3) rotPts(:,2)./rotPts(:,3)];
projPts = projPts .* camera.viewPlaneDistance + repmat(camera.center,[num2dPts 1]);

model_center = model3d.center_between_eyes;
%model_center = model3d.center_of_head;
rotCenter = (rot*model_center' + trans)';
projCenter = [rotCenter(:,1)./rotCenter(:,3) rotCenter(:,2)./rotCenter(:,3)];
projCenter = projCenter .* camera.viewPlaneDistance + camera.center;

rotCenterUp = (rot*(model_center + [0 0.2 0])' + trans)';
projCenterUp = [rotCenterUp(:,1)./rotCenterUp(:,3) rotCenterUp(:,2)./rotCenterUp(:,3)];
projCenterUp = projCenterUp .* camera.viewPlaneDistance + camera.center;

% show rotated, translated and projected 3d points
%im = imread(imgFile);
im = imread([dbpath facedata.image.db_id '/' facedata.image.filepath]);
disp(facedata.image.filepath);
figure;
imshow(im);
hold on;

showModel = false;
if showModel
    % show whole model
    myworld = vrworld('meanFaceSimplified.wrl');
    open(myworld);
    view(myworld);
    hold on;
    x = get(myworld);
    % add my code
    tmp_debug = x.Nodes(1);
    %
    tmp_debug = zeros(1,3);
    tmp_debug(1,:) = model3d.LeftBrowLeftCorner; 
    %
    %model_pts = x.Nodes(1).point;
    model_pts = tmp_debug;
    numMPts = size(model_pts,1);

    % rotate model points
    rotMPts = (rot*model_pts' + repmat(trans,[1 numMPts]))';

    % project model points
    projMPts = [rotMPts(:,1)./rotMPts(:,3) rotMPts(:,2)./rotMPts(:,3)];
    projMPts = projMPts .* camera.viewPlaneDistance + repmat(camera.center,[numMPts 1]);
    plot(projMPts(:,1),projMPts(:,2),'y.');

    close(myworld);
end

plot(projPts(1:end,1),projPts(1:end,2),'r.');
plot(projCenter(1),projCenter(2),'go');
plot(projCenterUp(1),projCenterUp(2),'bo');
plot(pts2d(:,1),pts2d(:,2),'b.');
for i=1:num2dPts
    text(pts2d(i,1)+10,pts2d(i,2),feature2dNames{i},'Color','r');
end
for i=1:num2dPts
    text(projPts(i,1)+10,projPts(i,2)+5,feature2dNames{i},'Color','b');
end
%set(gca,'YDir','reverse');
%axis equal;
tdx = [];
tdy = [];
imgsize = [facedata.image.height, facedata.image.width];

%draw_axis(im, headpose.yaw, headpose.pitch, headpose.roll, tdx, tdy, imgsize)

modelCenterDist = sqrt(sum(rotCenter.^2));
cameraModel3dYAngle = atan(rotCenter(2)/sqrt(rotCenter(3)^2 + rotCenter(1)^2));
cameraModel3dXAngle = atan(rotCenter(1)/sqrt(rotCenter(3)^2 + rotCenter(2)^2));
sphereCenterBorderAngle = asin(model3d.sphereRadius/modelCenterDist);
sphereProjTop    = tan(cameraModel3dYAngle - sphereCenterBorderAngle)*camera.viewPlaneDistance;
sphereProjBottom = tan(cameraModel3dYAngle + sphereCenterBorderAngle)*camera.viewPlaneDistance;
sphereProjLeft   = tan(cameraModel3dXAngle - sphereCenterBorderAngle)*camera.viewPlaneDistance;
sphereProjRight  = tan(cameraModel3dXAngle + sphereCenterBorderAngle)*camera.viewPlaneDistance;
sphereProjTop    = sphereProjTop + camera.center(2);
sphereProjBottom = sphereProjBottom + camera.center(2);
sphereProjLeft   = sphereProjLeft + camera.center(1);
sphereProjRight  = sphereProjRight + camera.center(1);

plot(projCenter(1),sphereProjTop,'g.')
plot(projCenter(1),sphereProjBottom,'g.')
plot(sphereProjLeft,projCenter(2),'g.')
plot(sphereProjRight,projCenter(2),'g.')


% rectangle
targetCenter = [64 37]; % all faces are centered on this point
w_out = 128;
h_out = 128;
normEyeDist = 50.0;

scale = max(sphereProjBottom-sphereProjTop,sphereProjRight-sphereProjLeft)/normEyeDist;

x = projCenter(1) - targetCenter(1)*scale;
y = projCenter(2) - targetCenter(2)*scale;
r = [x y w_out*scale h_out*scale];
rectangle('Position',r,'LineWidth',2,'EdgeColor','b');

img = im(r(2):min(r(2)+r(4),size(im,1)),r(1):min(r(1)+r(3),size(im,2)),:);
imgsize = [size(img,1),size(img,2)];
figure;
imshow(img);


%draw_axis(img, headpose.yaw * 180/pi, headpose.pitch* 180/pi, headpose.roll* 180/pi, tdx, tdy, imgsize)
%draw_axis(img, ruler_angles(3), ruler_angles(2), ruler_angles(1), tdx, tdy, imgsize)
% error: draw_axis(img, ruler(3), ruler(2), ruler(1), tdx, tdy, imgsize)

disp('yaw, pitch ,roll for ruler_angle0: AFLW提供！');
disp([headpose.yaw * 180/pi,headpose.pitch* 180/pi,headpose.roll* 180/pi]);

% for ruler_angle1 
% the following computing , refer to : Face-Yaw-Roll-Pitch-from-Pose-Estimation-using-OpenCV-master
pitch = (asin(sin(ruler_angle1(1))))* 180 /pi;
roll = -(asin(sin(ruler_angle1(3))))* 180 /pi;
yaw = (asin(sin(ruler_angle1(2)))) * 180 /pi;
disp('yaw, pitch ,roll for ruler_angle1: 近似！');
disp([yaw, pitch, roll]);

% for ruler_angle2
pitch = (asin(sin(ruler_angle2(1))))* 180 /pi;
roll = -(asin(sin(ruler_angle2(3))))* 180 /pi;
yaw = (asin(sin(ruler_angle2(2)))) * 180 /pi;
disp('yaw, pitch ,roll for ruler_angle2: 有點問題！');
disp([yaw, pitch, roll]);

% for ruler_angle3 
pitch = (asin(sin(ruler_angle3(1))))* 180 /pi;
roll = -(asin(sin(ruler_angle3(3))))* 180 /pi;
yaw = (asin(sin(ruler_angle3(2)))) * 180 /pi;
disp('yaw, pitch ,roll for ruler_angle3: 近似！');
disp([yaw, pitch, roll]);

% for ruler_angle4 
pitch = (asin(sin(ruler_angle4(1))))* 180 /pi;
roll = -(asin(sin(ruler_angle4(3))))* 180 /pi;
yaw = (asin(sin(ruler_angle4(2)))) * 180 /pi;
disp('yaw, pitch ,roll for ruler_angle4: 近似！');
disp([yaw, pitch, roll]);



draw_axis(img, yaw, pitch, roll, tdx, tdy, imgsize)
disp('');

function Eul = RotMat2Euler(R)

% Version 1.000 
%
% Code provided by Graham Taylor, Geoff Hinton and Sam Roweis 
%
% For more information, see:
%     http://www.cs.toronto.edu/~gwtaylor/publications/nips2006mhmublv
%
% Permission is granted for anyone to copy, use, modify, or distribute this
% program and accompanying programs and documents for any purpose, provided
% this copyright notice is retained and prominently displayed, along with
% a note saying that the original programs are available from our
% web page.
% The programs and documents are distributed without any warranty, express or
% implied.  As the programs were written for research purposes only, they have
% not been tested to the degree that would be advisable in any important
% application.  All use of these programs is entirely at the user's own
% risk.
%
% Finds one of two equivalent Euler angle representations for a Direction
% Cosine Matrix
% Assumes the DCM is in 'zyx' order
% Given R, the rotation matrix
% Returns a vector of Euler angles (in radians)
%  the first about x axis, the second about y axis, the third about z axis
% Based on an article by Gregory G. Slabaugh
%
% Usage Eul = RotMat2Euler(R)

%Note we need to treat the case of cos(E2) = +- pi/2 separately
%This corresponds to element R(1,3) = +- 1

if R(1,3) == 1 | R(1,3) == -1
  %special case
  E3 = 0; %set arbitrarily
  dlta = atan2(R(1,2),R(1,3));
  if R(1,3) == -1
    E2 = pi/2;
    E1 = E3 + dlta;
  else
    E2 = -pi/2;
    E1 = -E3 + dlta;
  end
else
  E2 = - asin(R(1,3));
  E1 = atan2(R(2,3)/cos(E2), R(3,3)/cos(E2));
  E3 = atan2(R(1,2)/cos(E2), R(1,1)/cos(E2));
end

Eul = [E1 E2 E3];
end


function eulerAngles = rotationMatrix2eulerAngles(R)

% eulerAngles = rotationMatrix2eulerAngles(R)

%

% This function returns the rotation angles in degrees about the x, y and z axis for a

% given rotation matrix

% 

% Copyright : This code is written by david zhao from SCUT,[email protected],com. The code

%              may be used, modified and distributed for research purposes with

%              acknowledgement of the author and inclusion this copyright information.

%

% Disclaimer : This code is provided as is without any warrantly.


if abs(R(3,1)) ~= 1
    theta1 = -asin(R(3,1));
    theta2 = pi - theta1;
    psi1 = atan2(R(3,2)/cos(theta1), R(3,3)/cos(theta1));
    psi2 = atan2(R(3,2)/cos(theta2), R(3,3)/cos(theta2));
    pfi1 = atan2(R(2,1)/cos(theta1), R(1,1)/cos(theta1));
    pfi2 = atan2(R(2,1)/cos(theta2), R(1,1)/cos(theta2));
    theta = theta1; % could be any one of the two
    psi = psi1;
    pfi = pfi1;
else
    phi = 0;
    delta = atan2(R(1,2), R(1,3));
    if R(3,1) == -1
        theta = pi/2;
        psi = phi + delta;
    else
        theta = -pi/2;
        psi = -phi + delta;
    end
end

%psi is along x-axis...........theta is along y-axis........pfi is along z

%axis

 eulerAngles = [psi theta pfi]; %for rad;

%eulerAngles = [psi*180/pi theta*180/pi pfi*180/pi]; %for degree;

end

結果：

yaw, pitch ,roll for ruler_angle0: AFLW提供！
  -26.1218  -13.7720   -0.3973

yaw, pitch ,roll for ruler_angle1: 近似！
  -23.2682  -14.2231   -0.3835

yaw, pitch ,roll for ruler_angle2: 有點問題！
  -25.5304   15.0904   -2.5019

yaw, pitch ,roll for ruler_angle3: 近似！
  -24.1540  -14.2231   -0.3835

yaw, pitch ,roll for ruler_angle4: 近似！
  -24.2173  -14.2231   -0.3835