头部姿态获取算法概述(坐标系转换)

头部姿态获取算法概述

—— 才疏学浅, 难免有错误和遗漏, 欢迎补充和勘误.

获取头部运动姿态的流程图如下：

图1. 获取头部运动姿态的流程图

• 首先通过Dlib库得到2D人脸68个特征关键点，再通过3D Morphable Model等模型匹配算法拟合3D人脸特征点。摄像机中的各个坐标系的位置关系如下：
  

图2. 各个坐标系的位置关系

• 其中，世界坐标系为：Ow-XwYwZw；相机坐标系为：0c-XcYcZc；图像坐标系为：o-xy；像素坐标系为：图像左上角-uv，单位为像素。P为世界坐标系中的一点； p为P在图像坐标系中的投影点；f为焦距，即Oc到o的距离。

1. 世界坐标系Ow-XwYwZw通过旋转和平移转换到相机坐标系0c-XcYcZc
   

图3. 世界、相机坐标系之间的旋转和平移转换

转换公式为：$\left[\begin{array}{c}Xc\\ Yc\\ Zc\\ 1\end{array}\right]=\left[\begin{array}{ll}R& T\\ \overrightarrow{0}& 1\end{array}\right]\left[\begin{array}{c}Xw\\ Yw\\ Zw\\ 1\end{array}\right]$

其中旋转矩阵R的规格是3*3，平移矩阵T的规格是3*1，且都为未知量。

---

2. 相机坐标系0c-XcYcZc通过三角形相似关系的透视投影转换到图像坐标系0-xy：
   

图4. 相机、图像坐标系之间的透视投影转换

$\because △ABOc\sim △oCOc\cdot$ 且 $△PBOc\sim △pCOc$
$\therefore \{\begin{array}{l}x=f\frac{Xc}{Zc}\\ y=f\frac{Yc}{Zc}\end{array}$
将上式写为矩阵表达式为：$Zc\left[\begin{array}{l}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}Xc\\ Yc\\ Zc\\ 1\end{array}\right]$

---

3. 图像坐标系0-xy通过伸缩和平移转换到像素坐标系uv：$p(u,v):\{\begin{array}{l}u=\frac{x}{dx}+u_0\\ v=\frac{y}{dy}+v_0\end{array}$
   

图5.图像、像素坐标系之间的伸缩和平移转换

其中 $dx$、 $dy$为单个像素在 $x$轴和$y$ 轴方向上的物理长度，将上式写为矩阵表达式为：

$\left[\begin{array}{l}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{l}x\\ y\\ 1\end{array}\right]$

---

• 至此，由世界坐标系0w-XwYwZw到像素坐标系uv之间的转换公式可合写为：
  
• 其中相机的内参只由相机本身决定，由张正友标定法等标定出来；相机的外参会随着世界坐标系中物体的位置而变化，当计算出相机畸变后，把3D参考点、2D关键点、相机内部参数和相机的畸变输入到OpenCV的solvePnP函数中求解出旋转矩阵R和平移矩阵T，最后通过欧拉角计算出驾驶人的头部运动姿态，即得到头部运动的pitch（俯仰角）、yaw（偏航角）和roll（翻滚角），如下图所示：
  

图6. pitch、yaw和roll角度的表示