第 5 节

相机标定与PNP算法（单目视觉测距）

作者

尚凡兴

协作者

韩文凯

董佳辉

发布于 2024/07/19更新于 2026/08/20

基础知识

pnp解算，指的是相机姿态估算，具体来讲就是通过给定几组特征点的三维世界坐标和二维图像坐标，和相机本身的内参(内参由相机标定得出)，解算出相机自身的坐标系此时相对表述三维世界的世界坐标系之间的变换关系，且这种变换关系一般由平移向量和旋转矩阵联合表述。pnp解算可以说是实现了单目测距的功能。

常见的坐标系与转换

常见的坐标系有:

世界坐标系: 目标在世界绝对坐标系中的位置，不随着相机位置的变化而变化。通常以mm或 m作为计量单位。
相机坐标系: 目标相对于相机的位置，会随着相机位置的变化而变化。通常以mm或m作为计量单位。
图像坐标系: 目标在图像中的位置，通常以像素作为计量单位。

$image\.png$

$屏幕截图 2023\-11\-18 182300\.png$

显然的，四个坐标系直接可以两两相互转换。所有的转换都可以用矩阵变换关系来表示，相机的标定过程实际上就是在4个坐标系转化的过程中求出相机的内参和外参的过程。

这4个坐标系分别是：世界坐标系（描述物体真实位置），相机坐标系（摄像头镜头中心），图像坐标系（图像传感器成像中心，图片中心，影布中心，单位mm），像素坐标系（图像左上角为原点，描述像素的位置，单位是多少行，多少列）。

世界坐标系 → 相机坐标系：等比例缩小，外加旋转平移，称之为刚体变换；求解摄像头外参（旋转和平移矩阵）；
相机坐标系 → 图像坐标系：相机能够将3D世界中的一个点转换为2D图像平面中的一个点，本质是经过了透视(projection)变换，所以需要乘上透视变换矩阵。同时，由于相机坐标系下和图像坐标系下的计量单位不同，还需要乘上内参矩阵进行变换；求解相机内参（摄像头矩阵和畸变系数）；
图像坐标系 → 像素坐标系：将图像坐标离散抽样；求解像素转化矩阵（可简单理解为原点从图片中心到左上角，单位毫米变行列）

$image\.png$

相机畸变

镜头畸变实际上是光学透镜固有的透视失真的总称，也就是因为透视原因造成的失真，这种失真对于照片的成像质量是非常不利的，毕竟摄影的目的是为了再现，而非夸张，但因为这是透镜的固有特性 (凸透镜汇聚光线、凹透镜发散光线)，所以无法消除，只能改善。高档镜头光学设计以及用料考究，利用镜片组的优化设计、选用高质量的光学玻璃(如萤石玻璃)来制造镜片，可以使透视变形降到很低的程度。但是完全消除畸变是不可能的，最高质量的镜头在极其严格的条件下测试，在镜头的边缘也会产生不同程度的变形和失真。

https://baike.baidu.com/item/%E9%95%9C%E5%A4%B4%E7%95%B8%E5%8F%98/5120871?fr=aladdin

摄像头参数： ⭐️

1）相机矩阵：包括焦距（fx，fy），光学中心（Cx，Cy），完全取决于相机本身，是相机的固有属性，只需要计算一次，可用矩阵表示如下：[fx, 0, Cx; 0, fy, Cy; 0,0,1]; 【本节相机标定需测出】

$image\.png$

s 是图像的 skew 参数，用于表示水平和垂直方向的像素缩放比例差异。通常情况下，s 很小，接近于 0。

2) 畸变系数：畸变数学模型的5个参数 D = （k1，k2， P1， P2， k3）；【本节相机标定需测出】

相机内参：“相机矩阵”+“畸变系数”统称为相机内参，在不考虑畸变的时候，相机矩阵也会被称为相机内参；

3）相机外参：通过旋转和平移变换将3D的坐标转换为相机2维的坐标，其中的旋转矩阵和平移矩阵就被称为相机的外参；描述的是将世界坐标系转换成相机坐标系的过程。【本节PnP算法需出】

张正友相机标定法

相机标定方法分类：

传统相机标定法、主动视觉相机标定法、相机自标定法。张正友相机标定法介于传统标定法和自标定法之间，但克服了传统标定法需要的高精度标定物的缺点。张氏标定法使用二维方格组成的标定板进行标定，采集标定板不同位姿图片，提取图片中角点像素坐标，通过单应矩阵计算出相机的内外参数初始值，利用非线性最小二乘法估计畸变系数，最后使用极大似然估计法优化参数。该方法操作简单，而且精度较高，可以满足大部分场合。

原理

懂得原理也没啥大用。opencv已经完全封装好了，如果仅仅想做个相机标定，可以跳过原理。第三小节的代码可以完全适用于队内的标定板，无需更改任何参数。

https://zhuanlan.zhihu.com/p/94244568

相机标定实现步骤(使用opencv && MATLAB)

手持标定板，用相机对其进行不同角度的拍摄，得到一组图像
对图像中的角点(Harris角点检测可以很容易检测出)检测，得到标定板角点的像素坐标值，根据已知的棋盘格大小和世界坐标系原点，计算得到标定板角点的物理坐标值
求解内参矩阵与外参矩阵。
求解畸变参数。
利用L-M (Levenberg-Marquardt)算法对上述参数进行优化，这一步是因为我们有很多张照，要综合多张照片的结果。

上述的步骤使用opencv非常容易实现。

相机标定板

$image\.png$

【这就是个标定板的图片，你可以把它缩放一下，要求你缩放之后能得到一个格子的尺寸】

【队内人员使用可以向我要，我缩放好了一个word可直接打印，单个格子的大小为25mm】

自己缩放的话一定要精准！！！！！！

截取图像C++代码⭐️

功能：拍摄一定数量的棋盘图照片，用于后续的相机标定。运行时，键盘按下 “k” 或者 “K” （注意是英文输入法下），截取一张图片；键盘按下 “q” 或者 “Q” ，退出程序运行，图像截取结束。截取的图片依次输出为 1.jpg、2.jpg、3.jpg…

#include <opencv2/opencv.hpp>
#include <string>
#include <iostream>

using namespace cv;
using namespace std;

int main()
{
        VideoCapture inputVideo(0);
        if (!inputVideo.isOpened())
        {
                cout << "Could not open the input video " << endl;
                return -1;
        }
        //////////////////////////////////////////////////////
        此处可以加上限制图片尺寸的实现
        //////////////////////////////////////////////////////

        Mat frame;
        string imgname;
        int f = 1;
        while (1)
        {
                inputVideo >> frame;
                if (frame.empty())
                        break;
                imshow("Camera", frame);
                char key = waitKey(1);
                if (key == 'q' || key == 'Q') // 退出运行
                        break;
                if (key == 'k' || key == 'K') // 截取图片
                {
                        cout << "frame:" << f << endl;
                        imgname = to_string(f++) + ".jpg";
                        imwrite(imgname, frame);//////这个地方可以加上你想保存的地址就不会默认保存到工程目录下了
                }
        }
        cout << "Finished writing" << endl;
        return 0;
}

截取完毕后输出图片在VS当前工程目录下：

$image\.png$

使用opencv代码标注：

队内标定代码(python)

import cv2
import numpy as np
import glob
#--------------------------------------------#
#设置寻找亚像素角点的参数
#采用的停止准则是最大循环次数30和最大误差容限0.001
#--------------------------------------------#
criteria = (cv2.TERM_CRITERIA_EPS +cv2.TERM_CRITERIA_MAX_ITER,30,0.001)#
#--------------------------------------------#
# 棋盘格模板规格
# 代表长宽角点的个数，注意棋盘格的顶点不被算入其中
#--------------------------------------------#
w=9
h=6
#--------------------------------------------#
#世界坐标系中的棋盘格点
#例如(0,0,θ)，(1,θ,θ)，(2,0,0)....,(8,5,0)
#去掉z坐标，记为二维矩阵
#--------------------------------------------#
objp = np.zeros((w*h,3),np.float32)
objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)
objp = objp*25 # 棋盘格每个ceil的边长25 mm
#--------------------------------------------#
# 储存棋盘格角点的世界坐标和图像坐标对
#--------------------------------------------#
objpoints = []# 在世界坐标系中的三维点
imgpoints = []#在图像平面的二维点

images = glob.glob('./chessImg/*.jpg') # 图像路径
cnt=0
for fname in images:
    img = cv2.imread(fname)
    # 获取画面中心点
    # 获取图像的长宽
    h1, w1 = img.shape[0]，img.shape[1]
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    u，v = img.shape[:2]#找到棋盘格角点
    ret，corners = cv2.findChessboardCorners(gray,(w,h),None)
    # 如果找到足够点对，将其存储起来
    if ret == True:
        print("cnt: \t",cnt)
        cnt = cnt +1
        # 在原角点的基础上寻找亚像素角点
        cv2.cornerSubPix(gray,corners, (11,11),(-1,-1),criteria)
        #追加进入世界三维点和平面二维点中
        objpoints.append(objp)
        imgpoints.append(corners)
        #将角点在图像上显示
        cv2.drawChessboardCorners(img,(w,h)，corners, ret)
        cv2.namedWindow('findCorners',cv2.WINDOW_NORMAL)
        cv2.resizeWindow('findCorners',640,480)
        cv2.imshow('findCorners',img)
        cv2.waitKey(200)
        if cnt> 100:
            # 太多的图片会导致计算缓慢，所以最大计算100张图片的、
            break
cv2.destroyAllWindows()
print('正在计算')
#标定
ret，mtx,dist,rvecs,tvecs = \
    cv2.calibrateCamera(objpoints,imgpoints, gray.shape[::-1]，None, None)

print("ret:",ret )
print("mtx:\n",mtx)        #内参数矩阵
print("dist畸变值:\n",dist)        # 畸变系数
print("rvecs旋转(向量)外参:\n",rvecs)# 旋转向量
print("tvecs平移(向量)外参:\n",tvecs)# 平移向量
newcameramtx,roi = cv2.get0ptimalNewCameraMatrix(mtx，dist，(u，v),0,(u，v))
print('newcameramtx外参',newcameramtx)

cv2.destoryAllWindows()

队内标定代码(C++)

首先新建文本文档 ”calibdata.txt”，将刚才输出的图片名写入文件 calibdata.txt 然后保存.

1.jpg
2.jpg
3.jpg
4.jpg
5.jpg
6.jpg
7.jpg
8.jpg
9.jpg
10.jpg

注意文件末尾不要留空行，否则会出现报错：

Assertion failed (dims <= 2 && step[0] > 0) in cv::Mat::locateROI

标定代码（C++）

#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <fstream>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace cv;
using namespace std;

void main()
{
        ifstream fin("calibdata.txt");             /* 标定所用图像文件的路径 */
        ofstream fout("caliberation_result.txt");  /* 保存标定结果的文件 */

                                                                                           // 读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
        int image_count = 0;  /* 图像数量 */
        Size image_size;      /* 图像的尺寸 */
        Size board_size = Size(9, 6);             /* 标定板上每行、列的角点数 */
        vector<Point2f> image_points_buf;         /* 缓存每幅图像上检测到的角点 */
        vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */
        string filename;      // 图片名
        vector<string> filenames;

        while (getline(fin, filename))
        {
                ++image_count;
                Mat imageInput = imread(filename);
                filenames.push_back(filename);

                // 读入第一张图片时获取图片大小
                if (image_count == 1)
                {
                        image_size.width = imageInput.cols;
                        image_size.height = imageInput.rows;
                }

                /* 提取角点 */
                if (0 == findChessboardCorners(imageInput, board_size, image_points_buf))
                {
                        cout << "can not find chessboard corners!\n";  // 找不到角点
                        exit(1);
                }
                else
                {
                        Mat view_gray;
                        cvtColor(imageInput, view_gray, CV_RGB2GRAY);  // 转灰度图

                        /* 亚像素精确化 */
                        // image_points_buf 初始的角点坐标向量，同时作为亚像素坐标位置的输出
                        // Size(5,5) 搜索窗口大小
                        // （-1，-1）表示没有死区
                        // TermCriteria 角点的迭代过程的终止条件, 可以为迭代次数和角点精度两者的组合
                        cornerSubPix(view_gray, image_points_buf, Size(5, 5), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

                        image_points_seq.push_back(image_points_buf);  // 保存亚像素角点

                        /* 在图像上显示角点位置 */
                        drawChessboardCorners(view_gray, board_size, image_points_buf, false); // 用于在图片中标记角点

                        imshow("Camera Calibration", view_gray);       // 显示图片

                        waitKey(500); //暂停0.5S
                }
        }
        int CornerNum = board_size.width * board_size.height;  // 每张图片上总的角点数

        //-------------以下是摄像机标定------------------

        /*棋盘三维信息*/
        Size square_size = Size(25, 25);         /* 实际测量得到的标定板上每个棋盘格的大小 */
        vector<vector<Point3f>> object_points;   /* 保存标定板上角点的三维坐标 */

        /*内外参数*/
        Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 摄像机内参数矩阵 */
        vector<int> point_counts;   // 每幅图像中角点的数量
        Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));       /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
        vector<Mat> tvecsMat;      /* 每幅图像的旋转向量 */
        vector<Mat> rvecsMat;      /* 每幅图像的平移向量 */

        /* 初始化标定板上角点的三维坐标 */
        int i, j, t;
        for (t = 0; t<image_count; t++)
        {
                vector<Point3f> tempPointSet;
                for (i = 0; i<board_size.height; i++)
                {
                        for (j = 0; j<board_size.width; j++)
                        {
                                Point3f realPoint;

                                /* 假设标定板放在世界坐标系中z=0的平面上 */
                                realPoint.x = i * square_size.width;
                                realPoint.y = j * square_size.height;
                                realPoint.z = 0;
                                tempPointSet.push_back(realPoint);
                        }
                }
                object_points.push_back(tempPointSet);
        }

        /* 初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 */
        for (i = 0; i<image_count; i++)
        {
                point_counts.push_back(board_size.width * board_size.height);
        }

        /* 开始标定 */
        // object_points 世界坐标系中的角点的三维坐标
        // image_points_seq 每一个内角点对应的图像坐标点
        // image_size 图像的像素尺寸大小
        // cameraMatrix 输出，内参矩阵
        // distCoeffs 输出，畸变系数
        // rvecsMat 输出，旋转向量
        // tvecsMat 输出，位移向量
        // 0 标定时所采用的算法
        calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);

        //------------------------标定完成------------------------------------

        // -------------------对标定结果进行评价------------------------------

        double total_err = 0.0;         /* 所有图像的平均误差的总和 */
        double err = 0.0;               /* 每幅图像的平均误差 */
        vector<Point2f> image_points2;  /* 保存重新计算得到的投影点 */
        fout << "每幅图像的标定误差：\n";

        for (i = 0; i<image_count; i++)
        {
                vector<Point3f> tempPointSet = object_points[i];

                /* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
                projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);

                /* 计算新的投影点和旧的投影点之间的误差*/
                vector<Point2f> tempImagePoint = image_points_seq[i];
                Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
                Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);

                for (int j = 0; j < tempImagePoint.size(); j++)
                {
                        image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
                        tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
                }
                err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
                total_err += err /= point_counts[i];
                fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
        }
        fout << "总体平均误差：" << total_err / image_count << "像素" << endl << endl;

        //-------------------------评价完成---------------------------------------------

        //-----------------------保存定标结果-------------------------------------------
        Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 保存每幅图像的旋转矩阵 */
        fout << "相机内参数矩阵：" << endl;
        fout << cameraMatrix << endl << endl;
        fout << "畸变系数：\n";
        fout << distCoeffs << endl << endl << endl;
        for (int i = 0; i<image_count; i++)
        {
                fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
                fout << tvecsMat[i] << endl;

                /* 将旋转向量转换为相对应的旋转矩阵 */
                Rodrigues(tvecsMat[i], rotation_matrix);
                fout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
                fout << rotation_matrix << endl;
                fout << "第" << i + 1 << "幅图像的平移向量：" << endl;
                fout << rvecsMat[i] << endl << endl;
        }
        fout << endl;

        //--------------------标定结果保存结束-------------------------------

        //----------------------显示定标结果--------------------------------

        Mat mapx = Mat(image_size, CV_32FC1);
        Mat mapy = Mat(image_size, CV_32FC1);
        Mat R = Mat::eye(3, 3, CV_32F);
        string imageFileName;
        std::stringstream StrStm;
        for (int i = 0; i != image_count; i++)
        {
                initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
                Mat imageSource = imread(filenames[i]);
                Mat newimage = imageSource.clone();
                remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
                StrStm.clear();
                imageFileName.clear();
                StrStm << i + 1;
                StrStm >> imageFileName;
                imageFileName += "_d.jpg";
                imwrite(imageFileName, newimage);
        }

        fin.close();
        fout.close();
        return;
}

角点检测效果：

$image\.png$

运行后VS当前工程目录下输出对应的标定后10张图片：

$image\.png$

并且在 caliberation_result.txt 中得到标定结果：

每幅图像的标定误差：
第1幅图像的平均误差：0.013196像素
第2幅图像的平均误差：0.0216657像素
第3幅图像的平均误差：0.0217481像素
第4幅图像的平均误差：0.0254981像素
第5幅图像的平均误差：0.0239333像素
第6幅图像的平均误差：0.017826像素
第7幅图像的平均误差：0.0232136像素
第8幅图像的平均误差：0.0231145像素
第9幅图像的平均误差：0.0470236像素
第10幅图像的平均误差：0.0154766像素
总体平均误差：0.0232695像素

相机内参数矩阵：
[505.1666120558573, 0, 310.1660811401983;
 0, 488.6105193422827, 249.9495562500046;
 0, 0, 1]

畸变系数：
[0.1311093141581356, -0.9654453502019078, 0.001118708198552768, -0.001126801336773416, 2.286059640823912]


第1幅图像的旋转向量：
[-37.91579109884059;
 -29.12008303891709;
 145.038522323583]
第1幅图像的旋转矩阵：
[-0.2578169368901203, -0.8295425114162431, -0.4953681951900302;
 0.9564626210175542, -0.2917064863318113, -0.009304860459955183;
 -0.1367833383450855, -0.4762001129621674, 0.8686332774917115]
第1幅图像的平移向量：
[2.171107396538949;
 2.104249697603109;
 0.3380832402207642]

第2幅图像的旋转向量：
[-29.36929014204156;
 -30.43767724876516;
 145.8627065313358]
第2幅图像的旋转矩阵：
[0.4953381063692663, -0.8244703490881966, -0.2736581879880385;
 0.8651086842696649, 0.4967904900977356, 0.06918217508622276;
 0.07891213328581717, -0.2710126425516693, 0.9593357195463131]
第2幅图像的平移向量：
[2.192414199084543;
 2.167143157075016;
 -0.02241130380397732]

第3幅图像的旋转向量：
[43.41090882554612;
 -29.90585262472088;
 130.4223820748588]
第3幅图像的旋转矩阵：
[-0.5974917709340989, -0.7122356798120717, 0.3684072747227861;
 0.4805682690426949, -0.6858362778564604, -0.5465187451179547;
 0.641917223990245, -0.1494956065529423, 0.7520594000250418]
第3幅图像的平移向量：
[0.06297801857808814;
 -2.912208294692916;
 -0.02236073998847246]

第4幅图像的旋转向量：
[22.3196598690343;
 -24.93883415944185;
 120.89012963209]
第4幅图像的旋转矩阵：
[0.9753046439544359, 0.2153626974780553, 0.04898734546682917;
 -0.2171670754976463, 0.9755052657394266, 0.03504194393081224;
 -0.04024068588767379, -0.04481500920033632, 0.9981845030602625]
第4幅图像的平移向量：
[-0.0839898065700953;
 -3.059111495460135;
 -0.651727632156657]
...

由此我们得到相机内参数矩阵和畸变系数，在后续的测距中将会用到（用相同的摄像头）

使用MATLAB软件标注:（高精度）

PnP测距算法

实现和原理

PnP(Perspective-n-Point)是求解3D到2D点的对应方法。它描述了当知道n个3D空间点及其位置，如何估计相机的位姿。如果两张图像中的一张特征点3D位置已知，那么至少需要3个点对(以及至少一个额外验证点验证结果)就可以计算相机的运动。

PnP问题的几何结构如下图所示，给定3D点的坐标以及对应2D点的坐标以及内参矩阵，求解相机的姿态。

$image\.png$

具体的公式推导参考

https://blog.csdn.net/weixin_44197735/article/details/124341862

opencv中不同pnp优化算法分析

在opencv中，官方专门为pnp解算分了一个cpp文件，其名称就叫做solvepnp.cpp文件另外由于 opencv是开源的，所以鼓励大家如果对opencv的某些函数或方法实现有钻研的想法，非常推荐直接看源码，不懂的地方百度一下，学习效率非常高。这里也很推荐一边看源码一边参照本文档的一些讲解与推荐的博客搭配食用。

pnp解算当前主流模式有Epnp，迭代法，P3P，DLS，Upnp等几种，这几种方法适用范围互相之间有重合也有区分。opencv对这几种方案都进行了实现，但遗憾的是如果查看了源码，就会发现其中Epnp、Upnp与DLS方法使用的一套相同的代码，都属于Epnp方法，这个是因为opencv2的版本里只实现了Epnp，迭代法，P3P，而后来在opencv3中才加入了Upnp与DLS方法，但并没有给出具体的实现，等待后续更新。而Epnp，迭代法，P3P三者之间简单的区别在下方的图表中列出

P3P（SOLVEPNP_P3P）	这个方法使用非常经典的Gao方法解P3P问题，求出4组可能的解，再通过对第四个点的重投影，返回重投影误差最小的点。	可以使用任意4个特征点求解，不要共面，特征点数量不为4时报错
迭代法（SOLVEPNP_ITERATIVE）	该方法基于Levenberg-Marquardt optimization迭代求解PNP问题，实质是迭代求出重投影误差最小的解，这个解显然不一定是正解。；实测该方法只有用4个共面的特征点时才能求出正确的解，使用5个特征点或4点非共面的特征点都得不到正确的位姿。	只能用4个共面的特征点来解位姿
EPnP（SOLVEPNP_EPNP）	该方法使用EfficientPNP方法求解问题。	对于N个特征点，只要N>3就能够求出正解。

EPnP

关于Epnp方法，opencv是专门准备了一个cpp文件的，但遗憾水平实在不足并不能读懂。其源码我也会附在文末，有兴趣有能力的同学可以尝试学习。这里仅对Epnp的算法略做阐述。

Epnp算法想要获得较精确的结果需要至少4组点，且这四组点不能共面。Epnp具有高精度、运算快的优点，这主要是由于它的主体思路是将给的4组点的三维、二维坐标均使用了一组假设的“控制点”来表述，且一般控制点为4个。具体来讲，就是将特征点的坐标看作是一组控制点的加权和，且总权重为1。Epnp的大致思路就是:选取适当的三维控制点——对特征点的三维坐标进行加权表示(可得到四个方程)——将控制点在图像坐标系下的二维坐标表示出来——对特征点的二维坐标进行加权表示(可得到四个方程)——对八个方程进行联立求解(此时方程中有八个未知数，分别是四个控制点的二维坐标(二个算一个)和四个权重)——对解得的结果进行数学处理，分离出旋转矩阵与平移向量。从上述过程中不难发现，四点中如果某点与其他三点确定的平面共面，则由该点所列出的两个方程一定可以被其他方程所表示，那么最终得到的方程组就会存在更多的解，从而降低了运算的精确性。解法的最根本依仗还是相机的成像原理。

Epnp的总体思路是比较容易理解的，但是其中涉及很多的线性代数知识，理解起来需要一定基础。鄙人发现了一篇讲解的非常详细易懂的文章《[PnP]PnP问题之EPnP解法》，辅助另一篇《深入EPnP算法》(这篇相对语言较为晦涩，建议先看前者)，对Epnp的学习掌握会有很大帮助。有兴趣的读者可以自行探索，鄙人也会在之后抽空学习。

P3P

p3p算法的原理也非常容易理解。它也同样服从于先求解一组3D-3D的点，然后求解相机外参。 p3p解法需要至少四个点，且不能共面。p3p算法将给定特征点其中不在同一直线上的三个点作为一个四面体的底面，通过对侧面三个三角形使用余弦定理描述三点连线形成的边来得到三个方程，联立化简求解后可以得到底边三条边长，进而求得四面体的侧边长，结合向量方向就可以得到相机坐标系下三个点的坐标。从此时就归结到3D-3D的外参求解问题了，不再过多赘述。关于 p3p的文档这里也推荐一篇《P3P位姿估计推导》，比较详细也容易理解。

容易发现，与Epnp不同的是，p3p算法是将给定的点(至少4个)中的不在同一直线上的3个点用于求解，剩余的点用于检验。

iteration

关于这种方法，遗憾的是不知道是不是方法的名称不全还是其他原因，我并没有能够找到一些能比较全面、详细地讲解solvepnp()中使用到的方法，其中非常粗略的讲到这个方法的文章有《PnP-迭代法》。此外在源码中发现这个方法下调用了cvFindExtrinsicCameraParams2函数，关于这个函数的原理在知乎上也有提到《opencv中cvFindExtrinsicCameraParams2函数的算法原理?》，建议有能力的同学可以尝试钻一钻这个函数及源码。

DLT

在查询众多资料过程中，还发现了一种方法叫DLT，直接线性变换。该方法详见《[PnP] PnP问题之DLT解法》，基本原理就是对最开始获得的图像坐标与世界坐标之间的变换关系进行化简，而后直接代入点的坐标得到一个方程组，对方程组进行求解可以得到一个相对较为精确的解，再对解的结果进行近似约束，最终可以得到此种方法下的最优解。

队内PnP代码剖析

PnP算法实现⭐️

#include <opencv2/opencv.hpp>
#include <math.h>

using namespace std;
using namespace cv;

int main(int argc, char **argv)
{
        Mat image = imread("14.jpg");

        // 2D 特征点像素坐标，这里是用PS找出，也可以用鼠标事件画出特征点
        vector<Point2d> image_points;
        image_points.push_back(Point2d(152, 92));
        image_points.push_back(Point2d(426, 94));
        image_points.push_back(Point2d(428, 394));
        image_points.push_back(Point2d(126, 380));

        // 画出四个特征点
        for (int i = 0; i < image_points.size(); i++)
        {
                circle(image, image_points[i], 3, Scalar(0, 0, 255), -1);
        }

        // 3D 特征点世界坐标，与像素坐标对应，单位是mm
        std::vector<Point3d> model_points;
        model_points.push_back(Point3d(-42.5f, -42.5f, 0)); // 左上角(-42.5mm,-42.5mm)
        model_points.push_back(Point3d(+42.5f, -42.5f, 0));
        model_points.push_back(Point3d(+42.5f, +42.5f, 0));
        model_points.push_back(Point3d(-42.5f, +42.5f, 0));
        //　注意世界坐标和像素坐标要一一对应

        // 相机内参矩阵和畸变系数均由相机标定结果得出
        // 相机内参矩阵
        Mat camera_matrix = (Mat_<double>(3, 3) << 505.1666120558573, 0, 310.1660811401983,
        0, 488.6105193422827, 249.9495562500046,
        0, 0, 1);
        // 相机畸变系数
        Mat dist_coeffs = (Mat_<double>(5, 1) << 0.1311093141581356, -0.9654453502019078,
        0.001118708198552768, -0.001126801336773416, 2.286059640823912);

        cout << "Camera Matrix " << endl << camera_matrix << endl << endl;
        // 旋转向量
        Mat rotation_vector;
        // 平移向量
        Mat translation_vector;

        // pnp求解
        solvePnP(model_points, image_points, camera_matrix, dist_coeffs, \
                rotation_vector, translation_vector, 0, CV_ITERATIVE);
        // 默认ITERATIVE方法，可尝试修改为EPNP（CV_EPNP）,P3P（CV_P3P）

        cout << "Rotation Vector " << endl << rotation_vector << endl << endl;
        cout << "Translation Vector" << endl << translation_vector << endl << endl;

        Mat Rvec;
        Mat_<float> Tvec;
        rotation_vector.convertTo(Rvec, CV_32F);  // 旋转向量转换格式
        translation_vector.convertTo(Tvec, CV_32F); // 平移向量转换格式

        Mat_<float> rotMat(3, 3);
        Rodrigues(Rvec, rotMat);
        // 旋转向量转成旋转矩阵
        cout << "rotMat" << endl << rotMat << endl << endl;

        Mat P_oc;
        P_oc = -rotMat.inv() * Tvec;//(核心公式)！！
        // 求解相机的世界坐标，得出p_oc的第三个元素即相机到物体的距离即深度信息，单位是mm
        cout << "P_oc" << endl << P_oc << endl;

        imshow("Output", image);
        waitKey(0);
}

注意相机内参数矩阵和畸变系数要根据自己的摄像头来调整；
2D 特征点像素坐标，可以用imageWatch找出，也可以用鼠标事件找出特征点；
3D 特征点世界坐标也要根据实际物体大小调整。

https://blog.csdn.net/cocoaqin/article/details/77848588

核心公式推导⭐️

相机坐标系到世界坐标系的变换：假设我们有相机的旋转矩阵 $R$ 和平移向量 $t$，可以将相机坐标系中的点 $P_c$ 转换为世界坐标系中的点 $P_w$。这个变换关系可以表示为：
$P_w=R\cdot P_c+t $
世界坐标系到相机坐标系的变换：现在假设我们有相机的旋转矩阵 $R$ 和平移向量 $t$的逆变换，我们可以将世界坐标系中的点 $P_w$ 转换为相机坐标系中的点 $P_c$。这个变换关系可以表示为： $P_c = R^ \cdot (P_w - t)$

$ P_c = R^ \cdot P_w - R^ \cdot t $

因为 $R $ 是一个正交矩阵（旋转矩阵的逆就是其转置），所以 $R^ = R^T $。所以可以写成：

$P_c = R^T \cdot P_w - R^T \cdot t $

$P_c = R^T \cdot P_w + T_c$ 这里 $T_c$ 就是 $-R^T\cdot t $

故：

$P_c=-R^T \cdot t

将点从相机坐标系投影到图像平面：最后，我们可以将相机坐标系中的点投影到图像平面上，得到图像上的点坐标。

$P_c=−rotMat.inv()×Tvec $

测试

图片中物体距摄像头大概14cm，边长8.5cm

$image\.png$

输出

ITERATIVE方法：

Camera Matrix
[505.1666120558573, 0, 310.1660811401983;
 0, 488.6105193422827, 249.9495562500046;
 0, 0, 1]

Rotation Vector
[-0.1157002831731034;
 0.1179963668031952;
 0.03141048973023804]

Translation Vector
[-9.106034353775875;
 -4.378822864319527;
 144.5117521867436]

rotMat
[0.99256271, -0.03807259, 0.11562786;
 0.024452539, 0.99283034, 0.1170042;
 -0.11925349, -0.11330661, 0.9863773]

P_oc
[26.378914;
 20.374874;
 -140.97787]

距离为 140.97787 mm

EPNP方法：

Camera Matrix
[505.1666120558573, 0, 310.1660811401983;
 0, 488.6105193422827, 249.9495562500046;
 0, 0, 1]

Rotation Vector
[-0.1616954415750413;
 0.06679582621203471;
 0.03007409518085349]

Translation Vector
[-8.402766835187963;
 -5.11616207912305;
 144.6472787556692]

rotMat
[0.99732399, -0.03530252, 0.064020529;
 0.024530273, 0.98651057, 0.16184933;
 -0.068870619, -0.15984578, 0.98473662]

P_oc
[18.467728;
 27.871765;
 -141.07347]

距离为 141.07347 mm

P3P方法：

Camera Matrix
[505.1666120558573, 0, 310.1660811401983;
 0, 488.6105193422827, 249.9495562500046;
 0, 0, 1]

Rotation Vector
[0.5013850510761655;
 0.606438260299431;
 0.03647540988139936]

Translation Vector
[-6.638223465259809;
 -3.076172335778562;
 146.0742567050607]

rotMat
[0.82479835, 0.11151069, 0.55432212;
 0.17714798, 0.8800413, -0.44061995;
 -0.53696018, 0.46161965, 0.70610273]

P_oc
[84.456192;
 -63.983349;
 -100.81914]

距离为 100.81914 mm

误差有点大…