精华内容
参与话题
问答
  • 摄像机矩阵由内参矩阵和外参矩阵组成,对摄像机矩阵进行QR分解可以得到内参矩阵和...外参包括旋转矩阵R3×3、平移向量T3×1,它们共同描述了如何把点从世界坐标系转换到摄像机坐标系,旋转矩阵描述了世界坐标系的坐...

    摄像机矩阵由内参矩阵和外参矩阵组成,对摄像机矩阵进行QR分解可以得到内参矩阵和外参矩阵。

    内参包括焦距、主点、倾斜系数、畸变系数

    (1)

    其中,fx,fy为焦距,一般情况下,二者相等,x0、y0为主点坐标(相对于成像平面),s为坐标轴倾斜参数,理想情况下为0

     

    外参包括旋转矩阵R3×3、平移向量T3×1,它们共同描述了如何把点从世界坐标系转换到摄像机坐标系,旋转矩阵描述了世界坐标系的坐标轴相对于摄像机坐标轴的方向,平移向量描述了在摄像机坐标系下空间原点的位置。

     

    分两种

    (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)求解

     

    代码如下:

     

    1. //opencv2.4.9 vs2012  
    2. #include <opencv2\opencv.hpp>  
    3. #include <fstream>  
    4. #include <iostream>  
    5.   
    6. using namespace std;  
    7. using namespace cv;  
    8.   
    9. Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3]);  
    10. Point3f uv2xyz(Point2f uvLeft,Point2f uvRight);  
    11.   
    12. //图片对数量  
    13. int PicNum = 14;  
    14.   
    15. //左相机内参数矩阵  
    16. float leftIntrinsic[3][3] = {4037.82450,             0,     947.65449,  
    17.                                       0,    3969.79038,     455.48718,  
    18.                                       0,             0,             1};  
    19. //左相机畸变系数  
    20. float leftDistortion[1][5] = {0.18962, -4.05566, -0.00510, 0.02895, 0};  
    21. //左相机旋转矩阵  
    22. float leftRotation[3][3] = {0.912333,       -0.211508,       0.350590,   
    23.                             0.023249,       -0.828105,      -0.560091,   
    24.                             0.408789,        0.519140,      -0.750590};  
    25. //左相机平移向量  
    26. float leftTranslation[1][3] = {-127.199992, 28.190639, 1471.356768};  
    27.   
    28. //右相机内参数矩阵  
    29. float rightIntrinsic[3][3] = {3765.83307,            0,     339.31958,  
    30.                                         0,  3808.08469,     660.05543,  
    31.                                         0,           0,             1};  
    32. //右相机畸变系数  
    33. float rightDistortion[1][5] = {-0.24195, 5.97763, -0.02057, -0.01429, 0};  
    34. //右相机旋转矩阵  
    35. float rightRotation[3][3] = {-0.134947,      0.989568,      -0.050442,   
    36.                               0.752355,      0.069205,      -0.655113,   
    37.                              -0.644788,     -0.126356,      -0.753845};  
    38. //右相机平移向量  
    39. float rightTranslation[1][3] = {50.877397, -99.796492, 1507.312197};  
    40.   
    41.   
    42. int main()  
    43. {  
    44.     //已知空间坐标求成像坐标  
    45.     Point3f point(700,220,530);  
    46.     cout<<"左相机中坐标:"<<endl;  
    47.     cout<<xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation)<<endl;  
    48.     cout<<"右相机中坐标:"<<endl;  
    49.     cout<<xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation)<<endl;  
    50.   
    51.     //已知左右相机成像坐标求空间坐标  
    52.     Point2f l = xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation);  
    53.     Point2f r = xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation);  
    54.     Point3f worldPoint;  
    55.     worldPoint = uv2xyz(l,r);  
    56.     cout<<"空间坐标为:"<<endl<<uv2xyz(l,r)<<endl;  
    57.   
    58.     system("pause");  
    59.   
    60.     return 0;  
    61. }  
    62.   
    63.   
    64. //************************************  
    65. // Description: 根据左右相机中成像坐标求解空间坐标  
    66. // Method:    uv2xyz  
    67. // FullName:  uv2xyz  
    68. // Access:    public   
    69. // Parameter: Point2f uvLeft  
    70. // Parameter: Point2f uvRight  
    71. // Returns:   cv::Point3f  
    72. // Author:    小白  
    73. // Date:      2017/01/10  
    74. // History:  
    75. //************************************  
    76. Point3f uv2xyz(Point2f uvLeft,Point2f uvRight)  
    77. {  
    78.     //  [u1]      |X|                     [u2]      |X|  
    79.     //Z*|v1| = Ml*|Y|                   Z*|v2| = Mr*|Y|  
    80.     //  [ 1]      |Z|                     [ 1]      |Z|  
    81.     //            |1|                               |1|  
    82.     Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);  
    83.     Mat mLeftTranslation = Mat(3,1,CV_32F,leftTranslation);  
    84.     Mat mLeftRT = Mat(3,4,CV_32F);//左相机M矩阵  
    85.     hconcat(mLeftRotation,mLeftTranslation,mLeftRT);  
    86.     Mat mLeftIntrinsic = Mat(3,3,CV_32F,leftIntrinsic);  
    87.     Mat mLeftM = mLeftIntrinsic * mLeftRT;  
    88.     //cout<<"左相机M矩阵 = "<<endl<<mLeftM<<endl;  
    89.   
    90.     Mat mRightRotation = Mat(3,3,CV_32F,rightRotation);  
    91.     Mat mRightTranslation = Mat(3,1,CV_32F,rightTranslation);  
    92.     Mat mRightRT = Mat(3,4,CV_32F);//右相机M矩阵  
    93.     hconcat(mRightRotation,mRightTranslation,mRightRT);  
    94.     Mat mRightIntrinsic = Mat(3,3,CV_32F,rightIntrinsic);  
    95.     Mat mRightM = mRightIntrinsic * mRightRT;  
    96.     //cout<<"右相机M矩阵 = "<<endl<<mRightM<<endl;  
    97.   
    98.     //最小二乘法A矩阵  
    99.     Mat A = Mat(4,3,CV_32F);  
    100.     A.at<float>(0,0) = uvLeft.x * mLeftM.at<float>(2,0) - mLeftM.at<float>(0,0);  
    101.     A.at<float>(0,1) = uvLeft.x * mLeftM.at<float>(2,1) - mLeftM.at<float>(0,1);  
    102.     A.at<float>(0,2) = uvLeft.x * mLeftM.at<float>(2,2) - mLeftM.at<float>(0,2);  
    103.   
    104.     A.at<float>(1,0) = uvLeft.y * mLeftM.at<float>(2,0) - mLeftM.at<float>(1,0);  
    105.     A.at<float>(1,1) = uvLeft.y * mLeftM.at<float>(2,1) - mLeftM.at<float>(1,1);  
    106.     A.at<float>(1,2) = uvLeft.y * mLeftM.at<float>(2,2) - mLeftM.at<float>(1,2);  
    107.   
    108.     A.at<float>(2,0) = uvRight.x * mRightM.at<float>(2,0) - mRightM.at<float>(0,0);  
    109.     A.at<float>(2,1) = uvRight.x * mRightM.at<float>(2,1) - mRightM.at<float>(0,1);  
    110.     A.at<float>(2,2) = uvRight.x * mRightM.at<float>(2,2) - mRightM.at<float>(0,2);  
    111.   
    112.     A.at<float>(3,0) = uvRight.y * mRightM.at<float>(2,0) - mRightM.at<float>(1,0);  
    113.     A.at<float>(3,1) = uvRight.y * mRightM.at<float>(2,1) - mRightM.at<float>(1,1);  
    114.     A.at<float>(3,2) = uvRight.y * mRightM.at<float>(2,2) - mRightM.at<float>(1,2);  
    115.   
    116.     //最小二乘法B矩阵  
    117.     Mat B = Mat(4,1,CV_32F);  
    118.     B.at<float>(0,0) = mLeftM.at<float>(0,3) - uvLeft.x * mLeftM.at<float>(2,3);  
    119.     B.at<float>(1,0) = mLeftM.at<float>(1,3) - uvLeft.y * mLeftM.at<float>(2,3);  
    120.     B.at<float>(2,0) = mRightM.at<float>(0,3) - uvRight.x * mRightM.at<float>(2,3);  
    121.     B.at<float>(3,0) = mRightM.at<float>(1,3) - uvRight.y * mRightM.at<float>(2,3);  
    122.   
    123.     Mat XYZ = Mat(3,1,CV_32F);  
    124.     //采用SVD最小二乘法求解XYZ  
    125.     solve(A,B,XYZ,DECOMP_SVD);  
    126.   
    127.     //cout<<"空间坐标为 = "<<endl<<XYZ<<endl;  
    128.   
    129.     //世界坐标系中坐标  
    130.     Point3f world;  
    131.     world.x = XYZ.at<float>(0,0);  
    132.     world.y = XYZ.at<float>(1,0);  
    133.     world.z = XYZ.at<float>(2,0);  
    134.   
    135.     return world;  
    136. }  
    137.   
    138. //************************************  
    139. // Description: 将世界坐标系中的点投影到左右相机成像坐标系中  
    140. // Method:    xyz2uv  
    141. // FullName:  xyz2uv  
    142. // Access:    public   
    143. // Parameter: Point3f worldPoint  
    144. // Parameter: float intrinsic[3][3]  
    145. // Parameter: float translation[1][3]  
    146. // Parameter: float rotation[3][3]  
    147. // Returns:   cv::Point2f  
    148. // Author:    小白  
    149. // Date:      2017/01/10  
    150. // History:  
    151. //************************************  
    152. Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3])  
    153. {  
    154.     //    [fx s x0]                         [Xc]        [Xw]        [u]   1     [Xc]  
    155.     //K = |0 fy y0|       TEMP = [R T]      |Yc| = TEMP*|Yw|        | | = —*K *|Yc|  
    156.     //    [ 0 0 1 ]                         [Zc]        |Zw|        [v]   Zc    [Zc]  
    157.     //                                                  [1 ]  
    158.     Point3f c;  
    159.     c.x = rotation[0][0]*worldPoint.x + rotation[0][1]*worldPoint.y + rotation[0][2]*worldPoint.z + translation[0][0]*1;  
    160.     c.y = rotation[1][0]*worldPoint.x + rotation[1][1]*worldPoint.y + rotation[1][2]*worldPoint.z + translation[0][1]*1;  
    161.     c.z = rotation[2][0]*worldPoint.x + rotation[2][1]*worldPoint.y + rotation[2][2]*worldPoint.z + translation[0][2]*1;  
    162.   
    163.     Point2f uv;  
    164.     uv.x = (intrinsic[0][0]*c.x + intrinsic[0][1]*c.y + intrinsic[0][2]*c.z)/c.z;  
    165.     uv.y = (intrinsic[1][0]*c.x + intrinsic[1][1]*c.y + intrinsic[1][2]*c.z)/c.z;  
    166.   
    167.     return uv;  
    168. }  


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

     

    1. //左相机旋转矩阵    
    2. float leftRotation[3][3] = {1,0,0,  0,1,0,  0,0,1 };  
    3. //左相机平移向量    
    4. 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)

     

     

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

    为什么要乘以16呢?

    因为在OpenCV2.0中,BM函数得出的结果是以16位符号数的形式的存储的,出于精度需要,所有的视差在输出时都扩大了16倍(2^4)。其具体代码表示如下:

     

    1. 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下实现的该方法的测距(转载)

     

     

    1. /******************************/  
    2. /*        立体匹配和测距        */  
    3. /******************************/  
    4.   
    5. #include <opencv2/opencv.hpp>    
    6. #include <iostream>    
    7.   
    8. using namespace std;  
    9. using namespace cv;  
    10.   
    11. const int imageWidth = 640;                             //摄像头的分辨率    
    12. const int imageHeight = 480;  
    13. Size imageSize = Size(imageWidth, imageHeight);  
    14.   
    15. Mat rgbImageL, grayImageL;  
    16. Mat rgbImageR, grayImageR;  
    17. Mat rectifyImageL, rectifyImageR;  
    18.   
    19. Rect validROIL;//图像校正之后,会对图像进行裁剪,这里的validROI就是指裁剪之后的区域    
    20. Rect validROIR;  
    21.   
    22. Mat mapLx, mapLy, mapRx, mapRy;     //映射表    
    23. Mat Rl, Rr, Pl, Pr, Q;              //校正旋转矩阵R,投影矩阵P 重投影矩阵Q  
    24. Mat xyz;              //三维坐标  
    25.   
    26. Point origin;         //鼠标按下的起始点  
    27. Rect selection;      //定义矩形选框  
    28. bool selectObject = false;    //是否选择对象  
    29.   
    30. int blockSize = 0, uniquenessRatio =0, numDisparities=0;  
    31. Ptr<StereoBM> bm = StereoBM::create(16, 9);  
    32.   
    33. /* 
    34. 事先标定好的相机的参数 
    35. fx 0 cx 
    36. 0 fy cy 
    37. 0 0  1 
    38. */  
    39. Mat cameraMatrixL = (Mat_<double>(3, 3) << 836.771593170594,0,319.970748854743,  
    40.     0,839.416501863912,228.788913693256,  
    41.     0, 0, 1);  
    42. Mat distCoeffL = (Mat_<double>(5, 1) << 0, 0, 0, 0, 0);  
    43.   
    44. Mat cameraMatrixR = (Mat_<double>(3, 3) << 838.101721655709,0,319.647150557935,  
    45.     0,840.636812165056,250.655818405938,  
    46.     0, 0, 1);  
    47. Mat distCoeffR = (Mat_<double>(5, 1) << 0, 0, 0, 0, 0);  
    48.   
    49. Mat T = (Mat_<double>(3, 1) << -39.7389449993974,0.0740619639178984,0.411914303245886);//T平移向量  
    50. Mat rec = (Mat_<double>(3, 1) << -0.00306, -0.03207, 0.00206);//rec旋转向量  
    51. Mat R = (Mat_<double>(3, 3) << 0.999957725513956,-0.00103511880221423,0.00913650447492805,  
    52.          0.00114462826834523,0.999927476064641,-0.0119888463633882,  
    53.          -0.00912343197938050,0.0119987974423658,0.999886389470751);//R 旋转矩阵  
    54.   
    55.   
    56. /*****立体匹配*****/  
    57. void stereo_match(int,void*)  
    58. {  
    59.     bm->setBlockSize(2*blockSize+5);     //SAD窗口大小,5~21之间为宜  
    60.     bm->setROI1(validROIL);  
    61.     bm->setROI2(validROIR);  
    62.     bm->setPreFilterCap(31);  
    63.     bm->setMinDisparity(0);  //最小视差,默认值为0, 可以是负值,int型  
    64.     bm->setNumDisparities(numDisparities*16+16);//视差窗口,即最大视差值与最小视差值之差,窗口大小必须是16的整数倍,int型  
    65.     bm->setTextureThreshold(10);   
    66.     bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配  
    67.     bm->setSpeckleWindowSize(100);  
    68.     bm->setSpeckleRange(32);  
    69.     bm->setDisp12MaxDiff(-1);  
    70.     Mat disp, disp8;  
    71.     bm->compute(rectifyImageL, rectifyImageR, disp);//输入图像必须为灰度图  
    72.     disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式  
    73.     reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时,ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16),才能得到正确的三维坐标信息。  
    74.     xyz = xyz * 16;  
    75.     imshow("disparity", disp8);  
    76. }  
    77.   
    78. /*****描述:鼠标操作回调*****/  
    79. static void onMouse(int event, int x, int y, int, void*)  
    80. {  
    81.     if (selectObject)  
    82.     {  
    83.         selection.x = MIN(x, origin.x);  
    84.         selection.y = MIN(y, origin.y);  
    85.         selection.width = std::abs(x - origin.x);  
    86.         selection.height = std::abs(y - origin.y);  
    87.     }  
    88.   
    89.     switch (event)  
    90.     {  
    91.     case EVENT_LBUTTONDOWN:   //鼠标左按钮按下的事件  
    92.         origin = Point(x, y);  
    93.         selection = Rect(x, y, 0, 0);  
    94.         selectObject = true;  
    95.         cout << origin <<"in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;  
    96.         break;  
    97.     case EVENT_LBUTTONUP:    //鼠标左按钮释放的事件  
    98.         selectObject = false;  
    99.         if (selection.width > 0 && selection.height > 0)  
    100.         break;  
    101.     }  
    102. }  
    103.   
    104.   
    105. /*****主函数*****/  
    106. int main()  
    107. {  
    108.     /* 
    109.     立体校正 
    110.     */  
    111.     //Rodrigues(rec, R); //Rodrigues变换  
    112.     stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,  
    113.         0, imageSize, &validROIL, &validROIR);  
    114.     initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);  
    115.     initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);  
    116.   
    117.     /* 
    118.     读取图片 
    119.     */  
    120.     rgbImageL = imread("左2.jpg", CV_LOAD_IMAGE_COLOR);  
    121.     cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);  
    122.     rgbImageR = imread("右2.jpg", CV_LOAD_IMAGE_COLOR);  
    123.     cvtColor(rgbImageR, grayImageR, CV_BGR2GRAY);  
    124.   
    125.     imshow("ImageL Before Rectify", grayImageL);  
    126.     imshow("ImageR Before Rectify", grayImageR);  
    127.   
    128.     /* 
    129.     经过remap之后,左右相机的图像已经共面并且行对准了 
    130.     */  
    131.     remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);  
    132.     remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);  
    133.   
    134.     /* 
    135.     把校正结果显示出来 
    136.     */  
    137.     Mat rgbRectifyImageL, rgbRectifyImageR;  
    138.     cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR);  //伪彩色图  
    139.     cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR);  
    140.   
    141.     //单独显示  
    142.     //rectangle(rgbRectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);  
    143.     //rectangle(rgbRectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);  
    144.     imshow("ImageL After Rectify", rgbRectifyImageL);  
    145.     imshow("ImageR After Rectify", rgbRectifyImageR);  
    146.   
    147.     //显示在同一张图上  
    148.     Mat canvas;  
    149.     double sf;  
    150.     int w, h;  
    151.     sf = 600. / MAX(imageSize.width, imageSize.height);  
    152.     w = cvRound(imageSize.width * sf);  
    153.     h = cvRound(imageSize.height * sf);  
    154.     canvas.create(h, w * 2, CV_8UC3);   //注意通道  
    155.   
    156.     //左图像画到画布上  
    157.     Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到画布的一部分    
    158.     resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);     //把图像缩放到跟canvasPart一样大小    
    159.     Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),                //获得被截取的区域      
    160.         cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));  
    161.     //rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8);                      //画上一个矩形    
    162.     cout << "Painted ImageL" << endl;  
    163.   
    164.     //右图像画到画布上  
    165.     canvasPart = canvas(Rect(w, 0, w, h));                                      //获得画布的另一部分    
    166.     resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);  
    167.     Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),  
    168.         cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));  
    169.     //rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);  
    170.     cout << "Painted ImageR" << endl;  
    171.   
    172.     //画上对应的线条  
    173.     for (int i = 0; i < canvas.rows; i += 16)  
    174.         line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);  
    175.     imshow("rectified", canvas);  
    176.   
    177.     /* 
    178.     立体匹配 
    179.     */  
    180.     namedWindow("disparity", CV_WINDOW_AUTOSIZE);  
    181.     // 创建SAD窗口 Trackbar  
    182.     createTrackbar("BlockSize:\n", "disparity",&blockSize, 8, stereo_match);  
    183.     // 创建视差唯一性百分比窗口 Trackbar  
    184.     createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);  
    185.     // 创建视差窗口 Trackbar  
    186.     createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);  
    187.     //鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数,一般取0)  
    188.     setMouseCallback("disparity", onMouse, 0);  
    189.     stereo_match(0,0);  
    190.   
    191.     waitKey(0);  
    192.     return 0;  
    193. }  


     

     

     

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

     

     

     

    1. //opencv2.4.9 vs2012  
    2. #include <opencv2\opencv.hpp>  
    3. #include <fstream>  
    4. #include <iostream>  
    5.   
    6. using namespace std;  
    7. using namespace cv;  
    8.   
    9. //图片对数量  
    10. #define  PicNum  14  
    11.   
    12. //左相机内参数矩阵  
    13. float leftIntrinsic[3][3] = {4037.82450,             0,     947.65449,  
    14.                                       0,    3969.79038,     455.48718,  
    15.                                       0,             0,             1};  
    16. //左相机畸变系数  
    17. float leftDistortion[1][5] = {0.18962, -4.05566, -0.00510, 0.02895, 0};  
    18. //左相机旋转矩阵  
    19. float leftRotation[3][3] = {0.912333,       -0.211508,       0.350590,   
    20.                             0.023249,       -0.828105,      -0.560091,   
    21.                             0.408789,        0.519140,      -0.750590};  
    22. //左相机平移向量  
    23. float leftTranslation[1][3] = {-127.199992, 28.190639, 1471.356768};  
    24.   
    25. //右相机内参数矩阵  
    26. float rightIntrinsic[3][3] = {3765.83307,            0,     339.31958,  
    27.                                         0,  3808.08469,     660.05543,  
    28.                                         0,           0,             1};  
    29. //右相机畸变系数  
    30. float rightDistortion[1][5] = {-0.24195, 5.97763, -0.02057, -0.01429, 0};  
    31. //右相机旋转矩阵  
    32. float rightRotation[3][3] = {-0.134947,      0.989568,      -0.050442,   
    33.                               0.752355,      0.069205,      -0.655113,   
    34.                              -0.644788,     -0.126356,      -0.753845};  
    35. //右相机平移向量  
    36. float rightTranslation[1][3] = {50.877397, -99.796492, 1507.312197};  
    37.   
    38. //球坐标数组  
    39. //大球  
    40. float rightDaqiu[PicNum][2] = {0};  
    41. float leftDaqiu[PicNum][2] = {0};  
    42. float worldDaqiu[PicNum][3] = {0};  
    43. //小球  
    44. float rightXiaoqiu[PicNum][2] = {0};  
    45. float leftXiaoqiu[PicNum][2] = {0};  
    46. float worldXiaoqiu[PicNum][3] = {0};  
    47. //花球  
    48. float rightHuaqiu[PicNum][2] = {0};  
    49. float leftHuaqiu[PicNum][2] = {0};  
    50. float worldHuaqiu[PicNum][3] = {0};  
    51.   
    52. void ContrastAndBright(double alpha, double beta);//调节亮度/对比度  
    53. void CorrectionProcess();//对素材校正畸变  
    54. void initPos();//手动赋值球的图像坐标  
    55. void Daqiu();//计算大球图像坐标  
    56. Mat PictureCorrection( Mat image ,float intrinsic[3][3],float distortion[1][5]);//单张图像畸变校正  
    57. Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3]);//从世界坐标转为图像坐标  
    58. Point3f uv2xyz(Point2f uvLeft,Point2f uvRight);//从图像坐标转为世界坐标  
    59.   
    60. int main()  
    61. {  
    62.     CorrectionProcess();//对素材校正畸变  
    63.     ContrastAndBright(2.5,50);//调节亮度/对比度  
    64.       
    65.     Daqiu();//自动计算大球坐标  
    66.       
    67.     initPos();//手动修正,如需验证数据,可以在该函数中修改  
    68.   
    69.     //求取大球的空间坐标  
    70.     cout<<"求解大球的世界坐标:"<<endl;  
    71.     for (int i=0; i<PicNum; i++)  
    72.     {  
    73.         Point2f l,r;  
    74.         Point3f worldPoint;  
    75.   
    76.         l.x = leftDaqiu[i][0];  
    77.         l.y = leftDaqiu[i][1];  
    78.         r.x = rightDaqiu[i][0];  
    79.         r.y = rightDaqiu[i][1];  
    80.   
    81.         worldPoint = uv2xyz(l,r);  
    82.         cout<< worldPoint <<endl;  
    83.         worldDaqiu[i][0] = worldPoint.x;  
    84.         worldDaqiu[i][1] = worldPoint.y;  
    85.         worldDaqiu[i][2] = worldPoint.z;  
    86.     }  
    87.   
    88.     cout<<"求解小球的世界坐标:"<<endl;  
    89.     for (int i=0; i<PicNum; i++)  
    90.     {  
    91.         Point2f l,r;  
    92.         Point3f worldPoint;  
    93.   
    94.         l.x = leftXiaoqiu[i][0];  
    95.         l.y = leftXiaoqiu[i][1];  
    96.         r.x = rightXiaoqiu[i][0];  
    97.         r.y = rightXiaoqiu[i][1];  
    98.   
    99.         worldPoint = uv2xyz(l,r);  
    100.         cout<< worldPoint <<endl;  
    101.         worldXiaoqiu[i][0] = worldPoint.x;  
    102.         worldXiaoqiu[i][1] = worldPoint.y;  
    103.         worldXiaoqiu[i][2] = worldPoint.z;  
    104.     }  
    105.   
    106.     cout<<"求解花球的世界坐标:"<<endl;  
    107.     for (int i=0; i<PicNum; i++)  
    108.     {  
    109.         Point2f l,r;  
    110.         Point3f worldPoint;  
    111.   
    112.         l.x = leftHuaqiu[i][0];  
    113.         l.y = leftHuaqiu[i][1];  
    114.         r.x = rightHuaqiu[i][0];  
    115.         r.y = rightHuaqiu[i][1];  
    116.   
    117.         worldPoint = uv2xyz(l,r);  
    118.         cout<< worldPoint <<endl;  
    119.         worldHuaqiu[i][0] = worldPoint.x;  
    120.         worldHuaqiu[i][1] = worldPoint.y;  
    121.         worldHuaqiu[i][2] = worldPoint.z;  
    122.     }  
    123.   
    124.     //csv文件写入部分  
    125.     ofstream oFile;  //定义文件输出流     
    126.   
    127.     oFile.open("三维坐标.csv", ios::out | ios::trunc);    //打开要输出的文件     这样就很容易的输出一个需要的excel 文件    
    128.     //写入大球数据  
    129.     oFile << "大球坐标" << endl;  
    130.     oFile << "左相机坐标,,,右相机坐标,,,世界坐标" << endl;  
    131.     oFile << "x,y,,x,y,,x,y,z" << endl;  
    132.     for (int i=0; i<PicNum ;i++)  
    133.     {  
    134.         oFile << leftDaqiu[i][0] <<","<< leftDaqiu[i][1] << ",," << rightDaqiu[i][0] <<","<< rightDaqiu[i][1]   
    135.               << ",," << worldDaqiu[i][0] <<","<<  worldDaqiu[i][1] <<","<<  worldDaqiu[i][2] << endl;  
    136.     }  
    137.   
    138.     //写入小球数据  
    139.     oFile << "小球坐标" << endl;  
    140.     oFile << "左相机坐标,,,右相机坐标,,,世界坐标" << endl;  
    141.     oFile << "x,y,,x,y,,x,y,z" << endl;  
    142.     for (int i=0; i<PicNum ;i++)  
    143.     {  
    144.         oFile << leftXiaoqiu[i][0] <<","<< leftXiaoqiu[i][1] << ",," << rightXiaoqiu[i][0] <<","<< rightXiaoqiu[i][1]   
    145.         << ",," << worldXiaoqiu[i][0] <<","<<  worldXiaoqiu[i][1] <<","<<  worldXiaoqiu[i][2] << endl;  
    146.     }  
    147.       
    148.   
    149.     //写入花球数据  
    150.     oFile << "花球坐标" << endl;  
    151.     oFile << "左相机坐标,,,右相机坐标,,,世界坐标" << endl;  
    152.     oFile << "x,y,,x,y,,x,y,z" << endl;  
    153.     for (int i=0; i<PicNum ;i++)  
    154.     {  
    155.         oFile << leftHuaqiu[i][0] <<","<< leftHuaqiu[i][1] << ",," << rightHuaqiu[i][0] <<","<< rightHuaqiu[i][1]   
    156.         << ",," << worldHuaqiu[i][0] <<","<<  worldHuaqiu[i][1] <<","<<  worldHuaqiu[i][2] << endl;  
    157.     }  
    158.   
    159.     //关闭文件  
    160.     oFile.close();    
    161.       
    162.   
    163.     //test  
    164.     已知空间坐标求成像坐标  
    165.     //Point3f point(700,220,530);  
    166.     //cout<<"左相机中坐标:"<<endl;  
    167.     //cout<<xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation)<<endl;  
    168.     //cout<<"右相机中坐标:"<<endl;  
    169.     //cout<<xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation)<<endl;  
    170.   
    171.     //已知左右相机成像坐标求空间坐标  
    172.     //Point2f l = xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation);  
    173.     //Point2f r = xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation);  
    174.     //Point3f worldPoint;  
    175.     //worldPoint = uv2xyz(l,r);  
    176.     //cout<<"空间坐标为:"<<endl<<uv2xyz(l,r)<<endl;  
    177.   
    178.   
    179.     /* 
    180.     //csv文件读取部分 
    181.  
    182.     string value;//临时字符串 
    183.     ifstream iFile("三维坐标.csv");//打开要读入的文件 
    184.     //循环行读取 
    185.     while (iFile.good()) 
    186.     { 
    187.         getline(iFile,value); 
    188.         //getline(iFile,value,','); //.csv文件用","作为分隔符 
    189.         cout<<value<<endl; 
    190.     } 
    191.     */  
    192.       
    193.     system("pause");  
    194.   
    195.     return 0;  
    196. }  
    197.   
    198. //************************************  
    199. // Description: 修正圆心坐标  
    200. // Method:    initPos  
    201. // FullName:  initPos  
    202. // Access:    public   
    203. // Returns:   void  
    204. // Author:    bhy  
    205. // Date:      2016/12/25  
    206. // History:  
    207. //************************************  
    208. void initPos()  
    209. {  
    210.     //手动修正  
    211.     leftDaqiu[0][0] = 1175;  leftDaqiu[0][1] = 7;     rightDaqiu[0][0] = 256;  rightDaqiu[0][1] = 1;  
    212.     leftDaqiu[1][0] = 823;   leftDaqiu[1][1] = 603;   rightDaqiu[1][0] = 289;  rightDaqiu[1][1] = 431;  
    213.     leftDaqiu[2][0] = 963;   leftDaqiu[2][1] = 360;   rightDaqiu[2][0] = 283;  rightDaqiu[2][1] = 169;  
    214.     leftDaqiu[3][0] = 1065;  leftDaqiu[3][1] = 180;   rightDaqiu[3][0] = 294;  rightDaqiu[3][1] = 1;  
    215.     leftDaqiu[4][0] = 1039;  leftDaqiu[4][1] = 217;   rightDaqiu[4][0] = 314;  rightDaqiu[4][1] = 68;  
    216.     leftDaqiu[5][0] = 896;   leftDaqiu[5][1] = 448;   rightDaqiu[5][0] = 378;  rightDaqiu[5][1] = 402;  
    217.     leftDaqiu[6][0] = 933;   leftDaqiu[6][1] = 376;   rightDaqiu[6][0] = 398;  rightDaqiu[6][1] = 347;  
    218.     leftDaqiu[7][0] = 868;   leftDaqiu[7][1] = 463;   rightDaqiu[7][0] = 423;  rightDaqiu[7][1] = 418;  
    219.     leftDaqiu[8][0] = 878;   leftDaqiu[8][1] = 417;   rightDaqiu[8][0] = 458;  rightDaqiu[8][1] = 466;  
    220.     leftDaqiu[9][0] = 860;   leftDaqiu[9][1] = 423;   rightDaqiu[9][0] = 481;  rightDaqiu[9][1] = 490;  
    221.     leftDaqiu[10][0] = 840;  leftDaqiu[10][1] = 442;  rightDaqiu[10][0] = 499; rightDaqiu[10][1] = 500;  
    222.     leftDaqiu[11][0] = 822;  leftDaqiu[11][1] = 414;  rightDaqiu[11][0] = 523; rightDaqiu[11][1] = 511;  
    223.     leftDaqiu[12][0] = 805;  leftDaqiu[12][1] = 406;  rightDaqiu[12][0] = 538; rightDaqiu[12][1] = 516;  
    224.     leftDaqiu[13][0] = 802;  leftDaqiu[13][1] = 402;  rightDaqiu[13][0] = 549; rightDaqiu[13][1] = 514;  
    225.   
    226.     leftXiaoqiu[0][0] = 1250;   leftXiaoqiu[0][1] = 120;   rightXiaoqiu[0][0] = 308;  rightXiaoqiu[0][1] = 313;  
    227.     leftXiaoqiu[1][0] = 1034;   leftXiaoqiu[1][1] = 481;   rightXiaoqiu[1][0] = 314;  rightXiaoqiu[1][1] = 482;  
    228.     leftXiaoqiu[2][0] = 1207;   leftXiaoqiu[2][1] = 228;   rightXiaoqiu[2][0] = 284;  rightXiaoqiu[2][1] = 186;  
    229.     leftXiaoqiu[3][0] = 1343;   leftXiaoqiu[3][1] = 55;    rightXiaoqiu[3][0] = 252;  rightXiaoqiu[3][1] = -20;  
    230.     leftXiaoqiu[4][0] = 1326;   leftXiaoqiu[4][1] = 102;   rightXiaoqiu[4][0] = 242;  rightXiaoqiu[4][1] = 23;  
    231.     leftXiaoqiu[5][0] = 1021;   leftXiaoqiu[5][1] = 625;   rightXiaoqiu[5][0] = 269;  rightXiaoqiu[5][1] = 632;  
    232.     leftXiaoqiu[6][0] = 1123;   leftXiaoqiu[6][1] = 489;   rightXiaoqiu[6][0] = 241;  rightXiaoqiu[6][1] = 458;  
    233.     leftXiaoqiu[7][0] = 1147;   leftXiaoqiu[7][1] = 475;   rightXiaoqiu[7][0] = 224;  rightXiaoqiu[7][1] = 404;  
    234.     leftXiaoqiu[8][0] = 1078;   leftXiaoqiu[8][1] = 595;   rightXiaoqiu[8][0] = 223;  rightXiaoqiu[8][1] = 558;  
    235.     leftXiaoqiu[9][0] = 1062;   leftXiaoqiu[9][1] = 635;   rightXiaoqiu[9][0] = 216;  rightXiaoqiu[9][1] = 598;  
    236.     leftXiaoqiu[10][0] = 1080;  leftXiaoqiu[10][1] = 619;  rightXiaoqiu[10][0] = 201; rightXiaoqiu[10][1] = 576;  
    237.     leftXiaoqiu[11][0] = 1054;  leftXiaoqiu[11][1] = 690;  rightXiaoqiu[11][0] = 189; rightXiaoqiu[11][1] = 633;  
    238.     leftXiaoqiu[12][0] = 1046;  leftXiaoqiu[12][1] = 724;  rightXiaoqiu[12][0] = 179; rightXiaoqiu[12][1] = 655;  
    239.     leftXiaoqiu[13][0] = 1046;  leftXiaoqiu[13][1] = 726;  rightXiaoqiu[13][0] = 172; rightXiaoqiu[13][1] = 656;  
    240.       
    241.     leftHuaqiu[0][0] = 1075;  leftHuaqiu[0][1] = 111;   rightHuaqiu[0][0] = 120;  rightHuaqiu[0][1] = -30;//出视场  
    242.     leftHuaqiu[1][0] = 708;   leftHuaqiu[1][1] = 810;   rightHuaqiu[1][0] = 142;  rightHuaqiu[1][1] = 382;  
    243.     leftHuaqiu[2][0] = 876;   leftHuaqiu[2][1] = 518;   rightHuaqiu[2][0] = 131;  rightHuaqiu[2][1] = 83;  
    244.     leftHuaqiu[3][0] = 1021;  leftHuaqiu[3][1] = 253;   rightHuaqiu[3][0] = 180;   rightHuaqiu[3][1] = -60;//出视场  
    245.     leftHuaqiu[4][0] = 1019;  leftHuaqiu[4][1] = 248;   rightHuaqiu[4][0] = 183;   rightHuaqiu[4][1] = -30;//出视场  
    246.     leftHuaqiu[5][0] = 764;   leftHuaqiu[5][1] = 654;   rightHuaqiu[5][0] = 278;  rightHuaqiu[5][1] = 437;  
    247.     leftHuaqiu[6][0] = 844;   leftHuaqiu[6][1] = 486;   rightHuaqiu[6][0] = 286;  rightHuaqiu[6][1] = 261;  
    248.     leftHuaqiu[7][0] = 852;   leftHuaqiu[7][1] = 425;   rightHuaqiu[7][0] = 305;  rightHuaqiu[7][1] = 206;  
    249.     leftHuaqiu[8][0] = 780;   leftHuaqiu[8][1] = 523;   rightHuaqiu[8][0] = 347;  rightHuaqiu[8][1] = 359;  
    250.     leftHuaqiu[9][0] = 757;   leftHuaqiu[9][1] = 530;   rightHuaqiu[9][0] = 363;  rightHuaqiu[9][1] = 382;  
    251.     leftHuaqiu[10][0] = 765;  leftHuaqiu[10][1] = 505;  rightHuaqiu[10][0] = 368; rightHuaqiu[10][1] = 345;  
    252.     leftHuaqiu[11][0] = 702;  leftHuaqiu[11][1] = 554;  rightHuaqiu[11][0] = 382; rightHuaqiu[11][1] = 398;  
    253.     leftHuaqiu[12][0] = 666;  leftHuaqiu[12][1] = 573;  rightHuaqiu[12][0] = 386; rightHuaqiu[12][1] = 408;  
    254.     leftHuaqiu[13][0] = 656;  leftHuaqiu[13][1] = 581;  rightHuaqiu[13][0] = 384; rightHuaqiu[13][1] = 398;  
    255. }  
    256.   
    257. //************************************  
    258. // 2016/12/2  by 小白  
    259. // Method:    uv2xyz  
    260. // FullName:  uv2xyz  
    261. // Access:    public   
    262. // Returns:   cv::Point3f       世界坐标  
    263. // Qualifier: 根据左右相机中成像坐标求解空间坐标  
    264. // Parameter: Point2f uvLeft        左相机中成像坐标  
    265. // Parameter: Point2f uvRight       右相机中成像坐标  
    266. //************************************  
    267. Point3f uv2xyz(Point2f uvLeft,Point2f uvRight)  
    268. {  
    269.     //  [u1]      |X|                     [u2]      |X|  
    270.     //Z*|v1| = Ml*|Y|                   Z*|v2| = Mr*|Y|  
    271.     //  [ 1]      |Z|                     [ 1]      |Z|  
    272.     //            |1|                               |1|  
    273.     Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);  
    274.     Mat mLeftTranslation = Mat(3,1,CV_32F,leftTranslation);  
    275.     Mat mLeftRT = Mat(3,4,CV_32F);//左相机M矩阵  
    276.     hconcat(mLeftRotation,mLeftTranslation,mLeftRT);  
    277.     Mat mLeftIntrinsic = Mat(3,3,CV_32F,leftIntrinsic);  
    278.     Mat mLeftM = mLeftIntrinsic * mLeftRT;  
    279.     //cout<<"左相机M矩阵 = "<<endl<<mLeftM<<endl;  
    280.   
    281.     Mat mRightRotation = Mat(3,3,CV_32F,rightRotation);  
    282.     Mat mRightTranslation = Mat(3,1,CV_32F,rightTranslation);  
    283.     Mat mRightRT = Mat(3,4,CV_32F);//右相机M矩阵  
    284.     hconcat(mRightRotation,mRightTranslation,mRightRT);  
    285.     Mat mRightIntrinsic = Mat(3,3,CV_32F,rightIntrinsic);  
    286.     Mat mRightM = mRightIntrinsic * mRightRT;  
    287.     //cout<<"右相机M矩阵 = "<<endl<<mRightM<<endl;  
    288.   
    289.     //最小二乘法A矩阵  
    290.     Mat A = Mat(4,3,CV_32F);  
    291.     A.at<float>(0,0) = uvLeft.x * mLeftM.at<float>(2,0) - mLeftM.at<float>(0,0);  
    292.     A.at<float>(0,1) = uvLeft.x * mLeftM.at<float>(2,1) - mLeftM.at<float>(0,1);  
    293.     A.at<float>(0,2) = uvLeft.x * mLeftM.at<float>(2,2) - mLeftM.at<float>(0,2);  
    294.   
    295.     A.at<float>(1,0) = uvLeft.y * mLeftM.at<float>(2,0) - mLeftM.at<float>(1,0);  
    296.     A.at<float>(1,1) = uvLeft.y * mLeftM.at<float>(2,1) - mLeftM.at<float>(1,1);  
    297.     A.at<float>(1,2) = uvLeft.y * mLeftM.at<float>(2,2) - mLeftM.at<float>(1,2);  
    298.   
    299.     A.at<float>(2,0) = uvRight.x * mRightM.at<float>(2,0) - mRightM.at<float>(0,0);  
    300.     A.at<float>(2,1) = uvRight.x * mRightM.at<float>(2,1) - mRightM.at<float>(0,1);  
    301.     A.at<float>(2,2) = uvRight.x * mRightM.at<float>(2,2) - mRightM.at<float>(0,2);  
    302.   
    303.     A.at<float>(3,0) = uvRight.y * mRightM.at<float>(2,0) - mRightM.at<float>(1,0);  
    304.     A.at<float>(3,1) = uvRight.y * mRightM.at<float>(2,1) - mRightM.at<float>(1,1);  
    305.     A.at<float>(3,2) = uvRight.y * mRightM.at<float>(2,2) - mRightM.at<float>(1,2);  
    306.   
    307.     //最小二乘法B矩阵  
    308.     Mat B = Mat(4,1,CV_32F);  
    309.     B.at<float>(0,0) = mLeftM.at<float>(0,3) - uvLeft.x * mLeftM.at<float>(2,3);  
    310.     B.at<float>(1,0) = mLeftM.at<float>(1,3) - uvLeft.y * mLeftM.at<float>(2,3);  
    311.     B.at<float>(2,0) = mRightM.at<float>(0,3) - uvRight.x * mRightM.at<float>(2,3);  
    312.     B.at<float>(3,0) = mRightM.at<float>(1,3) - uvRight.y * mRightM.at<float>(2,3);  
    313.   
    314.     Mat XYZ = Mat(3,1,CV_32F);  
    315.     //采用SVD最小二乘法求解XYZ  
    316.     solve(A,B,XYZ,DECOMP_SVD);  
    317.   
    318.     //cout<<"空间坐标为 = "<<endl<<XYZ<<endl;  
    319.   
    320.     //世界坐标系中坐标  
    321.     Point3f world;  
    322.     world.x = XYZ.at<float>(0,0);  
    323.     world.y = XYZ.at<float>(1,0);  
    324.     world.z = XYZ.at<float>(2,0);  
    325.   
    326.     return world;  
    327. }  
    328.   
    329. //************************************  
    330. // Description: 将空间坐标转换为像面坐标,用于检验  
    331. // Method:    xyz2uv  
    332. // FullName:  xyz2uv  
    333. // Access:    public   
    334. // Parameter: Point3f worldPoint  
    335. // Parameter: float intrinsic[3][3]  
    336. // Parameter: float translation[1][3]  
    337. // Parameter: float rotation[3][3]  
    338. // Returns:   cv::Point2f  
    339. // Author:    bhy  
    340. // Date:      2016/12/28  
    341. // History:  
    342. //************************************  
    343. Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3])  
    344. {  
    345.     //    [fx s x0]                         [Xc]        [Xw]        [u]   1     [Xc]  
    346.     //K = |0 fy y0|       TEMP = [R T]      |Yc| = TEMP*|Yw|        | | = —*K *|Yc|  
    347.     //    [ 0 0 1 ]                         [Zc]        |Zw|        [v]   Zc    [Zc]  
    348.     //                                                  [1 ]  
    349.     Point3f c;  
    350.     c.x = rotation[0][0]*worldPoint.x + rotation[0][1]*worldPoint.y + rotation[0][2]*worldPoint.z + translation[0][0]*1;  
    351.     c.y = rotation[1][0]*worldPoint.x + rotation[1][1]*worldPoint.y + rotation[1][2]*worldPoint.z + translation[0][1]*1;  
    352.     c.z = rotation[2][0]*worldPoint.x + rotation[2][1]*worldPoint.y + rotation[2][2]*worldPoint.z + translation[0][2]*1;  
    353.   
    354.     Point2f uv;  
    355.     uv.x = (intrinsic[0][0]*c.x + intrinsic[0][1]*c.y + intrinsic[0][2]*c.z)/c.z + 0.5;//加0.5去整 == 四舍五入  
    356.     uv.y = (intrinsic[1][0]*c.x + intrinsic[1][1]*c.y + intrinsic[1][2]*c.z)/c.z + 0.5;//加0.5去整 == 四舍五入  
    357.   
    358.     return uv;  
    359. }  
    360.   
    361.   
    362. //************************************  
    363. // Description: 对畸变校正文件夹下的图片批量进行亮度/对比度调节  
    364. // Method:    ContrastAndBright  
    365. // FullName:  ContrastAndBright  
    366. // Access:    public   
    367. // Parameter: double alpha  
    368. // Parameter: double beta  
    369. // Returns:   void  
    370. // Author:    bhy  
    371. // Date:      2016/12/28  
    372. // History:  
    373. //************************************  
    374. void ContrastAndBright(double alpha, double beta)  
    375. {  
    376.     //double alpha =3;    
    377.     //double beta = 40;   
    378.     cout<<"**********************************************"<<endl;  
    379.     cout<<"                 亮度/对比度调节              "<<endl;  
    380.     cout<<"**********************************************"<<endl;  
    381.   
    382.     cout<<"当前 alpha = "<<alpha<<endl;  
    383.     cout<<"当前 beta = "<<beta<<endl;  
    384.   
    385.     Mat src,dst;   
    386.   
    387.     system("md 亮度对比度\\rightky1");  
    388.   
    389.     //右相机调节  
    390.     //如果校正图像目录不存在,则创建该目录  
    391.   
    392.     for (int ii=1; ii<=PicNum; ii++)  
    393.     {  
    394.         cout<<"右:第"<<ii<<"张图片"<<endl;  
    395.   
    396.         char* filename = new char[100];  
    397.         sprintf(filename,"畸变校正/rightky1/r%d.bmp",ii);  
    398.         src = imread(filename);//顺次读入图片  
    399.         delete []filename;//释放字符串  
    400.           
    401.         dst = Mat::zeros(src.size(),src.type());//清空目标矩阵  
    402.         //根据alpha,beta重新计算灰度值  
    403.         for (int i = 0;i<src.rows;++i)    
    404.             for(int j= 0;j<src.cols;++j)    
    405.                 for(int k = 0;k<3;++k)    
    406.                     dst.at<Vec3b>(i,j)[k] = saturate_cast<uchar>(src.at<Vec3b>(i,j)[k]*alpha+beta);    
    407.   
    408.         char* output = new char[100];  
    409.         sprintf(output,"亮度对比度/rightky1/r%d.bmp",ii);  
    410.         imwrite(output,dst); //顺次保存校正图  
    411.         delete []output;//释放字符串  
    412.     }  
    413.   
    414.     //左相机调节  
    415.     //如果校正图像目录不存在,则创建该目录  
    416.     system("md 亮度对比度\\leftky1");  
    417.   
    418.     for (int ii=1; ii<=PicNum; ii++)  
    419.     {  
    420.         cout<<"左:第"<<ii<<"张图片"<<endl;  
    421.   
    422.         char* filename = new char[100];  
    423.         sprintf(filename,"畸变校正/leftky1/l%d.bmp",ii);  
    424.         src = imread(filename);//顺次读入图片  
    425.         delete []filename;//释放字符串  
    426.   
    427.         dst = Mat::zeros(src.size(),src.type());//清空目标矩阵  
    428.         //根据alpha,beta重新计算灰度值  
    429.         for (int i = 0;i<src.rows;++i)    
    430.             for(int j= 0;j<src.cols;++j)    
    431.                 for(int k = 0;k<3;++k)    
    432.                     dst.at<Vec3b>(i,j)[k] = saturate_cast<uchar>(src.at<Vec3b>(i,j)[k]*alpha+beta);    
    433.   
    434.         char* output = new char[100];  
    435.         sprintf(output,"亮度对比度/leftky1/l%d.bmp",ii);  
    436.         imwrite(output,dst); //顺次保存校正图  
    437.         delete []output;//释放字符串  
    438.     }  
    439.   
    440.     cout<<"**********************************************"<<endl;  
    441.     cout<<"             亮度/对比度调节结束              "<<endl;  
    442.     cout<<"**********************************************"<<endl;  
    443. }  
    444.   
    445. //************************************  
    446. // Description: 对素材文件夹中的图片批量进行畸变校正  
    447. // Method:    CorrectionProcess  
    448. // FullName:  CorrectionProcess  
    449. // Access:    public   
    450. // Returns:   void  
    451. // Author:    bhy  
    452. // Date:      2016/12/28  
    453. // History:  
    454. //************************************  
    455. void CorrectionProcess()  
    456. {  
    457.     cout<<"**********************************************"<<endl;  
    458.     cout<<"                    畸变校正                  "<<endl;  
    459.     cout<<"**********************************************"<<endl;  
    460.   
    461.     Mat image;  
    462.   
    463.     //使用畸变系数与内参校正右相机原图  
    464.     //如果校正图像目录不存在,则创建该目录  
    465.     system("md 畸变校正\\rightky1");  
    466.   
    467.     for (int i=1; i<=PicNum; i++)  
    468.     {  
    469.         cout<<"右:校正第"<<i<<"张图片"<<endl;  
    470.   
    471.         char* filename = new char[100];  
    472.         sprintf(filename,"素材/rightky1/r%d.bmp",i);  
    473.         image = imread(filename,IMREAD_GRAYSCALE);//顺次读入图片  
    474.         delete []filename;//释放字符串  
    475.   
    476.         char* output = new char[100];  
    477.         sprintf(output,"畸变校正/rightky1/r%d.bmp",i);  
    478.         imwrite(output,PictureCorrection(image,rightIntrinsic,rightDistortion)); //顺次保存校正图  
    479.         delete []output;//释放字符串  
    480.     }  
    481.   
    482.     //使用畸变系数与内参校正左相机原图  
    483.     //如果校正图像目录不存在,则创建该目录  
    484.     system("md 畸变校正\\leftky1");  
    485.   
    486.     for (int i=1; i<=PicNum; i++)  
    487.     {  
    488.         cout<<"左:校正第"<<i<<"张图片"<<endl;  
    489.   
    490.         char* filename = new char[100];  
    491.         sprintf(filename,"素材/leftky1/l%d.bmp",i);  
    492.         image = imread(filename,IMREAD_GRAYSCALE);//顺次读入图片  
    493.         delete []filename;//释放字符串  
    494.   
    495.         char* output = new char[100];  
    496.         sprintf(output,"畸变校正/leftky1/l%d.bmp",i);  
    497.         imwrite(output,PictureCorrection(image,leftIntrinsic,leftDistortion)); //顺次保存校正图  
    498.         delete []output;//释放字符串  
    499.     }  
    500.     cout<<"**********************************************"<<endl;  
    501.     cout<<"                  畸变校正结束                "<<endl;  
    502.     cout<<"**********************************************"<<endl;  
    503. }  
    504.   
    505. //************************************  
    506. // Method:    PictureCorrection  
    507. // FullName:  PictureCorrection  
    508. // Access:    public   
    509. // Returns:   cv::Mat   校正图  
    510. // Qualifier: 根据畸变系数与内参校正一张图片  
    511. // Parameter: Mat image  
    512. // Parameter: float intrinsic[3][3]     内参  
    513. // Parameter: float distortion[1][5]    畸变矩阵  
    514. //************************************  
    515. Mat PictureCorrection( Mat image ,float intrinsic[3][3],float distortion[1][5])  
    516. {  
    517.     Size image_size = image.size();  
    518.   
    519.     Mat intrinsic_matrix = Mat(3,3,CV_32FC1,intrinsic);  
    520.     Mat distortion_coeffs = Mat(1,5,CV_32FC1,distortion);  
    521.     Mat R = Mat::eye(3,3,CV_32F);         
    522.     Mat mapx = Mat(image_size,CV_32FC1);  
    523.     Mat mapy = Mat(image_size,CV_32FC1);      
    524.     initUndistortRectifyMap(intrinsic_matrix,distortion_coeffs,R,intrinsic_matrix,image_size,CV_32FC1,mapx,mapy);  
    525.     Mat t = image.clone();  
    526.     cv::remap( image, t, mapx, mapy, INTER_LINEAR);  
    527.     return t;  
    528. }  
    529.   
    530.   
    531. //************************************  
    532. // Description: 采用hough变换求取大球圆心  
    533. // Method:    Daqiu  
    534. // FullName:  Daqiu  
    535. // Access:    public   
    536. // Returns:   void  
    537. // Author:    bhy  
    538. // Date:      2016/12/28  
    539. // History:  
    540. //************************************  
    541. void Daqiu()  
    542. {  
    543.     cout<<"**********************************************"<<endl;  
    544.     cout<<"                  计算大球圆心                "<<endl;  
    545.     cout<<"**********************************************"<<endl;  
    546.   
    547.     Mat img;  
    548.     //右相机 ContrastAndBright(2.5,50);  
    549.     system("md 大球圆心\\rightky1");  
    550.   
    551.     cout<<"右图:"<<endl;  
    552.   
    553.     for (int i=1; i<=PicNum; i++)  
    554.     {  
    555.         stringstream strStm;  
    556.         string strFileName;  
    557.         strStm << i;  
    558.         strStm >> strFileName;  
    559.         strFileName = "亮度对比度/rightky1/r" + strFileName + ".bmp";  
    560.         img = imread(strFileName,IMREAD_GRAYSCALE);  
    561.   
    562.         GaussianBlur(img,img,Size(5,5),0);  
    563.   
    564.         vector<Vec3f> circles;  
    565.         HoughCircles( img, circles, CV_HOUGH_GRADIENT, 3 ,70, 70, 30, 95 ,100);//hough圆变换  
    566.         Point2f center(0,0);  
    567.         float radius;  
    568.         for (int j = 0; j < circles.size(); j++)  
    569.         {  
    570.             if (circles[j][0] > center.x && circles[j][0] < img.cols/2)  
    571.             {  
    572.                 center.x = circles[j][0];  
    573.                 center.y = circles[j][1];  
    574.                 radius = circles[j][2];//半径   
    575.             }  
    576.         }  
    577.   
    578.         rightDaqiu[i-1][0] = center.x;  
    579.         rightDaqiu[i-1][1] = center.y;  
    580.   
    581.         CvScalar color = CV_RGB(0,0,0);  
    582.         circle( img, (Point)center, radius, color, 3, 8, 0);//绘制圆  
    583.         circle( img, (Point)center, 3, color, 3, 8, 0);//绘制圆心  
    584.         cout<<"圆心:"<<center<<endl;//为了保证精度,以原值输出  
    585.   
    586.         char* output = new char[100];  
    587.         sprintf(output,"大球圆心/rightky1/r%d.bmp",i);  
    588.         imwrite(output,img); //顺次保存校正图  
    589.         delete []output;//释放字符串  
    590.     }  
    591.   
    592.     system("md 大球圆心\\leftky1");  
    593.   
    594.     cout<<"左图:"<<endl;  
    595.   
    596.     for (int i=1; i<=PicNum; i++)  
    597.     {  
    598.         stringstream strStm;  
    599.         string strFileName;  
    600.         strStm << i;  
    601.         strStm >> strFileName;  
    602.         strFileName = "亮度对比度/leftky1/l" + strFileName + ".bmp";  
    603.         img = imread(strFileName,IMREAD_GRAYSCALE);  
    604.   
    605.         GaussianBlur(img,img,Size(5,5),0);  
    606.   
    607.         vector<Vec3f> circles;  
    608.         HoughCircles( img, circles, CV_HOUGH_GRADIENT, 3 ,70, 30, 50, 100 ,110);//hough圆变换  
    609.         Point2f center(0,1024);  
    610.         float radius;  
    611.         for (int j = 0; j < circles.size(); j++)  
    612.         {  
    613.             if (circles[j][1]<center.y)  
    614.             {  
    615.                 center.x = circles[j][0];  
    616.                 center.y = circles[j][1];  
    617.                 radius = circles[j][2];//半径  
    618.             }  
    619.         }  
    620.         leftDaqiu[i-1][0] = center.x;  
    621.         leftDaqiu[i-1][1] = center.y;  
    622.   
    623.         CvScalar color = CV_RGB(0,0,0);  
    624.         circle( img, center, radius, color, 3, 8, 0);//绘制圆  
    625.         circle( img, center, 3, color, 3, 8, 0);//绘制圆心  
    626.         cout<<"圆心:"<<center<<endl;//为了保证精度,以原值输出  
    627.   
    628.         char* output = new char[100];  
    629.         sprintf(output,"大球圆心/leftky1/l%d.bmp",i);  
    630.         imwrite(output,img); //顺次保存校正图  
    631.         delete []output;//释放字符串  
    632.     }  
    633.   
    634.     cout<<"**********************************************"<<endl;  
    635.     cout<<"                  圆心计算结束                "<<endl;  
    636.     cout<<"**********************************************"<<endl;  
    637. }  


     

    运行该程序,会提取生成的“三维坐标.csv”中的空间坐标数据,并绘制运动轨迹需要注意的是,在“三维坐标.csv”文件中直接修改圆心坐标没有用,需要在工程中的initPos()修改。

    [cpp] view plain copy

    1. clc;clear;  
    2. M = csvread('三维坐标.csv',3,6,[3,6,16,8]);  
    3. x = M(:,1);  
    4. y = M(:,2);  
    5. z = M(:,3);  
    6. plot3(x,y,z,'r');  
    7. %legend('大球');  
    8. hold on  
    9.   
    10. M = csvread('三维坐标.csv',20,6,[20,6,33,8]);  
    11. x = M(:,1);  
    12. y = M(:,2);  
    13. z = M(:,3);  
    14. plot3(x,y,z,'g');  
    15. %legend('小球');  
    16. hold on  
    17.   
    18. M = csvread('三维坐标.csv',37,6,[37,6,50,8]);  
    19. x = M(:,1);  
    20. y = M(:,2);  
    21. z = M(:,3);  
    22. plot3(x,y,z,'b');  
    23. %legend('花球');  
    24. hold on  
    25.   
    26. legend('大球','小球','花球');  
    27. title('小球运动轨迹');  
    28. xlabel('x');  
    29. ylabel('y');  
    30. zlabel('z');  
    31. grid on  
    32. axis square  


    最后绘制的轨迹图

     

     补一张世界坐标系的图

    展开全文
  • 图像处理、立体视觉等等方向常常涉及到四个坐标系:世界坐标系、相机坐标系、图像坐标系、像素坐标系。例如下图: 构建世界坐标系只是为了更好的描述相机的位置在哪里,在双目视觉中一般将世界坐标系原点定在左相机...

    转:https://blog.csdn.net/chentravelling/article/details/53558096

     

    1.正文

    图像处理、立体视觉等等方向常常涉及到四个坐标系:世界坐标系、相机坐标系、图像坐标系、像素坐标系。例如下图:
    这里写图片描述
    构建世界坐标系只是为了更好的描述相机的位置在哪里,在双目视觉中一般将世界坐标系原点定在左相机或者右相机或者二者X轴方向的中点。
    接下来的重点,就是关于这几个坐标系的转换。也就是说,一个现实中的物体是如何在图像中成像的。

    1.1世界坐标系与相机坐标系

    这里写图片描述

    于是,从世界坐标系到相机坐标系,涉及到旋转和平移(其实所有的运动也可以用旋转矩阵和平移向量来描述)。绕着不同的坐标轴旋转不同的角度,得到相应的旋转矩阵,如下图所示:
    这里写图片描述

    那么从世界坐标系到相机坐标系的转换关系如下所示:
    这里写图片描述

    1.2相机坐标系与图像坐标系

    从相机坐标系到图像坐标系,属于透视投影关系,从3D转换到2D。
    这里写图片描述
    此时投影点p的单位还是mm,并不是pixel,需要进一步转换到像素坐标系。

    1.3图像坐标系与像素坐标系

    像素坐标系和图像坐标系都在成像平面上,只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成像平面的交点,通常情况下是成像平面的中点或者叫principal point。图像坐标系的单位是mm,属于物理单位,而像素坐标系的单位是pixel,我们平常描述一个像素点都是几行几列。所以这二者之间的转换如下:其中dx和dy表示每一列和每一行分别代表多少mm,即1pixel=dx mm
    这里写图片描述

    那么通过上面四个坐标系的转换就可以得到一个点从世界坐标系如何转换到像素坐标系的。
    这里写图片描述

    其中相机的内参和外参可以通过张正友标定获取(戳这里查看张正友标定的资料)。通过最终的转换关系来看,一个三维中的坐标点,的确可以在图像中找到一个对应的像素点,但是反过来,通过图像中的一个点找到它在三维中对应的点就很成了一个问题,因为我们并不知道等式左边的Zc的值。
    关于三维重建不是我的方向,但是深度值的获取是我项目中的一个需要解决的问题,这将涉及到后面的立体视觉知识。

    展开全文
  • 像素坐标系: 如下图所示:像素坐标系u-v的原点为O0, 横坐标u和纵坐标v分别是图像所在的行和列, 在视觉处理库OpenCV中,u对应x,v对应y; 图像坐标系: 图像坐标系x-y的原点是O1,为像素坐标系的中点, 假设(u0...

    像素坐标系:
    如下图所示:像素坐标系u-v的原点为O0,
    横坐标u和纵坐标v分别是图像所在的行和列,
    在视觉处理库OpenCV中,u对应x,v对应y;
    在这里插入图片描述
    图像坐标系:
    图像坐标系x-y的原点是O1,为像素坐标系的中点,
    假设(u0,v0)代表O1在u-v坐标系下的坐标,dx和dy分别表示每个像素在横轴x和纵轴y的物理尺寸;
    则图像坐标系和像素坐标系的关系如下:
    在这里插入图片描述
    假设物理坐标系中的单位为毫米,那么dx的单位为毫米/像素。
    那么x/dx的单位就是像素了,
    即和u的单位一样。
    为了方便,
    将上式写成矩阵形式:
    在这里插入图片描述
    相机坐标系 以光心为相机坐标系的原点,以平行于图像的x和y方向为Xc轴和Yc轴,Zc轴和光轴平行,Xc,Yc,Zc互相垂直,单位是长度单位。
    图像物理坐标系 以主光轴和图像平面交点为坐标原点,x和y方向如图所示,单位是长度单位。
    图像像素坐标系 以图像的顶点为坐标原点,u和v方向平行于x和y方向,单位是以像素计。
    针孔相机的模型:三维世界中的物体,经过相机成像系统,变成二维图像过程如下所示。
    在这里插入图片描述
    在这里插入图片描述
    相机坐标系通过内参矩阵转换到图像像素坐标系
    这一步是经过两步实现的,一般情况下,相机成像过程中都会存在畸变,因此在标定的过程中需要求取畸变系数。
    畸变
    畸变一般可以分为:径向畸变、切向畸变
    在这里插入图片描述
    其中,k1,k2,k3径向畸变系数,p1,p2是切向畸变系数。畸变发生在相机坐标系转图像物理坐标系的过程中。操作的对象是相机坐标系。
    在这里插入图片描述

    展开全文
  • https://blog.csdn.net/m0_37598482/article/details/78647793

    原文链接:https://blog.csdn.net/m0_37598482/article/details/78647793

    展开全文
  • 1、世界坐标系到相机坐标系之间的转换:世界坐标系是真实世界的基准坐标系,我们需要知道相机坐标系下的点在世界坐标中的位置,利用齐次坐标转换矩阵。 2、相机坐标系到图像...3、图像最坐标系到像素坐标系: ...
  • 世界坐标系-相机坐标系:相机坐标系相对于世界坐标系的转换矩阵 相机坐标系-图像坐标系:属于透视投影关系,真实世界中的景象投射到相机中的转换矩阵,...图像坐标系-像素坐标系:像素和图像间的比例关系 参考 ...
  • 世界坐标系-相机坐标系-像素坐标系世界坐标系,相机坐标系,图像物理坐标系,图像像素坐标系世界坐标系-相机坐标系,外参矩阵相机坐标系-图像物理坐标系-图像像素坐标系,内参矩阵 世界坐标系,相机坐标系,图像物理...
  • 世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换 图像处理、立体视觉等方向常常涉及到四个坐标系:世界坐标系、相机坐标系、图像坐标系、像素坐标系。例如下图: 构建世界坐标系只是为了更好的描述...
  • 像素坐标系与图像坐标系

    万次阅读 2018-07-26 15:53:07
    1. 像素坐标系: 如下图所示:像素坐标系u-v的原点为O0, 横坐标u和纵坐标v分别是图像所在的行和列, 在视觉处理库OpenCV中,u对应x,v对应y; 2. 图像坐标系: 图像坐标系x-y的原点是O1,为像素坐标系的中点...
  • 1.像素坐标系、像平面坐标希、相机坐标系、世界坐标系的定义;2.四个坐标系之间的相互转换关系
  • 一、像素坐标系 OpenCV中像素坐标系的坐标原点在图像的左上角,这种坐标系在结构体Mat,Rect,Point中都是适用的。在Point(x,y)和Rect(x,y)中,第一个参数x代表的是元素所在图像的列数,第二个参数y代表的是元素所在...
  • 世界坐标系,相机坐标系,图像坐标系,图像像素坐标系这四个坐标系的转换实质就是刚体变换、透视投影和数字化图像这几个成像里的步骤。 一、世界坐标系到相机坐标系 世界坐标系,也称为测量坐标系,它是一个三维...
  • 最近在网上看到了很多关于坐标系转换的帖子,但是其内容很多都是相互转载(甚至还有一部分是错误的),同时大部分的文章内容都是告诉你四种坐标系间的相互转化的数学公式,看完之后很多时候还是不知所云,本文意在...
  • OpenGL坐标系和像素坐标系

    千次阅读 2016-05-27 13:06:58
    像素坐标系是同样的中心点,右上是正方向,只是没有做归一化处理,是以像素为单位的。那么,归一化的坐标系和像素坐标最大的区别就是在于,像素是实际的单位,会根据像素数值所变化。而归一化坐标是一个比例,只和...
  • 相机成像原理:世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换
  •   0.前言 最近整理了“相机成像原理”和“视差与深度信息”相关的资料,然后做成了PPT,以备自己用,也提供给相关的...图像处理、立体视觉等等方向常常涉及到四个坐标系:世界坐标系、相机坐标系、图像坐标系、...
  • 一、各坐标系介绍图像处理、立体视觉经常涉及到世界坐标系、相机坐标系、图像坐标系和像素坐标系。如下图所示:世界坐标系是为了确定相机的位置,在双目视觉中一般将世界坐标系原点定在左相机、右相机或两者X轴方向...
  • 1.我做了手眼标定之后,得到了坐标从相机坐标系到机械臂基坐标系的转换方法,但是如何得到坐标从像素坐标系到相机坐标系的转换关系,这样我就可以将坐标从像素坐标系转换到机器人基坐标系,就可以引导机器人抓取目标...
  • 最近在做双目测距,需要用到相机标定、矫正等,但首先需要...首先要理解相机模型中的四个平面坐标系之间的关系:像素平面坐标系(u,v)、图像坐标系(x,y)、相机坐标系(Xc,Yc,Zc)和世界坐标系(Xw,Yw,Zw); ...
  • https://blog.csdn.net/humanking7/article/details/44756073

空空如也

1 2 3 4 5 ... 20
收藏数 41,871
精华内容 16,748
关键字:

像素坐标系