利用二维图像进行头部姿态估计

3D头部姿态估计（ubuntu操作系统，基于opencv3.2+Dlib19.4+python2.7）打开摄像头，可实现实时（realtime）姿态检测。

坐标变换：世界坐标系旋转、转换矩阵将3D点从世界坐标系变换到相机坐标系中.也就是通过算法完成世界坐标系（3D）、2Dlandmark输入image、相机坐标系之间的映射转换和标定。

2D输入landmark与3D世界坐标系对应点

即利用已有的输入世界坐标系的6点参数和landmark的6点参数，他们分别是

landmark6点参数

The 3D coordinates of the various facial features shown above are inworld coordinates. If we knew the rotation and translation ( i.e. pose ), we could transform the 3D points in world coordinates to 3D points incamera coordinates.

本程序最终结果为三个参数：

Yaw:摇头  左正右负

Pitch:点头 上负下正

Roll:摆头（歪头）左负 右正

结果角度表示

参考博客内容：http://blog.csdn.net/shenxiaolu1984/article/details/50165635

使用opencv的库函数solvepnp， (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points,camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE)

可以得到rotation_vector 但是这个旋转向量为3*1，其输出值不为而旋转矩阵为3*3的，只有转换成旋转矩阵才能与Euler坐标进行转换（ roll yaw pitch），故需要转换成旋转矩阵（vector->matrix）经查找需要使用opencv中的cv2.Rodrigues函数进行转换成旋转矩阵，调用时需要注意在python2.7中按如下方式调用R=cv2.Rodrigues(rotation_vector)[0]，再借用MATLAB中的转换代码rot2euler函数：

function [euler] = Rot2Euler(R)

q0 = sqrt( 1 + R(1,1) + R(2,2) + R(3,3) ) / 2;

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];

但是我们发现第pitch坐标抖动很厉害，而且是在正负之间震荡，但是在一定范围内是随点头的幅度呈近似线性关系，所以考虑是坐标角度变换的问题，在减去180度发现是从0度直接到-360，所以是互补的问题：

if pitch1<=-180:

pitch1=pitch1+360

此时在正常范围内进行测试，基本准确。

在测试时不能启动，shape没有定义，思考后发现是当开启摄像头后未检测到人脸，就不会有数据传入shape，这样就是未定义了，在前面测量下dets=detector(frame,1)中dets的个数，如果小于1直接显示未检测到人脸，返回即可。

运行结果如下：

运行结果

参考材料：http://www.learnopencv.com/head-pose-estimation-using-opencv-and-dlib/

其他参考博客：

http://blog.csdn.net/taily_duan/article/details/60580694