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

最新推荐文章于 2025-05-29 16:17:37 发布

原创

最新推荐文章于 2025-05-29 16:17:37 发布 · 1.4w 阅读

标签

#3d #线性代数 #矩阵

本文详细介绍了PnP问题在3D视觉中的核心原理，包括直接线性变换DLT的推导，以及如何利用OpenCV解决实际的人脸姿态估计。通过人脸2D-3D对应点，展示了如何求解相机位姿参数。

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 <opencv2/opencv.hpp>


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

最低0.47元/天解锁文章