双目已知图像坐标求空间坐标opencv

//opencv2.4.9 vs2012
#include 
#include 
#include 
 
using namespace std;
using namespace cv;
 
Point2f xyz2uv（Point3f worldPointfloat intrinsic[3][3]float translation[1][3]float rotation[3][3]）;
Point3f uv2xyz（Point2f uvLeftPoint2f uvRight）;
 
//图片对数量
int PicNum = 14;
 
//左相机内参数矩阵
float leftIntrinsic[3][3] = {4037.82450			 0		947.65449
									  0	3969.79038		455.48718
									  0			 0				1};
//左相机畸变系数
float leftDistortion[1][5] = {0.18962 -4.05566 -0.00510 0.02895 0};
//左相机旋转矩阵
float leftRotation[3][3] = {0.912333		-0.211508		 0.350590 
							0.023249		-0.828105		-0.560091 
							0.408789		 0.519140		-0.750590};
//左相机平移向量
float leftTranslation[1][3] = {-127.199992 28.190639 1471.356768};
 
//右相机内参数矩阵
float rightIntrinsic[3][3] = {3765.83307			 0		339.31958
										0	3808.08469		660.05543
										0			 0				1};
//右相机畸变系数
float rightDistortion[1][5] = {-0.24195 5.97763 -0.02057 -0.01429 0};
//右相机旋转矩阵
float rightRotation[3][3] = {-0.134947		 0.989568		-0.050442 
							  0.752355		 0.069205		-0.655113 
							 -0.644788		-0.126356		-0.753845};
//右相机平移向量
float rightTranslation[1][3] = {50.877397 -99.796492 1507.312197};
 
 
int main（）
{
	//已知空间坐标求成像坐标
	/*Point3f point（700220530）;
	cout<<“左相机中坐标：“<	cout<	cout<<“右相机中坐标：“<	cout<
	//已知左右相机成像坐标求空间坐标
	//Point2f l = xyz2uv（pointleftIntrinsicleftTranslationleftRotation）;
	//Point2f r = xyz2uv（pointrightIntrinsicrightTranslationrightRotation）;
	Point2f l =（2000 1000）;
	Point2f r =（10001000）;
	Point3f worldPoint;
	worldPoint = uv2xyz（lr）;
	cout<<“空间坐标为:“< 
	system（“pause“）;
 
	return 0;
}
 
 
//************************************
// Description: 根据左右相机中成像坐标求解空间坐标
// Method:    uv2xyz
// FullName:  uv2xyz
// Access:    public 
// Parameter: Point2f uvLeft
// Parameter: Point2f uvRight
// Returns:   cv::Point3f
// Author:    小白
// Date:      2017/01/10
// History:
//************************************
Point3f uv2xyz（Point2f uvLeftPoint2f uvRight）
{
	//  [u1]      |X|					  [u2]      |X|
	//Z*|v1| = Ml*|Y|					Z*|v2| = Mr*|Y|
	//  [ 1]      |Z|					  [ 1]      |Z|
	//			  |1|								|1|
	Mat mLeftRotation = Mat（33CV_32FleftRotation）;
	Mat mLeftTranslation = Mat（31CV_32FleftTranslation）;
	Mat mLeftRT = Mat（34CV_32F）;//左相机M矩阵
	hconcat（mLeftRotationmLeftTranslationmLeftRT）;
	Mat mLeftIntrinsic = Mat（33CV_32FleftIntrinsic）;
	Mat mLeftM = mLeftIntrinsic * mLeftRT;
	//cout<<“左相机M矩阵 = “< 
	Mat mRightRotation = Mat（33CV_32FrightRotation）;
	Mat mRightTranslation = Mat（31CV_32FrightTranslation）;
	Mat mRightRT = Mat（34CV_32F）;//右相机M矩阵
	hconcat（mRightRotationmRightTranslationmRightRT）;
	Mat mRightIntrinsic = Mat（3

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      80384  2018-11-12 17:44  xyz2uvDebugmyfirst_opencv.exe

     文件     419016  2018-11-12 17:44  xyz2uvDebugmyfirst_opencv.ilk

     文件    2108416  2018-11-12 17:44  xyz2uvDebugmyfirst_opencv.pdb

     文件      26563  2018-11-08 19:15  xyz2uvgirl.jpg

     文件       1322  2018-11-12 17:43  xyz2uvmyfirst_opencvDebugcl.command.1.tlog

     文件      19944  2018-11-12 17:43  xyz2uvmyfirst_opencvDebugCL.read.1.tlog

     文件        802  2018-11-12 17:43  xyz2uvmyfirst_opencvDebugCL.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink-cvtres.read.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink-cvtres.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink-rc.read.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink-rc.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.2920-cvtres.read.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.2920-cvtres.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.2920-rc.read.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.2920-rc.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.2920.read.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.2920.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.3688-cvtres.read.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.3688-cvtres.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.3688-rc.read.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.3688-rc.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.3688.read.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.3688.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.4136-cvtres.read.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.4136-cvtres.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.4136-rc.read.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.4136-rc.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.4136.read.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.4136.write.1.tlog

     文件          2  2018-11-12 17:44  xyz2uvmyfirst_opencvDebuglink.6344-cvtres.read.1.tlog

............此处省略51个文件信息