SolvePNP. Compensate for head rotation

edit

Hi all! I need to orientate detected face points to parallel with camera view.

I use the following code:

public void CalculateEulerAngles(int faceIndex)
{
    var objectPoints = new List<Point3f>
    {
       ...
    };

    var marks = Faces[faceIndex].Marks;
    var imagePoints = new List<Point2f>
    {
       ...
    };

    var focalLength = Image.Cols;
    var center = new Point2f(Image.Cols / 2f, Image.Rows / 2f);
    var cameraMatrix = new double[,] {{focalLength, 0, center.X}, {0, focalLength, center.Y}, {0, 0, 1}};

    double[] rvec, tvec;
    Cv2.SolvePnP(objectPoints, imagePoints, cameraMatrix, null, out rvec, out tvec);

    double[,] rvecRodrigues;
    Cv2.Rodrigues(rvec, out rvecRodrigues);

    double[] eulerAngles;
    double[,] camMatrix;
    GetEulerAngles(rvecRodrigues, out eulerAngles, out camMatrix);

    //

    Point2f[] points;
    double[,] jacobian;
    Cv2.ProjectPoints(objectPoints, rvec, tvec, cameraMatrix, null, out points, out jacobian);

    for (int i = 0; i < points.Length; i++)
        Cv2.Circle(Image, (int)points[i].X, (int)points[i].Y, 5, Scalar.Aqua);

    //
}

void GetEulerAngles(double[,] rvec, out double[] euler, out double[,] camMatrix)
{
    double[] transVect, eulerAngles;
    double[,] cameraMatrix,rotMatrix,rotMatrixX,rotMatrixY,rotMatrixZ;
    double[,] projMatrix =
    {
        {rvec[0, 0], rvec[0, 1], rvec[0, 2], 0},
        {rvec[1, 0], rvec[1, 1], rvec[1, 2], 0},
        {rvec[2, 0], rvec[2, 1], rvec[2, 2], 0}
    };

    Cv2.DecomposeProjectionMatrix(projMatrix, out cameraMatrix, out rotMatrix, out transVect,
                                    out rotMatrixX, out rotMatrixY, out rotMatrixZ, out eulerAngles);

    euler = eulerAngles;
    camMatrix = cameraMatrix;
}

But, I don't know how I can rotate points use calculated Euler angles to orientate parallel with camera view, so points always "flatted" to camera. Thanks guys