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

概述

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

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

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

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

$$P=-inverse(R)\ast T$$

准备工作

1.相机标定简介

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：

• 机器视觉的相机标定到底是什么？
• （五）单目摄像头标定与畸变矫正（C++，opencv）

2.标定过程

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

例如：

• 这部分需要用自己的摄像头进行测试，如果没有别的摄像头可以用笔记本自带的摄像头。

VideoCapture inputVideo(0);

• openCV的安装目录中有标定用到的标定板（棋盘图 chessboard.png ）。
  我的 chessboard.png 路径：

E:\openCV3.4.3\opencv\sources\samples\data

chessboard.png：

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

3.截取图像C++代码

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

#include 
#include 
#include 

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当前工程目录下：

这里我拍摄了10张图片。

4.标定C++代码

Ref：
openCV—相机内外参标定
【笔记】Opencv张正友相机标定傻瓜教程

首先新建文本文档 ”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 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 

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(7, 7);             /* 标定板上每行、列的角点数 */
	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(10, 10);         /* 实际测量得到的标定板上每个棋盘格的大小 */
	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;
}

角点检测效果：

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

并且在 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]
... 

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

PnP测距

代码

本来想直接贴这部分的代码，毕竟前面的准备工作在网上搜一下有很多可以参考的，不过为了方便读者使用，还是简单介绍一下前面的部分。

版本：VS2015 + openCV3.4.3

#include 
#include 

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));
	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 特征点像素坐标，我这里是用PS找出，也可以用鼠标事件找出特征点；
• 3D 特征点世界坐标也要根据实际物体大小调整。

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

测试

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

输出

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

误差有点大…