c++ - 如何使openCV的solvePNP头部姿势估计输出更准确

Question

我正在使用OpenCV Solventpnp进行实时头部姿势估计(x，y，z，俯仰，偏航，横滚)，我正在使用该信息向用户显示视频游戏中的正确 View ，因此，如果他以相同的 Angular 摇头视频游戏(例如Xbox Kinect)中会发生这种情况。
问题是，即使用户的头部保持恒定，输出的声音也会很嘈杂(显示用户在不动时头部移动)，或者当用户在移动头部时输出仍会不平滑并且坐标会在整个位置上跳跃。
我尝试添加更多的2d点(面部界标)和用于solvepnp甚至solvepnpransac的不同标志，但是没有任何效果。
这是我正在使用的代码

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;
}

Answer 1

您的问题是您对头部位置的测量很嘈杂。应对您的噪声测量的一种(最好是最好的)解决方案是卡尔曼滤波器。二维情况下没有间距，偏航和滚动的结果如下所示:https://www.youtube.com/watch?v=7Z_8-Ew7u2k。
正如您在视频中看到的那样，卡尔曼滤波器(蓝/红线)并不直接跟随噪声测量(绿点)，而是“随心所欲”。
但是，设计卡尔曼滤波器并非易事。首先，您应该处理一个简单的问题，以了解卡尔曼滤波的工作原理，然后再回到最初的问题。不幸的是，这就是我能为您做的一切。
编辑
我刚刚发现了一篇有关3维头部跟踪的有趣论文，其中没有俯仰，偏航和横摇。 Head Pose Estimation Based on Head Tracking and the Kalman Filter 。您会在此处找到卡尔曼滤波器所需的矩阵。
我认为应该在某个地方针对您的问题提供直接可行的解决方案。但是很遗憾，我必须承认我找不到它，对不起。