相机标定——图像坐标与世界坐标转换

环境：windows10+OpenCV3.1

前提：完成相机标定获得内参数矩阵和畸变系数

一、图像坐标与世界坐标转换方法简述

下图为OpenCV文档中图像坐标与世界坐标的关系图，(u,v)为图像坐标，(x,y,z)为世界坐标。

下式为图像坐标与世界坐标的转换公式，第一个矩阵为相机内参数矩阵，第二个矩阵为相机外参数矩阵，该式中图像坐标已知，相机内参数矩阵通过标定已获取，还欠缺s和相机外参数矩阵。

转换公式转换为如下左式，其中M为相机内参数矩阵，R为旋转矩阵，t为平移矩阵，Z为实际坐标系原点与相机坐标系原点在Z轴上的距离。左式进行转换后可得出右式，在右式中，当R，M，t和Z已知时，s为唯一变量，所以可以求得s。

外参数矩阵通过solvepnp函数获得。

bool solvePnP(InputArray objectPoints, 
	      InputArray imagePoints, 
	      InputArray cameraMatrix, 
	      InputArray distCoeffs, 
              OutputArray rvec, 
              OutputArray tvec, 
              bool useExtrinsicGuess=false, 
              int flags=ITERATIVE )

第一个参数objectPoints，输入世界坐标系中点的坐标；

第二个参数imagePoints，输入对应图像坐标系中点的坐标；

第三个参数cameraMatrix, 相机内参数矩阵；

第四个参数distCoeffs, 畸变系数；

第五个参数rvec, 旋转矩阵，需输入一个非空Mat；

第六个参数tvec, 平移矩阵，需输入一个非空Mat；

第七个参数useExtrinsicGuess, 默认为false，如果设置为true则输出输入的旋转矩阵和平移矩阵；

第八个参数flags，选择采用的算法；

注意：solvePnP的参数rvec和tvec应该都是double类型的

二、程序实现

（1）计算参数s和旋转平移矩阵，需要输入一系列的世界坐标系的点及其对应的图像坐标系的点。

        //输入参数
	Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 摄像机内参数矩阵 */
	Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
	double zConst = 0;//实际坐标系的距离
	
	//计算参数
	double s;
	Mat rotationMatrix = Mat (3, 3, DataType<double>::type);
	Mat tvec = Mat (1, 3, cv::DataType<double>::type);
	void calcParameters(vector<cv::Point2f> imagePoints, vector<cv::Point3f> objectPoints)
	{
		//计算旋转和平移矩阵
		Mat rvec(1, 3, cv::DataType<double>::type);
		cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
		cv::Rodrigues(rvec, rotationMatrix);
		cv::Mat imagePoint = cv::Mat::ones(3, 1, cv::DataType<double>::type); //u,v=1,1,1

		//计算参数S
		cv::Mat tempMat, tempMat2;
		tempMat = rotationMatrix.inv() * cameraMatrix.inv() * imagePoint;
		tempMat2 = rotationMatrix.inv() * tvec;
		s = zConst + tempMat2.at<double>(2, 0);
		s /= tempMat.at<double>(2, 0);
	}

（2）根据输入的图像坐标计算世界坐标。

	Point3f getWorldPoints(Point2f inPoints)
	{
		cv::Mat imagePoint = cv::Mat::ones(3, 1, cv::DataType<double>::type); //u,v,1
		imagePoint.at<double>(0, 0) = inPoints.x;
		imagePoint.at<double>(1, 0) = inPoints.y;
		Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * imagePoint - tvec);
		Point3f worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));
		return worldPoint;
	}

参考

http://answers.opencv.org/question/62779/image-coordinate-to-world-coordinate-opencv/

https://stackoverflow.com/questions/12299870/computing-x-y-coordinate-3d-from-image-point

相机标定——图像坐标与世界坐标转换

猜你喜欢