双目相机测距代码演示

双目测距的操作流程有四步：相机标定——双目校正——双目匹配——计算深度，具体内容参考： https://blog.csdn.net/qq_38236355/article/details/88933839

其中相机标定通常用matlab现成的工具箱进行标定，具体操作参考： https://blog.csdn.net/qq_38236355/article/details/89280633

我们接下来在完成相机标定的基础上，用标定得到的数据，按上述流程对双目深度进行计算。如果用的是任意的两个单目摄像头拼成的双目相机，则需要按上述所说进行双目校正；如果用的是成品的双目相机，则不需要进行双目校正，商家已经对相机校正好了。

一、双目校正+BM算法双目匹配生成视差图+生成深度图

#include <opencv2\opencv.hpp>
#include <iostream>
using namespace std;
using namespace cv;const int imagewidth = 752;
const int imageheigh = 480; // 摄像头分辨率752*480
Size imageSize = Size(imagewidth, imageheigh);Mat grayImageL;
Mat grayImageR;
Mat rectifyImageL, rectifyImageR;
Mat disp, disp8;//视差图Rect vaildROIL;
Rect vaildROIR;//图像校正后，会对图像进行裁剪，这里的vaildROIR就是指裁剪之后的区域Mat maplx, maply, mapRx, mapRy;//映射表
Mat Rl, Rr, Pl, Pr, Q; //校正旋转矩阵R，投影矩阵P，重投影矩阵Q
Mat xyz;
int blockSize = 0, uniquenessRatio = 0, numDisparities = 0;
Ptr<StereoBM>bm = StereoBM::create(16, 9);//事先用matlab标定好的相机参数
//matlab里的左相机标定参数矩阵
Mat cameraMatrixL = (Mat_<float>(3, 3) << 165.9419, -0.2471, 372.8349, 0, 164.8281, 325.8182, 0, 0, 1);
//matlab里左相机畸变参数
Mat distCoeffL = (Mat_<double>(5, 1) << -0.0218, -0.0014, -0.0104, -0.0025, -0.000024286);
//matlab右相机标定矩阵
Mat cameraMatrixR = (Mat_<double>(3, 3) << 168.2781, 0.1610, 413.2010, 0, 167.7710, 304.7487, 0, 0, 1);
//matlab右相机畸变参数
Mat distCoffR = (Mat_<double>(5, 1) << -0.0332, 0.0033, -0.0090, -0.0029, -0.00038324);
//matlab T 平移参数
Mat T = (Mat_<double>(3, 1) << -117.2789, -0.8970, 0.9281);
//旋转矩阵
Mat R = (Mat_<double>(3, 3) << 1.0000, -0.0040, -0.000052, 0.0040, 1.0000, -0.0041, 0.0000683, 0.0041, 1.0000);
//立体匹配 BM算法
void stereo_match(int, void*)
{bm->setBlockSize(2 * blockSize + 5);//SAD窗口大小bm->setROI1(vaildROIL);bm->setROI2(vaildROIR);bm->setPreFilterCap(31);bm->setMinDisparity(0);//最小视差，默认值为0bm->setNumDisparities(numDisparities * 16 + 16);//视差窗口，最大视差值与最小视差值之差bm->setTextureThreshold(10);bm->setUniquenessRatio(uniquenessRatio);//可以防止误匹配bm->setSpeckleWindowSize(100);bm->setSpeckleRange(32);bm->setDisp12MaxDiff(-1);bm->compute(rectifyImageL, rectifyImageR, disp);//生成视差图disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16) * 16.));//计算出的视差是CV_16S格式,将其转化为CV_8U的形式reprojectImageTo3D(disp, xyz, Q, true);xyz = xyz * 16;//在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。imshow("disp image", disp8);
}
//生成深度图
void disp2Depth(int, void*)
{Mat depthMap = Mat(disp.size(), CV_16UC1, 255);int type = disp.type();float fx = cameraMatrixL.at<float>(0, 0);float fy = cameraMatrixL.at<float>(1, 1);float cx = cameraMatrixL.at<float>(0, 2);float cy = cameraMatrixL.at<float>(1, 2);float baseline = 40; //基线距离40mmif (type == CV_16S){int height = disp.rows;int width = disp.cols;short* dispData = (short*)disp.data;ushort* depthData = (ushort*)depthMap.data;for (int i = 0; i < height; i++){for (int j = 0; j < width; j++){int id = i * width + j;if (!dispData[id]){continue;}depthData[id] = ushort((float)fx*baseline / ((float)dispData[id]));}}}else{cout << "please onfirm dispImage's type" << endl;}Mat depthMap8;depthMap.convertTo(depthMap8, CV_8U);//计算出的深度图是CV_16U格式imshow("depth image", depthMap8);
}
int main()
{//双目校正stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY, -1, imageSize, &vaildROIL, &vaildROIR);initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl , imageSize, CV_32FC1, maplx, maply);initUndistortRectifyMap(cameraMatrixR, distCoffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);//读取图像grayImageL = imread("D:/opencv learning/diannao_left.png", 0);grayImageR = imread("D:/opencv learning/diannao_right.png", 0);if (grayImageR.empty() || grayImageL.empty()){printf("can not load image...");return -1;}imshow("imageL before rectify", grayImageL);imshow("imageR bedore rectify", grayImageR);//经过remap之后，左右相机的图像已经共面并且行对准了remap(grayImageL, rectifyImageL, maplx, maply, INTER_LINEAR);remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);//把校正结果显示出来imshow("imageL after rectify", rectifyImageL);imshow("imageR after rectify", rectifyImageR);//显示在同一张图上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_8UC1);//左图像画到画布上Mat canvaspart = canvas(Rect(w * 0, 0, w, h));resize(rectifyImageL, canvaspart, canvaspart.size(), 0, 0, INTER_AREA);Rect vroil(cvRound(vaildROIL.x*sf), cvRound(vaildROIL.y*sf), cvRound(vaildROIL.width*sf), cvRound(vaildROIL.height*sf));cout << "Painted imagel" << endl;//右图像画到画布上canvaspart = canvas(Rect(w, 0, w, h));resize(rectifyImageR, canvaspart, canvaspart.size(), 0, 0, INTER_LINEAR);Rect vroiR(cvRound(vaildROIR.x*sf), cvRound(vaildROIR.y*sf), cvRound(vaildROIR.width*sf), cvRound(vaildROIR.height*sf));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("disp image", CV_WINDOW_AUTOSIZE);createTrackbar("blocksize:\n", "disp image", &blockSize, 8, stereo_match);createTrackbar("UniquenessRatio:\n", "disp image", &uniquenessRatio, 50, stereo_match);createTrackbar("NumDisparities:\n", "disp image", &numDisparities, 16, stereo_match);stereo_match(0, 0);disp2Depth(0, 0);waitKey(0);return 0;
}

二、成品双目无双目校正，BM算法双目匹配生成视差图+生成深度图

#include <opencv2/opencv.hpp>
#include <iostream>
using namespace std;
using namespace cv;const int imagewidth = 752;
const int imageheigh = 480; // 摄像头分辨率752*480
Size imageSize = Size(imagewidth, imageheigh);int blockSize = 0, uniquenessRatio = 0, numDisparities = 0;
Ptr<StereoBM> bm = StereoBM::create(16, 9); //用Block Matching算法,实现双目匹配Mat grayImageL, grayImageR , disp, disp8;Rect vaildROIL = Rect(0, 0, 752, 480);
Rect vaildROIR = Rect(0, 0, 752, 480);Mat cameraMatrixL = (Mat_<float>(3, 3) << 165.9419, -0.2471, 372.8349, 0, 164.8281, 325.8182, 0, 0, 1);void stereo_match(int, void*); //双目匹配,生成视差图（xl-xr）
void disp2Depth(int, void*); //视差图转为深度图int main()
{grayImageL = imread("D:/opencv learning/liangdui_left.png", 0);grayImageR = imread("D:/opencv learning/liangdui_right.png", 0);if (grayImageL.empty() || grayImageR.empty()){printf("can not load image...");}imshow("input ImageL", grayImageL);imshow("input ImageR", grayImageR);//显示在同一张图上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_8UC1);//左图像画到画布上Mat canvaspart = canvas(Rect(w * 0, 0, w, h));resize(grayImageL, canvaspart, canvaspart.size(), 0, 0, INTER_AREA);Rect vroil(cvRound(vaildROIL.x*sf), cvRound(vaildROIL.y*sf), cvRound(vaildROIL.width*sf), cvRound(vaildROIL.height*sf));cout << "Painted imagel" << endl;//右图像画到画布上canvaspart = canvas(Rect(w, 0, w, h));resize(grayImageR, canvaspart, canvaspart.size(), 0, 0, INTER_LINEAR);Rect vroiR(cvRound(vaildROIR.x*sf), cvRound(vaildROIR.y*sf), cvRound(vaildROIR.width*sf), cvRound(vaildROIR.height*sf));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);createTrackbar("BlockSize:\n", "disparity", &blockSize, 8, stereo_match);//创建SAD窗口TrackbarcreateTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);//创建视差唯一性百分比窗口 TrackbarcreateTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);// 创建视差窗口 Trackbarstereo_match(0, 0);disp2Depth(0, 0);waitKey(0);return 0;
}
void stereo_match(int, void*)
{bm->setBlockSize(2 * blockSize + 5); //SAD窗口大小，5-21之间为宜bm->setROI1(vaildROIL);bm->setROI2(vaildROIR);bm->setPreFilterCap(31);bm->setMinDisparity(0); //最小视差bm->setNumDisparities(numDisparities * 16 + 16);bm->setTextureThreshold(10);bm->setUniquenessRatio(uniquenessRatio); //uniquenessRatio主要可以防止误匹配bm->setSpeckleRange(32);bm->setSpeckleWindowSize(100);bm->setDisp12MaxDiff(-1);bm->compute(grayImageL, grayImageR, disp); //输入图像必须为灰度图,输出视差图disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式imshow("disparity", disp8);
}
void disp2Depth(int, void*)
{Mat depthMap = Mat(disp.size(), CV_16UC1, 255);int type = disp.type();float fx = cameraMatrixL.at<float>(0, 0);float fy = cameraMatrixL.at<float>(1, 1);float cx = cameraMatrixL.at<float>(0, 2);float cy = cameraMatrixL.at<float>(1, 2);float baseline = 40; //基线距离40mmif (type == CV_16S){int height = disp.rows;int width = disp.cols;short* dispData = (short*)disp.data;ushort* depthData = (ushort*)depthMap.data;for (int i = 0; i < height; i++){for (int j = 0; j < width; j++){int id = i * width + j;if (!dispData[id]){continue;}depthData[id] = ushort((float)fx*baseline / ((float)dispData[id]));}}}else{cout << "please onfirm dispImage's type" << endl;}Mat depthMap8;depthMap.convertTo(depthMap8, CV_8U);//计算出的深度图是CV_16U格式imshow("depth image", depthMap8);
}

我拍了一幅电脑的图片，效果如下图所示：

左右原图：

校正后放到同一平面：

视差图：

深度图：

双目相机测距代码演示相关推荐

【OpenCV】双目相机测距及其深度恢复原理及其算法流程
1. 数学模型 2.整体流程获取标定与图像数据==>stereoRectify==>initUndistortRectifyMap==>remap==>bg/sgbm恢复出视 ...
OpenCV双目相机拍照及图片分割
最近在做双目相机测距及三维重建,我从tb买了一个双目相机,第一步需要通过棋盘图来对双目相机进行标定,由于双目相机拍出来的左右相机的图片是一张图,需要进行分割. #include<iostream ...
ROS+Opencv的双目相机标定和orbslam双目参数匹配
本文承接ROS调用USB双目摄像头模组目录先完成单目标定双目标定生成可用于ORB-SLAM2的yaml文件生成可用于ORB-SLAM3的yaml文件参考按照上面链接配置好后,执行 ros ...
双目相机标定以及立体测距原理及OpenCV实现
转载双目相机标定以及立体测距原理及OpenCV实现 http://blog.csdn.net/dcrmg/article/details/52986522?locationNum=15&fp ...
双目测距------双目相机V1.0，将双目相机采集到任意一点的深度数据进行串口传输(带源码)
Depth2Uart 双目测距------双目相机V1.0,将双目相机采集到任意一点的深度数据进行串口传输一.项目说明/Overview 所实现的功能:基于Intel Realsense官方提供的S ...
双目相机标定以及立体测距原理及OpenCV实现(下)
前篇:双目相机标定以及立体测距原理及实现(上) 双目相机标定后,可以看到左右相机对应匹配点基本上已经水平对齐. 之后在该程序基础上运行stereo_match.cpp,求左右相机的视差. 注:下边Op ...
SLAM学习之路---双目相机照片生成点云（附C++代码）
一.概述本文记录如何根据双目相机得到的两帧图片数据来计算出三维点云.这里主要涉及到双目视觉的成像原理,在<视觉SLAM十四讲>第2版的第五讲中有详细介绍.本文的代码根据书上给出的代码进行 ...
【双目相机】双目摄像头测距并导出世界坐标数据进入txt【python】
1.双目测距,通过鼠标点击像素导出像素的世界坐标代码如下,核心是threeD,图片内所有像素对应的世界坐标都储存在threeD内 #从excel里读取数据 #144行fps帧率不准 import c ...
双目相机标定以及立体测距原理及实现(上)
作者丨童虎编辑丨3D视觉开发者社区单目相机标定的目标是获取相机的内参和外参,内参(1/dx,1/dy,Cx,Cy,f)表征了相机的内部结构参数,外参是相机的旋转矩阵R和平移向量t.内参中dx和dy ...

双目相机测距代码演示

双目相机测距代码演示相关推荐

最新文章

热门文章