疑點解答：

攝像機矩陣由內參矩陣和外參矩陣組成，對攝像機矩陣進行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??

最后繪制的軌跡圖

?

?補一張世界坐標系的圖