3D视觉(六)：PnP问题（pespective-n-point）

3D视觉(六)：PnP问题（pespective-n-point）

PnP问题，是指已知3D点(x, y, z)及其在相机上的投影(u，v)，求解相机位姿变换R、T。
投影方程可表示为：
这里K为相机内参矩阵，是已知的。我们要做的就是，从n对这样的2D-3D对应关系中，恢复出相机姿态变换，即旋转矩阵R和平移向量t。

一、算法原理

典型的PnP问题求解方式有很多种，例如P3P、直接线性变换DLT、EPnP、UPnP，另外还有非线性的Bundle Adjustment。下面简单推导一下直接线性变换DLT的原理。

考虑某个空间点P，它的齐次坐标为P=(X, Y, Z, 1).T，投影到图像中得到特征点x1=(u1, v1, 1).T。我们定义增广矩阵 [R|t] 为一个3*4矩阵，模型的数学表达式为：

用最后一行把s消去，得到两个约束：

为简化表示，定义T的行向量：

则上面两个约束可以转化成矩阵形式：

可以看到，每个特征点能提供两个关于旋转平移矩阵T的线性约束。假设一共拥有N个特征点，则可列出如下线性方程组：

旋转平移矩阵T一共有12维，因此最少通过6对匹配点即可实现矩阵T的线性求解，这种方法称为DLT。当匹配点大于6对时，也可以使用SVD等方法对超定方程求最小二乘解。

二、实验过程

利用人脸关键点2D图像坐标，和3D人脸模板关键点坐标，求解头部姿态。

人脸2D关键点图像坐标如下：

3D人脸模板关键点的3D坐标如下：

利用cv::solvePnP函数，求解位姿变换结果：

头部姿态可视化效果如下：

三、源码

#include 


using namespace std;
using namespace cv;


// reference: https://learnopencv.com/head-pose-estimation-using-opencv-and-dlib/


int main(int argc, char **argv)
{
    
    // Read input image
    cv::Mat im = cv::imread("../headPose.jpg");
    cout << "img cols and rows: " << im.cols << "  " << im.rows << endl;
    
    // 2D image points coordinate. If you change the image, you need to change vector
    std::vector<cv::Point2d> 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 coordinate.
    std::vector<cv::Point3d> 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 parameter matrix.
    // Approximate focal length.
    // Assuming no lens distortion.
    double focal_length = im.cols; 
    Point2d center = cv::Point2d(im.cols/2, im.rows/2);
    cv::Mat camera_matrix = (cv::Mat_<double>(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<double>::type); 
    
    cout << endl << "Approximate Camera Matrix: " << endl << camera_matrix << endl;
    cout << endl << "Approximate Distort Coeffs: " << endl << dist_coeffs.t() << endl << endl;
    
    // Output rotation and translation, Rotation is in axis-angle form and matrix form.
    cv::Mat rotation_vector; 
    cv::Mat rotation_matrix; 
    cv::Mat translation_vector;
    
    // Solve for pose.
    // The output result of cv::solvepnp function is a rotation vector, which needs to be converted into a matrix by Rodrigues formula.
    cv::solvePnP(model_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector);
    cv::Rodrigues(rotation_vector, rotation_matrix);
    cout << "Rotation Vector: " << endl << rotation_vector << endl << endl;
    cout << "Rotation Matrix: " << endl << rotation_matrix << endl << endl;
    cout << "Translation Vector:" << endl << translation_vector << endl << endl;
    
    // 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<Point3d> nose_end_point3D;
    vector<Point2d> 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);
    cout << "project results: " << nose_end_point2D << endl << endl;
    
    // Draw landmark points and projecting line
    for(int i=0; i < image_points.size(); i++)
    {
        circle(im, image_points[i], 3, Scalar(0, 255, 255), -1);
    }
    
    cv::line(im,image_points[0], nose_end_point2D[0], cv::Scalar(0, 0, 255), 3);
    
    // Display image.
    cv::imshow("im", im);
    cv::waitKey(0);
    cv::imwrite("../result.png", im);

}

四、项目链接

如果代码跑不通，或者想直接使用数据集，可以去下载项目链接：
https://blog.csdn.net/Twilight737