【opencv3】详述PnP测距完整流程（附C++代码）

我们只要获得特征点的世界坐标（三维坐标）、2D坐标（像素坐标）、相机内参矩阵、相机畸变参数矩阵以上四个参数即可以解得相机与标志物之间的外参（旋转矩阵R、平移矩阵T），并以此求得相机的世界坐标（以标志物为世界坐标平面，且原点为标志物已知某一点）。

Ref： PnP 单目相机位姿估计（二）：solvePnP利用二维码求解相机世界坐标

相机内参矩阵、相机畸变参数矩阵可通过相机标定获取。

最后由旋转向量和平移矩阵求出深度信息： 先将旋转向量转化为旋转矩阵再转置，与平移矩阵相乘，得到的z坐标即深度信息。

P = − i n v e r s e ( R ) ∗ T P = -inverse (R) * T P=−inverse(R)∗T

1.为什么需要对摄像头进行标定？ ⭐️ 摄像头存在畸变，畸变可以拓宽视野，但会影响图像识别和测量的精度。 2.摄像头参数： ⭐️ 1）相机矩阵：包括焦距（fx，fy），光学中心（Cx，Cy），完全取决于相机本身，是相机的固有属性，只需要计算一次，可用矩阵表示如下：[fx, 0, Cx; 0, fy, cy; 0,0,1]; 2) 畸变系数：畸变数学模型的5个参数 D = （k1，k2， P1， P2， k3）； 3）相机内参：相机矩阵和畸变系数统称为相机内参，在不考虑畸变的时候，相机矩阵也会被称为相机内参； 4） 相机外参：通过旋转和平移变换将3D的坐标转换为相机2维的坐标，其中的旋转矩阵和平移矩阵就被称为相机的外参；描述的是将世界坐标系转换成相机坐标系的过程。 3.摄像头标定的流程： ⭐️ 相机的标定过程实际上就是在4个坐标系转化的过程中求出相机的内参和外参的过程。这4个坐标系分别是：世界坐标系（描述物体真实位置），相机坐标系（摄像头镜头中心），图像坐标系（图像传感器成像中心，图片中心，影布中心，单位mm），像素坐标系（图像左上角为原点，描述像素的位置，单位是多少行，多少列）。 （1）世界坐标系 → 相机坐标系：等比例缩小，外加旋转平移，称之为刚体变换；求解摄像头外参（旋转和平移矩阵）； （2）相机坐标系 → 图像坐标系：称为投影；求解相机内参（摄像头矩阵和畸变系数）； （3）图像坐标系 → 像素坐标系：将图像坐标离散抽样；求解像素转化矩阵（可简单理解为原点从图片中心到左上角，单位厘米变行列） 4.相机标定方法分类： ⭐️ 传统相机标定法、主动视觉相机标定法、相机自标定法。张正友相机标定法介于传统标定法和自标定法之间，但克服了传统标定法需要的高精度标定物的缺点。张氏标定法使用二维方格组成的标定板进行标定，采集标定板不同位姿图片，提取图片中角点像素坐标，通过单应矩阵计算出相机的内外参数初始值，利用非线性最小二乘法估计畸变系数，最后使用极大似然估计法优化参数。该方法操作简单，而且精度较高，可以满足大部分场合。

Ref：

（1）首先用摄像头拍摄一定数量的 标定板（棋盘图） 照片，大概10-20张左右，角度尽可能不同，拍照时要把完整的棋盘拍进去，不然找不到角点。

例如：

chessboard.png： （2）拍摄之后进行相机内外参标定，这里我采用张正友相机标定法进行标定。

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