繼上篇雙目視覺下的空間座標計算：http://blog.csdn.net/qq_15947787/article/details/53366592

結合普通相機標定流程：http://blog.csdn.net/qq_15947787/article/details/51471535

單目標定測量這裡只是簡單的通過一個相機測量桌面（標定板平面為固定平面）上其他物體的尺寸。

影象上的點與世界座標系下的點與相機的內外參滿足這樣一個關係：

其中Zc是攝像機座標系下的一個座標分量，最後可以消去。

把上式的內外參乘進去，形成下式：

所以空間中的一個點（X,Y,Z）通過這個關係式可以找到唯一對應的影象上點座標（u,v）。而影象上點座標（u,v）通過這個關係式可以找到滿足等式的一個面（Z值不確定），兩個方程，三個未知量。

而雙目構成的系統，則有四個方程，三個未知量，通過最小二乘法可以得到（X,Y,Z）。

將上式寫成方程形式：

而我們要求固定平面下的（X,Y），Z為常數，可以修改上式為：

解出X,Y即可得到空間座標

————————————————————————————————

以上是簡單的求解過程，首先是通過opencv單目標定得到相機內外參，以及每張標定板圖的旋轉矩陣和平移向量，計算出重對映誤差，我這裡在重對映誤差最小的一張圖上建立空間座標系，由於標定板是水平放置在桌面上的，所以Z軸方向就是物體的高度方向，然後在物體上找到邊緣點，通過上面計算得到空間的座標。

單位都是mm，標定板一個格子是41mm，根據博文：http://blog.csdn.net/shenxiaolu1984/article/details/50165635

可以知道這張標定圖的世界座標系情況，但是我在試驗的時候，發現卻不符合。但是不管符合不符合世界座標系的情況，而且找不到這個世界座標系的位置，但是計算的距離都是非常準確的。

//計算空間座標
Point3f uv2xyz(Point2f uv)  
{  
	//棋盤格上兩個點測量
	//uv.x = 711;
	//uv.y = 917;  計算點（-48.9， -176.7）
	//uv.x = 630;
	//uv.y = 932;  計算點（-89.1， -184.6）
	//棋盤格大小41mm，測量結果為40.9689
	//誤差0.03mm

	//   |u|      |X|  
	//Zc*|v| = Ml*|Y| 
	//   |1|      |Z| 
	//            |1| 
	Mat camRT = Mat(3, 4, CV_64F);//相機M矩陣
	hconcat(camRotation, camTranslation, camRT);  
	Mat camM = camIntrinsic * camRT;

	//最小二乘法A矩陣  
	Mat A = Mat(2,2,CV_64F);  
	double aaa = uv.x * camM.at<double>(2,0);
	double bbb = camM.at<double>(0,0);
	A.at<double>(0,0) = uv.x * camM.at<double>(2,0) - camM.at<double>(0,0);  
	A.at<double>(0,1) = uv.x * camM.at<double>(2,1) - camM.at<double>(0,1);  

	A.at<double>(1,0) = uv.y * camM.at<double>(2,0) - camM.at<double>(1,0);  
	A.at<double>(1,1) = uv.y * camM.at<double>(2,1) - camM.at<double>(1,1);  

	//最小二乘法B矩陣  
	Mat B = Mat(2,1,CV_64F);  
	float Z = 200;//物體高度
	B.at<double>(0,0) = camM.at<double>(0,3) - uv.x * camM.at<double>(2,3) - Z * (uv.x * camM.at<double>(2,2) - camM.at<double>(0,2));  
	B.at<double>(1,0) = camM.at<double>(1,3) - uv.y * camM.at<double>(2,3) - Z * (uv.y * camM.at<double>(2,2) - camM.at<double>(1,2));  

	Mat XY = Mat(2,1,CV_64F);  
	//採用SVD最小二乘法求解XY 
	solve(A,B,XY,DECOMP_SVD);  

	//世界座標系中座標  
	Point3f world;  
	world.x = XY.at<double>(0,0);  
	world.y = XY.at<double>(1,0);  
	world.z = Z;  

	return world;  
}