参考python代码实现：https://github.com/fisakhan/Face_Pose  pose_detection_scrfd.py

def find_pose(points):
  """
  Parameters
  ----------
  points : float32, Size = (5,2)
  coordinates of landmarks for the selected faces.
  Returns
  -------
  float32, float32, float32
  """
  LMx = points[:,0]#points[0:5]# horizontal coordinates of landmarks
  LMy = points[:,1]#[5:10]# vertical coordinates of landmarks

  dPx_eyes = max((LMx[1] - LMx[0]), 1)
  dPy_eyes = (LMy[1] - LMy[0])
  angle = np.arctan(dPy_eyes / dPx_eyes) # angle for rotation based on slope

  alpha = np.cos(angle)
  beta = np.sin(angle)

  # rotated landmarks
  LMxr = (alpha * LMx + beta * LMy + (1 - alpha) * LMx[2] / 2 - beta * LMy[2] / 2)
  LMyr = (-beta * LMx + alpha * LMy + beta * LMx[2] / 2 + (1 - alpha) * LMy[2] / 2)

  # average distance between eyes and mouth
  dXtot = (LMxr[1] - LMxr[0] + LMxr[4] - LMxr[3]) / 2
  dYtot = (LMyr[3] - LMyr[0] + LMyr[4] - LMyr[1]) / 2

  # average distance between nose and eyes
  dXnose = (LMxr[1] - LMxr[2] + LMxr[4] - LMxr[2]) / 2
  dYnose = (LMyr[3] - LMyr[2] + LMyr[4] - LMyr[2]) / 2

  # relative rotation 0 degree is frontal 90 degree is profile
  Xfrontal = (-90+90 / 0.5 * dXnose / dXtot) if dXtot != 0 else 0
  Yfrontal = (-90+90 / 0.5 * dYnose / dYtot) if dYtot != 0 else 0

  return angle * 180 / np.pi, Xfrontal, Yfrontal

std::vector<float> find_pose(const std::vector<cv::Point2d>& facial5Pts)
{
    const float PI_VALUE = 3.1415926;
    cv::Point2d eye_l = facial5Pts[0];
    cv::Point2d eye_r = facial5Pts[1];
    cv::Point2d nose =  facial5Pts[2];
    cv::Point2d mouth_l = facial5Pts[3];
    cv::Point2d mouth_r = facial5Pts[4];

    float dx = std::max(facial5Pts[1].x - facial5Pts[0].x, 1.0);
    float dy = facial5Pts[1].y - facial5Pts[0].y;
    double angle = atan(dy / dx);
    double alpha = cos(angle);
    double beta = sin(angle);

    // rotated landmarks
    std::vector<float> lmx_rot;
    lmx_rot.resize(5);
    std::vector<float> lmy_rot;
    lmy_rot.resize(5);
    float LM_nose_x = facial5Pts[2].x;
    float LM_nose_y = facial5Pts[2].y;
    //LMxr = (alpha * LMx + beta * LMy + (1 - alpha) * LMx[2] / 2 - beta * LMy[2] / 2)
    //LMyr = (-beta * LMx + alpha * LMy + beta * LMx[2] / 2 + (1 - alpha) * LMy[2] / 2)
    for(int i=0; i<5; i++){
        float LMx = facial5Pts[i].x;
        float LMy = facial5Pts[i].y;
        lmx_rot[i] = alpha * LMx + beta * LMy + (1 - alpha) * LM_nose_x / 2 - beta * LM_nose_y / 2;
        lmy_rot[i] = -beta * LMx + alpha * LMy + beta * LM_nose_x / 2 + (1 - alpha) * LM_nose_y / 2;
    }
    //# average distance between eyes and mouth
    //dXtot = (LMxr[1] - LMxr[0] + LMxr[4] - LMxr[3]) / 2
    //dYtot = (LMyr[3] - LMyr[0] + LMyr[4] - LMyr[1]) / 2
    float dXtot = (lmx_rot[1] - lmx_rot[0] + lmx_rot[4] - lmx_rot[3])/2;
    float dYtot = (lmy_rot[3] - lmy_rot[0] + lmy_rot[4] - lmy_rot[1])/2;
    //# average distance between nose and eyes
    //dXnose = (LMxr[1] - LMxr[2] + LMxr[4] - LMxr[2]) / 2
    //dYnose = (LMyr[3] - LMyr[2] + LMyr[4] - LMyr[2]) / 2
    float dXnose = (lmx_rot[1] - lmx_rot[2] + lmx_rot[4] - lmx_rot[2])/2;
    float dYnose = (lmy_rot[3] - lmy_rot[2] + lmy_rot[4] - lmy_rot[2])/2;

    //# relative rotation 0 degree is frontal 90 degree is profile
    //Xfrontal = (-90+90 / 0.5 * dXnose / dXtot) if dXtot != 0 else 0
    //Yfrontal = (-90+90 / 0.5 * dYnose / dYtot) if dYtot != 0 else 0
    float Xfrontal = 0;
    float Yfrontal = 0;
    if(dXtot != 0){
        Xfrontal = -90+90 / 0.5 * dXnose / dXtot;
    }
    if(dYtot != 0){
        Yfrontal = -90+90 / 0.5 * dYnose / dYtot;
    }
    float roll = angle * 180/PI_VALUE;
    float yaw = Xfrontal;
    float pitch = Yfrontal;
    std::vector<float> result = {roll, yaw, pitch};
    std::cout << "roll:" << result[0] << " yaw:" << result[1] << " pitch:" << result[2] << std::endl;
    return result;


}