OpenCV的solvePnP函数和Dlib估计头部姿势

一、姿势估计概述

1、概述

在许多应用中，我们需要知道头部是如何相对于相机倾斜的。例如，在虚拟现实应用程序中，可以使用头部的姿势来渲染场景的右视图。在驾驶员辅助系统中，在车辆中观察驾驶员面部的摄像头可以使用头部姿势估计来查看驾驶员是否正在注意道路。当然，人们可以使用基于头部姿势的手势来控制免提应用程序/游戏。例如，从左到右偏头可能表示“否”。

2、姿态估计

在计算机视觉中，物体的姿态是指它相对于相机的相对方向和位置。您可以通过相对于相机移动对象或相对于对象移动相机来更改姿势。

姿态估计问题在计算机视觉术语中通常称为Perspective-n-Point问题或 PNP。在这个问题中，目标是在我们有一个校准过的相机时找到物体的位姿，并且我们知道物体上n 个3D 点的位置以及相应的 2D 投影图片。

3、在数学上表示相机运动

3D 刚体相对于相机只有两种运动。

平移：将相机从其当前的 3D 位置移动(X, Y, Z)到新的 3D 位置(X', Y', Z')称为翻译。如您所见，平移有 3 个自由度——您可以在 X、Y 或 Z 方向上移动。翻译由向量表示\mathbf{t}这等于（X' - X，Y' - Y，Z' - Z）.

旋转：您还可以围绕X,是和Z轴。因此，旋转也具有三个自由度。表示旋转的方式有很多种。您可以使用欧拉角（roll、pitch 和 yaw）来表示它，a3\次3 旋转矩阵，或旋转方向（即轴）和角度。

因此，估计 3D 对象的姿态意味着找到 6 个数字——三个用于平移，三个用于旋转。

4、姿势估计需要什么

要计算图像中对象的 3D 姿势，您需要以下信息

（1）几个点的 2D 坐标

您需要图像中几个点的 2D (x,y) 位置。对于人脸，你可以选择眼角、鼻尖、嘴角等。Dlib 的面部特征检测器为我们提供了许多可供选择的点。在本教程中，我们将使用鼻尖、下巴、左眼左眼角、右眼右眼角、左嘴角和右嘴角。

（2）相同点的 3D 位置

您还需要 2D 特征点的 3D 位置。您可能会认为需要照片中人物的 3D 模型才能获得 3D 位置。理想情况下是的，但在实践中，你不会。一个通用的 3D 模型就足够了。你从哪里得到一个头部的 3D 模型？好吧，您真的不需要完整的 3D 模型。您只需要一些任意参考框架中几个点的 3D 位置。在本教程中，我们将使用以下 3D 点。

鼻尖 : ( 0.0, 0.0, 0.0)

下巴：（0.0，-330.0，-65.0）

左眼左眼角：(-225.0f, 170.0f, -135.0)

右眼角：( 225.0, 170.0, -135.0)

左嘴角：(-150.0, -150.0, -125.0)

右嘴角：(150.0, -150.0, -125.0)

请注意，上述点位于某个任意参考系/坐标系中。这称为世界坐标（在 OpenCV 文档中也称为模型坐标）。

图像处理中涉及到的四个坐标系：

（3）相机的内在参数

如前所述，在这个问题中，假设相机是经过校准的。换句话说，需要知道相机的焦距、图像中的光学中心和径向畸变参数。所以你需要校准你的相机。不过可以不使用精确的 3D 模型，而使用近似状态。可以通过图像的中心来近似光学中心，通过以像素为单位的图像宽度来近似焦距，并假设不存在径向畸变。

5、姿势估计算法

有几种姿势估计算法。第一个已知的算法可以追溯到 1841 年。这里简要介绍一下。

有三个坐标系。上面显示的各种面部特征的 3D 坐标是世界坐标。如果我们知道旋转和平移（即姿势），我们可以将世界坐标中的 3D 点转换为相机坐标中的 3D 点。使用相机的内在参数（焦距，光学中心等），可以将相机坐标中的3D点投影到图像平面（即图像坐标系）上。

在上图中，O是相机的中心，图中所示的平面是图像平面。我们找出投影的方程3D点的到图像平面上。

关于3D到2D的投影可以简单参考下面链接

https://skydance.blog.csdn.net/article/details/124991406

假设我们知道位置（U、V、W）一个 3D 点磷在世界坐标中。如果我们知道旋转R（一个 3×3 矩阵）和平移t（一个 3×1 向量），相对于相机坐标的世界坐标，我们可以计算位置(X, Y, Z)点的P在相机坐标系中使用以下等式。

在扩展形式中，上面的等式看起来像这样

在线性代数中，如果我们知道足够数量的点对应（即(X, Y, Z)和（U、V、W）)，上面是一个线性方程组，其中和是未知数，您可以轻松解决未知数。

我们知道 3D 模型上的许多点（即（U、V、W）)，但我们不知道(X, Y, Z). 我们只知道二维点的位置（即(x, y)）。在没有径向畸变的情况下，坐标(x, y)点的p图像坐标由下式给出

其中和是 x 和 y 方向上的焦距，并且是光学中心。当涉及径向失真时，事情会变得稍微复杂一些，暂将其省略。

那个怎么样s在等式中？这是一个未知的比例因子。它存在于等式中，因为在任何图像中我们都不知道深度。如果你加入任何一点磷在 3D 中到中心○相机，点p，射线与像平面相交的地方是P. 请注意，沿着连接相机中心和点的射线的所有点P产生相同的图像。

上述形式的方程可以通过使用称为直接线性变换 (DLT)的方法使用一些代数魔法来求解。您可以在发现方程几乎是线性但偏离未知比例的问题时随时使用 DLT。

6、Levenberg-Marquardt 优化

上面提到的 DLT 解决方案不是很准确，原因如下。一、旋转具有三个自由度，但 DLT 解决方案中使用的矩阵表示有 9 个数字。DLT 解决方案中没有任何东西强制估计的 3×3 矩阵成为旋转矩阵。更重要的是，DLT 解决方案不会最小化正确的目标函数。理想情况下，我们希望最小化下面描述的重投影误差。

如果我们知道正确的姿势 (和)，我们可以通过将 3D 点投影到 2D 图像上来预测图像上 3D 面部点的 2D 位置。换句话说，如果我们知道和我们可以找到重点p在每个 3D 点的图像中磷.

我们还知道 2D 面部特征点（使用 Dlib 或手动点击）。我们可以查看投影的 3D 点和 2D 面部特征之间的距离。当估计的姿势完美时，投影到图像平面上的 3D 点将与 2D 面部特征几乎完美地对齐。当姿态估计不正确时，我们可以计算重投影误差度量——投影的 3D 点和 2D 面部特征点之间的距离平方和。

如前所述，姿态的近似估计（和) 可以使用 DLT 解决方案找到。改进 DLT 解决方案的一种简单方法是随机改变姿势 (和) 并检查重投影误差是否减小。如果是这样，我们可以接受新的姿势估计。我们可以继续扰动和一次又一次地寻找更好的估计。虽然这个过程会起作用，但它会很慢。事实证明，有原则性的方法可以迭代地改变和使重投影误差减小。

二、solvePnP函数

1、函数原型

如OpenCV文档中可见，姿势估计有一系列solvePnP函数，这里仅介绍solvePnP函数。

此函数使用不同的方法返回将对象坐标系中表示的 3D 点转换为相机坐标系的旋转和平移矢量：

P3P 方法（SOLVEPNP_P3P、SOLVEPNP_AP3P）：需要 4 个输入点才能返回唯一解。

SOLVEPNP_IPPE 输入点必须 >= 4 并且对象点必须共面。

SOLVEPNP_IPPE_SQUARE 适用于标记姿势估计的特殊情况。 输入点数必须为 4。对象点必须按顺序定义。

对于所有其他标志，输入点的数量必须 >= 4，并且对象点可以采用任何配置。

bool cv::solvePnP (InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=SOLVEPNP_ITERATIVE)

2、参数详解

objectPoints对象坐标空间中的对象点数组，Nx3 1通道或 1xN/Nx1 3通道，其中 N 是点数。 vector 也可以在这里传递。

imagePoints对应图像点的数组，Nx2 1通道或 1xN/Nx1 2通道，其中 N 是点数。 vector 也可以在这里传递。

cameraMatrix输入相机固有矩阵

distCoeffs4、5、8、12 的失真系数 (k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy]]]]) 的输入向量 或 14 个元素。 如果向量为 NULL/空，则假定零失真系数。

rvec输出旋转矢量（参见 Rodrigues），它与 tvec 一起将点从模型坐标系带到相机坐标系。

tvec输出平移向量。

useExtrinsicGuess用于 SOLVEPNP_ITERATIVE 的参数。 如果为真 (1)，则函数使用提供的 rvec 和 tvec 值分别作为旋转和平移向量的初始近似值，并进一步优化它们。

flags解决 PnP 问题的方法：参见 calib3d_solvePnP_flags

三、OpenCV源码

1、源码路径

opencv\modules\calib3d\src\solvepnp.cpp

int solvePnPGeneric( InputArray _opoints, InputArray _ipoints,
                     InputArray _cameraMatrix, InputArray _distCoeffs,
                     OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
                     bool useExtrinsicGuess, SolvePnPMethod flags,
                     InputArray _rvec, InputArray _tvec,
                     OutputArray reprojectionError) {
    CV_INSTRUMENT_REGION();
 
    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
    CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess)
                || (npoints >= 3 && flags == SOLVEPNP_SQPNP) )
               && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
 
    opoints = opoints.reshape(3, npoints);
    ipoints = ipoints.reshape(2, npoints);
 
    if( flags != SOLVEPNP_ITERATIVE )
        useExtrinsicGuess = false;
 
    if (useExtrinsicGuess)
        CV_Assert( !_rvec.empty() && !_tvec.empty() );
 
    if( useExtrinsicGuess )
    {
        int rtype = _rvec.type(), ttype = _tvec.type();
        Size rsize = _rvec.size(), tsize = _tvec.size();
        CV_Assert( (rtype == CV_32FC1 || rtype == CV_64FC1) &&
                   (ttype == CV_32FC1 || ttype == CV_64FC1) );
        CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
                   (tsize == Size(1, 3) || tsize == Size(3, 1)) );
    }
 
    Mat cameraMatrix0 = _cameraMatrix.getMat();
    Mat distCoeffs0 = _distCoeffs.getMat();
    Mat cameraMatrix = Mat_(cameraMatrix0);
    Mat distCoeffs = Mat_(distCoeffs0);
 
    vector vec_rvecs, vec_tvecs;
    if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
    {
        if (flags == SOLVEPNP_DLS)
        {
            CV_LOG_DEBUG(NULL, "Broken implementation for SOLVEPNP_DLS. Fallback to EPnP.");
        }
        else if (flags == SOLVEPNP_UPNP)
        {
            CV_LOG_DEBUG(NULL, "Broken implementation for SOLVEPNP_UPNP. Fallback to EPnP.");
        }
 
        Mat undistortedPoints;
        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
        epnp PnP(cameraMatrix, opoints, undistortedPoints);
 
        Mat rvec, tvec, R;
        PnP.compute_pose(R, tvec);
        Rodrigues(R, rvec);
 
        vec_rvecs.push_back(rvec);
        vec_tvecs.push_back(tvec);
    }
    else if (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P)
    {
        vector rvecs, tvecs;
        solveP3P(opoints, ipoints, _cameraMatrix, _distCoeffs, rvecs, tvecs, flags);
        vec_rvecs.insert(vec_rvecs.end(), rvecs.begin(), rvecs.end());
        vec_tvecs.insert(vec_tvecs.end(), tvecs.begin(), tvecs.end());
    }
    else if (flags == SOLVEPNP_ITERATIVE)
    {
        Mat rvec, tvec;
        if (useExtrinsicGuess)
        {
            rvec = _rvec.getMat();
            tvec = _tvec.getMat();
        }
        else
        {
            rvec.create(3, 1, CV_64FC1);
            tvec.create(3, 1, CV_64FC1);
        }
 
        CvMat c_objectPoints = cvMat(opoints), c_imagePoints = cvMat(ipoints);
        CvMat c_cameraMatrix = cvMat(cameraMatrix), c_distCoeffs = cvMat(distCoeffs);
        CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec);
        cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
                                     (c_distCoeffs.rows && c_distCoeffs.cols) ? &c_distCoeffs : 0,
                                     &c_rvec, &c_tvec, useExtrinsicGuess );
 
        vec_rvecs.push_back(rvec);
        vec_tvecs.push_back(tvec);
    }
    else if (flags == SOLVEPNP_IPPE)
    {
        CV_DbgAssert(isPlanarObjectPoints(opoints, 1e-3));
        Mat undistortedPoints;
        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
 
        IPPE::PoseSolver poseSolver;
        Mat rvec1, tvec1, rvec2, tvec2;
        float reprojErr1, reprojErr2;
        try
        {
            poseSolver.solveGeneric(opoints, undistortedPoints, rvec1, tvec1, reprojErr1, rvec2, tvec2, reprojErr2);
 
            if (reprojErr1 < reprojErr2)
            {
                vec_rvecs.push_back(rvec1);
                vec_tvecs.push_back(tvec1);
 
                vec_rvecs.push_back(rvec2);
                vec_tvecs.push_back(tvec2);
            }
            else
            {
                vec_rvecs.push_back(rvec2);
                vec_tvecs.push_back(tvec2);
 
                vec_rvecs.push_back(rvec1);
                vec_tvecs.push_back(tvec1);
            }
        }
        catch (...) { }
    }
    else if (flags == SOLVEPNP_IPPE_SQUARE)
    {
        CV_Assert(npoints == 4);
 
#if defined _DEBUG || defined CV_STATIC_ANALYSIS
        double Xs[4][3];
        if (opoints.depth() == CV_32F)
        {
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    Xs[i][j] = opoints.ptr(0)[i](j);
                }
            }
        }
        else
        {
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    Xs[i][j] = opoints.ptr(0)[i](j);
                }
            }
        }
 
        const double equalThreshold = 1e-9;
        //Z must be zero
        for (int i = 0; i < 4; i++)
        {
            CV_DbgCheck(Xs[i][2], approxEqual(Xs[i][2], 0, equalThreshold), "Z object point coordinate must be zero!");
        }
        //Y0 == Y1 && Y2 == Y3
        CV_DbgCheck(Xs[0][1], approxEqual(Xs[0][1], Xs[1][1], equalThreshold), "Object points must be: Y0 == Y1!");
        CV_DbgCheck(Xs[2][1], approxEqual(Xs[2][1], Xs[3][1], equalThreshold), "Object points must be: Y2 == Y3!");
        //X0 == X3 && X1 == X2
        CV_DbgCheck(Xs[0][0], approxEqual(Xs[0][0], Xs[3][0], equalThreshold), "Object points must be: X0 == X3!");
        CV_DbgCheck(Xs[1][0], approxEqual(Xs[1][0], Xs[2][0], equalThreshold), "Object points must be: X1 == X2!");
        //X1 == Y1 && X3 == Y3
        CV_DbgCheck(Xs[1][0], approxEqual(Xs[1][0], Xs[1][1], equalThreshold), "Object points must be: X1 == Y1!");
        CV_DbgCheck(Xs[3][0], approxEqual(Xs[3][0], Xs[3][1], equalThreshold), "Object points must be: X3 == Y3!");
#endif
 
        Mat undistortedPoints;
        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
 
        IPPE::PoseSolver poseSolver;
        Mat rvec1, tvec1, rvec2, tvec2;
        float reprojErr1, reprojErr2;
        try
        {
            poseSolver.solveSquare(opoints, undistortedPoints, rvec1, tvec1, reprojErr1, rvec2, tvec2, reprojErr2);
 
            if (reprojErr1 < reprojErr2)
            {
                vec_rvecs.push_back(rvec1);
                vec_tvecs.push_back(tvec1);
 
                vec_rvecs.push_back(rvec2);
                vec_tvecs.push_back(tvec2);
            }
            else
            {
                vec_rvecs.push_back(rvec2);
                vec_tvecs.push_back(tvec2);
 
                vec_rvecs.push_back(rvec1);
                vec_tvecs.push_back(tvec1);
            }
        } catch (...) { }
    }
    else if (flags == SOLVEPNP_SQPNP)
    {
        Mat undistortedPoints;
        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
 
        sqpnp::PoseSolver solver;
        solver.solve(opoints, undistortedPoints, vec_rvecs, vec_tvecs);
    }
    /*else if (flags == SOLVEPNP_DLS)
    {
        Mat undistortedPoints;
        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
        dls PnP(opoints, undistortedPoints);
        Mat rvec, tvec, R;
        bool result = PnP.compute_pose(R, tvec);
        if (result)
        {
            Rodrigues(R, rvec);
            vec_rvecs.push_back(rvec);
            vec_tvecs.push_back(tvec);
        }
    }
    else if (flags == SOLVEPNP_UPNP)
    {
        upnp PnP(cameraMatrix, opoints, ipoints);
        Mat rvec, tvec, R;
        PnP.compute_pose(R, tvec);
        Rodrigues(R, rvec);
        vec_rvecs.push_back(rvec);
        vec_tvecs.push_back(tvec);
    }*/
    else
        CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, "
            "SOLVEPNP_EPNP, SOLVEPNP_DLS, SOLVEPNP_UPNP, SOLVEPNP_AP3P, SOLVEPNP_IPPE, SOLVEPNP_IPPE_SQUARE or SOLVEPNP_SQPNP");
 
    CV_Assert(vec_rvecs.size() == vec_tvecs.size());
 
    int solutions = static_cast(vec_rvecs.size());
 
    int depthRot = _rvecs.fixedType() ? _rvecs.depth() : CV_64F;
    int depthTrans = _tvecs.fixedType() ? _tvecs.depth() : CV_64F;
    _rvecs.create(solutions, 1, CV_MAKETYPE(depthRot, _rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
    _tvecs.create(solutions, 1, CV_MAKETYPE(depthTrans, _tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
 
    for (int i = 0; i < solutions; i++)
    {
        Mat rvec0, tvec0;
        if (depthRot == CV_64F)
            rvec0 = vec_rvecs[i];
        else
            vec_rvecs[i].convertTo(rvec0, depthRot);
 
        if (depthTrans == CV_64F)
            tvec0 = vec_tvecs[i];
        else
            vec_tvecs[i].convertTo(tvec0, depthTrans);
 
        if (_rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR)
        {
            Mat rref = _rvecs.getMat_();
 
            if (_rvecs.depth() == CV_32F)
                rref.at(0,i) = Vec3f(rvec0.at(0,0), rvec0.at(1,0), rvec0.at(2,0));
            else
                rref.at(0,i) = Vec3d(rvec0.at(0,0), rvec0.at(1,0), rvec0.at(2,0));
        }
        else
        {
            _rvecs.getMatRef(i) = rvec0;
        }
 
        if (_tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR)
        {
 
            Mat tref = _tvecs.getMat_();
 
            if (_tvecs.depth() == CV_32F)
                tref.at(0,i) = Vec3f(tvec0.at(0,0), tvec0.at(1,0), tvec0.at(2,0));
            else
                tref.at(0,i) = Vec3d(tvec0.at(0,0), tvec0.at(1,0), tvec0.at(2,0));
        }
        else
        {
            _tvecs.getMatRef(i) = tvec0;
        }
    }
 
    if (reprojectionError.needed())
    {
        int type = (reprojectionError.fixedType() || !reprojectionError.empty())
                ? reprojectionError.type()
                : (max(_ipoints.depth(), _opoints.depth()) == CV_64F ? CV_64F : CV_32F);
 
        reprojectionError.create(solutions, 1, type);
        CV_CheckType(reprojectionError.type(), type == CV_32FC1 || type == CV_64FC1,
                     "Type of reprojectionError must be CV_32FC1 or CV_64FC1!");
 
        Mat objectPoints, imagePoints;
        if (opoints.depth() == CV_32F)
        {
            opoints.convertTo(objectPoints, CV_64F);
        }
        else
        {
            objectPoints = opoints;
        }
        if (ipoints.depth() == CV_32F)
        {
            ipoints.convertTo(imagePoints, CV_64F);
        }
        else
        {
            imagePoints = ipoints;
        }
 
        for (size_t i = 0; i < vec_rvecs.size(); i++)
        {
            vector projectedPoints;
            projectPoints(objectPoints, vec_rvecs[i], vec_tvecs[i], cameraMatrix, distCoeffs, projectedPoints);
            double rmse = norm(Mat(projectedPoints, false), imagePoints, NORM_L2) / sqrt(2*projectedPoints.size());
 
            Mat err = reprojectionError.getMat();
            if (type == CV_32F)
            {
                err.at(static_cast(i)) = static_cast(rmse);
            }
            else
            {
                err.at(static_cast(i)) = rmse;
            }
        }
    }
 
    return solutions;
}

bool solvePnP( InputArray opoints, InputArray ipoints,
               InputArray cameraMatrix, InputArray distCoeffs,
               OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, int flags )
{
    CV_INSTRUMENT_REGION();
 
    vector rvecs, tvecs;
    int solutions = solvePnPGeneric(opoints, ipoints, cameraMatrix, distCoeffs, rvecs, tvecs, useExtrinsicGuess, (SolvePnPMethod)flags, rvec, tvec);
 
    if (solutions > 0)
    {
        int rdepth = rvec.empty() ? CV_64F : rvec.depth();
        int tdepth = tvec.empty() ? CV_64F : tvec.depth();
        rvecs[0].convertTo(rvec, rdepth);
        tvecs[0].convertTo(tvec, tdepth);
    }
 
    return solutions > 0;
}

四、效果图像示例

这里面部特征点的位置是硬编码的，可以使用dlib进行面部特征点的定位，然后更改image_points。

https://skydance.blog.csdn.net/article/details/107896225

可以简单参考上面的链接

#include 
 
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 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 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_(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::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 nose_end_point3D;
    vector 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);
 
}

python版本

#!/usr/bin/env python
 
import cv2
import numpy as np
 
# 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.CV_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
cv2.imshow("Output", im)
cv2.waitKey(0)

https://blog.csdn.net/bashendixie5/article/details/125689183

https://blog.csdn.net/weixin_41010198/article/details/116028666