前一段時間，研究了下相機標定。關於opencv相機標定，包括標定完後，世界座標到 影象座標的轉換，以評估影象的標定誤差，這些網上有很多資源和原始碼。

可是，相機標定完之後，我們想要的是，知道了影象座標，想要得到它的世界座標，或者我們已知影象上兩個點之間的畫素距離，現在我們想知道兩個點之間的實際距離。

樓主在網上搜了很多資源，問了很多人，都沒有相關的程式碼，可以得到這樣的結論：opencv沒有提供現成的函式，滿足從影象座標到世界座標的轉換。

所以，我們最想要的這一步，是需要自己寫的。（如果我理解的不對，希望看到這篇博文的人，能夠批評指正）

相機標定的主要思路為：

一、獲取十幾張不同角度拍攝的圖片，角點檢測，得到每個角點的座標；

二、分別定義十幾張照片中，世界座標系下的角點座標，一般x、y為等間距，z為0 ；

三、開始標定，主要函式為calibrateCamera；

四、得到了相機內參和畸變係數，這是標定完後相機的屬性，還會得到外參，外參代表著每張圖片所在的平面；

五、opencv提供了世界座標到影象座標的轉換函式，主要用來評估標定的誤差；

六、我們最想要的，根據影象座標到世界座標的轉換，本質上就是矩陣的運算，需要自己寫；

下面貼出程式碼，開發環境opencv2.4.9+vs2013

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

using namespace std;
using namespace cv;

const int imageWidth = 1600;                             //攝像頭的解析度  
const int imageHeight = 1200;
const int boardWidth = 39;                               //橫向的角點數目  
const int boardHeight = 39;                              //縱向的角點資料  
const int boardCorner = boardWidth * boardHeight;       //總的角點資料  
const int frameNumber =7;                             //相機標定時需要採用的影象幀數  
const int squareSize = 10;                              //標定板黑白格子的大小 單位mm  
const Size boardSize = Size(boardWidth, boardHeight);   //  

Mat intrinsic;                                          //相機內參數  
Mat distortion_coeff;                                   //相機畸變引數  
vector<Mat> rvecs;                                        //旋轉向量  
vector<Mat> tvecs;                                        //平移向量  
vector<vector<Point2f>> corners;                        //各個影象找到的角點的集合 和objRealPoint 一一對應  
vector<vector<Point3f>> objRealPoint;                   //各副影象的角點的實際物理座標集合  


vector<Point2f> corner;                                   //某一副影象找到的角點  

Mat rgbImage, grayImage;

/*計算標定板上模組的實際物理座標*/
void calRealPoint(vector<vector<Point3f>>& obj, int boardwidth, int boardheight, int imgNumber, int squaresize)
{
	//  Mat imgpoint(boardheight, boardwidth, CV_32FC3,Scalar(0,0,0));  
	vector<Point3f> imgpoint;
	for (int rowIndex = 0; rowIndex < boardheight; rowIndex++)
	{
		for (int colIndex = 0; colIndex < boardwidth; colIndex++)
		{
			//  imgpoint.at<Vec3f>(rowIndex, colIndex) = Vec3f(rowIndex * squaresize, colIndex*squaresize, 0);  
			imgpoint.push_back(Point3f(colIndex * squaresize, rowIndex * squaresize, 0));
		}
	}
	for (int imgIndex = 0; imgIndex < imgNumber; imgIndex++)
	{
		obj.push_back(imgpoint);
	}
}

/*設定相機的初始引數 也可以不估計*/

void CalibrationEvaluate(void)//標定結束後進行評價
{
	double err=0;
	double total_err=0;
	//calibrateCamera(objRealPoint, corners, Size(imageWidth, imageHeight), intrinsic, distortion_coeff, rvecs, tvecs, 0);
	cout << "每幅影象的定標誤差：" << endl;
	for (int i = 0; i < corners.size(); i++)
	{
		vector<Point2f> image_points2;
		vector<Point3f> tempPointSet = objRealPoint[i];
		projectPoints(tempPointSet, rvecs[i], tvecs[i], intrinsic, distortion_coeff, image_points2);




		vector<Point2f> tempImagePoint = corners[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 + total_err;
		cout << "第" << i + 1 << "幅影象的平均誤差：" << err << "畫素" << endl;
	}
	cout << "總體平均誤差：" << total_err / (corners.size() + 1) << "畫素" << endl;
}

void guessCameraParam(void)
{
	/*分配記憶體*/
	intrinsic.create(3, 3, CV_64FC1);
	distortion_coeff.create(5, 1, CV_64FC1);

	/*
	fx 0 cx
	0 fy cy
	0 0  1
	*/
	intrinsic.at<double>(0, 0) = 256.8093262;   //fx         
	intrinsic.at<double>(0, 2) = 160.2826538;   //cx  
	intrinsic.at<double>(1, 1) = 254.7511139;   //fy  
	intrinsic.at<double>(1, 2) = 127.6264572;   //cy  

	intrinsic.at<double>(0, 1) = 0;
	intrinsic.at<double>(1, 0) = 0;
	intrinsic.at<double>(2, 0) = 0;
	intrinsic.at<double>(2, 1) = 0;
	intrinsic.at<double>(2, 2) = 1;

	/*
	k1 k2 p1 p2 p3
	*/
	distortion_coeff.at<double>(0, 0) = -0.193740;  //k1  
	distortion_coeff.at<double>(1, 0) = -0.378588;  //k2  
	distortion_coeff.at<double>(2, 0) = 0.028980;   //p1  
	distortion_coeff.at<double>(3, 0) = 0.008136;   //p2  
	distortion_coeff.at<double>(4, 0) = 0;          //p3  
}

void outputCameraParam(void)
{
	/*儲存資料*/
	//cvSave("cameraMatrix.xml", &intrinsic);  
	//cvSave("cameraDistoration.xml", &distortion_coeff);  
	//cvSave("rotatoVector.xml", &rvecs);  
	//cvSave("translationVector.xml", &tvecs);  
	/*輸出資料*/
	cout << "fx :" << intrinsic.at<double>(0, 0) << endl << "fy :" << intrinsic.at<double>(1, 1) << endl;
	cout << "cx :" << intrinsic.at<double>(0, 2) << endl << "cy :" << intrinsic.at<double>(1, 2) << endl;

	cout << "k1 :" << distortion_coeff.at<double>(0, 0) << endl;
	cout << "k2 :" << distortion_coeff.at<double>(1, 0) << endl;
	cout << "p1 :" << distortion_coeff.at<double>(2, 0) << endl;
	cout << "p2 :" << distortion_coeff.at<double>(3, 0) << endl;
	cout << "p3 :" << distortion_coeff.at<double>(4, 0) << endl;
}

//int _tmain(int argc, _TCHAR* argv[])
int main()
{
	Mat img;
	int goodFrameCount = 0;
	namedWindow("chessboard");
	cout << "按Q退出 ..." << endl;
	while (goodFrameCount < frameNumber)
	{
		
		char filename[100];
		sprintf_s(filename, "chao%d.bmp", goodFrameCount);
		//sprintf_s(filename, "chess%d.jpg", goodFrameCount);
		goodFrameCount++;
		
		rgbImage = imread(filename, 1);
		cvtColor(rgbImage, grayImage, CV_BGR2GRAY);
		imshow("Camera", grayImage);

		bool isFind = findChessboardCorners(rgbImage, boardSize, corner, 0);
		//bool isFind = findChessboardCorners(rgbImage, boardSize, corner, CV_CALIB_CB_NORMALIZE_IMAGE);
		if (isFind == true) //所有角點都被找到 說明這幅影象是可行的  
		{
			/*
			Size(5,5) 搜尋視窗的一半大小
			Size(-1,-1) 死區的一半尺寸
			TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1)迭代終止條件
			*/
			cornerSubPix(grayImage, corner, Size(5, 5), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1));
			drawChessboardCorners(rgbImage, boardSize, corner, isFind);
			imshow("chessboard", rgbImage);
			corners.push_back(corner);
			//string filename = "res\\image\\calibration";  
			//filename += goodFrameCount + ".jpg";  
			//cvSaveImage(filename.c_str(), &IplImage(rgbImage));       //把合格的圖片儲存起來  

			cout << "The image is good" << endl;
		}
		else
		{
			cout << "The image is bad please try again" << endl;
		}
		//  cout << "Press any key to continue..." << endl;  
		//  waitKey(0);  

		if (waitKey(10) == 'q')
		{
			break;
		}
		//  imshow("chessboard", rgbImage);  
	}

	/*
	影象採集完畢 接下來開始攝像頭的校正
	calibrateCamera()
	輸入引數 objectPoints  角點的實際物理座標
	imagePoints   角點的影象座標
	imageSize     影象的大小
	輸出引數
	cameraMatrix  相機的內參矩陣
	distCoeffs    相機的畸變引數
	rvecs         旋轉向量(外引數)
	tvecs         平移向量(外引數）
	*/

	/*設定實際初始引數 根據calibrateCamera來 如果flag = 0 也可以不進行設定*/
	guessCameraParam();
	cout << "guess successful" << endl;
	/*計算實際的校正點的三維座標*/
	calRealPoint(objRealPoint, boardWidth, boardHeight, frameNumber, squareSize);
	cout << "cal real successful" << endl;
	/*標定攝像頭*/
	calibrateCamera(objRealPoint, corners, Size(imageWidth, imageHeight), intrinsic, distortion_coeff, rvecs, tvecs, 0);
	cout << "calibration successful" << endl;
	/*儲存並輸出引數*/
	outputCameraParam();
	CalibrationEvaluate();
	cout << "out successful" << endl;

	/*顯示畸變校正效果*/
	Mat cImage;
	undistort(rgbImage, cImage, intrinsic, distortion_coeff);
	imshow("Corret Image", cImage);
	cout << "Correct Image" << endl;
	cout << "Wait for Key" << endl;

	waitKey(0);
	system("pause");
	return 0;
}
 

以上只貼出了部分程式碼，完整的C++程式碼，可到 Hust平凡之路相機標定工程 下載。