头部姿态估计——在solvePnP之后输出欧拉角

最新推荐文章于 2024-06-02 10:22:54 发布

wangturbo

最新推荐文章于 2024-06-02 10:22:54 发布

阅读量5.8k

点赞数 7

CC 4.0 BY-SA版权

分类专栏：机器视觉文章标签：计算机视觉 python 算法

本文链接：https://blog.youkuaiyun.com/weixin_41856614/article/details/107488878

机器视觉专栏收录该内容

2 篇文章

订阅专栏

本文详细介绍头部姿态估计过程，包括在2D图像中寻找面部特征点，如眼角、鼻尖、嘴角等，并使用3D模型进行拟合。通过解决PnP问题，计算相机矩阵和畸变系数，最终求解出头部在三个自由度上的欧拉角。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

背景知识

基于LearnOpenCV的文章，对头部姿态估计分为以下几个部分，即

在2D图像中寻找到面部特征点，即眼角、鼻尖、下巴等等。该博客做了非常详细的介绍，但本文中考虑到镜像的问题，特征点的标记选取的不同，如下

FACIAL_LANDMARKS_68_IDXS = OrderedDict([
	("Nose tip", 30),
	("Chin", 8),
	("Left eye left corner", 45),
	("Right eye right corner", 36),
	("Left Mouth corner", 54),
	("Right mouth corner", 48),
])

即将左眼角和右眼角、左嘴角和右嘴角的位置替换
2. 第二步是采用3D模型，这里直接采用LearnOpenCV文章给出的3D模型坐标，拟合出来的效果也是满足要求的。
在这里插入图片描述
3D模型点坐标如下：

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 corner
    (-150.0, -150.0, -125.0),  # Left Mouth corner
    (150.0, -150.0, -125.0)  # Right mouth corner
])

对世界坐标系和相机坐标系的转换进行计算，原则上相机是需要进行标定的，但是在这里不标定，不考虑镜头的径向畸变，也基本能满足要求。
文章介绍了solvePnP和solvePnPRansac，后者利用随机抽样一致算法（Random sample consensus，RANSAC）的思想，虽然计算出的姿态更加精确，但速度慢、没法实现实时系统。这里只介绍solvePnP的算法。

 size = im.shape
 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"
        )
  dist_coeffs = np.zeros((4, 1))  # 假设没有发生镜头畸变
        (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs,  flags=cv2.SOLVEPNP_ITERATIVE)  # dist_coeffs为摄像机的畸变系数

其中rotation_vector和translation_vector分别是旋转向量和平移向量。在得到这两项值之后，则需要计算头部在三个自由度的旋转方向上的欧拉角。
头部姿态的三个自由度方向，图片来自网络

求解欧拉角

欧拉角的求解有多种方法，可以直接由定义求解，也可以利用OpenCV内置的decomposeProjectionMatrix的方法求解，程序如下:

def GetEuler(rotation_vector):
    """
    此函数用于从旋转向量计算欧拉角
    :param rotation_vector: 输入为旋转向量
    :return: 返回欧拉角在三个轴上的值
    """
    rvec_matrix = cv2.Rodrigues(rotation_vector)[0]
    proj_matrix = np.hstack((rvec_matrix, translation_vector))
    eulerAngles = -cv2.decomposeProjectionMatrix(proj_matrix)[6]
    yaw = eulerAngles[1]
    pitch = eulerAngles[0]
    roll = eulerAngles[2]
    rot_params = np.array([yaw, pitch, roll])
    return rot_params

OpenCV的问答区下还给出了C语言的版本

void getEulerAngles(Mat &rotCamerMatrix,Vec3d &eulerAngles){

    Mat cameraMatrix,rotMatrix,transVect,rotMatrixX,rotMatrixY,rotMatrixZ;
    double* _r = rotCamerMatrix.ptr<double>();
    double projMatrix[12] = {_r[0],_r[1],_r[2],0,
                          _r[3],_r[4],_r[5],0,
                          _r[6],_r[7],_r[8],0};

    decomposeProjectionMatrix( Mat(3,4,CV_64FC1,projMatrix),
                               cameraMatrix,
                               rotMatrix,
                               transVect,
                               rotMatrixX,
                               rotMatrixY,
                               rotMatrixZ,
                               eulerAngles);
}
Vec3d eulerAngles;
getEulerAngles(rotCamerMatrix1,eulerAngles);
yaw   = eulerAngles[1]; 
pitch = eulerAngles[0];
roll  = eulerAngles[2];