# 【opencv】双目视觉下空间坐标计算/双目测距 6/13更新

（1）

Mat jiaozheng( Mat image )
{
Size image_size = image.size();
float intrinsic[3][3] = {589.2526583947847,0,321.8607532099886,0,585.7784771038199,251.0338528599469,0,0,1};
float distortion[1][5] = {-0.5284205687061442, 0.3373615384253201, -0.002133029981628697, 0.001511983002864886, -0.1598661778309496};
Mat intrinsic_matrix = Mat(3,3,CV_32FC1,intrinsic);
Mat distortion_coeffs = Mat(1,5,CV_32FC1,distortion);
Mat R = Mat::eye(3,3,CV_32F);
Mat mapx = Mat(image_size,CV_32FC1);
Mat mapy = Mat(image_size,CV_32FC1);
initUndistortRectifyMap(intrinsic_matrix,distortion_coeffs,R,intrinsic_matrix,image_size,CV_32FC1,mapx,mapy);
Mat t = image.clone();
cv::remap( image, t, mapx, mapy, INTER_LINEAR);
return t;
}


（2）

（3）

（4）

## （2）像面坐标系——>世界坐标系

（5）          （6）

（7）

（8）

（9）

（10）

//opencv2.4.9 vs2012
#include <opencv2\opencv.hpp>
#include <fstream>
#include <iostream>

using namespace std;
using namespace cv;

Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3]);
Point3f uv2xyz(Point2f uvLeft,Point2f 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(700,220,530);
cout<<"左相机中坐标："<<endl;
cout<<xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation)<<endl;
cout<<"右相机中坐标："<<endl;
cout<<xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation)<<endl;

//已知左右相机成像坐标求空间坐标
Point2f l = xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation);
Point2f r = xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation);
Point3f worldPoint;
worldPoint = uv2xyz(l,r);
cout<<"空间坐标为:"<<endl<<uv2xyz(l,r)<<endl;

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 uvLeft,Point2f uvRight)
{
//  [u1]      |X|					  [u2]      |X|
//Z*|v1| = Ml*|Y|					Z*|v2| = Mr*|Y|
//  [ 1]      |Z|					  [ 1]      |Z|
//			  |1|								|1|
Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);
Mat mLeftTranslation = Mat(3,1,CV_32F,leftTranslation);
Mat mLeftRT = Mat(3,4,CV_32F);//左相机M矩阵
hconcat(mLeftRotation,mLeftTranslation,mLeftRT);
Mat mLeftIntrinsic = Mat(3,3,CV_32F,leftIntrinsic);
Mat mLeftM = mLeftIntrinsic * mLeftRT;
//cout<<"左相机M矩阵 = "<<endl<<mLeftM<<endl;

Mat mRightRotation = Mat(3,3,CV_32F,rightRotation);
Mat mRightTranslation = Mat(3,1,CV_32F,rightTranslation);
Mat mRightRT = Mat(3,4,CV_32F);//右相机M矩阵
hconcat(mRightRotation,mRightTranslation,mRightRT);
Mat mRightIntrinsic = Mat(3,3,CV_32F,rightIntrinsic);
Mat mRightM = mRightIntrinsic * mRightRT;
//cout<<"右相机M矩阵 = "<<endl<<mRightM<<endl;

//最小二乘法A矩阵
Mat A = Mat(4,3,CV_32F);
A.at<float>(0,0) = uvLeft.x * mLeftM.at<float>(2,0) - mLeftM.at<float>(0,0);
A.at<float>(0,1) = uvLeft.x * mLeftM.at<float>(2,1) - mLeftM.at<float>(0,1);
A.at<float>(0,2) = uvLeft.x * mLeftM.at<float>(2,2) - mLeftM.at<float>(0,2);

A.at<float>(1,0) = uvLeft.y * mLeftM.at<float>(2,0) - mLeftM.at<float>(1,0);
A.at<float>(1,1) = uvLeft.y * mLeftM.at<float>(2,1) - mLeftM.at<float>(1,1);
A.at<float>(1,2) = uvLeft.y * mLeftM.at<float>(2,2) - mLeftM.at<float>(1,2);

A.at<float>(2,0) = uvRight.x * mRightM.at<float>(2,0) - mRightM.at<float>(0,0);
A.at<float>(2,1) = uvRight.x * mRightM.at<float>(2,1) - mRightM.at<float>(0,1);
A.at<float>(2,2) = uvRight.x * mRightM.at<float>(2,2) - mRightM.at<float>(0,2);

A.at<float>(3,0) = uvRight.y * mRightM.at<float>(2,0) - mRightM.at<float>(1,0);
A.at<float>(3,1) = uvRight.y * mRightM.at<float>(2,1) - mRightM.at<float>(1,1);
A.at<float>(3,2) = uvRight.y * mRightM.at<float>(2,2) - mRightM.at<float>(1,2);

//最小二乘法B矩阵
Mat B = Mat(4,1,CV_32F);
B.at<float>(0,0) = mLeftM.at<float>(0,3) - uvLeft.x * mLeftM.at<float>(2,3);
B.at<float>(1,0) = mLeftM.at<float>(1,3) - uvLeft.y * mLeftM.at<float>(2,3);
B.at<float>(2,0) = mRightM.at<float>(0,3) - uvRight.x * mRightM.at<float>(2,3);
B.at<float>(3,0) = mRightM.at<float>(1,3) - uvRight.y * mRightM.at<float>(2,3);

Mat XYZ = Mat(3,1,CV_32F);
//采用SVD最小二乘法求解XYZ
solve(A,B,XYZ,DECOMP_SVD);

//cout<<"空间坐标为 = "<<endl<<XYZ<<endl;

//世界坐标系中坐标
Point3f world;
world.x = XYZ.at<float>(0,0);
world.y = XYZ.at<float>(1,0);
world.z = XYZ.at<float>(2,0);

return world;
}

//************************************
// Description: 将世界坐标系中的点投影到左右相机成像坐标系中
// Method:    xyz2uv
// FullName:  xyz2uv
// Access:    public
// Parameter: Point3f worldPoint
// Parameter: float intrinsic[3][3]
// Parameter: float translation[1][3]
// Parameter: float rotation[3][3]
// Returns:   cv::Point2f
// Author:    小白
// Date:      2017/01/10
// History:
//************************************
Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3])
{
//    [fx s x0]							[Xc]		[Xw]		[u]	  1		[Xc]
//K = |0 fy y0|       TEMP = [R T]		|Yc| = TEMP*|Yw|		| | = —*K *|Yc|
//    [ 0 0 1 ]							[Zc]		|Zw|		[v]	  Zc	[Zc]
//													[1 ]
Point3f c;
c.x = rotation[0][0]*worldPoint.x + rotation[0][1]*worldPoint.y + rotation[0][2]*worldPoint.z + translation[0][0]*1;
c.y = rotation[1][0]*worldPoint.x + rotation[1][1]*worldPoint.y + rotation[1][2]*worldPoint.z + translation[0][1]*1;
c.z = rotation[2][0]*worldPoint.x + rotation[2][1]*worldPoint.y + rotation[2][2]*worldPoint.z + translation[0][2]*1;

Point2f uv;
uv.x = (intrinsic[0][0]*c.x + intrinsic[0][1]*c.y + intrinsic[0][2]*c.z)/c.z;
uv.y = (intrinsic[1][0]*c.x + intrinsic[1][1]*c.y + intrinsic[1][2]*c.z)/c.z;

return uv;
}



2017/5/26补充

matlab或者opencv标定完都是在左相机上建立世界坐标系，于是上面代码对应的改为：

//左相机旋转矩阵
float leftRotation[3][3] = {1,0,0,  0,1,0,  0,0,1 };
//左相机平移向量
float leftTranslation[1][3] = {0,0,0};

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

像面坐标系——>世界坐标系还有一种模型是光轴平行模型

（11）

（12）

（13）

Point p;
p.x =294,p.y=189;
cout<<p<< "in world coordinate: " << xyz.at<Vec3f>(p)*16 <<endl;


dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);

/******************************/
/*        立体匹配和测距        */
/******************************/

#include <opencv2/opencv.hpp>
#include <iostream>

using namespace std;
using namespace cv;

const int imageWidth = 640;                             //摄像头的分辨率
const int imageHeight = 480;
Size imageSize = Size(imageWidth, imageHeight);

Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;

Rect validROIL;//图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域
Rect validROIR;

Mat mapLx, mapLy, mapRx, mapRy;     //映射表
Mat Rl, Rr, Pl, Pr, Q;              //校正旋转矩阵R，投影矩阵P 重投影矩阵Q
Mat xyz;              //三维坐标

Point origin;         //鼠标按下的起始点
Rect selection;      //定义矩形选框
bool selectObject = false;    //是否选择对象

int blockSize = 0, uniquenessRatio =0, numDisparities=0;
Ptr<StereoBM> bm = StereoBM::create(16, 9);

/*

fx 0 cx
0 fy cy
0 0  1
*/
Mat cameraMatrixL = (Mat_<double>(3, 3) << 836.771593170594,0,319.970748854743,
0,839.416501863912,228.788913693256,
0, 0, 1);
Mat distCoeffL = (Mat_<double>(5, 1) << 0, 0, 0, 0, 0);

Mat cameraMatrixR = (Mat_<double>(3, 3) << 838.101721655709,0,319.647150557935,
0,840.636812165056,250.655818405938,
0, 0, 1);
Mat distCoeffR = (Mat_<double>(5, 1) << 0, 0, 0, 0, 0);

Mat T = (Mat_<double>(3, 1) << -39.7389449993974,0.0740619639178984,0.411914303245886);//T平移向量
Mat rec = (Mat_<double>(3, 1) << -0.00306, -0.03207, 0.00206);//rec旋转向量
Mat R = (Mat_<double>(3, 3) << 0.999957725513956,-0.00103511880221423,0.00913650447492805,
0.00114462826834523,0.999927476064641,-0.0119888463633882,
-0.00912343197938050,0.0119987974423658,0.999886389470751);//R 旋转矩阵

/*****立体匹配*****/
void stereo_match(int,void*)
{
bm->setROI1(validROIL);
bm->setROI2(validROIR);
bm->setPreFilterCap(31);
bm->setMinDisparity(0);  //最小视差，默认值为0, 可以是负值，int型
bm->setNumDisparities(numDisparities*16+16);//视差窗口，即最大视差值与最小视差值之差,窗口大小必须是16的整数倍，int型
bm->setTextureThreshold(10);
bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配
bm->setSpeckleWindowSize(100);
bm->setSpeckleRange(32);
bm->setDisp12MaxDiff(-1);
Mat disp, disp8;
bm->compute(rectifyImageL, rectifyImageR, disp);//输入图像必须为灰度图
disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式
reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
xyz = xyz * 16;
imshow("disparity", disp8);
}

/*****描述：鼠标操作回调*****/
static void onMouse(int event, int x, int y, int, void*)
{
if (selectObject)
{
selection.x = MIN(x, origin.x);
selection.y = MIN(y, origin.y);
selection.width = std::abs(x - origin.x);
selection.height = std::abs(y - origin.y);
}

switch (event)
{
case EVENT_LBUTTONDOWN:   //鼠标左按钮按下的事件
origin = Point(x, y);
selection = Rect(x, y, 0, 0);
selectObject = true;
cout << origin <<"in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
break;
case EVENT_LBUTTONUP:    //鼠标左按钮释放的事件
selectObject = false;
if (selection.width > 0 && selection.height > 0)
break;
}
}

/*****主函数*****/
int main()
{
/*
立体校正
*/
//Rodrigues(rec, R); //Rodrigues变换
stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
0, imageSize, &validROIL, &validROIR);
initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);

/*
读取图片
*/
cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);
cvtColor(rgbImageR, grayImageR, CV_BGR2GRAY);

imshow("ImageL Before Rectify", grayImageL);
imshow("ImageR Before Rectify", grayImageR);

/*
经过remap之后，左右相机的图像已经共面并且行对准了
*/
remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

/*
把校正结果显示出来
*/
Mat rgbRectifyImageL, rgbRectifyImageR;
cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR);  //伪彩色图
cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR);

//单独显示
//rectangle(rgbRectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);
//rectangle(rgbRectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);
imshow("ImageL After Rectify", rgbRectifyImageL);
imshow("ImageR After Rectify", rgbRectifyImageR);

//显示在同一张图上
Mat canvas;
double sf;
int w, h;
sf = 600. / MAX(imageSize.width, imageSize.height);
w = cvRound(imageSize.width * sf);
h = cvRound(imageSize.height * sf);
canvas.create(h, w * 2, CV_8UC3);   //注意通道

//左图像画到画布上
Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到画布的一部分
resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);     //把图像缩放到跟canvasPart一样大小
Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),                //获得被截取的区域
cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
//rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8);                      //画上一个矩形
cout << "Painted ImageL" << endl;

//右图像画到画布上
canvasPart = canvas(Rect(w, 0, w, h));                                      //获得画布的另一部分
resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
//rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);
cout << "Painted ImageR" << endl;

//画上对应的线条
for (int i = 0; i < canvas.rows; i += 16)
line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
imshow("rectified", canvas);

/*
立体匹配
*/
namedWindow("disparity", CV_WINDOW_AUTOSIZE);
createTrackbar("BlockSize:\n", "disparity",&blockSize, 8, stereo_match);
// 创建视差唯一性百分比窗口 Trackbar
createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);
// 创建视差窗口 Trackbar
createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);
//鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数，一般取0)
setMouseCallback("disparity", onMouse, 0);
stereo_match(0,0);

waitKey(0);
return 0;
}

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

//opencv2.4.9 vs2012
#include <opencv2\opencv.hpp>
#include <fstream>
#include <iostream>

using namespace std;
using namespace cv;

//图片对数量
#define  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};

//球坐标数组
//大球
float rightDaqiu[PicNum][2] = {0};
float leftDaqiu[PicNum][2] = {0};
float worldDaqiu[PicNum][3] = {0};
//小球
float rightXiaoqiu[PicNum][2] = {0};
float leftXiaoqiu[PicNum][2] = {0};
float worldXiaoqiu[PicNum][3] = {0};
//花球
float rightHuaqiu[PicNum][2] = {0};
float leftHuaqiu[PicNum][2] = {0};
float worldHuaqiu[PicNum][3] = {0};

void ContrastAndBright(double alpha, double beta);//调节亮度/对比度
void CorrectionProcess();//对素材校正畸变
void initPos();//手动赋值球的图像坐标
void Daqiu();//计算大球图像坐标
Mat PictureCorrection( Mat image ,float intrinsic[3][3],float distortion[1][5]);//单张图像畸变校正
Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3]);//从世界坐标转为图像坐标
Point3f uv2xyz(Point2f uvLeft,Point2f uvRight);//从图像坐标转为世界坐标

int main()
{
CorrectionProcess();//对素材校正畸变
ContrastAndBright(2.5,50);//调节亮度/对比度

Daqiu();//自动计算大球坐标

initPos();//手动修正,如需验证数据，可以在该函数中修改

//求取大球的空间坐标
cout<<"求解大球的世界坐标："<<endl;
for (int i=0; i<PicNum; i++)
{
Point2f l,r;
Point3f worldPoint;

l.x = leftDaqiu[i][0];
l.y = leftDaqiu[i][1];
r.x = rightDaqiu[i][0];
r.y = rightDaqiu[i][1];

worldPoint = uv2xyz(l,r);
cout<< worldPoint <<endl;
worldDaqiu[i][0] = worldPoint.x;
worldDaqiu[i][1] = worldPoint.y;
worldDaqiu[i][2] = worldPoint.z;
}

cout<<"求解小球的世界坐标："<<endl;
for (int i=0; i<PicNum; i++)
{
Point2f l,r;
Point3f worldPoint;

l.x = leftXiaoqiu[i][0];
l.y = leftXiaoqiu[i][1];
r.x = rightXiaoqiu[i][0];
r.y = rightXiaoqiu[i][1];

worldPoint = uv2xyz(l,r);
cout<< worldPoint <<endl;
worldXiaoqiu[i][0] = worldPoint.x;
worldXiaoqiu[i][1] = worldPoint.y;
worldXiaoqiu[i][2] = worldPoint.z;
}

cout<<"求解花球的世界坐标："<<endl;
for (int i=0; i<PicNum; i++)
{
Point2f l,r;
Point3f worldPoint;

l.x = leftHuaqiu[i][0];
l.y = leftHuaqiu[i][1];
r.x = rightHuaqiu[i][0];
r.y = rightHuaqiu[i][1];

worldPoint = uv2xyz(l,r);
cout<< worldPoint <<endl;
worldHuaqiu[i][0] = worldPoint.x;
worldHuaqiu[i][1] = worldPoint.y;
worldHuaqiu[i][2] = worldPoint.z;
}

//csv文件写入部分
ofstream oFile;  //定义文件输出流

oFile.open("三维坐标.csv", ios::out | ios::trunc);    //打开要输出的文件 	 这样就很容易的输出一个需要的excel 文件
//写入大球数据
oFile << "大球坐标" << endl;
oFile << "左相机坐标,,,右相机坐标,,,世界坐标" << endl;
oFile << "x,y,,x,y,,x,y,z" << endl;
for (int i=0; i<PicNum ;i++)
{
oFile << leftDaqiu[i][0] <<","<< leftDaqiu[i][1] << ",," << rightDaqiu[i][0] <<","<< rightDaqiu[i][1]
<< ",," << worldDaqiu[i][0] <<","<<  worldDaqiu[i][1] <<","<<  worldDaqiu[i][2] << endl;
}

//写入小球数据
oFile << "小球坐标" << endl;
oFile << "左相机坐标,,,右相机坐标,,,世界坐标" << endl;
oFile << "x,y,,x,y,,x,y,z" << endl;
for (int i=0; i<PicNum ;i++)
{
oFile << leftXiaoqiu[i][0] <<","<< leftXiaoqiu[i][1] << ",," << rightXiaoqiu[i][0] <<","<< rightXiaoqiu[i][1]
<< ",," << worldXiaoqiu[i][0] <<","<<  worldXiaoqiu[i][1] <<","<<  worldXiaoqiu[i][2] << endl;
}

//写入花球数据
oFile << "花球坐标" << endl;
oFile << "左相机坐标,,,右相机坐标,,,世界坐标" << endl;
oFile << "x,y,,x,y,,x,y,z" << endl;
for (int i=0; i<PicNum ;i++)
{
oFile << leftHuaqiu[i][0] <<","<< leftHuaqiu[i][1] << ",," << rightHuaqiu[i][0] <<","<< rightHuaqiu[i][1]
<< ",," << worldHuaqiu[i][0] <<","<<  worldHuaqiu[i][1] <<","<<  worldHuaqiu[i][2] << endl;
}

//关闭文件
oFile.close();

//test
////已知空间坐标求成像坐标
//Point3f point(700,220,530);
//cout<<"左相机中坐标："<<endl;
//cout<<xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation)<<endl;
//cout<<"右相机中坐标："<<endl;
//cout<<xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation)<<endl;

//已知左右相机成像坐标求空间坐标
//Point2f l = xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation);
//Point2f r = xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation);
//Point3f worldPoint;
//worldPoint = uv2xyz(l,r);
//cout<<"空间坐标为:"<<endl<<uv2xyz(l,r)<<endl;

/*
//csv文件读取部分

string value;//临时字符串
ifstream iFile("三维坐标.csv");//打开要读入的文件
//循环行读取
while (iFile.good())
{
getline(iFile,value);
//getline(iFile,value,','); //.csv文件用","作为分隔符
cout<<value<<endl;
}
*/

system("pause");

return 0;
}

//************************************
// Description: 修正圆心坐标
// Method:    initPos
// FullName:  initPos
// Access:    public
// Returns:   void
// Author:    bhy
// Date:      2016/12/25
// History:
//************************************
void initPos()
{
//手动修正
leftDaqiu[0][0] = 1175;  leftDaqiu[0][1] = 7;     rightDaqiu[0][0] = 256;  rightDaqiu[0][1] = 1;
leftDaqiu[1][0] = 823;   leftDaqiu[1][1] = 603;   rightDaqiu[1][0] = 289;  rightDaqiu[1][1] = 431;
leftDaqiu[2][0] = 963;   leftDaqiu[2][1] = 360;   rightDaqiu[2][0] = 283;  rightDaqiu[2][1] = 169;
leftDaqiu[3][0] = 1065;  leftDaqiu[3][1] = 180;   rightDaqiu[3][0] = 294;  rightDaqiu[3][1] = 1;
leftDaqiu[4][0] = 1039;  leftDaqiu[4][1] = 217;   rightDaqiu[4][0] = 314;  rightDaqiu[4][1] = 68;
leftDaqiu[5][0] = 896;   leftDaqiu[5][1] = 448;   rightDaqiu[5][0] = 378;  rightDaqiu[5][1] = 402;
leftDaqiu[6][0] = 933;   leftDaqiu[6][1] = 376;   rightDaqiu[6][0] = 398;  rightDaqiu[6][1] = 347;
leftDaqiu[7][0] = 868;   leftDaqiu[7][1] = 463;   rightDaqiu[7][0] = 423;  rightDaqiu[7][1] = 418;
leftDaqiu[8][0] = 878;   leftDaqiu[8][1] = 417;   rightDaqiu[8][0] = 458;  rightDaqiu[8][1] = 466;
leftDaqiu[9][0] = 860;   leftDaqiu[9][1] = 423;   rightDaqiu[9][0] = 481;  rightDaqiu[9][1] = 490;
leftDaqiu[10][0] = 840;  leftDaqiu[10][1] = 442;  rightDaqiu[10][0] = 499; rightDaqiu[10][1] = 500;
leftDaqiu[11][0] = 822;  leftDaqiu[11][1] = 414;  rightDaqiu[11][0] = 523; rightDaqiu[11][1] = 511;
leftDaqiu[12][0] = 805;  leftDaqiu[12][1] = 406;  rightDaqiu[12][0] = 538; rightDaqiu[12][1] = 516;
leftDaqiu[13][0] = 802;  leftDaqiu[13][1] = 402;  rightDaqiu[13][0] = 549; rightDaqiu[13][1] = 514;

leftXiaoqiu[0][0] = 1250;   leftXiaoqiu[0][1] = 120;   rightXiaoqiu[0][0] = 308;  rightXiaoqiu[0][1] = 313;
leftXiaoqiu[1][0] = 1034;   leftXiaoqiu[1][1] = 481;   rightXiaoqiu[1][0] = 314;  rightXiaoqiu[1][1] = 482;
leftXiaoqiu[2][0] = 1207;   leftXiaoqiu[2][1] = 228;   rightXiaoqiu[2][0] = 284;  rightXiaoqiu[2][1] = 186;
leftXiaoqiu[3][0] = 1343;   leftXiaoqiu[3][1] = 55;    rightXiaoqiu[3][0] = 252;  rightXiaoqiu[3][1] = -20;
leftXiaoqiu[4][0] = 1326;   leftXiaoqiu[4][1] = 102;   rightXiaoqiu[4][0] = 242;  rightXiaoqiu[4][1] = 23;
leftXiaoqiu[5][0] = 1021;   leftXiaoqiu[5][1] = 625;   rightXiaoqiu[5][0] = 269;  rightXiaoqiu[5][1] = 632;
leftXiaoqiu[6][0] = 1123;   leftXiaoqiu[6][1] = 489;   rightXiaoqiu[6][0] = 241;  rightXiaoqiu[6][1] = 458;
leftXiaoqiu[7][0] = 1147;   leftXiaoqiu[7][1] = 475;   rightXiaoqiu[7][0] = 224;  rightXiaoqiu[7][1] = 404;
leftXiaoqiu[8][0] = 1078;   leftXiaoqiu[8][1] = 595;   rightXiaoqiu[8][0] = 223;  rightXiaoqiu[8][1] = 558;
leftXiaoqiu[9][0] = 1062;   leftXiaoqiu[9][1] = 635;   rightXiaoqiu[9][0] = 216;  rightXiaoqiu[9][1] = 598;
leftXiaoqiu[10][0] = 1080;  leftXiaoqiu[10][1] = 619;  rightXiaoqiu[10][0] = 201; rightXiaoqiu[10][1] = 576;
leftXiaoqiu[11][0] = 1054;  leftXiaoqiu[11][1] = 690;  rightXiaoqiu[11][0] = 189; rightXiaoqiu[11][1] = 633;
leftXiaoqiu[12][0] = 1046;  leftXiaoqiu[12][1] = 724;  rightXiaoqiu[12][0] = 179; rightXiaoqiu[12][1] = 655;
leftXiaoqiu[13][0] = 1046;  leftXiaoqiu[13][1] = 726;  rightXiaoqiu[13][0] = 172; rightXiaoqiu[13][1] = 656;

leftHuaqiu[0][0] = 1075;  leftHuaqiu[0][1] = 111;   rightHuaqiu[0][0] = 120;  rightHuaqiu[0][1] = -30;//出视场
leftHuaqiu[1][0] = 708;   leftHuaqiu[1][1] = 810;   rightHuaqiu[1][0] = 142;  rightHuaqiu[1][1] = 382;
leftHuaqiu[2][0] = 876;   leftHuaqiu[2][1] = 518;   rightHuaqiu[2][0] = 131;  rightHuaqiu[2][1] = 83;
leftHuaqiu[3][0] = 1021;  leftHuaqiu[3][1] = 253;   rightHuaqiu[3][0] = 180;   rightHuaqiu[3][1] = -60;//出视场
leftHuaqiu[4][0] = 1019;  leftHuaqiu[4][1] = 248;   rightHuaqiu[4][0] = 183;   rightHuaqiu[4][1] = -30;//出视场
leftHuaqiu[5][0] = 764;   leftHuaqiu[5][1] = 654;   rightHuaqiu[5][0] = 278;  rightHuaqiu[5][1] = 437;
leftHuaqiu[6][0] = 844;   leftHuaqiu[6][1] = 486;   rightHuaqiu[6][0] = 286;  rightHuaqiu[6][1] = 261;
leftHuaqiu[7][0] = 852;   leftHuaqiu[7][1] = 425;   rightHuaqiu[7][0] = 305;  rightHuaqiu[7][1] = 206;
leftHuaqiu[8][0] = 780;   leftHuaqiu[8][1] = 523;   rightHuaqiu[8][0] = 347;  rightHuaqiu[8][1] = 359;
leftHuaqiu[9][0] = 757;   leftHuaqiu[9][1] = 530;   rightHuaqiu[9][0] = 363;  rightHuaqiu[9][1] = 382;
leftHuaqiu[10][0] = 765;  leftHuaqiu[10][1] = 505;  rightHuaqiu[10][0] = 368; rightHuaqiu[10][1] = 345;
leftHuaqiu[11][0] = 702;  leftHuaqiu[11][1] = 554;  rightHuaqiu[11][0] = 382; rightHuaqiu[11][1] = 398;
leftHuaqiu[12][0] = 666;  leftHuaqiu[12][1] = 573;  rightHuaqiu[12][0] = 386; rightHuaqiu[12][1] = 408;
leftHuaqiu[13][0] = 656;  leftHuaqiu[13][1] = 581;  rightHuaqiu[13][0] = 384; rightHuaqiu[13][1] = 398;
}

//************************************
// 2016/12/2  by 小白
// Method:    uv2xyz
// FullName:  uv2xyz
// Access:    public
// Returns:   cv::Point3f		世界坐标
// Qualifier: 根据左右相机中成像坐标求解空间坐标
// Parameter: Point2f uvLeft		左相机中成像坐标
// Parameter: Point2f uvRight		右相机中成像坐标
//************************************
Point3f uv2xyz(Point2f uvLeft,Point2f uvRight)
{
//  [u1]      |X|					  [u2]      |X|
//Z*|v1| = Ml*|Y|					Z*|v2| = Mr*|Y|
//  [ 1]      |Z|					  [ 1]      |Z|
//			  |1|								|1|
Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);
Mat mLeftTranslation = Mat(3,1,CV_32F,leftTranslation);
Mat mLeftRT = Mat(3,4,CV_32F);//左相机M矩阵
hconcat(mLeftRotation,mLeftTranslation,mLeftRT);
Mat mLeftIntrinsic = Mat(3,3,CV_32F,leftIntrinsic);
Mat mLeftM = mLeftIntrinsic * mLeftRT;
//cout<<"左相机M矩阵 = "<<endl<<mLeftM<<endl;

Mat mRightRotation = Mat(3,3,CV_32F,rightRotation);
Mat mRightTranslation = Mat(3,1,CV_32F,rightTranslation);
Mat mRightRT = Mat(3,4,CV_32F);//右相机M矩阵
hconcat(mRightRotation,mRightTranslation,mRightRT);
Mat mRightIntrinsic = Mat(3,3,CV_32F,rightIntrinsic);
Mat mRightM = mRightIntrinsic * mRightRT;
//cout<<"右相机M矩阵 = "<<endl<<mRightM<<endl;

//最小二乘法A矩阵
Mat A = Mat(4,3,CV_32F);
A.at<float>(0,0) = uvLeft.x * mLeftM.at<float>(2,0) - mLeftM.at<float>(0,0);
A.at<float>(0,1) = uvLeft.x * mLeftM.at<float>(2,1) - mLeftM.at<float>(0,1);
A.at<float>(0,2) = uvLeft.x * mLeftM.at<float>(2,2) - mLeftM.at<float>(0,2);

A.at<float>(1,0) = uvLeft.y * mLeftM.at<float>(2,0) - mLeftM.at<float>(1,0);
A.at<float>(1,1) = uvLeft.y * mLeftM.at<float>(2,1) - mLeftM.at<float>(1,1);
A.at<float>(1,2) = uvLeft.y * mLeftM.at<float>(2,2) - mLeftM.at<float>(1,2);

A.at<float>(2,0) = uvRight.x * mRightM.at<float>(2,0) - mRightM.at<float>(0,0);
A.at<float>(2,1) = uvRight.x * mRightM.at<float>(2,1) - mRightM.at<float>(0,1);
A.at<float>(2,2) = uvRight.x * mRightM.at<float>(2,2) - mRightM.at<float>(0,2);

A.at<float>(3,0) = uvRight.y * mRightM.at<float>(2,0) - mRightM.at<float>(1,0);
A.at<float>(3,1) = uvRight.y * mRightM.at<float>(2,1) - mRightM.at<float>(1,1);
A.at<float>(3,2) = uvRight.y * mRightM.at<float>(2,2) - mRightM.at<float>(1,2);

//最小二乘法B矩阵
Mat B = Mat(4,1,CV_32F);
B.at<float>(0,0) = mLeftM.at<float>(0,3) - uvLeft.x * mLeftM.at<float>(2,3);
B.at<float>(1,0) = mLeftM.at<float>(1,3) - uvLeft.y * mLeftM.at<float>(2,3);
B.at<float>(2,0) = mRightM.at<float>(0,3) - uvRight.x * mRightM.at<float>(2,3);
B.at<float>(3,0) = mRightM.at<float>(1,3) - uvRight.y * mRightM.at<float>(2,3);

Mat XYZ = Mat(3,1,CV_32F);
//采用SVD最小二乘法求解XYZ
solve(A,B,XYZ,DECOMP_SVD);

//cout<<"空间坐标为 = "<<endl<<XYZ<<endl;

//世界坐标系中坐标
Point3f world;
world.x = XYZ.at<float>(0,0);
world.y = XYZ.at<float>(1,0);
world.z = XYZ.at<float>(2,0);

return world;
}

//************************************
// Description: 将空间坐标转换为像面坐标，用于检验
// Method:    xyz2uv
// FullName:  xyz2uv
// Access:    public
// Parameter: Point3f worldPoint
// Parameter: float intrinsic[3][3]
// Parameter: float translation[1][3]
// Parameter: float rotation[3][3]
// Returns:   cv::Point2f
// Author:    bhy
// Date:      2016/12/28
// History:
//************************************
Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3])
{
//    [fx s x0]							[Xc]		[Xw]		[u]	  1		[Xc]
//K = |0 fy y0|       TEMP = [R T]		|Yc| = TEMP*|Yw|		| | = —*K *|Yc|
//    [ 0 0 1 ]							[Zc]		|Zw|		[v]	  Zc	[Zc]
//													[1 ]
Point3f c;
c.x = rotation[0][0]*worldPoint.x + rotation[0][1]*worldPoint.y + rotation[0][2]*worldPoint.z + translation[0][0]*1;
c.y = rotation[1][0]*worldPoint.x + rotation[1][1]*worldPoint.y + rotation[1][2]*worldPoint.z + translation[0][1]*1;
c.z = rotation[2][0]*worldPoint.x + rotation[2][1]*worldPoint.y + rotation[2][2]*worldPoint.z + translation[0][2]*1;

Point2f uv;
uv.x = (intrinsic[0][0]*c.x + intrinsic[0][1]*c.y + intrinsic[0][2]*c.z)/c.z + 0.5;//加0.5去整 == 四舍五入
uv.y = (intrinsic[1][0]*c.x + intrinsic[1][1]*c.y + intrinsic[1][2]*c.z)/c.z + 0.5;//加0.5去整 == 四舍五入

return uv;
}

//************************************
// Description: 对畸变校正文件夹下的图片批量进行亮度/对比度调节
// Method:    ContrastAndBright
// FullName:  ContrastAndBright
// Access:    public
// Parameter: double alpha
// Parameter: double beta
// Returns:   void
// Author:    bhy
// Date:      2016/12/28
// History:
//************************************
void ContrastAndBright(double alpha, double beta)
{
//double alpha =3;
//double beta = 40;
cout<<"**********************************************"<<endl;
cout<<"                 亮度/对比度调节              "<<endl;
cout<<"**********************************************"<<endl;

cout<<"当前 alpha = "<<alpha<<endl;
cout<<"当前 beta = "<<beta<<endl;

Mat src,dst;

system("md 亮度对比度\\rightky1");

//右相机调节
//如果校正图像目录不存在，则创建该目录

for (int ii=1; ii<=PicNum; ii++)
{
cout<<"右：第"<<ii<<"张图片"<<endl;

char* filename = new char[100];
sprintf(filename,"畸变校正/rightky1/r%d.bmp",ii);
delete []filename;//释放字符串

dst = Mat::zeros(src.size(),src.type());//清空目标矩阵
//根据alpha,beta重新计算灰度值
for (int i = 0;i<src.rows;++i)
for(int j= 0;j<src.cols;++j)
for(int k = 0;k<3;++k)
dst.at<Vec3b>(i,j)[k] = saturate_cast<uchar>(src.at<Vec3b>(i,j)[k]*alpha+beta);

char* output = new char[100];
sprintf(output,"亮度对比度/rightky1/r%d.bmp",ii);
imwrite(output,dst); //顺次保存校正图
delete []output;//释放字符串
}

//左相机调节
//如果校正图像目录不存在，则创建该目录
system("md 亮度对比度\\leftky1");

for (int ii=1; ii<=PicNum; ii++)
{
cout<<"左：第"<<ii<<"张图片"<<endl;

char* filename = new char[100];
sprintf(filename,"畸变校正/leftky1/l%d.bmp",ii);
delete []filename;//释放字符串

dst = Mat::zeros(src.size(),src.type());//清空目标矩阵
//根据alpha,beta重新计算灰度值
for (int i = 0;i<src.rows;++i)
for(int j= 0;j<src.cols;++j)
for(int k = 0;k<3;++k)
dst.at<Vec3b>(i,j)[k] = saturate_cast<uchar>(src.at<Vec3b>(i,j)[k]*alpha+beta);

char* output = new char[100];
sprintf(output,"亮度对比度/leftky1/l%d.bmp",ii);
imwrite(output,dst); //顺次保存校正图
delete []output;//释放字符串
}

cout<<"**********************************************"<<endl;
cout<<"             亮度/对比度调节结束              "<<endl;
cout<<"**********************************************"<<endl;
}

//************************************
// Description: 对素材文件夹中的图片批量进行畸变校正
// Method:    CorrectionProcess
// FullName:  CorrectionProcess
// Access:    public
// Returns:   void
// Author:    bhy
// Date:      2016/12/28
// History:
//************************************
void CorrectionProcess()
{
cout<<"**********************************************"<<endl;
cout<<"                    畸变校正                  "<<endl;
cout<<"**********************************************"<<endl;

Mat image;

//使用畸变系数与内参校正右相机原图
//如果校正图像目录不存在，则创建该目录
system("md 畸变校正\\rightky1");

for (int i=1; i<=PicNum; i++)
{
cout<<"右：校正第"<<i<<"张图片"<<endl;

char* filename = new char[100];
sprintf(filename,"素材/rightky1/r%d.bmp",i);
delete []filename;//释放字符串

char* output = new char[100];
sprintf(output,"畸变校正/rightky1/r%d.bmp",i);
imwrite(output,PictureCorrection(image,rightIntrinsic,rightDistortion)); //顺次保存校正图
delete []output;//释放字符串
}

//使用畸变系数与内参校正左相机原图
//如果校正图像目录不存在，则创建该目录
system("md 畸变校正\\leftky1");

for (int i=1; i<=PicNum; i++)
{
cout<<"左：校正第"<<i<<"张图片"<<endl;

char* filename = new char[100];
sprintf(filename,"素材/leftky1/l%d.bmp",i);
delete []filename;//释放字符串

char* output = new char[100];
sprintf(output,"畸变校正/leftky1/l%d.bmp",i);
imwrite(output,PictureCorrection(image,leftIntrinsic,leftDistortion)); //顺次保存校正图
delete []output;//释放字符串
}
cout<<"**********************************************"<<endl;
cout<<"                  畸变校正结束                "<<endl;
cout<<"**********************************************"<<endl;
}

//************************************
// Method:    PictureCorrection
// FullName:  PictureCorrection
// Access:    public
// Returns:   cv::Mat	校正图
// Qualifier: 根据畸变系数与内参校正一张图片
// Parameter: Mat image
// Parameter: float intrinsic[3][3]		内参
// Parameter: float distortion[1][5]	畸变矩阵
//************************************
Mat PictureCorrection( Mat image ,float intrinsic[3][3],float distortion[1][5])
{
Size image_size = image.size();

Mat intrinsic_matrix = Mat(3,3,CV_32FC1,intrinsic);
Mat distortion_coeffs = Mat(1,5,CV_32FC1,distortion);
Mat R = Mat::eye(3,3,CV_32F);
Mat mapx = Mat(image_size,CV_32FC1);
Mat mapy = Mat(image_size,CV_32FC1);
initUndistortRectifyMap(intrinsic_matrix,distortion_coeffs,R,intrinsic_matrix,image_size,CV_32FC1,mapx,mapy);
Mat t = image.clone();
cv::remap( image, t, mapx, mapy, INTER_LINEAR);
return t;
}

//************************************
// Description: 采用hough变换求取大球圆心
// Method:    Daqiu
// FullName:  Daqiu
// Access:    public
// Returns:   void
// Author:    bhy
// Date:      2016/12/28
// History:
//************************************
void Daqiu()
{
cout<<"**********************************************"<<endl;
cout<<"                  计算大球圆心                "<<endl;
cout<<"**********************************************"<<endl;

Mat img;
//右相机 ContrastAndBright(2.5,50);
system("md 大球圆心\\rightky1");

cout<<"右图："<<endl;

for (int i=1; i<=PicNum; i++)
{
stringstream strStm;
string strFileName;
strStm << i;
strStm >> strFileName;
strFileName = "亮度对比度/rightky1/r" + strFileName + ".bmp";

GaussianBlur(img,img,Size(5,5),0);

vector<Vec3f> circles;
HoughCircles( img, circles, CV_HOUGH_GRADIENT, 3 ,70, 70, 30, 95 ,100);//hough圆变换
Point2f center(0,0);
for (int j = 0; j < circles.size(); j++)
{
if (circles[j][0] > center.x && circles[j][0] < img.cols/2)
{
center.x = circles[j][0];
center.y = circles[j][1];
}
}

rightDaqiu[i-1][0] = center.x;
rightDaqiu[i-1][1] = center.y;

CvScalar color = CV_RGB(0,0,0);
circle( img, (Point)center, radius, color, 3, 8, 0);//绘制圆
circle( img, (Point)center, 3, color, 3, 8, 0);//绘制圆心
cout<<"圆心:"<<center<<endl;//为了保证精度，以原值输出

char* output = new char[100];
sprintf(output,"大球圆心/rightky1/r%d.bmp",i);
imwrite(output,img); //顺次保存校正图
delete []output;//释放字符串
}

system("md 大球圆心\\leftky1");

cout<<"左图："<<endl;

for (int i=1; i<=PicNum; i++)
{
stringstream strStm;
string strFileName;
strStm << i;
strStm >> strFileName;
strFileName = "亮度对比度/leftky1/l" + strFileName + ".bmp";

GaussianBlur(img,img,Size(5,5),0);

vector<Vec3f> circles;
HoughCircles( img, circles, CV_HOUGH_GRADIENT, 3 ,70, 30, 50, 100 ,110);//hough圆变换
Point2f center(0,1024);
for (int j = 0; j < circles.size(); j++)
{
if (circles[j][1]<center.y)
{
center.x = circles[j][0];
center.y = circles[j][1];
}
}
leftDaqiu[i-1][0] = center.x;
leftDaqiu[i-1][1] = center.y;

CvScalar color = CV_RGB(0,0,0);
circle( img, center, radius, color, 3, 8, 0);//绘制圆
circle( img, center, 3, color, 3, 8, 0);//绘制圆心
cout<<"圆心:"<<center<<endl;//为了保证精度，以原值输出

char* output = new char[100];
sprintf(output,"大球圆心/leftky1/l%d.bmp",i);
imwrite(output,img); //顺次保存校正图
delete []output;//释放字符串
}

cout<<"**********************************************"<<endl;
cout<<"                  圆心计算结束                "<<endl;
cout<<"**********************************************"<<endl;
}

clc;clear;
x = M(:,1);
y = M(:,2);
z = M(:,3);
plot3(x,y,z,'r');
%legend('大球');
hold on

x = M(:,1);
y = M(:,2);
z = M(:,3);
plot3(x,y,z,'g');
%legend('小球');
hold on

x = M(:,1);
y = M(:,2);
z = M(:,3);
plot3(x,y,z,'b');
%legend('花球');
hold on

legend('大球','小球','花球');
title('小球运动轨迹');
xlabel('x');
ylabel('y');
zlabel('z');
grid on
axis square

补一张世界坐标系的图

• 本文已收录于以下专栏：
• opencv

举报原因： 您举报文章：【opencv】双目视觉下空间坐标计算/双目测距 6/13更新 色情 政治 抄袭 广告 招聘 骂人 其他 (最多只允许输入30个字)