ysh329
commented
5 years ago 7. 头部姿态估计实现代码
具体代码说明，见前文内容。这部分是对前文内容基于LM方法的具体实现，LM方法调用OpenCV的solvePnP接口：
import cv2
import numpy as np
import math
def get_angle(self, img, five_points_dict):
img_size = img.shape
if type(five_points_dict)==tuple or five_points_dict==None:
print("[ERROR] no face found or many faces")
return None
image_points = np.array([five_points_dict['nose'],\
five_points_dict['left_eye'],\
five_points_dict['right_eye'],\
five_points_dict['mouse_left'],\
five_points_dict['mouse_right'],\
])
# 3D model points.
model_points = np.array([
(0.0, 0.0, 0.0), # Nose tip
(-165.0, 170.0, -135.0), # Left eye left corner
(165.0, 170.0, -135.0), # Right eye right corner
(-150.0, -150.0, -125.0), # Left Mouth corner
(150.0, -150.0, -125.0) # Right mouth corner
])
# Camera internals
focal_length = img_size[1]
center = (img_size[1] / 2, img_size[0] / 2)
camera_matrix = np.array(
[[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1]], dtype="double"
)
dist_coeffs = np.zeros((4, 1)) # Assuming no lens distortion
(success, rotation_vector, translation_vector) = cv2.solvePnP(model_points,\
image_points,\
camera_matrix,\
dist_coeffs,\
flags=cv2.CV_ITERATIVE)
# calculate rotation angles
theta = cv2.norm(rotation_vector)
# transformed to quaterniond
w = np.cos(theta / 2)
x = np.sin(theta / 2) * rotation_vector[0] / theta
y = np.sin(theta / 2) * rotation_vector[1] / theta
z = np.sin(theta / 2) * rotation_vector[2] / theta
# quaterniondToEulerAngle
ysqr = y * y
xsqr = x * x
zsqr = z * z
# pitch (x-axis rotation)
t0 = 2.0 * (w * x + y * z)
t1 = 1.0 - 2.0 * (xsqr + ysqr)
pitch = math.atan2(t0, t1)
pitch = pitch * 180 / math.pi
# yaw (y-axis rotation)
t2 = 2.0 * (w * y - z * x)
t2 = 1.0 if t2 > 1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
yaw = math.asin(t2)
yaw = yaw * 180 / math.pi
# roll (z-axis rotation)
t3 = 2.0 * (w * z + x * y)
t4 = 1.0 - 2.0 * (ysqr + zsqr)
roll = math.atan2(t3, t4)
roll = roll * 180 / math.pi
if roll > 90:
roll = (roll - 180) % 180
if roll < -90:
roll = (roll + 180) % 180
angle_dict = {"pitch": pitch,\
"yaw": yaw,\
"roll": roll}
return angle_dict8. 头部姿态估计的评价准则
上面公式不好想的话，可以这么想：我们已知过点
注：红色的牛顿法的迭代路径，绿色的是梯度下降法的迭代路径。
liser0908
1. 什么是物体姿态和PNP问题
在计算机视觉中，物体的姿势指的是其相对于相机的相对取向和位置，一般用旋转矩阵、旋转向量、四元数或欧拉角表示（这四个量也可以互相转换）。一般而言，欧拉角可读性更好一些，也可以可视化（见下图，分别对应欧拉角的三个角度），所以常用欧拉角表示。欧拉角包含3个角度：pitch、yaw、roll，这三个角度也称为姿态角。
确定物体的姿态就是要计算其相对相机的欧拉角，刚提到也可以是旋转矩阵。旋转矩阵可以和欧拉角相互转换，一般而言，得到的欧拉角可以可视化到二维图像上，见下图。
姿势估计问题通常称为Perspective-n-Point问题或计算机视觉术语中的PNP。在该问题中，目标是在我们有校准相机时找到物体的姿势或运动（即找到欧拉角或旋转矩阵），还需要知道物体上的n个3D点的位置以及相应的相机3D到图像2D平面的投影。即，我们需要三个东西（后文会说到为什么需要这三样）：
2. 如何在数学上表示物体相对相机的运动？
在此前，我们先看看怎么表示物体在3D世界坐标中的运动（即如何让3D世界坐标下的物体运动到相机3D世界中去）。3D刚性物体相对于相机的运动，仅具有两种运动：
（X，Y，Z）移动到新的3D位置（X'，Y'，Z'）称为平移，平移具有3个自由度，即可以沿X，Y或Z方向移动。平移由向量t表示（X' - X，Y' - Y，Z' - Z）。3x3维度的旋转矩阵或者3x1维度的旋转向量，或者用一个旋转方向（即轴）和角度来定义。因此，估计3D对象中某一个点的运动，意味着找到该点的平移与旋转的6个参数 （这里，假定旋转用
1x3维度的旋转向量表示）：3个用于平移，3个用于旋转。那么我们就描述完了如何将3D世界中的坐标点转换到3D相机世界中去，这是第一个转换。但需要注意的是，我们需要知道物体3D世界中的点，而且一个假定是面部特征的3d坐标是基于世界坐标。这部分内容就是将物体在3D世界中的点，平移旋转到3D相机世界坐标的方法，但我只知道相机参数，并不知道物体在3D相机世界中的坐标。
3. 姿势估计算法如何工作？
别急，你看上图里有三个坐标系（世界3D坐标World Coordinates、相机3D坐标Camera Coordinates、投影平面坐标即图像Image Plane）。前文中有一张图片显示就是面部特征在世界3D坐标中的样子，以及对应2D图像Image Plane上的样子。如果我们知道，从人脸在世界3D坐标中的位置，到人脸在相机3D世界的位置的旋转和平移（即姿势/物体运动）参数，那么就可知道人脸在相机3D坐标中的位置。
仍旧看上图，可以发现相机3D坐标Camera Coordinates与投影平面坐标即图像Image Plane，二者的X和Y是一样的。现在我们知道世界坐标中位置P的3D点为（U，V，W）。如果知道相对于相机坐标的世界坐标的旋转矩阵R（3×3矩阵）和平移b（3×1矢量），就可以使用以下等式计算相机坐标系（X，Y，Z）中点的位置P。
也可以写成扩展的形式：
如果我们知道足够数量的点对，即（X，Y，Z）和与之对应的（U，V，W），上面就是一个线性方程组，可以求解得到旋转R和平移b的值。但该方法也有缺点（具体见下文方法2），正如前面所说，描述物体运动需要旋转和平移，对应3个方向6个值，这个方法一个3D点的平移参数算出是3个值，但是旋转参数却有9个（假定我们这里不考虑使用
3x3维度的旋转矩阵，而是想要使用3x1维度的旋转向量）。3.1 方法1：直接线性变换（DLT）求解参数
在上面的方程中，我们有很多已知的3D世界坐标点，即（U，V，W），但不知道具体的3D相机坐标（X，Y，Z），但知道3D相机坐标中的X和Y，即2D图像上的关键点。在没有径向畸变（即z点的畸变）的情况下，2D图像坐标中P点即（x，y）有以下公式：
PS：下面公式描述了，将相机坐标（X，Y，Z）转换即投影为平面坐标（x，y，1）的过程，已知平面坐标和相机参数，通过下面公式算出相机坐标（X，Y，Z）：
其中，
f_x和f_y是在x和y方向上的焦距，并且（c_x，c_y）是光学中心。当涉及径向变形时，事情变得稍微复杂一些，为了简单起见不考虑这一项。s是一个我们未知的比例因子，表示深度信息。举个例子，若将3D世界坐标中某个点P与3D相机坐标中对应的点P连接起来，即光线，那么这个光线会与图像平面相交得到二维平面上的点P。这个s就是这个过程中变换的深度信息，但我们这里因为未知，统统将所有点s考虑为一样的。
PS：以下公式，将3D世界坐标（U，V，W）转换为3D相机坐标，已知3D世界坐标和上面公式算出的3D相机坐标，因而可以算出
3x3的旋转矩阵和3x1的平移向量。基于上面的两个公式，先用第一个公式计算出Z值，再用第二个公式计算出旋转矩阵，依次列出方程组，依次求解得到3D相机坐标的值Z、旋转R和平移T参数。这种方法称为直接线性变换（Direct Linear Transform，DLT）。然后再讲旋转矩阵转换为欧拉角。
下面是补充内容，将
3x3维度的旋转矩阵转换为欧拉角。3.2 方法2：Levenberg-Marquardt
实际中，方法1的直接线性变换（Direct Linear Transform，DLT）计算结果不准确，因为：
3x3矩阵的意义是旋转矩阵（这里我有点懵，可能是说，3x3维度的这个矩阵其实际意义很难说带有旋转的意思在里面，因为没有最小化目标函数，这个算出的3x3的矩阵与旋转的参数意义不明确）；前文中的公式中，我们已经知道，若得到了正确的姿态R和t参数，就可以基于3D点信息以3D点投射到2D图像上的方式，来得到2D点信息。换句话说，知道了姿态参数R和t就可以知道每个3D点投影的2D点。
实际计算姿态前，我们已知2D点位置信息（比方用Dlib或手动选择点击得到关键点），假设也已知旋转R和平移t参数，那么基于2D点、R和t，我们可以算出2D点对应投影出的3D点（这个3D点是我们用参数R和t估算的），因为我们也知道真实的3D点，进而可以得到估算的3D点与真实的3D点二者的投影误差（re-projection error），即所有相对应点的距离平方和。
用前文中提到的两个公式（如上，再次列出），直接线性变换（DLT，direct linear transform）就是对姿态参数R和t的估计，想要改进一个最简单的方法就是修正R和t参数，看重投影误差有没有降低。如果降低，则接受新的R和t参数。但这个方法没有修正目标（每次加一点轻微扰动），而且很慢，一种名为LM的优化方法（Levenberg-Marquardt optimization），可以很好地解决这个问题，具体参考维基百科。
OpenCV的API
solvePnP（PnP，Perspective-n-Point，即从3D点找到其对应的2D点），可以使用基于LM方法（Levenberg-Marquardt）进行姿态估计，只需要定义flag参数为CV_ITERATIVE即可（当然还有其他方法，见后文API说明），无论哪种方法的使用，都需要定义这三个参数：objectPoints；imagePoints。如人脸关键点位置信息（五点如左右眼睛、嘴唇左右、鼻子）；distCoeffs（一般是固定的，其中定义了焦距，光学中心等）。确保或者以该参数校准相机，不会有误差、错误的结果等。有了上面三个参数交给LM优化算法，它就会使用相机的固有参数，结合世界坐标中的3D点与图像平面上的点信息，估计姿态的旋转R和平移t参数（即图像坐标系），反复迭代计算并寻找重投影误差最小时的旋转和平移参数，即我们最后期望的结果。
实际上，OpenCV有两个用来解决计算物体姿态函数接口：
solvePnP、solvePnPRansac，第二个solvePnPRansac利用随机抽样一致算法（Random sample consensus，RANSAC）的思想，虽然计算出的姿态更加精确，但速度慢、没法实现实时系统，所以我们这里只关注第一个solvePnP函数，OpenCV提供其C++与Python接口，具体说明如下：4. Levenberg-Marquardt优化算法
【见后文】【见后文】【见后文】【见后文】【见后文】【见后文】 【】【】【】【】【】【】【】【】【】【】【】【】【】
5. 旋转参数转换为欧拉角
副标题：旋转向量（角轴）、四元数与欧拉角的转换
前面我们使用OpenCV的
solvePnP接口，得到了旋转向量R和平移向量t。现在我们需要将旋转向量R转换为欧拉角。后面有与之对应实现的具体代码。这个代码的过程包含以下两个转换部分：rotation_vector——> 四元数（quaterniond，即w,x,y,z）；w,x,y,z）——>对应的欧拉角（pitch、yaw、roll）。注：更多旋转方面的转换公式见后面参考链接。
5.1 旋转向量（角轴）转为四元数
假设某个旋转是绕单位向量
n = [n_x, n_y, n_z]^T进行角度为\theta的旋转，则有四元数q：q = [cos(\theta / 2), n_x sin(\theta / 2), n_y sin(\theta / 2), n_z sin(\theta / 2)] ^ T，q = [w, x, y, z]^T，|q|^2 = w^2 + x^2 + y^2 + z^2 = 1该转换公式中的
\theta加上2pi，得到一个相同的旋转，其对应的四元数变成了-q，即任意的旋转都可以由两个互为相反数的四元数表示。对应如下代码：
5.2 四元数转为欧拉角
得到四元数后，再计算欧拉角，公式如下：
由于
arctan和arcsin的取值范围在[−π/2, π/2]，之间只有180°，而绕某个轴旋转时范围是360°，因此要使用atan2函数代替arctan函数：从四元数
[w, x, y, z]转换为欧拉角的代码实现：计算得到的欧拉角是弧度，需要将弧度转换为角度，才是我们姿态估计时候的欧拉角，转换公式如下：
6. 欧拉角与头部姿态
知道了欧拉角后，要确定头部姿态需要计算不同姿态对应的三个角度（pitch、yaw、roll）的值范围。常用的头部姿态有如下五个：左右摇头（水平45°倾斜，或者说是绕x轴旋转），左右转头（yaw，绕z轴旋转），抬头点头（一般看点头，不看抬头因为抬头和人脸正面差不多）。
这个参数需要调并校准，我在网上找了很多人脸图，然后对其旋转，得到3组图片：
分别先跑网络得到关键点，观察计算出的角度，确认每张图片的角度范围。得出：
这个对应姿态的三个角度参数需要自己调整，从而判断当前的头部属于哪种姿态。