C++ 如何使openCV解算PNP头部姿势估计输出更准确

我使用OpenCV solvepnp进行实时头部姿势估计（x、y、z、俯仰、偏航、滚动），我使用这些信息向用户显示视频游戏中的正确视图，因此，如果用户转动头部，视频游戏中也会发生同样的事情（如Xbox Kinect）。 问题是，即使当用户的头部保持不变时，am得到的输出也是嘈杂的（显示用户在不移动头部时移动了头部），或者当用户移动头部时，输出是不平滑的，坐标到处跳跃。 我尝试为solvepnp甚至solvepnpransac添加更多2d点（面部地标）和不同的标志，但没有任何效果。 这是我正在使用的代码

double getCordinates(double *listPtr, int size, int imCols, int imRows, int position)
{

    // 2D image points.
    std::vector<cv::Point2d> image_points;
    // Nose tip
    image_points.push_back(cv::Point2d(*(listPtr + 0), *(listPtr + 1)));
    // Chin
    image_points.push_back(cv::Point2d(*(listPtr + 2), *(listPtr + 3)));
    // Left eye left corner
    image_points.push_back(cv::Point2d(*(listPtr + 4), *(listPtr + 5)));
    // Right eye right corner
    image_points.push_back(cv::Point2d(*(listPtr + 6), *(listPtr + 7)));
    // Left Mouth corner
     image_points.push_back(cv::Point2d(*(listPtr + 8), *(listPtr + 9)));
    // Right mouth corner
     image_points.push_back(cv::Point2d(*(listPtr + 10), *(listPtr + 11)));


    // 3D model points.
    std::vector<cv::Point3d> model_points;
    // Nose tip
    model_points.push_back(cv::Point3d(0.0, 0.0, 0.0));
    // Chin
    model_points.push_back(cv::Point3d(0.0, -330.0, -65.0));
    // Left eye
    model_points.push_back(cv::Point3d(-225.0, 170.0, -135.0));
    // Right eye
    model_points.push_back(cv::Point3d(225.0, 170.0, -135.0));
    // Left Mouth
    model_points.push_back(cv::Point3d(-150.0, -150.0, -125.0));
    // Right mouth
    model_points.push_back(cv::Point3d(150.0, -150.0, -125.0));
 

    // Camera internals
    double focal_length = imCols;
    // Approximate focal length.
    cv::Point2d center = cv::Point2d(imCols / 2, imRows / 2);
    cv::Mat camera_matrix = (cv::Mat_<double>(3, 3) << focal_length, 0, center.x, 0, focal_length, center.y, 0, 0, 1);
    // Assuming no lens distortion
    cv::Mat dist_coeffs = cv::Mat::zeros(4, 1, cv::DataType<double>::type);
    // 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, false,0);


    // convert rotation vector to retotation matrix

    cv::Mat rotation_matrix;
    cv::Rodrigues(rotation_vector, rotation_matrix);
    // get eulerAngles
    cv::Vec3d eulerAngles;
    cv::Mat cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ;
    double *_r = rotation_matrix.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], 1};

    cv::decomposeProjectionMatrix(cv::Mat(3, 4, CV_64FC1, projMatrix),
                                  cameraMatrix,
                                  rotMatrix,
                                  transVect,
                                  rotMatrixX,
                                  rotMatrixY,
                                  rotMatrixZ,
                                  eulerAngles);

    double xTurn = eulerAngles[0];
    double yTurn = eulerAngles[1];
    double zTurn = eulerAngles[2];

    if (position == 1)
        return translation_vector.ptr<double>()[0];
    else if (position == 2)
        return translation_vector.ptr<double>()[1];
    else if (position == 3)
        return translation_vector.ptr<double>()[2];
    else if (position == 4)
        return xTurn;
    else if (position == 5)
        return yTurn;
    else if (position == 6)
        return zTurn;

    return xTurn;
}

double-getCordinates（double*listPtr、int-size、int-imCols、int-imRows、int-position）
{
//二维图像点。
std：：矢量图像_点；
//鼻尖
图像点。向后推（cv:：Point2d（*（listPtr+0），*（listPtr+1））；
//下巴
图像点。向后推（cv:：Point2d（*（listPtr+2），*（listPtr+3））；
//左眼左角
图像点。向后推（cv:：Point2d（*（listPtr+4），*（listPtr+5））；
//右眼右角
图像点。向后推（cv:：Point2d（*（listPtr+6），*（listPtr+7））；
//左口角
图像点。向后推（cv:：Point2d（*（listPtr+8），*（listPtr+9））；
//右口角
图像点。向后推（cv:：Point2d（*（listPtr+10），*（listPtr+11））；
//三维模型点。
std：：向量模型_点；
//鼻尖
模型点。向后推（cv:：Point3d（0.0,0.0,0.0））；
//下巴
模型点。向后推（cv:：Point3d（0.0，-330.0，-65.0））；
//左眼
模型点。向后推（cv:：Point3d（-225.0170.0，-135.0））；
//右眼
模型点。向后推（cv:：Point3d（225.0170.0，-135.0））；
//左嘴
模型点。向后推（cv:：Point3d（-150.0，-150.0，-125.0））；
//右嘴
模型点。向后推（cv:：Point3d（150.0，-150.0，-125.0））；
//摄像机内部
双焦距=imCols；
//近似焦距。
cv:：Point2d center=cv:：Point2d（imCols/2，imRows/2）；
cv:：Mat camera_matrix=（cv:：Mat_（3，3）您的问题是头部位置的测量值有噪声。处理噪声测量值的一个（可能是最好的）解决方案是卡尔曼滤波器。对于没有俯仰、偏航和横摇的二维情况，结果如下所示：

正如您在视频中看到的，卡尔曼滤波器（蓝/红线）不会直接跟踪噪声测量值（绿点），而是“根据您的需要”

然而，设计一个卡尔曼滤波器并不是一件小事。首先，你应该处理一个简单的问题来学习卡尔曼滤波是如何工作的，然后回到你原来的问题。不幸的是，这就是我能为你做的一切

编辑
我刚刚找到一篇有趣的论文，是关于没有俯仰、偏航和横摇的三维头部跟踪。你会在那里找到卡尔曼滤波器所需的矩阵

<>我认为应该有一个直接的工作解决方案来解决你的问题。但是，我必须承认我找不到它，抱歉。
一个通用的C++编程建议（对这个特殊问题没有帮助，但会提高代码质量）：使用双指针（双*）作为指向列表的指针被认为是不好的做法。请改用std:：vector。它的工作原理与您的std:：vector完全相同。好的，谢谢！谢谢您的回答，我将开始研究如何设计卡尔曼滤波器。有一个教程解释如何使用OpenCV和卡尔曼滤波器跟踪球。也许这不是最好的教程这段视频看起来很有趣。