立体视觉标定源代码C++,简单粗暴!粗暴·······

疑点解答:

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

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

(1)

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

 

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

转::标定双目后,首先要根据其畸变系数来校正原图,可以参考

http://blog.csdn.net/qq_15947787/article/details/51471535

 

[cpp] view plain copy

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

 

 

 

 

 

 

 

 

校正完成后就可以进行坐标计算了,分两种

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

 

代码如下:

[cpp] view plain copy

  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. }  


2017/5/26补充 

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

[cpp] view plain copy

  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)

 

光轴平行模型要得到视差图,http://blog.csdn.net/wangchao7281/article/details/52506691?locationNum=7

可以参考opencv的例子,例子的使用方法可以参考http://blog.csdn.net/t247555529/article/details/48046859

得到视差图后可以调用cvReprojectImageTo3D输出的三维坐标

看到很多人输出三维坐标时z出现10000,那个其实是输出方式不对,应该是下面这样

[cpp] view plain copy

  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)。其具体代码表示如下:

[cpp] view plain copy

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

 

[cpp] view plain copy

  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. }  

 

 

 

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

最后是完整的大作业代码,感觉自己写的并不是很好,敷衍了事~~

运行该程序,会对素材文件夹中图像进行处理,生成三个文件夹,分别是“大球圆心”、“畸变校正”、“亮度对比度”以及一个csv文件,文件名为“三维坐标.csv”,里面记录了计算得到的左右相机中球的像面坐标和解算出的空间坐标。该程序默认对所有计算出的圆心坐标进行了重新赋值,如需修改圆心坐标,需在工程中的initPos()修改赋值语句,如需程序自动计算,在主函数中注释该指令即可,但是自动计算的圆心并不准确。

 

[cpp] view plain copy

  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  


最后绘制的轨迹图

 

 补一张世界坐标系的图

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/252002.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

11. 临时表

-- 查询5大洲国家总数 SELECT continent,COUNT(*) FROM country GROUP BY continent;-- 演示临时表 CREATE TEMPORARY TABLE tmp_table ( continent VARCHAR(20), COUNT INT );INSERT INTO tmp_table SELECT Asia AS continent,COUNT(*) FROM country WHERE continent Asia;…

MongoDB负载信息一目了然 阿里云HDM重磅发布MongoDB监控和诊断功

2019独角兽企业重金招聘Python工程师标准>>> 混合云数据库管理&#xff08;HDM&#xff09;的统一监控、告警、诊断功能新增了对MongoDB的支持。 通过直观的方式将MongoDB多个维度的负载信息统一整合&#xff0c;不仅可以清晰的查看实时负载信息&#xff0c;也可以方…

在iview的Table中添加Select(render)

首先对Render进行分析&#xff0c;在iview官方的文档中&#xff0c;找到了table插入Button的例子&#xff1a; [javascript] view plaincopy { title: Action, key: action, width: 150, align: center, render: (h, params) > { return h(div, [ h(Butt…

JavaScript中call和apply方法

1 /*2 在js中 call和apply常用于绑定作用域3 */4 //1 简单的绑定5 function sum(a,b){6 return ab;7 }8 //将sum的功能绑定给test2来执行9 function test2(a,b){ 10 return sum.call(this,a,b); 11 } 12 // call 和apply的区别是 apply接收数组作为参数…

工业机械人运动学正逆解,简单粗暴!!!!!!

ur机械臂是六自由度机械臂&#xff0c;由D-H参数法确定它的运动学模型&#xff0c;连杆坐标系的建立如上图所示。 转动关节θi是关节变量&#xff0c;连杆偏移di是常数。 关节编号 α&#xff08;绕x轴&#xff09; a&#xff08;沿x轴&#xff09; θ&#xff08;绕z轴&am…

python opencv立体测距 立体匹配BM算法

立体标定应用标定数据转换成深度图标定 在开始之前&#xff0c;需要准备的当然是两个摄相头&#xff0c;根据你的需求将两个摄像头进行相对位置的固定&#xff0c;我是按平行来进行固定的&#xff08;如果为了追求两个双目图像更高的生命度&#xff0c;也可以将其按一定钝角固…

对于python 作用域新的理解

今天看Python习题&#xff0c;看到如下题目 def num():return [lambda x: i*x for i in range(4)] print([m(2) for m in num()])  # 求输出结果是什么 我看了半天才明白这应该是一个列表生成式&#xff0c;列表中的元素为四个匿名函数&#xff0c;我本以为每个匿名函数应该是…

Vue基础学习(一)------内部指令

一.v-if v-else v-show 指令 1.v-if v-if:是vue 的一个内部指令&#xff0c;指令用在我们的html中,用来判断是否加载html的DOM 现在举个栗子&#xff0c;判断用户的登录操作&#xff0c;用isLogin作为一个判断字段&#xff0c;登录成功&#xff0c;就显示用户的名称 代码&…

【bzoj3555】[Ctsc2014]企鹅QQ 简单哈希

传送门 题目分析 题意即求有多少对字符串只相差一个字符&#xff0c;枚举删除每个字符后的哈希&#xff0c; 看有多少相等即可。 比如有如下字符串&#xff1a;$Sd123$&#xff0c;其中S部分的哈希值为H&#xff0c;删除的是d&#xff0c;则原字符串的哈希值为$$(((H * T d) *…

StereoRectify()函数定义及用法畸变矫正与立体校正

畸变矫正是上一篇博文的遗留问题&#xff0c;当畸变系数和内外参数矩阵标定完成后&#xff0c;就应该进行畸变的矫正&#xff0c;以达到消除畸变的目的&#xff0c;此其一。 在该系列第一部分的博文中介绍的立体成像原理中提到&#xff0c;要通过两幅图像估计物点的深度信息&a…

死磕 java集合之TreeMap源码分析(三)- 内含红黑树分析全过程

2019独角兽企业重金招聘Python工程师标准>>> 欢迎关注我的公众号“彤哥读源码”&#xff0c;查看更多源码系列文章, 与彤哥一起畅游源码的海洋。 删除元素 删除元素本身比较简单&#xff0c;就是采用二叉树的删除规则。 &#xff08;1&#xff09;如果删除的位置有两…

Linux:进程实例信息(/proc)

https://blog.csdn.net/test1280/article/details/73632333 Linux:进程实例信息&#xff08;/proc&#xff09; 问几个问题&#xff1a; 1.怎么知道一个进程对应哪个可执行文件&#xff1f; 2.怎么知道一个进程的资源限制&#xff1f; 3.怎么知道一个进程所处的环境&#xff1f…

四元素理解

旋转变换_四元数 2017年03月29日 11:59:38 csxiaoshui 阅读数&#xff1a;5686 1.简介 四元数是另一种描述三维旋转的方式&#xff0c;四元数使用4个分量来描述旋转&#xff0c;四元数的描述方式如下&#xff1a; qsxiyjzk,(s,x,y,z∈ℝ&#xff09;i2j2k2ijk−1 四元数的由…

31、SAM文件中flag含义解释工具--转载

转载&#xff1a;http://www.cnblogs.com/nkwy2012/p/6362996.html SAM是Sequence Alignment/Map 的缩写。像bwa等软件序列比对结果都会输出这样的文件。samtools网站上有专门的文档介绍SAM文件。具体地址&#xff1a;http://samtools.sourceforge.net/SAM1.pdf很多人困惑SAM文…

《Head First设计模式》批注系列(一)——观察者设计模式

最近在读《Head First设计模式》一书&#xff0c;此系列会引用源书内容&#xff0c;但文章内容会更加直接&#xff0c;以及加入一些自己的理解。 观察者模式&#xff08;有时又被称为模型-视图&#xff08;View&#xff09;模式、源-收听者(Listener)模式或从属者模式&#xff…

PYPL 4 月排行:Python 最流行,Java 还行不行?

开发四年只会写业务代码&#xff0c;分布式高并发都不会还做程序员&#xff1f; PYPL 发布了 4 月份的编程语言排行榜。 前五的分别是&#xff1a;Python、Java、Javascript、C# 和 PHP。可以看到&#xff0c;榜单没有什么大变化&#xff0c;但是相比去年 4 月份&#xff0c;…

两个向量的旋转矩阵与四元素

两向量的夹角 2017年06月20日 17:38:11 csxiaoshui 阅读数&#xff1a;36764 怎么计算两个向量间的夹角呢&#xff1f; 这里主要分两种情况&#xff0c;对于二维向量和三维向量来分别讨论。 1. 二维向量 二维向量的情况相对简单&#xff0c;根据向量间的点乘关系 v1⋅v2|…

顺序表

一、数据是如何在内存中存储的&#xff1f; 32位系统中char&#xff0c;int型数据在内存中的存储方式&#xff1a; char占1byte&#xff08;8bit&#xff09;int占4byte&#xff08;32bit&#xff09;假设我们有一个int类型的值&#xff0c;它从0x01开始&#xff0c;一个int占据…

Establishing SSL connection without server's identity verification is not recommended.

完全描述:Establishing SSL connection without servers identity verification is not recommended. According to MySQL 5.5.45, 5.6.26 and 5.7.6 requirements SSL connection must be established by default if explicit option isnt set. For compliance with existing …

四元素的真面目..........简单粗暴

作者&#xff1a;Yang Eninala 链接&#xff1a;https://www.zhihu.com/question/23005815/answer/33971127 来源&#xff1a;知乎 著作权归作者所有。商业转载请联系作者获得授权&#xff0c;非商业转载请注明出处。 根据我的理解&#xff0c;大多数人用汉密尔顿四元数就只…