opencv立体相机标定 opencv相机标定原理

目录

• 1. 坐标系转换

• 1.1 各个坐标系的定义

• 1.1.1 像素坐标系
• 1.1.2 图像坐标系
• 1.1.3 相机坐标系
• 1.1.4 世界坐标系

• 1.2 相机的内参和外参

• 2. 图像畸变及畸变矫正

• 2.1 相机的畸变模型

• 2.1.1 径向畸变（参数：k1,k2,k3）
• 2.1.2 切向畸变 (参数：p1,p2)

• 2.2 畸变矫正

• 3. 相机标定代码解读

• 3.1 角点检测
• 3.2 标定参数
• 3.3 计算标定误差
• 3.4 畸变矫正
• 3.5 完整代码

• 4. 实际应用

在机器视觉领域，相机的标定是一个关键的环节，它决定了机器视觉系统能否有效的定位，能否有效的计算目标物。相机的标定基本上可以分为两种，第一种是相机的自标定；第二种是依赖于标定参照物的标定方法。前者是相机拍摄周围物体，通过数字图像处理的方法和相关的几何计算得到相机参数，但是这种方法标定的结果误差较大，不适合于高精度应用场合。后者是通过标定参照物，由相机成像，并通过数字图像处理的方法，以及后期的空间算术运算计算相机的内参和外参。这种方法标定的精度高，适用于对精度要求高的应用场合。本文主要写一写后者，至于前者，是一个研究的难点和热点，以后有空再写。

1. 坐标系转换

坐标系转换部分摘抄至这篇博文

三维空间中的点如何一一对应到图像上？
图像上的点又如何反映射到三维空间的点上？
这就涉及世界坐标到像素坐标的转换。

小孔成像的原理可以用下图来说明：

1.1 各个坐标系的定义

为了说明白，建议先介绍图像的坐标系，再逐步推广到世界坐标系，最后说明各个坐标系是如何变化的，从而给出相机的内参和外参。

1.1.1 像素坐标系

像素坐标就是像素在图像中的位置。一般像素坐标系的左上角的顶点就是原点，水平向右是u，垂直向下是v轴。

1.1.2 图像坐标系

在像素坐标系中，每个像素的坐标是用像素来表示的，然而，像素的表示方法却不能反应图像中物体的物理尺寸，因此，有必要将像素坐标转换为图像坐标。

将像素坐标系的原点平移到图像的中心，就定为图像坐标系的原点，图像坐标系的x轴与像素坐标系的u轴平行，方向相同，而图像坐标系的y轴与像素坐标系的v轴平行，方向相同。

在图中，假设图像中心的像素坐标是（u0,v0），相机中感光器件每个像素的物理尺寸是dx * dy，那么，图像坐标系的坐标（x,y）与像素坐标系的坐标（u,v）之间的关系可以表示为：

写成矩阵的形式为：

改写为齐次坐标的形式：

1.1.3 相机坐标系

相机坐标系是以相机的光轴作为Z轴，光线在相机光学系统的中心位置就是原点Oc（实际上就是透镜的中心）,相机坐标系的水平轴Xc与垂直轴Yc分别于图像坐标系的X轴和Y轴平行。在图中，相机坐标系的原点与图像坐标系的原点之间的距离OcOi之间的距离为f(也就是焦距)。

如果有一个物体成像到图像坐标系，则可以用下图来表示（B点是相机坐标系中物体的点坐标，P是图像坐标系中成像的坐标）：

可以知道相机坐标系与图像坐标系的关系为：

好了，那么为什么OcOi这个距离是焦距呢？为什么像素坐标系会在相机坐标系的前面呢，按道理说，相机坐标系是以相机的透镜中心为原点，那像素坐标系和图像坐标系为什么不在后面呢？这里做一个说明。

（1）本文的第一个图就是小孔成像的原理图。像平面就是成像的位置，这个是用户自己设定的，就是CCD传感器的位置，而焦平面就是镜头的焦距所在平面。当像平面刚好和焦平面重合时，此时所成的像是最清晰的。所以，这也就是为什么上面的公式中相机坐标系的原点到图像坐标系的原点的距离就是焦距。（实际上，由于物理条件的限制，像平面和焦平面是不可能完全重合的。）

（2）同样是本文的第一个图，我们可以看到像平面在光学系统的右面，而在推导相机标定的坐标系关系时，却认为光线先通过成像平面，再在相机坐标系上汇聚到一个点，实际上，如果用下图来说明，可能就更清楚一点。就是推导的时候，把像平面用虚拟像平面代替了。

1.1.4 世界坐标系

当对相机坐标系安装一定的参数，分别绕着X，Y，Z轴做平移和旋转后，就得到在世界坐标系中的坐标。

平移表示：

而对于旋转，可以采用如下的方法，给定一个基本旋转矩阵和基本矩阵

对于三坐标轴旋转，当绕着X轴旋转时，保持基本矩阵的第1列不变，有如下的旋转矩阵:

当绕着Y轴旋转时，保持基本矩阵的第2列不变，有如下的旋转矩阵:

当绕着Z轴旋转时，保持基本矩阵的第3列不变，有如下的旋转矩阵:

那么，整个相机坐标系到世界坐标系的变换公式为:

1.2 相机的内参和外参

通过前面的几个步骤，我们已经得到了各个坐标系之间的相互转换关系，进一步的就可以得到从像素坐标系到世界坐标系的变换关系：

2. 图像畸变及畸变矫正

2.1 相机的畸变模型

透镜自身形状对光线传播的影响——径向畸变
组装过程中，透镜和成像平面未完成平行——切向畸变

2.1.1 径向畸变（参数：k1,k2,k3）

在针孔模型中，直线投影至像素平面仍为直线；实际照片中，直线在图片中将变为曲线，且 越靠近边缘，该现象越明显

由于实际加工的透镜往往中心对称，因此形成的畸变通常径向对称

径向畸变分为两类： 桶形畸变, 枕形畸变

径向畸变的矫正公式如下

式中，（x,y）是理想的无畸变的坐标（图像坐标系），（xdr,ydr）是畸变后图像像素点的坐标，而且，

2.1.2 切向畸变 (参数：p1,p2)

切向畸变可以这样理解，当透镜与成像平面不行时，就产生了畸变，类似于透视变换。

切向畸变的矫正公式如下

2.2 畸变矫正

通过上面介绍的径向畸变和切向畸变模型，可以得到两个模型最终作用于真实图像后的矫正模型。

为什么是相加呢，可以这样理解，畸变就是距离的变化，而距离的变化反映到坐标值上就是加减的关系。

3. 相机标定代码解读

这部分内容使用了Gitee上的代码 https://gitee.com/chengwen12333/opencv-camera-calibration_retest 如图所示，在距离相机200mm的平台上，将标定板以不同的姿态摆放，拍摄18张图片，尽量使得所有标定板的位置能够充满相机视野。标定板中的角点个数为15*9，每个格子的实际边长为3mm。

3.1 角点检测

对18张图像进行角点检测，得到18 x 15 x 9个角点坐标

cv::findChessboardCorners(gray, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), image_points_buf, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);

得到285个角点坐标：

3.2 标定参数

calibrateCamera(objpoints, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);

进行相机标定，得到4个内参参数，5个畸变参数，18*6个外参参数（每张图像6个外参参数）

3.3 计算标定误差

根据每张图像的外参矩阵和世界坐标，计算其像素坐标，跟角点检测的像素坐标进行比较，计算标定误差。

projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);

3.4 畸变矫正

initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);

根据每张图像的内参矩阵和畸变矩阵，计算出映射矩阵mapx和mapy，使用mapx和mapy对原图 imageSource 进行映射，得到矫正后的图像 newimage 。

3.5 完整代码

#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;

#define calibration

int CHECKERBOARD[2]{ 15,19 };

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

	// 读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
	Size image_size;      /* 图像的尺寸 */
	Size board_size = Size(19, 15);           /* 标定板上每行、列的角点数 */
	vector<Point2f> image_points_buf;         /* 缓存每幅图像上检测到的角点 */
	vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */
	string filename;      // 图片名
	vector<string> filenames;
	std::string modelPath = "./image/model/*.bmp";
	std::vector<cv::String> images;
	cv::glob(modelPath, images);
	cv::Mat frame, gray;
	double Kernelsize_cali[2]{3.0, 3.0};      /* 标定板的宽度: 我们使用的是3.0mm * 3.0mm */

	std::vector<std::vector<cv::Point3f> > objpoints;  /* 真实的坐标点，通过横轴第一个点相对应起来的 */
	std::vector<cv::Point3f> objp;        
	
	for (int i{ 0 }; i < CHECKERBOARD[1]; i++)
	{
		for (int j{ 0 }; j < CHECKERBOARD[0]; j++)
		{
			objp.push_back(cv::Point3f(j * Kernelsize_cali[0], i * Kernelsize_cali[1], 0));
			cout << "(x,y,z):(" << j << "," << i << "," << 0 << ")" << endl;
		}
		cout << "one success!" << endl;
	}

	for (int i{ 0 }; i < images.size(); i++)
	{
		frame = cv::imread(images[i], 1);
		if (frame.empty()){ continue; }
		cout << "the current image is " << i << "th" << endl;
		cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);

		//交点提取
		bool success = cv::findChessboardCorners(gray, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), image_points_buf, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
		if (success)
		{
			// 终止条件达到 阈值 或 最大次数 时终止
			cv::TermCriteria criteria(TermCriteria::EPS | TermCriteria::Type::MAX_ITER, 30, 0.001);
			
			// 为给定的二维点细化像素坐标
			cv::cornerSubPix(gray, image_points_buf, cv::Size(11, 11), cv::Size(-1, -1), criteria);

			// 在棋盘上显示检测到的角点
			cv::drawChessboardCorners(frame, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), image_points_buf, success);

			objpoints.push_back(objp);
			image_points_seq.push_back(image_points_buf);
		}
		else
		{
			cout << "some errors happened in cornerSubPix" << endl;
		}
	}

	image_size = cv::Size(frame.cols, frame.rows);
	int CornerNum = board_size.width * board_size.height;  // 每张图片上总的角点数

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

	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;       /* 每幅图像的平移向量 */

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

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

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

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

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

	for (int i = 0; i<images.size(); i++)
	{
		vector<Point3f> tempPointSet = objpoints[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);
		}

		//求真实角点像素和重投影像素坐标两者之间的L2范数
		err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
		total_err += err /= point_counts[i];
		fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
	}
	fout << "总体平均误差：" << total_err / images.size() << "像素" << 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<images.size(); 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 <images.size(); i++)
	{
		initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
		Mat imageSource = imread(images[i]);
		Mat newimage = imageSource.clone();
		remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
		StrStm.clear();
		imageFileName.clear();
		StrStm << i + 1;
		StrStm >> imageFileName;
		imageFileName += "./images/result/_d.jpg";
		imwrite(imageFileName, newimage);
	}

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

4. 实际应用

需要进行相机标定的场景非常多，但主要目的都是希望根据目标点的像素位置映射出其在世界坐标系中的真实位置。本文从距离测量的方面对相机标定的应用进行说明。

如图所示，机械臂在抓取工件之前，需要知道工件的 x, y 坐标位置，由于工件在z坐标的位置可以认为始终保持不变，因此可以设为固定参数。此外，还需要知道相机镜头与工件之间的距离H(下图中为Zc)和相机内参矩阵，结合 x,y 平面标定的相机外参矩阵，即可计算出工件的世界坐标。

计算公式如下：

使用opencv函数进行相机标定，结果如下图所示。将矩阵参数带入上图，即可算出像素点的世界坐标。

我们以第一张标定图像为例，将其前两个角点坐标映射到世界坐标，计算其实际距离。

p1,p2之间的距离为63.6649, w1,w2之间的距离为0.2036。相关代码如下：

//------------------像素坐标映射到世界坐标------------------

vector<Point2f> image_points;
Mat pixelImage = imread("./images/result/1_d.jpg", 0);

cv::findChessboardCorners(pixelImage, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), image_points, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);

Point2f p1 = image_points[0];
Point2f p2 = image_points[1];

double z = 200;       /* 物距: 200mm */
cv::Mat i1 = (cv::Mat_<float>(3, 1) << image_points[0].x, image_points[0].y, 1);       /* 像素坐标i1 */
cv::Mat i2 = (cv::Mat_<float>(3, 1) << image_points[1].x, image_points[1].y, 1);
cv::Mat K1, K2, K3, w1, w2;
cv::Mat M1 = (cv::Mat_<float>(4, 4) << -0.6061, 0.7425, 0.2854, 0.0346,       
									   -0.7928, -0.5932, -0.1403, 0.0217,
									   0.0652, -0.3113, 0.9481, -0.0067,
									   0, 0, 0, 1);
cv::invert(M1, K1, DECOMP_LU);

cv::Mat M3 = (cv::Mat_<float>(3, 3) << 7816.12, 0, 1891.21,     
									   0, 7814.15, 1318.65,
									   0, 0, 1);
cv::invert(M3, K3, DECOMP_LU);

cv::Mat M2 = (cv::Mat_<float>(3, 3) << 8, 0, 0,     /* f = 8mm */
	0, 8, 0,
	0, 0, 1);
cv::Mat zero = (cv::Mat_<float>(1, 3) << 0, 0, 0);
cv::invert(M2, K2, DECOMP_LU);
cv::vconcat(K2, zero, K2);

w1 = z * K1 * K2 * K3 * i1;         /* 从 像素坐标i1 映射到 世界坐标w1 */
w2 = z * K1 * K2 * K3 * i2;

double pixelLen = norm(i1, i2, NORM_L2);        /* 像素距离 */
double realLen = norm(w1, w2, NORM_L2);         /* 实际距离 */