单目双视三维坐标确定

1、三维坐标计算

//左相机内参数矩阵
float leftIntrinsic[3][3] = { 1474.84107104330, -0.00321871344116866, 654.395068297042,
0, 1474.82163568531, 491.277561988847,
0, 0, 1 };

//左相机畸变系数
float leftDistortion[1][5] = { -0.135390683382819, 0.133022010409548, 0.000407393933022971,0.000191027564674195, 0 };
//左相机旋转矩阵
float leftRotation[3][3] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
//左相机平移向量
float leftTranslation[1][3] = { 0, 0, 0 };

//右相机内参数矩阵
float rightIntrinsic[3][3] = { 1474.84107104330, -0.00321871344116866, 654.395068297042,
0, 1474.82163568531, 491.277561988847,
0, 0, 1 };
//右相机畸变系数
float rightDistortion[1][5] = { -0.135390683382819, 0.133022010409548, 0.000407393933022971,0.000191027564674195, 0 };
//右相机旋转矩阵
float rightRotation[3][3] = {  0.9989, -0.0416, -0.0230,
0.0407, 0.9984, -0.0383,
0.0245, 0.0373, 0.9990};
//右相机平移向量
float rightTranslation[1][3] = { -77.4641, -5.3798, 16.4835};

//************************************
// Description: 根据左右相机中成像坐标求解空间坐标
// Method:    uv2xyz
// FullName:  uv2xyz
// Access:    public 
// Parameter: Point2f uvLeft
// Parameter: Point2f uvRight
// Returns:   cv::Point3f
// History:
//************************************
Point3f uv2xyz(Point2f uvLeft, Point2f uvRight)
{
	//  [u1]      |X|					  [u2]      |X|
	//Z*|v1| = Ml*|Y|					Z*|v2| = Mr*|Y|
	//  [ 1]