摄像机矩阵由内参矩阵和外参矩阵组成,对摄像机矩阵进行QR分解可以得到内参矩阵和外参矩阵。
内参包括焦距、主点、倾斜系数、畸变系数
(1)
其中,fx,fy为焦距,一般情况下,二者相等,x0、y0为主点坐标(相对于成像平面),s为坐标轴倾斜参数,理想情况下为0
外参包括旋转矩阵R3×3、平移向量T3×1,它们共同描述了如何把点从世界坐标系转换到摄像机坐标系,旋
转矩阵描述了世界坐标系的坐标轴相对于摄像机坐标轴的方向,平移向量描述了在摄像机
坐标系下空间原点的位置。
标定双目后,首先要根据其畸变系数来校正原图,可以参考
http://blog.csdn.net/qq_15947787/article/details/51471535
- 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;
- }
校正完成后就可以进行坐标计算了,分两种
(1)世界坐标系——>像面坐标系
首先将世界坐标系——>摄像机坐标系
已知某点在世界坐标系中的坐标为(Xw, Yw, Zw),由旋转和平移矩阵可得摄像机坐标系和世界坐标系的关系为
(2)
然后将摄像机坐标系——>像面坐标系
(3)
其中[u v 1]T为点在图像坐标系中的坐标,[Xc Yc Zc 1]T为点在摄像机坐标系中的坐标,K为摄像机内参数矩阵。
这样最终可以得到:
(4)
(2)像面坐标系——>世界坐标系
光轴会聚模型:

对于两相机分别有:
(5)
(6)
公式56,左边Z应分别为Zc1,Zc2
其中,
(7)
这样可以把(5)(6)写成
(8)
公式8左边Z应为Zc1
(9)
公式9左边Z应为Zc2
将(8)(9)整理可以得到
(10)
以上公式参考评论6楼 wisemanjack
采用最小二乘法求解X,Y,Z,在opencv中可以用solve(A,B,XYZ,DECOMP_SVD)求解
代码如下:
-
- #include
- #include
- #include
-
- 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<<"左相机中坐标:"<
- cout<
- cout<<"右相机中坐标:"<
- cout<
-
-
- Point2f l = xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation);
- Point2f r = xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation);
- Point3f worldPoint;
- worldPoint = uv2xyz(l,r);
- cout<<"空间坐标为:"<
-
- system("pause");
-
- return 0;
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- Point3f uv2xyz(Point2f uvLeft,Point2f uvRight)
- {
-
-
-
-
- Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);
- Mat mLeftTranslation = Mat(3,1,CV_32F,leftTranslation);
- Mat mLeftRT = Mat(3,4,CV_32F);
- hconcat(mLeftRotation,mLeftTranslation,mLeftRT);
- Mat mLeftIntrinsic = Mat(3,3,CV_32F,leftIntrinsic);
- Mat mLeftM = mLeftIntrinsic * mLeftRT;
-
-
- Mat mRightRotation = Mat(3,3,CV_32F,rightRotation);
- Mat mRightTranslation = Mat(3,1,CV_32F,rightTranslation);
- Mat mRightRT = Mat(3,4,CV_32F);
- hconcat(mRightRotation,mRightTranslation,mRightRT);
- Mat mRightIntrinsic = Mat(3,3,CV_32F,rightIntrinsic);
- Mat mRightM = mRightIntrinsic * mRightRT;
-
-
-
- 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);
-
-
- 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);
-
- solve(A,B,XYZ,DECOMP_SVD);
-
-
-
-
- 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;
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3])
- {
-
-
-
-
- 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};
畸变矩阵(默认获得5个即便参数k1,k2,p1,p2,k3)
——————————————————————————————————————————————————————————————
像面坐标系——>世界坐标系还有一种模型是光轴平行模型
双目立体视觉三位测量是基于视差原理:
(11)


(12)
这里,除cx‘外的所有参数都来自于左图像,cx’是主点在右图像上的x坐标。如果主光线在无穷远处相交,那么cx=cx‘,并且右下角的项为0,给定一个二维齐次点和其关联的视差d,我们可以将此点投影到三维中:
(13)
三维坐标就是(X / W , Y / W , Z / W)
光轴平行模型要得到视差图,http://blog.csdn.net/wangchao7281/article/details/52506691?locationNum=7
可以参考opencv的例子,例子的使用方法可以参考http://blog.csdn.net/t247555529/article/details/48046859
得到视差图后可以调用cvReprojectImageTo3D输出的三维坐标
看到很多人输出三维坐标时z出现10000,那个其实是输出方式不对,应该是下面这样
- Point p;
- p.x =294,p.y=189;
- cout<
"in world coordinate: "
<< xyz.at(p)*16 <
为什么要乘以16呢?
因为在OpenCV2.0中,BM函数得出的结果是以16位符号数的形式的存储的,出于精度需要,所有的视差在输出时都扩大了16倍(2^4)。其具体代码表示如下:
- dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);
可以看到,原始视差在左移8位(256)并且加上一个修正值之后又右移了4位,最终的结果就是左移4位
因此,在实际求距离时,cvReprojectTo3D出来的X/W,Y/W,Z/W都要乘以16 (也就是W除以16),才能得到正确的三维坐标信息
下面是在opencv3.0下实现的该方法的测距(转载)
-
-
-
-
- #include
- #include
-
- 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;
- Rect validROIR;
-
- Mat mapLx, mapLy, mapRx, mapRy;
- Mat Rl, Rr, Pl, Pr, Q;
- Mat xyz;
-
- Point origin;
- Rect selection;
- bool selectObject = false;
-
- int blockSize = 0, uniquenessRatio =0, numDisparities=0;
- Ptr bm = StereoBM::create(16, 9);
-
-
-
-
-
-
-
- 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);
- Mat rec = (Mat_<double>(3, 1) << -0.00306, -0.03207, 0.00206);
- Mat R = (Mat_<double>(3, 3) << 0.999957725513956,-0.00103511880221423,0.00913650447492805,
- 0.00114462826834523,0.999927476064641,-0.0119888463633882,
- -0.00912343197938050,0.0119987974423658,0.999886389470751);
-
-
-
- void stereo_match(int,void*)
- {
- bm->setBlockSize(2*blockSize+5);
- bm->setROI1(validROIL);
- bm->setROI2(validROIR);
- bm->setPreFilterCap(31);
- bm->setMinDisparity(0);
- bm->setNumDisparities(numDisparities*16+16);
- bm->setTextureThreshold(10);
- bm->setUniquenessRatio(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.));
- reprojectImageTo3D(disp, xyz, Q, true);
- 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(origin) << endl;
- break;
- case EVENT_LBUTTONUP:
- selectObject = false;
- if (selection.width > 0 && selection.height > 0)
- break;
- }
- }
-
-
-
- int main()
- {
-
-
-
-
- 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);
-
-
-
-
- rgbImageL = imread("左2.jpg", CV_LOAD_IMAGE_COLOR);
- cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);
- rgbImageR = imread("右2.jpg", CV_LOAD_IMAGE_COLOR);
- cvtColor(rgbImageR, grayImageR, CV_BGR2GRAY);
-
- imshow("ImageL Before Rectify", grayImageL);
- imshow("ImageR Before Rectify", grayImageR);
-
-
-
-
- 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);
-
-
-
-
- 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);
- Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),
- cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
-
- 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));
-
- 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);
-
- createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);
-
- createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);
-
- setMouseCallback("disparity", onMouse, 0);
- stereo_match(0,0);
-
- waitKey(0);
- return 0;
- }
————————————————————————————————————————
最后是完整的大作业代码,感觉自己写的并不是很好,敷衍了事~~
运行该程序,会对素材文件夹中图像进行处理,生成三个文件夹,分别是“大球圆心”、“畸变校正”、“亮度对比度”以及一个csv文件,文件名为“三维坐标.csv”,里面记录了计算得到的左右相机中球的像面坐标和解算出的空间坐标。该程序默认对所有计算出的圆心坐标进行了重新赋值,如需修改圆心坐标,需在工程中的initPos()修改赋值语句,如需程序自动计算,在主函数中注释该指令即可,但是自动计算的圆心并不准确。
-
- #include
- #include
- #include
-
- 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<<"求解大球的世界坐标:"<
- for (int i=0; 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 <
- worldDaqiu[i][0] = worldPoint.x;
- worldDaqiu[i][1] = worldPoint.y;
- worldDaqiu[i][2] = worldPoint.z;
- }
-
- cout<<"求解小球的世界坐标:"<
- for (int i=0; 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 <
- worldXiaoqiu[i][0] = worldPoint.x;
- worldXiaoqiu[i][1] = worldPoint.y;
- worldXiaoqiu[i][2] = worldPoint.z;
- }
-
- cout<<"求解花球的世界坐标:"<
- for (int i=0; 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 <
- worldHuaqiu[i][0] = worldPoint.x;
- worldHuaqiu[i][1] = worldPoint.y;
- worldHuaqiu[i][2] = worldPoint.z;
- }
-
-
- ofstream oFile;
-
- oFile.open("三维坐标.csv", ios::out | ios::trunc);
-
- oFile << "大球坐标" << endl;
- oFile << "左相机坐标,,,右相机坐标,,,世界坐标" << endl;
- oFile << "x,y,,x,y,,x,y,z" << endl;
- for (int i=0; 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
- {
- 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
- {
- 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();
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- system("pause");
-
- return 0;
- }
-
-
-
-
-
-
-
-
-
-
-
- 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;
- }
-
-
-
-
-
-
-
-
-
-
-
- Point3f uv2xyz(Point2f uvLeft,Point2f uvRight)
- {
-
-
-
-
- Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);
- Mat mLeftTranslation = Mat(3,1,CV_32F,leftTranslation);
- Mat mLeftRT = Mat(3,4,CV_32F);
- hconcat(mLeftRotation,mLeftTranslation,mLeftRT);
- Mat mLeftIntrinsic = Mat(3,3,CV_32F,leftIntrinsic);
- Mat mLeftM = mLeftIntrinsic * mLeftRT;
-
-
- Mat mRightRotation = Mat(3,3,CV_32F,rightRotation);
- Mat mRightTranslation = Mat(3,1,CV_32F,rightTranslation);
- Mat mRightRT = Mat(3,4,CV_32F);
- hconcat(mRightRotation,mRightTranslation,mRightRT);
- Mat mRightIntrinsic = Mat(3,3,CV_32F,rightIntrinsic);
- Mat mRightM = mRightIntrinsic * mRightRT;
-
-
-
- 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);
-
-
- 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);
-
- solve(A,B,XYZ,DECOMP_SVD);
-
-
-
-
- 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;
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3])
- {
-
-
-
-
- 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;
- uv.y = (intrinsic[1][0]*c.x + intrinsic[1][1]*c.y + intrinsic[1][2]*c.z)/c.z + 0.5;
-
- return uv;
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- void ContrastAndBright(double alpha, double beta)
- {
-
-
- cout<<"**********************************************"<
- cout<<" 亮度/对比度调节 "<
- cout<<"**********************************************"<
-
- cout<<"当前 alpha = "<
- cout<<"当前 beta = "<
-
- Mat src,dst;
-
- system("md 亮度对比度\\rightky1");
-
-
-
-
- for (int ii=1; ii<=PicNum; ii++)
- {
- cout<<"右:第"<"张图片"<
-
- char* filename = new char[100];
- sprintf(filename,"畸变校正/rightky1/r%d.bmp",ii);
- src = imread(filename);
- delete []filename;
-
- dst = Mat::zeros(src.size(),src.type());
-
- for (int i = 0;i
- for(int j= 0;j
- for(int k = 0;k<3;++k)
- dst.at(i,j)[k] = saturate_cast(src.at(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<<"左:第"<"张图片"<
-
- char* filename = new char[100];
- sprintf(filename,"畸变校正/leftky1/l%d.bmp",ii);
- src = imread(filename);
- delete []filename;
-
- dst = Mat::zeros(src.size(),src.type());
-
- for (int i = 0;i
- for(int j= 0;j
- for(int k = 0;k<3;++k)
- dst.at(i,j)[k] = saturate_cast(src.at(i,j)[k]*alpha+beta);
-
- char* output = new char[100];
- sprintf(output,"亮度对比度/leftky1/l%d.bmp",ii);
- imwrite(output,dst);
- delete []output;
- }
-
- cout<<"**********************************************"<
- cout<<" 亮度/对比度调节结束 "<
- cout<<"**********************************************"<
- }
-
-
-
-
-
-
-
-
-
-
-
- void CorrectionProcess()
- {
- cout<<"**********************************************"<
- cout<<" 畸变校正 "<
- cout<<"**********************************************"<
-
- Mat image;
-
-
-
- system("md 畸变校正\\rightky1");
-
- for (int i=1; i<=PicNum; i++)
- {
- cout<<"右:校正第"<"张图片"<
-
- char* filename = new char[100];
- sprintf(filename,"素材/rightky1/r%d.bmp",i);
- image = imread(filename,IMREAD_GRAYSCALE);
- 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<<"左:校正第"<"张图片"<
-
- char* filename = new char[100];
- sprintf(filename,"素材/leftky1/l%d.bmp",i);
- image = imread(filename,IMREAD_GRAYSCALE);
- delete []filename;
-
- char* output = new char[100];
- sprintf(output,"畸变校正/leftky1/l%d.bmp",i);
- imwrite(output,PictureCorrection(image,leftIntrinsic,leftDistortion));
- delete []output;
- }
- cout<<"**********************************************"<
- cout<<" 畸变校正结束 "<
- cout<<"**********************************************"<
- }
-
-
-
-
-
-
-
-
-
-
-
- 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;
- }
-
-
-
-
-
-
-
-
-
-
-
-
- void Daqiu()
- {
- cout<<"**********************************************"<
- cout<<" 计算大球圆心 "<
- cout<<"**********************************************"<
-
- Mat img;
-
- system("md 大球圆心\\rightky1");
-
- cout<<"右图:"<
-
- for (int i=1; i<=PicNum; i++)
- {
- stringstream strStm;
- string strFileName;
- strStm << i;
- strStm >> strFileName;
- strFileName = "亮度对比度/rightky1/r" + strFileName + ".bmp";
- img = imread(strFileName,IMREAD_GRAYSCALE);
-
- GaussianBlur(img,img,Size(5,5),0);
-
- vector circles;
- HoughCircles( img, circles, CV_HOUGH_GRADIENT, 3 ,70, 70, 30, 95 ,100);
- Point2f center(0,0);
- float radius;
- 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];
- radius = circles[j][2];
- }
- }
-
- 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<<"圆心:"<
-
- char* output = new char[100];
- sprintf(output,"大球圆心/rightky1/r%d.bmp",i);
- imwrite(output,img);
- delete []output;
- }
-
- system("md 大球圆心\\leftky1");
-
- cout<<"左图:"<
-
- for (int i=1; i<=PicNum; i++)
- {
- stringstream strStm;
- string strFileName;
- strStm << i;
- strStm >> strFileName;
- strFileName = "亮度对比度/leftky1/l" + strFileName + ".bmp";
- img = imread(strFileName,IMREAD_GRAYSCALE);
-
- GaussianBlur(img,img,Size(5,5),0);
-
- vector circles;
- HoughCircles( img, circles, CV_HOUGH_GRADIENT, 3 ,70, 30, 50, 100 ,110);
- Point2f center(0,1024);
- float radius;
- for (int j = 0; j < circles.size(); j++)
- {
- if (circles[j][1]
- {
- center.x = circles[j][0];
- center.y = circles[j][1];
- radius = circles[j][2];
- }
- }
- 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<<"圆心:"<
-
- char* output = new char[100];
- sprintf(output,"大球圆心/leftky1/l%d.bmp",i);
- imwrite(output,img);
- delete []output;
- }
-
- cout<<"**********************************************"<
- cout<<" 圆心计算结束 "<
- cout<<"**********************************************"<
- }
运行该程序,会提取生成的“三维坐标.csv”中的空间坐标数据,并绘制运动轨迹需要注意的是,在“三维坐标.csv”文件中直接修改圆心坐标没有用,需要在工程中的initPos()修改。
- clc;clear;
- M = csvread('三维坐标.csv',3,6,[3,6,16,8]);
- x = M(:,1);
- y = M(:,2);
- z = M(:,3);
- plot3(x,y,z,'r');
- %legend('大球');
- hold on
-
- M = csvread('三维坐标.csv',20,6,[20,6,33,8]);
- x = M(:,1);
- y = M(:,2);
- z = M(:,3);
- plot3(x,y,z,'g');
- %legend('小球');
- hold on
-
- M = csvread('三维坐标.csv',37,6,[37,6,50,8]);
- 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
最后绘制的轨迹图

补一张世界坐标系的图