云社区 博客 博客详情

双目 机器视觉-- 测距

一颗小树x 发表于 2020-12-02 22:47:41 2020-12-02
0
0

【摘要】 目录 1.双目图片--测距: 流程说明: 2. 实时采集数据,进行测距 首先进行双目定标,获取双目摄像头内部的参数后,进行测距。 注意:双目定标的效果会影响测距的精准度,建议大家在做双目定标时,做好一些(尽量让误差小)   本次的双目视觉测距,基于BM算法。 1.双目图片--测距: 效果:   本人通过测试,误差是1cm.   其中参数:B...

目录

1.双目图片--测距:

流程说明:

2. 实时采集数据,进行测距


首先进行双目定标,获取双目摄像头内部的参数后,进行测距。

注意:双目定标的效果会影响测距的精准度,建议大家在做双目定标时,做好一些(尽量让误差小)

 

本次的双目视觉测距,基于BM算法。

1.双目图片--测距:

效果:

 

本人通过测试,误差是1cm.

 

其中参数:BlockSize、UniquenessRatio、NumDisparities 根据实际情况来调整;

 

源代码:


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

 

流程说明:

先采集左右摄像头的图片,然后,修改一下指定的图片,可以进行测距。

里面有双目摄像头的参数,具体需要自己定标和矫正后,然后,填入。

双目定标可以参考:双目视觉 定标+矫正 (基于MATLAB)

双目数据转化可以参考:双目视觉 三维重建、测距 ---准备工作(数据转化)

 

详细讲解摄像头参数:

1)Mat cameraMatrixL                                                                左相机的内参矩阵

2)Mat distCoeffL = (Mat_<double>(5, 1) .......                          左相机 畸变参数    即K1K2P1P2K3

3) Mat cameraMatrixR                                                               右相机的内参矩阵

4)Mat distCoeffR = (Mat_<double>(5, 1)  .......                          右相机畸变参数    即K1K2P1P2K3

5) Mat T = (Mat_<double>(3, 1) << -1.210187345641146e+02, 0.519235426836325, -0.425535566316217);//  相机的 平移向量

6) Mat rec = (Mat_<double>(3, 3) << 0.99934112270088...................        相机的旋转向量 

一共6个相机参数,1、2是 左相机的参数; 3、4是 右相机的参数; 5、6是相机(相对)整体的参数。

 

 

 

2. 实时采集数据,进行测距

效果:

 

源代码:


  
  1. /******************************/
  2. /* 立体匹配和测距 */
  3. /******************************/
  4. #include <opencv2/opencv.hpp>
  5. #include <iostream>
  6. #include <math.h>
  7. using namespace std;
  8. using namespace cv;
  9. const int imageWidth = 640; //摄像头的分辨率
  10. const int imageHeight = 360;
  11. Vec3f point3;
  12. float d;
  13. Size imageSize = Size(imageWidth, imageHeight);
  14. Mat rgbImageL, grayImageL;
  15. Mat rgbImageR, grayImageR;
  16. Mat rectifyImageL, rectifyImageR;
  17. Rect validROIL;//图像校正之后,会对图像进行裁剪,这里的validROI就是指裁剪之后的区域
  18. Rect validROIR;
  19. Mat mapLx, mapLy, mapRx, mapRy; //映射表
  20. Mat Rl, Rr, Pl, Pr, Q; //校正旋转矩阵R,投影矩阵P 重投影矩阵Q
  21. Mat xyz; //三维坐标
  22. Point origin; //鼠标按下的起始点
  23. Rect selection; //定义矩形选框
  24. bool selectObject = false; //是否选择对象
  25. int blockSize = 0, uniquenessRatio = 0, numDisparities = 0;
  26. Ptr<StereoBM> bm = StereoBM::create(16, 9);
  27. /*事先标定好的左相机的内参矩阵
  28. fx 0 cx
  29. 0 fy cy
  30. 0 0 1
  31. */
  32. Mat cameraMatrixL = (Mat_<double>(3, 3) << 418.523322187048, -1.26842201390676, 343.908870120890,
  33. 0, 421.222568242056, 235.466208987968,
  34. 0, 0, 1);
  35. //获得的畸变参数
  36. /*418.523322187048 0 0
  37. -1.26842201390676 421.222568242056 0
  38. 344.758267538961 243.318992284899 1 */ //2
  39. Mat distCoeffL = (Mat_<double>(5, 1) << 0.006636837611004, 0.050240447649195, 0.006681263320267, 0.003130367429418, 0);
  40. //[0.006636837611004,0.050240447649195] [0.006681263320267,0.003130367429418]
  41. /*事先标定好的右相机的内参矩阵
  42. fx 0 cx
  43. 0 fy cy
  44. 0 0 1
  45. */
  46. Mat cameraMatrixR = (Mat_<double>(3, 3) << 417.417985082506, 0.498638151824367, 309.903372309072,
  47. 0, 419.795432389420, 230.6,
  48. 0, 0, 1);
  49. /*
  50. 417.417985082506 0 0
  51. 0.498638151824367 419.795432389420 0
  52. 309.903372309072 236.256106972796 1
  53. */ //2
  54. Mat distCoeffR = (Mat_<double>(5, 1) << -0.038407383078874, 0.236392800301615, 0.004121779274885, 0.002296129959664, 0);
  55. //[-0.038407383078874,0.236392800301615] [0.004121779274885,0.002296129959664]
  56. Mat T = (Mat_<double>(3, 1) << -1.210187345641146e+02, 0.519235426836325, -0.425535566316217);//T平移向量
  57. //[-1.210187345641146e+02,0.519235426836325,-0.425535566316217]
  58. //对应Matlab所得T参数
  59. //Mat rec = (Mat_<double>(3, 1) << -0.00306, -0.03207, 0.00206);//rec旋转向量,对应matlab om参数 我
  60. Mat rec = (Mat_<double>(3, 3) << 0.999341122700880, -0.00206388651740061, 0.0362361815232777,
  61. 0.000660748031451783, 0.999250989651683, 0.0386913826603732,
  62. -0.0362888948713456, -0.0386419468010579, 0.998593969567432); //rec旋转向量,对应matlab om参数 我
  63. /* 0.999341122700880 0.000660748031451783 -0.0362888948713456
  64. -0.00206388651740061 0.999250989651683 -0.0386419468010579
  65. 0.0362361815232777 0.0386913826603732 0.998593969567432 */
  66. //Mat T = (Mat_<double>(3, 1) << -48.4, 0.241, -0.0344);//T平移向量
  67. //[-1.210187345641146e+02,0.519235426836325,-0.425535566316217]
  68. //对应Matlab所得T参数
  69. Mat R;//R 旋转矩阵
  70. /*****立体匹配*****/
  71. void stereo_match(int, void*)
  72. {
  73. bm->setBlockSize(2 * blockSize + 5); //SAD窗口大小,5~21之间为宜
  74. bm->setROI1(validROIL);
  75. bm->setROI2(validROIR);
  76. bm->setPreFilterCap(31);
  77. bm->setMinDisparity(0); //最小视差,默认值为0, 可以是负值,int型
  78. bm->setNumDisparities(numDisparities * 16 + 16);//视差窗口,即最大视差值与最小视差值之差,窗口大小必须是16的整数倍,int型
  79. bm->setTextureThreshold(10);
  80. bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配
  81. bm->setSpeckleWindowSize(100);
  82. bm->setSpeckleRange(32);
  83. bm->setDisp12MaxDiff(-1);
  84. Mat disp, disp8;
  85. bm->compute(rectifyImageL, rectifyImageR, disp);//输入图像必须为灰度图
  86. disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式
  87. reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时,ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16),才能得到正确的三维坐标信息。
  88. xyz = xyz * 16;
  89. imshow("disparity", disp8);
  90. }
  91. /*****描述:鼠标操作回调*****/
  92. static void onMouse(int event, int x, int y, int, void*)
  93. {
  94. if (selectObject)
  95. {
  96. selection.x = MIN(x, origin.x);
  97. selection.y = MIN(y, origin.y);
  98. selection.width = std::abs(x - origin.x);
  99. selection.height = std::abs(y - origin.y);
  100. }
  101. switch (event)
  102. {
  103. case EVENT_LBUTTONDOWN: //鼠标左按钮按下的事件
  104. origin = Point(x, y);
  105. selection = Rect(x, y, 0, 0);
  106. selectObject = true;
  107. //cout << origin << "in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
  108. point3 = xyz.at<Vec3f>(origin);
  109. point3[0];
  110. //cout << "point3[0]:" << point3[0] << "point3[1]:" << point3[1] << "point3[2]:" << point3[2]<<endl;
  111. cout << "世界坐标:" << endl;
  112. cout << "x: " << point3[0] << " y: " << point3[1] << " z: " << point3[2] << endl;
  113. d = point3[0] * point3[0]+ point3[1] * point3[1]+ point3[2] * point3[2];
  114. d = sqrt(d); //mm
  115. // cout << "距离是:" << d << "mm" << endl;
  116. d = d / 10.0; //cm
  117. cout << "距离是:" << d << "cm" << endl;
  118. // d = d/1000.0; //m
  119. // cout << "距离是:" << d << "m" << endl;
  120. break;
  121. case EVENT_LBUTTONUP: //鼠标左按钮释放的事件
  122. selectObject = false;
  123. if (selection.width > 0 && selection.height > 0)
  124. break;
  125. }
  126. }
  127. /*****主函数*****/
  128. int main()
  129. {
  130. /*
  131. 立体校正
  132. */
  133. Rodrigues(rec, R); //Rodrigues变换
  134. stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
  135. 0, imageSize, &validROIL, &validROIR);
  136. initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);
  137. initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
  138. /*
  139. 打开摄像头
  140. */
  141. VideoCapture cap;
  142. cap.open(1); //打开相机,电脑自带摄像头一般编号为0,外接摄像头编号为1,主要是在设备管理器中查看自己摄像头的编号。
  143. cap.set(CV_CAP_PROP_FRAME_WIDTH, 2560); //设置捕获视频的宽度
  144. cap.set(CV_CAP_PROP_FRAME_HEIGHT, 720); //设置捕获视频的高度
  145. if (!cap.isOpened()) //判断是否成功打开相机
  146. {
  147. cout << "摄像头打开失败!" << endl;
  148. return -1;
  149. }
  150. Mat frame, frame_L, frame_R;
  151. cap >> frame; //从相机捕获一帧图像
  152. cout << "Painted ImageL" << endl;
  153. cout << "Painted ImageR" << endl;
  154. while (1) {
  155. double fScale = 0.5; //定义缩放系数,对2560*720图像进行缩放显示(2560*720图像过大,液晶屏分辨率较小时,需要缩放才可完整显示在屏幕)
  156. Size dsize = Size(frame.cols*fScale, frame.rows*fScale);
  157. Mat imagedst = Mat(dsize, CV_32S);
  158. resize(frame, imagedst, dsize);
  159. char image_left[200];
  160. char image_right[200];
  161. frame_L = imagedst(Rect(0, 0, 640, 360)); //获取缩放后左Camera的图像
  162. // namedWindow("Video_L", 1);
  163. // imshow("Video_L", frame_L);
  164. frame_R = imagedst(Rect(640, 0, 640, 360)); //获取缩放后右Camera的图像
  165. // namedWindow("Video_R", 2);
  166. // imshow("Video_R", frame_R);
  167. cap >> frame;
  168. /*
  169. 读取图片
  170. */
  171. //rgbImageL = imread("image_left_1.jpg", CV_LOAD_IMAGE_COLOR);
  172. cvtColor(frame_L, grayImageL, CV_BGR2GRAY);
  173. //rgbImageR = imread("image_right_1.jpg", CV_LOAD_IMAGE_COLOR);
  174. cvtColor(frame_R, grayImageR, CV_BGR2GRAY);
  175. // imshow("ImageL Before Rectify", grayImageL);
  176. // imshow("ImageR Before Rectify", grayImageR);
  177. /*
  178. 经过remap之后,左右相机的图像已经共面并且行对准了
  179. */
  180. remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
  181. remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
  182. /*
  183. 把校正结果显示出来
  184. */
  185. Mat rgbRectifyImageL, rgbRectifyImageR;
  186. cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR); //伪彩色图
  187. cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR);
  188. //单独显示
  189. //rectangle(rgbRectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);
  190. //rectangle(rgbRectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);
  191. // imshow("ImageL After Rectify", rgbRectifyImageL);
  192. // imshow("ImageR After Rectify", rgbRectifyImageR);
  193. //显示在同一张图上
  194. Mat canvas;
  195. double sf;
  196. int w, h;
  197. sf = 600. / MAX(imageSize.width, imageSize.height);
  198. w = cvRound(imageSize.width * sf);
  199. h = cvRound(imageSize.height * sf);
  200. canvas.create(h, w * 2, CV_8UC3); //注意通道
  201. //左图像画到画布上
  202. Mat canvasPart = canvas(Rect(w * 0, 0, w, h)); //得到画布的一部分
  203. resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA); //把图像缩放到跟canvasPart一样大小
  204. Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf), //获得被截取的区域
  205. cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
  206. //rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8); //画上一个矩形
  207. // cout << "Painted ImageL" << endl;
  208. //右图像画到画布上
  209. canvasPart = canvas(Rect(w, 0, w, h)); //获得画布的另一部分
  210. resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
  211. Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
  212. cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
  213. //rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);
  214. // cout << "Painted ImageR" << endl;
  215. //画上对应的线条
  216. for (int i = 0; i < canvas.rows; i += 16)
  217. line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
  218. imshow("rectified", canvas);
  219. /*
  220. 立体匹配
  221. */
  222. namedWindow("disparity", CV_WINDOW_AUTOSIZE);
  223. // 创建SAD窗口 Trackbar
  224. createTrackbar("BlockSize:\n", "disparity", &blockSize, 8, stereo_match);
  225. // 创建视差唯一性百分比窗口 Trackbar
  226. createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);
  227. // 创建视差窗口 Trackbar
  228. createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);
  229. //鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数,一般取0)
  230. setMouseCallback("disparity", onMouse, 0);
  231. stereo_match(0, 0);
  232. waitKey(10);
  233. } //wheil
  234. return 0;
  235. }

 

希望对你有帮助。

 

补充说明:

1.关于如何求出世界坐标?

1)x,y,z 是由

Vec3f point3;

point3 = xyz.at<Vec3f>(origin); 来转化的。

cout << "x: " << point3[0] << "  y: " << point3[1] << "  z: " << point3[2] << endl;
 

2)x,y,z求平方和后开根号,是两点的距离公式,即点(0,0,0)------双目摄像头的中心点,和点(x,y,z)进行两点求距离。

 

 

 

文章来源: guo-pu.blog.csdn.net,作者:一颗小树x,版权归原作者所有,如需转载,请联系作者。

原文链接:guo-pu.blog.csdn.net/article/details/86744936

登录后可下载附件,请登录或者注册

【版权声明】本文为华为云社区用户原创内容,转载时必须标注文章的来源(华为云社区),文章链接,文章作者等基本信息,否则作者和本社区有权追究责任。如果您发现本社区中有涉嫌抄袭的内容,欢迎发送邮件至:huaweicloud.bbs@huawei.com进行举报,并提供相关证据,一经查实,本社区将立刻删除涉嫌侵权内容。
评论文章 //点赞 收藏 0
点赞
分享文章到微博
分享文章到朋友圈

上一篇:机器学习算法 综述(入门)

下一篇:软考免费刷题 解决方法

评论 (0)


登录后可评论,请 登录注册

评论