原文：Head Pose Estimation using OpenCV and Dlib - 2016.09.26

主要介绍如何采用 OpenCV 和 Dlib 库，实现图片中人头部姿态估计.

image

在很多场景中，需要知道人头部相对于相机的倾斜程度. 例如，在 VR 领域，可以采用头部姿态来渲染场景的视角. 在辅助驾驶系统中，对准司机脸部的相机，可以根据头部姿态估计，判断司机是否注意力集中. 此外，还可以采用给予手势的头部姿态来控制 hands-free 应用(或游戏). 例如，将头部从左到有扭转，可以表示 NO. 但，如果是在印度南部，这个信号表示 YES.

本文作者认为，头部姿态估计是很有应用价值的.

1. 什么是姿态估计？

在 CV 领域中，物体的姿态是其相对于相机的方向(orientation)和位置(position). 通过移动物体相对于相机的位置，或者移动相机相对于物体的位置，即可改变物体的姿态.

这里所描述的姿态估计问题通常是 Perspective-n-Point (PNP) 问题，其旨在，对于标定相机(calibrated camera)，找出物体的姿态；其中，已知物体的 n 个 3D 点，以及其在图片中所对应的 2D 投影.

对于一个标定好的摄像机，根据目标在 3D 世界坐标系中 n 个点的坐标和对应投影到 2D 图像坐标系中的点集之间的变换关系矩阵进行求解.

2. 相机运动(camera motion) 的数学描述

3D 刚体仅有两种相对于相机的运动：

[1] - 平移

将相机从其当前 3D 位置 $(X, Y, Z)$ 移动到新的 3D 位置 $(X', Y', Z')$，即为 平移.

易知，其有 3 个自由度(degrees of freedom)，可以分别在 X, Y 或 Z 方向进行评议.

平移可以表示为向量：$\mathbf{t} = (X'-X, Y'-Y, Z'-Z)$.

[2] - 旋转

关于 X, Y 和 Z 轴，旋转相机. 因此，旋转 也有三个自由度.

旋转有很多种表示方式，可以是欧拉角(Euler angles) (roll,pitch,yaw) 或者，3x3 旋转矩阵(rotation matrix)，或者旋转和角度的方向(direction of rotation (i.e. axis ) and angle).

因此，3D 物体的姿态估计，即是计算 6 个变量 - 三个平移和三个旋转.

3. 姿态估计所需信息

image

为了计算图片中物体的 3D 姿态，需要知道一下信息：

[1] - 一些点的 2D 坐标位置

图片中一些点的 2D 坐标 (x, y).

对于人脸而言，往往选择眼角(corners of the eyes), 鼻尖(tip of the nose), 嘴角(corners of the mouth) 等位置的点. Dlib 库的 人脸关键点检测 可以定位很多人脸关键点.

这里采用的人脸关键点有：鼻尖，下巴，左眼的左角，右眼的右角，左嘴角，右嘴角.

[2] - 同一批点的 3D 位置

同时还需要 2D 特征点对应的 3D 位置.

可能首先会想到，需要获取图片中人体的 3D 模型，才能得到 3D 位置. 而，实际上，是不需要的. 通用 3D 模型即可.

如何获取头部的 3D 模型呢？ 实际上不需要 完整的 3D 模型，只需要任意坐标系中一些点的 3D 位置. 这里采用如下的 3D 点：

  • (1) - 鼻尖：( 0.0, 0.0, 0.0)
  • (2) - 下巴：( 0.0, -330.0, -65.0)
  • (3) - 左眼左角：(-225.0f, 170.0f, -135.0)
  • (4) - 右眼右角：( 225.0, 170.0, -135.0)
  • (5) - 左嘴角：(-150.0, -150.0, -125.0)
  • (6) - 右嘴角：(150.0, -150.0, -125.0)

要注意的是，这些点是在任意坐标系统的. 即世界坐标系(World Coordinates).

[3] - 相机的内在参数(intrinsic parameters)

正如上面所述，这里需要首先进行相机标定的(calibrated). 即，需要知道相机的焦距(focal length)，图像的光学中心(optical center)和径向畸变参数(radial distortion parameters).

由于不是采用精度 3D 模型来逼近问题，可以采用图片中心(image center)来逼近光学中心(optical center)，采用图像的宽度(像素值)来逼近焦距(focal length)，假设径向畸变(radial disortion) 不存在的话.

进行相机参数估计时首先需要对相机进行标定，精确的相机标定需要使用张正友的棋盘格标定，这里还是进行近似.

相机的内参数矩阵需要设定相机的焦距、图像的中心位置并且假设不存在径向畸变. 这里设置相机焦距为图像的宽度(以像素为单位)，图像中心位置为(image.width/2,image.height/2).

From: https://blog.csdn.net/cdknight_happy/article/details/79975060

4. 姿态估计算法原理

姿态估计有很多常用算法，最早的算法可以追溯到 1841 年. 这里不涉及到这些算法细节的解释，只是利用其思想.

这里需要用到三个坐标系统. 人脸特征的 3D 坐标是以世界坐标系进行表示的，如果旋转和平移是已知的(例如，pose)，那么可以将世界坐标(world coordinates)中的 3D 点转换到相机坐标系(camera coordinates)中的 3D 点. 然后，采用相机的内在参数(焦距、光学中心等)，将相机坐标系中的 3D 点再投影到图像平面(如，图像坐标系).

image

下面将图像成像公式分解，以理解上面的坐标系统的工作原理. 如上图中，$o$ 是相机中心，图中的平面即为图像平面. 目的是，找出 3D 点 $P$ 到图像平面的投影 $p$ 的等式.

假设，已知世界坐标系中的 3D 点 $P$ 的位置 $(U, V, W)$. 如果还知道相对于相机坐标系中的世界坐标系的旋转矩阵 $\mathbf{R} $ (3x3 矩阵)和平移向量 $\mathbf{t}$ (3x1 向量)，则可以采用下面的等式计算相机坐标系中点 $P$ 的坐标 $(X, Y, Z)$:

image

其展开形式类似于：

image

如果具备线性代数知识，可以看出上述等式中，如果变量数(如，$(X, Y, Z)$ 和 $(U, V, W)$ )已知的足够多，该等式是线性系统的等式. 其中，$r_{ij}$ 和 $(t_x, t_y, t_z)$ 是未知的，是可以求解未知量.

下一章节中会涉及到的，只有 $(X, Y, Z)$ 是已知的，因此该等式不是简单的线性系统问题.

5. 直接线性变换

虽然已知 3D 模型中的很多点，如$(U, V, W)$，但是不知道 $(X, Y, Z)$. 而仅知道 2D 点的坐标未知，如$(x, y)$.

假设不存在径向畸变(radial disortion)的情况下，图像坐标系中的点 $p$ 的坐标 $(x, y)$ 的计算为：

image

其中，$f_x$ 和 $f_y$ 是在 x 和 y 方向上的焦距长度，$(c_x, c_y)$ 是光学中心.

如果存在径向畸变，则会变得复杂一些. 不过这里先跳过.

公式中的 $s$ 是未知的缩放因子. 其在公式中的原因是，在任何图片中深度(depth)都是未知的. 如果将点 $P$ 的 3D 坐标加入到相机的的中心 $o$，则，点 $p$ 是射线 $oP$ 与图像平面的交点. 沿着射线的加入到相机中心和点 $P$ 的所有点，产生相同的图像. 换句话说，采用等式(3)，只能得到 $(X, Y, Z)$，最多还能得到尺度 $s$.

这就会使得等式 (2) 变得复杂. 且更类似于：

image

幸运的是，等式(4) 还是可以采用直接线性变换(Direct Linear Transform (DLT)) 方法进行求解的. DLT 方法可以用于求解等式接近与线性系统，但又包含未知尺度因子的等式问题.

6. Levenberg-Marquardt Optimization

DLT 方法可能是不够精确的，其主要原因是：

[1] - 旋转 $\mathbf{R}$ 包含三个自由度，但 DLT 算法中的矩阵表示要采用 9 个数字. 并没有限制该估计的 3x3 矩阵必须为旋转矩阵.

[2] - 更重要的是，DLT 方法并不是对正确的目标函数进行最小化. 理想情况下，想要最小化的是重投影误差(reprojection error).

如等式 (2) 和等式(3) 所示，如果正确的姿态($\mathbf{R}​$ 和 $\mathbf{t} ​$)是已知的，通过将 3D 点投影到 2D 图像，可以预测出 3D 人脸关键点的 2D 位置点.换句话说，如果已知 $\mathbf{R}​$ 和 $\mathbf{t}​$，可以找到每个 3D 点 $P​$ 的投影点 $p​$.

另外还知道 2D 人脸特征点(可以采用 Dlib 库或手工选取). 则可以计算投影的 3D 点和 2D 人脸特征点之间的距离. 如果估计的人脸姿态是很完美的，则 3D 点投影到图像平面，会与 2D 人脸特征点高度重合(line up). 如果估计的人脸姿态不够好，则需要计算重投影误差，其计算的是投影的 3D 点和 2D 人脸特征点之间的平方差距离.

前面说过，DLT 方法可以用于姿态( $\mathbf{R}$ 和 $\mathbf{t}$)的逼近估计. DLT 方法的一种直接的提升方案是，随机轻微的改变姿态( $\mathbf{R}$ 和 $\mathbf{t}$) ，然后检查重投影误差的衰减情况. 如果是衰减的，则可以接受为新的姿态估计. 然后，继续保持调整 $\mathbf{R}$ 和 $\mathbf{t}$，以寻找更好的姿态估计. 虽然该方案有效，但速度会非常慢. 实践证明， $\mathbf{R}$ 和 $\mathbf{t}$ 迭代改变的有几种主要方式，以使得重投影误差衰减，这种方式被成为 Levenberg-Marquardtoptimization. 详细细节可参考 Wikipedia.

7. OpenCV solvePnP 函数

OpenCV 中，solvePnP 函数和 solvePnPRansac 函数可以用于估计姿态.

solvePnP 函数实现了几种姿态估计算法，可以根据参数 flag 进行选择. 默认 flag 是 SOLVEPNP_ITERATIVE，其是采用 Levenberg-Marquardt 优化的 DTL 方案；SOLVEPNP_P3P 仅采用 3 个点用于计算姿态，只有在采用 solvePnPRansac 函数时才被使用.

OpenCV 3中，包含了新的算法，SOLVEPNP_DLS 和 SOLVEPNP_UPNP. 其中，有意思的是，SOLVEPNP_UPNP 还尝试估计相机的内在参数.

// 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 - 世界坐标系空间的物体点数组. 一般是 N 个 3D 点的向量(推荐). 也可以是 Nx3 (或 3xN) 的单通道矩阵，或者 Nx1(或 1xN) 的 3 通道矩阵.

imagePoints - 对应的图像点数组. 一般可以是 N 个 2D 点的向量. 也可以是 Nx2 (或 2xN) 的单通道矩阵，或者 Nx1(或 1xN) 的 2 通道矩阵.

cameraMatrix - 输入的相机矩阵:

$$ \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_x & c_y \\ 0 & 0 & 1 \end{bmatrix} $$

其中，$f_x$ 和 $f_y$ 可以采用图像的宽度进行逼近，$c_x$ 和 $c_y$ 可以是图像中心的坐标.

distCoeffs - 包含 4， 5， 8 或12 个元素的畸变相关系数的输入向量 $(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6], [s_1, s_2, s_3, s_4]])$ . 如果该向量为 NULL 或空，则假设是 0 畸变的. 通常该值为 NULL.

rvec - 输出的旋转向量

tvec - 输出的平移向量

useExtrinsicGuess - SOLVEPNP_ITERATIVE 涉及的参数. 如果该值为 True，则函数会采用提供的 rvec 和 tvec 值分别作为旋转向量和平移向量的初始化逼近，再进行进一步的优化.

flags - PnP 问题求解方法：

SOLVEPNP_ITERATIVE Iterative method is based on Levenberg-Marquardt optimization. In this case, the function finds such a pose that minimizes reprojection error, that is the sum of squared distances between the observed projections imagePoints and the projected (using projectPoints() ) objectPoints .

SOLVEPNP_P3P Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification for the Perspective-Three-Point Problem”. In this case, the function requires exactly four object and image points.

SOLVEPNP_EPNP Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient Perspective-n-Point Camera Pose Estimation”.

The flags below are only available for OpenCV 3

SOLVEPNP_DLS Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis. “A Direct Least-Squares (DLS) Method for PnP”.

SOLVEPNP_UPNP Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto, F.Moreno-Noguer. “Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation”. In this case the function also estimates the parameters f_x and f_y assuming that both have the same value. Then the cameraMatrix is updated with the estimated focal length.

8. OpenCV solvePnPRansac 函数

solvePnPRansac 函数与 solvePnP 函数非常相似，除了其采用了 Random Sample Consensus ( RANSAC ) 以进行更鲁棒的姿态估计.

// C++: 
void solvePnPRansac(InputArray objectPoints, 
                    InputArray imagePoints, 
                    InputArray cameraMatrix, 
                    InputArray distCoeffs, 
                    OutputArray rvec, 
                    OutputArray tvec, 
                    bool useExtrinsicGuess=false, 
                    int iterationsCount=100, 
                    float reprojectionError=8.0, 
                    int minInliersCount=100, 
                    OutputArray inliers=noArray(), 
                    int flags=ITERATIVE)
# Python: 
cv2.solvePnPRansac(
    objectPoints, 
    imagePoints, 
    cameraMatrix, 
    distCoeffs[,
               rvec[, 
                    tvec[,
                         useExtrinsicGuess[,
                                           iterationsCount[, 
                                                           reprojectionError[, 
                                                                             minInliersCount[, 
                                                                                             inliers[, flags]
                                                                                            ]
                                                                            ]
                                                          ]
                                          ]
                        ]
                   ]
              ]
) → rvec, tvec, inliers

新增参数：

iterationsCount – The number of times the minimum number of points are picked and the parameters estimated.
reprojectionError – As mentioned earlier in RANSAC the points for which the predictions are close enough are called “inliers”. This parameter value is the maximum allowed distance between the observed and computed point projections to consider it an inlier.
minInliersCount – Number of inliers. If the algorithm at some stage finds more inliers than minInliersCount , it finishes.
inliers – Output vector that contains indices of inliers in objectPoints and imagePoints .

9. OpenCV POSIT

OpenCV 早期采用了 POSIT 的姿态估计算法，其是 C 代码的 API (cvPosit)，并不是 C++ API 的一部分.

10. OpenCV 姿态估计的实现源码

人脸特征点的位置是硬编码的，如果需要使用自定义的图片，需要修改 image_points 向量.

image

headPose.jpg

10.1. C++ 实现

#include <opencv2/opencv.hpp>
 
using namespace std;
using namespace cv;
 
int main(int argc, char **argv)
{
    // Read input image
    cv::Mat im = cv::imread("headPose.jpg");
     
    // 2D image points. If you change the image, you need to change vector
    std::vector<cv::Point2d> image_points;
    image_points.push_back( cv::Point2d(359, 391) );    // Nose tip
    image_points.push_back( cv::Point2d(399, 561) );    // Chin
    image_points.push_back( cv::Point2d(337, 297) );     // Left eye left corner
    image_points.push_back( cv::Point2d(513, 301) );    // Right eye right corner
    image_points.push_back( cv::Point2d(345, 465) );    // Left Mouth corner
    image_points.push_back( cv::Point2d(453, 469) );    // Right mouth corner
     
    // 3D model points.
    std::vector<cv::Point3d> model_points;
    model_points.push_back(cv::Point3d(0.0f, 0.0f, 0.0f));               // Nose tip
    model_points.push_back(cv::Point3d(0.0f, -330.0f, -65.0f));          // Chin
    model_points.push_back(cv::Point3d(-225.0f, 170.0f, -135.0f));       // Left eye left corner
    model_points.push_back(cv::Point3d(225.0f, 170.0f, -135.0f));        // Right eye right corner
    model_points.push_back(cv::Point3d(-150.0f, -150.0f, -125.0f));      // Left Mouth corner
    model_points.push_back(cv::Point3d(150.0f, -150.0f, -125.0f));       // Right mouth corner
     
    // Camera internals
    double focal_length = im.cols; // Approximate focal length.
    Point2d center = cv::Point2d(im.cols/2,im.rows/2);
    cv::Mat camera_matrix = (cv::Mat_<double>(3,3) << focal_length, 0, center.x, 0 , focal_length, center.y, 0, 0, 1);
    cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<double>::type); // Assuming no lens distortion
     
    cout << "Camera Matrix " << endl << camera_matrix << endl ;
    // Output rotation and translation
    cv::Mat rotation_vector; // Rotation in axis-angle form
    cv::Mat translation_vector;
     
    // Solve for pose
    cv::solvePnP(model_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector);
 
     
    // Project a 3D point (0, 0, 1000.0) onto the image plane.
    // We use this to draw a line sticking out of the nose
     
    vector<Point3d> nose_end_point3D;
    vector<Point2d> nose_end_point2D;
    nose_end_point3D.push_back(Point3d(0,0,1000.0));
     
    projectPoints(nose_end_point3D, rotation_vector, translation_vector, camera_matrix, dist_coeffs, nose_end_point2D);
     
     
    for(int i=0; i < image_points.size(); i++)
    {
        circle(im, image_points[i], 3, Scalar(0,0,255), -1);
    }
     
    cv::line(im,image_points[0], nose_end_point2D[0], cv::Scalar(255,0,0), 2);
     
    cout << "Rotation Vector " << endl << rotation_vector << endl;
    cout << "Translation Vector" << endl << translation_vector << endl;
     
    cout <<  nose_end_point2D << endl;
     
    // Display image.
    cv::imshow("Output", im);
    cv::waitKey(0);
 
}

10.2. Python 实现

#!/usr/bin/env python
import cv2
import numpy as np
import matplotlib.pyplot as plt

# Read Image
im = cv2.imread("headPose.jpg")
size = im.shape

# 2D image points. If you change the image, you need to change vector
image_points = np.array([
    (359, 391),  # Nose tip
    (399, 561),  # Chin
    (337, 297),  # Left eye left corner
    (513, 301),  # Right eye right corne
    (345, 465),  # Left Mouth corner
    (453, 469)  # Right mouth corner
], dtype="double")

# 3D model points.
model_points = np.array([
    (0.0, 0.0, 0.0),  # Nose tip
    (0.0, -330.0, -65.0),  # Chin
    (-225.0, 170.0, -135.0),  # Left eye left corner
    (225.0, 170.0, -135.0),  # Right eye right corne
    (-150.0, -150.0, -125.0),  # Left Mouth corner
    (150.0, -150.0, -125.0)  # Right mouth corner
])

# Camera internals
focal_length = size[1]
center = (size[1] / 2, size[0] / 2)
camera_matrix = np.array(
    [[focal_length, 0, center[0]],
     [0, focal_length, center[1]],
     [0, 0, 1]], dtype="double")

print("Camera Matrix :\n {0}".format(camera_matrix))

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.cv2.SOLVEPNP_ITERATIVE) #

print("Rotation Vector:\n {0}".format(rotation_vector))
print("Translation Vector:\n {0}".format(translation_vector))

# Project a 3D point (0, 0, 1000.0) onto the image plane.
# We use this to draw a line sticking out of the nose

(nose_end_point2D, jacobian) = cv2.projectPoints(
    np.array([(0.0, 0.0, 1000.0)]),
    rotation_vector,
    translation_vector,
    camera_matrix,
    dist_coeffs)

for p in image_points:
    cv2.circle(im, (int(p[0]), int(p[1])), 3, (0, 0, 255), -1)

p1 = (int(image_points[0][0]), int(image_points[0][1]))
p2 = (int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))

cv2.line(im, p1, p2, (255, 0, 0), 2)

# Display image
plt.imshow(im[:, :,::-1])
plt.axis("off")
plt.show()

输入如：

Camera Matrix :
 [[1.20e+03 0.00e+00 6.00e+02]
 [0.00e+00 1.20e+03 3.37e+02]
 [0.00e+00 0.00e+00 1.00e+00]]
Rotation Vector:
 [[-0.0581844 ]
 [ 2.20231074]
 [ 0.01887552]]
Translation Vector:
 [[  449.53365497]
 [  -95.72475146]
 [-2344.07917989]]

image

Last modification：April 24th, 2019 at 09:06 pm
© 允许规范转载