TA的每日心情 | 慵懒 2024-5-31 23:20 |
---|
签到天数: 302 天 连续签到: 2 天 [LV.8]以坛为家I
|
(一)
根据上一张帖子标定的结果:- Mat cameraMatrixL = (Mat_<double>(3, 3) << 274.07311, 0, 334.46683,
- 0, 273.77609, 230.80436,
- 0, 0, 1);
- Mat distCoeffL = (Mat_<double>(5, 1) << -0.21470, 0.03484, -0.00121, -0.00279, 0.00000);
- Mat cameraMatrixR = (Mat_<double>(3, 3) << 284.34939, 0, 283.30332,
- 0, 285.84734, 243.33090,
- 0, 0, 1);
- Mat distCoeffR = (Mat_<double>(5, 1) << -0.21396, 0.03400, -0.00154, 0.00204, 0.00000);
- Mat T = (Mat_<double>(3, 1) << -90.67152, -2.93721, 16.09332);//T平移向量
- Mat rec = (Mat_<double>(3, 1) << 0.02428, 0.15673, -0.00825);//rec旋转向量
- Mat R;//R 旋转矩阵
复制代码 带入双目测距程序里面:- /******************************/
- /* 立体匹配和测距 */
- /******************************/
- #include <opencv2/opencv.hpp>
- #include <imgproc.hpp>
- #include <iostream>
- #include <time.h>
- using namespace std;
- using namespace cv;
- const int imageWidth = 640; //摄像头的分辨率
- const int imageHeight = 480;
- Size imageSize = Size(imageWidth, imageHeight);
- Mat rgbImageL, grayImageL;
- Mat rgbImageR, grayImageR;
- Mat rectifyImageL, rectifyImageR;
- Rect validROIL;//图像校正之后,会对图像进行裁剪,这里的validROI就是指裁剪之后的区域
- Rect validROIR;
- Mat mapLx, mapLy, mapRx, mapRy; //映射表
- Mat Rl, Rr, Pl, Pr, Q; //校正旋转矩阵R,投影矩阵P 重投影矩阵Q
- Mat xyz; //三维坐标
- Point origin; //鼠标按下的起始点
- Rect selection; //定义矩形选框
- bool selectObject = false; //是否选择对象
- int blockSize = 0, uniquenessRatio =0, numDisparities=0;
- Ptr<StereoBM> bm = StereoBM::create(16, 9);
- /*
- 事先标定好的相机的参数
- fx 0 cx
- 0 fy cy
- 0 0 1
- */
- Mat cameraMatrixL = (Mat_<double>(3, 3) << 274.07311, 0, 334.46683,
- 0, 273.77609, 230.80436,
- 0, 0, 1);
- Mat distCoeffL = (Mat_<double>(5, 1) << -0.21470, 0.03484, -0.00121, -0.00279, 0.00000);
- Mat cameraMatrixR = (Mat_<double>(3, 3) << 284.34939, 0, 283.30332,
- 0, 285.84734, 243.33090,
- 0, 0, 1);
- Mat distCoeffR = (Mat_<double>(5, 1) << -0.21396, 0.03400, -0.00154, 0.00204, 0.00000);
- Mat T = (Mat_<double>(3, 1) << -90.67152, -2.93721, 16.09332);//T平移向量
- Mat rec = (Mat_<double>(3, 1) << 0.02428, 0.15673, -0.00825);//rec旋转向量
- Mat R;//R 旋转矩阵
- /*Mat cameraMatrixL = (Mat_<double>(3, 3) << 268.0870787552206, 0, 335.7271612649919,
- 0, 268.0500620563969, 228.181598739472,
- 0, 0, 1);
- Mat distCoeffL = (Mat_<double>(5, 1) << -0.2320648740970932, 0.04510313360911739, -9.418532128676933e-05, -0.0008271590194063145, 0);
- Mat cameraMatrixR = (Mat_<double>(3, 3) << 268.0746191870755, 0, 313.554845275748,
- 0, 267.8032936757982, 237.0287771634367,
- 0, 0, 1);
- Mat distCoeffR = (Mat_<double>(5, 1) << -0.2391280628833069, 0.04899490848168394, 7.734678017422217e-05, 7.975132256511924e-05, 0);
- Mat T = (Mat_<double>(3, 1) << -90.67152, -2.93721, 16.09332);//T平移向量
- Mat rec = (Mat_<double>(3, 1) << 0.02428, 0.15673, -0.00825);//rec旋转向量
- Mat R;//R 旋转矩阵*/
- /*****立体匹配*****/
- void stereo_match(int,void*)
- {
- bm->setBlockSize(2*blockSize+5); //SAD窗口大小,5~21之间为宜
- bm->setROI1(validROIL);
- bm->setROI2(validROIR);
- bm->setPreFilterCap(31);
- bm->setMinDisparity(0); //最小视差,默认值为0, 可以是负值,int型
- bm->setNumDisparities(numDisparities*16+16);//视差窗口,即最大视差值与最小视差值之差,窗口大小必须是16的整数倍,int型
- bm->setTextureThreshold(10);
- bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配
- bm->setSpeckleWindowSize(100);
- bm->setSpeckleRange(32);
- bm->setDisp12MaxDiff(-1);
- Mat disp, disp8;
- bm->compute(rectifyImageL, rectifyImageR, disp);//输入图像必须为灰度图
- disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式
- reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时,ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16),才能得到正确的三维坐标信息。
- xyz = xyz * 16;
- Point p;
- p.x = 400; p.y = 300;
- cout << "in world coordinate: " << xyz.at<Vec3f>(p) << endl;
- imshow("disparity", disp8);
- }
- /*****描述:鼠标操作回调*****/
- static void onMouse(int event, int x, int y, int, void*)
- {
- if (selectObject)
- {
- selection.x = MIN(x, origin.x);
- selection.y = MIN(y, origin.y);
- selection.width = std::abs(x - origin.x);
- selection.height = std::abs(y - origin.y);
- }
- switch (event)
- {
- case EVENT_LBUTTONDOWN: //鼠标左按钮按下的事件
- origin = Point(x, y);
- selection = Rect(x, y, 0, 0);
- selectObject = true;
- cout << origin <<"in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
- break;
- case EVENT_LBUTTONUP: //鼠标左按钮释放的事件
- selectObject = false;
- if (selection.width > 0 && selection.height > 0)
- break;
- }
- }
- /*****主函数*****/
- int main()
- {
- int counter_frame = 0;
- /*
- 立体校正
- */
- Rodrigues(rec, R); //Rodrigues变换
- stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
- 0, imageSize, &validROIL, &validROIR);
- initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
- initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
- VideoCapture left("Left.avi");
- VideoCapture right("Right.avi");
- Mat left_frame,right_frame;
- if(!left.isOpened() || !right.isOpened())
- {
- cout<<"open error"<<endl;
- return -1;
- }
- clock_t start_clock,end_clock;
- start_clock = clock();
- while(1)
- {
- if(counter_frame++ >= 30)
- {
- counter_frame = 0;
- end_clock = clock();
- std::cout << end_clock - start_clock << std::endl;
- start_clock = clock();
- }
- /*
- 读取图片
- */
- left >> rgbImageL;
- right >> rgbImageR;
- cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);
- cvtColor(rgbImageR, grayImageR, CV_BGR2GRAY);
- imshow("ImageL Before Rectify", grayImageL);
- imshow("ImageR Before Rectify", grayImageR);
- /*
- 经过remap之后,左右相机的图像已经共面并且行对准了
- */
- remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
- remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
- /*
- 把校正结果显示出来
- */
- Mat rgbRectifyImageL, rgbRectifyImageR;
- cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR); //伪彩色图
- cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR);
- //单独显示
- //rectangle(rgbRectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);
- //rectangle(rgbRectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);
- imshow("ImageL After Rectify", rgbRectifyImageL);
- imshow("ImageR After Rectify", rgbRectifyImageR);
- //显示在同一张图上
- Mat canvas;
- double sf;
- int w, h;
- sf = 600. / MAX(imageSize.width, imageSize.height);
- w = cvRound(imageSize.width * sf);
- h = cvRound(imageSize.height * sf);
- canvas.create(h, w * 2, CV_8UC3); //注意通道
- //左图像画到画布上
- Mat canvasPart = canvas(Rect(w * 0, 0, w, h)); //得到画布的一部分
- resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA); //把图像缩放到跟canvasPart一样大小
- Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf), //获得被截取的区域
- cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
- //rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8); //画上一个矩形
- cout << "Painted ImageL" << endl;
- //右图像画到画布上
- canvasPart = canvas(Rect(w, 0, w, h)); //获得画布的另一部分
- resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
- Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
- cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
- //rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);
- cout << "Painted ImageR" << endl;
- //画上对应的线条
- for (int i = 0; i < canvas.rows; i += 16)
- line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
- imshow("rectified", canvas);
- /*
- 立体匹配
- */
- namedWindow("disparity", CV_WINDOW_AUTOSIZE);
- // 创建SAD窗口 Trackbar
- createTrackbar("BlockSize:\n", "disparity",&blockSize, 8, stereo_match);
- // 创建视差唯一性百分比窗口 Trackbar
- createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);
- // 创建视差窗口 Trackbar
- createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);
- //鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数,一般取0)
- setMouseCallback("disparity", onMouse, 0);
- stereo_match(0,0);
- waitKey(30);
- }
- return 0;
- }
复制代码 (二)双目视频获取
由于Khadas的usb线宽不足以采集手头两个usb摄像头1280*720的分辨率图片,春节将至,快递都停了,一时间又不好再买新的摄像头,只好用pc采集视频流做成avi再拷贝到开发板代替双目摄像头:
代码:- #include <opencv2/core/core.hpp>
- #include <opencv2/highgui/highgui.hpp>
- using namespace cv;
- int main()
- {
- int counter;
- VideoCapture capture_left(0);//如果是笔记本,0打开的是自带的摄像头,1 打开外接的相机
- VideoCapture capture_right(1);
- double rate = 25.0;//视频的帧率
- Size videoSize(640,480);
- VideoWriter writer_left("Left2.avi", CV_FOURCC('M', 'J', 'P', 'G'), rate, videoSize);
- VideoWriter writer_right("Right2.avi", CV_FOURCC('M', 'J', 'P', 'G'), rate, videoSize);
- Mat left,right;
- while (capture_left.isOpened() || capture_right.isOpened())
- {
- capture_left >> left;
- capture_right >> right;
- writer_left << left;
- writer_right << right;
- imshow("video_left2", left);
- imshow("video_right2", right);
- if (waitKey(20) == 27)//27是键盘摁下esc时,计算机接收到的ascii码值
- {
- break;
- }
- if(counter++ >=200)
- break;
- }
- capture_left.release();
- capture_right.release();
- return 0;
- }
复制代码 (三)效果
部分结构的log:- in world coordinate: [306.073, 641.233, 2638.34]
- Painted ImageL
- Painted ImageR
- in world coordinate: [215.967, 452.458, 1861.64]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [385.141, 806.884, 3319.92]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [491.67, 1030.07, 4238.19]
- Painted ImageL
- Painted ImageR
- in world coordinate: [248.478, 520.57, 2141.88]
- Painted ImageL
- Painted ImageR
- in world coordinate: [320.951, 672.404, 2766.6]
- Painted ImageL
- Painted ImageR
- in world coordinate: [332.496, 696.591, 2866.11]
- Painted ImageL
- Painted ImageR
- in world coordinate: [378.828, 793.657, 3265.49]
- Painted ImageL
- Painted ImageR
- in world coordinate: [192.571, 403.442, 1659.96]
- Painted ImageL
- Painted ImageR
- in world coordinate: [212.981, 446.203, 1835.9]
- Painted ImageL
- Painted ImageR
- in world coordinate: [249.821, 523.384, 2153.46]
- Painted ImageL
- Painted ImageR
- in world coordinate: [316.555, 663.193, 2728.7]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [2310.85, 4841.31, 19919.5]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [385.141, 806.884, 3319.92]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [2567.61, 5379.23, 22132.8]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [inf, inf, inf]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- 9302492
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [2432.47, 5096.11, 20967.9]
- Painted ImageL
- Painted ImageR
- in world coordinate: [249.821, 523.384, 2153.46]
- Painted ImageL
- Painted ImageR
- in world coordinate: [192.571, 403.442, 1659.96]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [253.939, 532.012, 2188.96]
- Painted ImageL
- Painted ImageR
- in world coordinate: [inf, inf, inf]
- Painted ImageL
- Painted ImageR
- in world coordinate: [inf, inf, inf]
- Painted ImageL
- Painted ImageR
- in world coordinate: [711.03, 1489.63, 6129.08]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [1711.74, 3586.15, 14755.2]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [872.018, 1826.91, 7516.79]
- Painted ImageL
- Painted ImageR
- in world coordinate: [inf, inf, inf]
- Painted ImageL
- Painted ImageR
- in world coordinate: [inf, inf, inf]
- Painted ImageL
- Painted ImageR
- in world coordinate: [440.162, 922.153, 3794.19]
- Painted ImageL
- Painted ImageR
- in world coordinate: [563.622, 1180.81, 4858.41]
- Painted ImageL
- Painted ImageR
- in world coordinate: [270.275, 566.235, 2329.77]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [inf, inf, inf]
- Painted ImageL
- Painted ImageR
- in world coordinate: [385.141, 806.884, 3319.92]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [216.981, 454.583, 1870.38]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- Painted ImageL
- Painted ImageR
- in world coordinate: [-2888.56, -6051.63, 160000]
- 9273217
- Painted ImageL
- Painted ImageR
复制代码 大部分结构都落在9s左右(30帧9s),而这个在i5 12g 2400ddr配置的pc机上成绩在2s左右。稍后会推出在树莓派上运行的结果,对比pc机肯定是被虐,那么khadas又能不能虐树莓派呢?有点小期待。本来也想在banana pi berry上试一下的,可惜捣鼓了一个星期,系统刷锅好几遍都没能给banana pi装上opencv3.4,大概是因为系统依赖有点问题,只能作罢。
|
|