欧拉角（Euler Angle）
pitch：俯仰角，围绕X轴旋转;
yaw：偏航角，围绕Y轴旋转;
roll：翻滚角，围绕Z轴旋转。
第一步：用Opencv的solvePnP函数解出旋转向量
C++:
bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=SOLVEPNP_ITERATIVE )
Python:
cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, flags]]]]) → retval, rvec, tvec
-
输入
objectPoints - 世界坐标系下的控制点的坐标，单位mm，vector的数据类型在这里可以使用;
imagePoints - 在图像坐标系下对应的控制点的坐标，vector在这里可以使用;
cameraMatrix - 相机的内参矩阵;
distCoeffs - 相机的畸变系数;
flags - 默认使用CV_ITERATIV迭代法，还有SOLVEPNP _P3P、SOLVEPNP _EPNP、SOLVEPNP _DLS、SOLVEPNP _UPNP;
useExtrinsicGuess - 默认False，表示是否有初始化已知的旋转向量和平移向量;
（其中相机内参矩阵与畸变系数可由张正友相机标定法得到） -
输出
rvec - 输出的旋转向量，使坐标点从世界坐标系旋转到相机坐标系;
tvec - 输出的平移向量，使坐标点从世界坐标系平移到相机坐标系;
其中3D model，站坑以后看，得到3D点
https://github.com/YadiraF/PRNet
LEFT_EYEBROW_LEFT = [6.825897, 6.760612, 4.402142]
LEFT_EYEBROW_RIGHT = [1.330353, 7.122144, 6.903745]
RIGHT_EYEBROW_LEFT = [-1.330353, 7.122144, 6.903745]
RIGHT_EYEBROW_RIGHT = [-6.825897, 6.760612, 4.402142]
LEFT_EYE_LEFT = [5.311432, 5.485328, 3.987654]
LEFT_EYE_RIGHT = [1.789930, 5.393625, 4.413414]
RIGHT_EYE_LEFT = [-1.789930, 5.393625, 4.413414]
RIGHT_EYE_RIGHT = [-5.311432, 5.485328, 3.987654]
NOSE_LEFT = [2.005628, 1.409845, 6.165652]
NOSE_RIGHT = [-2.005628, 1.409845, 6.165652]
MOUTH_LEFT = [2.774015, -2.080775, 5.048531]
MOUTH_RIGHT = [-2.774015, -2.080775, 5.048531]
LOWER_LIP = [0.000000, -3.116408, 6.097667]
CHIN = [0.000000, -7.415691, 4.070434]
landmarks_3D = np.float32([LEFT_EYEBROW_LEFT,
LEFT_EYEBROW_RIGHT,
RIGHT_EYEBROW_LEFT,
RIGHT_EYEBROW_RIGHT,
LEFT_EYE_LEFT,
LEFT_EYE_RIGHT,
RIGHT_EYEBROW_LEFT,
RIGHT_EYEBROW_RIGHT,
NOSE_LEFT,
NOSE_RIGHT,
MOUTH_LEFT,
MOUTH_RIGHT,
LOWER_LIP,
CHIN])
第二步：旋转向量 ->旋转矩阵->欧拉角
- 旋转矩阵->旋转向量
C++:
void Rodrigues(InputArray src, OutputArray dst, OutputArray jacobian=noArray())
Python:
cv2.Rodrigues(src[, dst[, jacobian]]) → dst, jacobian
src – 输入，旋转向量(3x1 or 1x3) or 旋转矩阵 (3x3);
dst – 输出，旋转矩阵(3x3) or 旋转向量 (3x1 or 1x3);
jacobian – 可选项，输出雅克比矩阵（39或者93），输入数组对输出数组的偏导数;
利用solvePnP得到的旋转向量，由Rodrigues可得到旋转矩阵，下面可由旋转矩阵求出欧拉角。
- 判断是否为旋转矩阵
def isRotationMatrix(R):
Rt = np.transpose(R) #旋转矩阵R的转置
shouldBeIdentity = np.dot(Rt, R) #R的转置矩阵乘以R
I = np.identity(3, dtype=R.dtype) # 3阶单位矩阵
n = np.linalg.norm(I - shouldBeIdentity) #np.linalg.norm默认求二范数
return n < 1e-6 # 目的是判断矩阵R是否正交矩阵（旋转矩阵按道理须为正交矩阵，如此其返回值理论为0）
旋转矩阵为正交矩阵，根据正交矩阵的特性进行判断R*RT=E。
- 旋转矩阵->欧拉角
def rotationMatrixToAngles(R):
assert (isRotationMatrix(R)) #判断是否是旋转矩阵（用到正交矩阵特性）
sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0]) #矩阵元素下标都从0开始（对应公式中是sqrt(r11*r11+r21*r21)），sy=sqrt(cosβ*cosβ)
singular = sy < 1e-6 # 判断β是否为正负90°
if not singular: #β不是正负90°
x = math.atan2(R[2, 1], R[2, 2])
y = math.atan2(-R[2, 0], sy)
z = math.atan2(R[1, 0], R[0, 0])
else: #β是正负90°
x = math.atan2(-R[1, 2], R[1, 1])
y = math.atan2(-R[2, 0], sy) #当z=0时，此公式也OK，上面图片中的公式也是OK的
z = 0
x = x*180.0/3.141592653589793
y = y*180.0/3.141592653589793
z = z*180.0/3.141592653589793
return np.array([x, y, z])
参考：
https://docs.opencv.org/3.0-beta/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#solvepnp
https://www.zhihu.com/question/47736315/answer/236284413
