1、引言
在一年之前小编写了一篇双目测距的博文,引入了大量的童鞋阅读,其博文介绍了详细的相机标定与双目测距过程和代码
https://blog.csdn.net/xiao__run/article/details/78900652
摄像头如前面文章所示,大家可自行购买,小编就是在这家购买
https://shop224405513.taobao.com/search.htm?spm=a1z10.1-c-s.0.0.751b3e49u0Kz6o&search=y
文章评论特别多,由此可见很多读者遇到了很多的问题,有标定不准的,测距距离不准,误差特别大,视差图很差的等各种问题。今天小编再写一篇博文,可能对您有所帮助。
双目测距流程如图
2 标定
首先准备一张标定板,标定板要比较大,不能反光等特性,小博的标定板如图
接下来拍照,大约二十张,就可以了,我重新修改了下拍照程序
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;
int main()
{
/*
//双目摄像头,两个usb
cv::VideoCapture capl(0);
cv::VideoCapture capr(1);
int i = 0;
cv::Mat cam_left;
cv::Mat cam_right;
char filename_l[15];
char filename_r[15];
while(capl.read(cam_left) && capr.read(cam_right))
{
cv::imshow("cam_left", cam_left);
cv::imshow("cam_right", cam_right);
char c = cv::waitKey(1);
char s[40];
if(c==' ') //按空格采集图像
{
sprintf(filename_l, "/home/lqx/ClionProjects/Calibration/left_img/left%d.jpg",i);
imwrite(filename_l, cam_left);
sprintf(filename_r, "/home/lqx/ClionProjects/Calibration/right_img/right%d.jpg",i++);
imwrite(filename_r, cam_right);
//printf(s, "%s%d%s\n", "the ",i++,"th image");
cout << "save the "<< i <<"th image\n"<< endl;
}
if(c=='q' || c=='Q') // 按q退出
{
break;
}
}
return 0;
*/
//双目摄像头,一个usb
//图像大小为640*240,左右各为320*240
cv::VideoCapture cap(0);
int i = 0;
cap.set(CV_CAP_PROP_FRAME_WIDTH,640);//可以
cap.set(CV_CAP_PROP_FRAME_HEIGHT,240);
cv::Mat cam, cam_left,cam_right;
char filename_l[15];
char filename_r[15];
while(cap.read(cam) )
{
cam_right=cam(Rect(0,0,320,240));
cam_left=cam(Rect(320,0,320,240));
cv::imshow("cam_left", cam_left);
cv::imshow("cam_right", cam_right);
char c = cv::waitKey(1);
char s[40];
if(c==' ') //按空格采集图像
{
sprintf(filename_l, "left_img/left%d.jpg",i);
imwrite(filename_l, cam_left);
sprintf(filename_r, "right_img/right%d.jpg",i++);
imwrite(filename_r, cam_right);
//printf(s, "%s%d%s\n", "the ",i++,"th image");
cout << "save the "<< i <<"th image\n"<< endl;
}
if(c=='q' || c=='Q') // 按q退出
{
break;
}
}
return 0;
}
调用上述代码即可得到左右二十多张图像。
接下来我们使用matlab工具箱的标定工具进行标定,也可以使用我上面的博文进行标定。我建议使用matlab标定,结果会更准确点。
标定得到内参和外参如下:
//T 矩阵参数
Mat T = Mat::zeros(3,1,CV_64F);
T.at<double>(0,0)= 119.5815;
T.at<double>(1,0)=-0.5328;
T.at<double>(2,0)=-1.7296;
//R矩阵
// Rodrigues(rec,R);
// cout<<R<<endl;
Mat R=Mat::eye(3,3,CV_64F);
R.at<double>(0,1)= 0.000055498;
R.at<double>(0,2)= -0.0184;
R.at<double>(1,0)= -0.0001629;
R.at<double>(1,2)= 0.0118;
R.at<double>(2,0)= 0.0184;
R.at<double>(2,1)= -0.0118;
//*******************左相机参数**********************************
//*******************左相机参数**********************************
double fx=202.0386,fy=202.6377;
double cx=156.0528,cy=116.9724;
Size size(320,240);
Rect left(0,0,320,240);
Rect right(320,0,320,240);
Mat cameraMatrixL=Mat ::eye(3,3,CV_64F);
Mat cameraMatrixR=Mat ::eye(3,3,CV_64F);
//左边相机内参
cameraMatrixL.at<double>(0,0)=fx;
cameraMatrixL.at<double>(0,2)=cx;
cameraMatrixL.at<double>(1,1)=fy;
cameraMatrixL.at<double>(1,2)=cy;
//左边相机畸变
Mat distCoffsL=Mat::zeros(5,1,CV_64F);
distCoffsL.at<double>(0,0)=-0.0446;
distCoffsL.at<double>(1,0)=0.0597;
distCoffsL.at<double>(2,0)=0;
distCoffsL.at<double>(3,0)=0.;
distCoffsL.at<double>(4,0)=0;
//*******************右相机参数**********************************
cameraMatrixR.at<double>(0,0)=204.1203;
cameraMatrixR.at<double>(0,2)=164.9597;
cameraMatrixR.at<double>(1,1)=204.5797;
cameraMatrixR.at<double>(1,2)=117.9534;
Mat distCoffsR=Mat::zeros(5,1,CV_64F);
distCoffsR.at<double>(0,0)=-0.0352;
distCoffsR.at<double>(1,0)=0.0345;
distCoffsR.at<double>(2,0)=0.;
distCoffsR.at<double>(3,0)=0;
distCoffsR.at<double>(4,0)=0;
3、 BM与SGBM匹配算法
SGBM是一种立体匹配算法,准确度和速度适中,工程中比较常用
oid stereo_SGBM_match(int, void*)
{
int mindisparity = 0;
int ndisparities = 64;
int SADWindowSize = 11;
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(mindisparity, ndisparities, SADWindowSize);
int P1 = 8 * left_camera_calibration.channels() * SADWindowSize* SADWindowSize;
int P2 = 32 * left_camera_calibration.channels() * SADWindowSize* SADWindowSize;
sgbm->setP1(P1);
sgbm->setP2(P2);
sgbm->setPreFilterCap(15);
sgbm->setUniquenessRatio(6);
sgbm->setSpeckleRange(2);
sgbm->setSpeckleWindowSize(100);
sgbm->setDisp12MaxDiff(1);
//sgbm->setNumDisparities(1);
sgbm->setMode(cv::StereoSGBM::MODE_HH);
Mat disp,disp8U;
sgbm->compute(left_camera_calibration, right_camrera_calibration, disp);
disp.convertTo(disp, CV_32F, 1.0 / 16); //除以16得到真实视差值
disp8U = Mat(disp.rows, disp.cols, CV_8UC1); //显示
//normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);
disp.convertTo(disp8U,CV_8U,255/(numDisparities*16.));
reprojectImageTo3D(disp,xyz,Q);
xyz=xyz*16;
imshow("disparity",disp8U);
}
BM算法之前博文已经提过
oid stereo_match(int, void*)
{
bm->setBlockSize(2*blockSize+5);
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(uniquenessRation);
bm->setSpeckleWindowSize(100);
bm->setSpeckleRange(32);
bm->setDisp12MaxDiff(-1);
Mat disp,disp8;
bm->compute(left_camera_calibration,right_camrera_calibration,disp);
disp.convertTo(disp8,CV_8U,255/((numDisparities*16+16)*16.));
reprojectImageTo3D(disp,xyz,Q);
xyz=xyz*16;
imshow("disparity",disp8);
}
4、视差图测距
我们先看下效果图,效果蛮不错,误差1m以内大约1cm以内;
可以看到本文的测距方法还是挺准,视差图效果也不错。
最后给出部分工程代码吧,此代码未经我优化,仅仅作为学习使用,其实经优化,在树莓派等设备上也能实时生成视差图测距。
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;
int framewidth=320;
int frameheight=240;
Size imageSize=Size(framewidth,frameheight);
Mat left_camera,right_camera,left_camera_calibration,right_camrera_calibration,xyz;
Point origin;
Rect selection;
bool selectObject= false;
int blockSize=0,uniquenessRation=0,numDisparities=0;
Rect validROIL,validROIR;
Mat view,rview,mapL1,map_L2,mapR1,mapR2,RL,PL,RR,PR,Q;
Ptr<StereoBM>bm=StereoBM::create(16,9);
int mindisparity = 0;
int ndisparities = 64;
int SADWindowSize = 11;
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(mindisparity, ndisparities, SADWindowSize);
//*******************bm**********************************
//*******************bm**********************************
/*
void stereo_match(int, void*)
{
bm->setBlockSize(2*blockSize+5);
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(uniquenessRation);
bm->setSpeckleWindowSize(100);
bm->setSpeckleRange(32);
bm->setDisp12MaxDiff(-1);
Mat disp,disp8;
bm->compute(left_camera_calibration,right_camrera_calibration,disp);
disp.convertTo(disp8,CV_8U,255/((numDisparities*16+16)*16.));
reprojectImageTo3D(disp,xyz,Q);
xyz=xyz*16;
imshow("disparity",disp8);
}
*/
//*******************bm**********************************
//*******************bm**********************************
void stereo_SGBM_match(int, void*)
{
int P1 = 8 * left_camera_calibration.channels() * SADWindowSize* SADWindowSize;
int P2 = 32 * left_camera_calibration.channels() * SADWindowSize* SADWindowSize;
sgbm->setP1(P1);
sgbm->setP2(P2);
sgbm->setPreFilterCap(15);
sgbm->setUniquenessRatio(6);
sgbm->setSpeckleRange(2);
sgbm->setSpeckleWindowSize(100);
sgbm->setDisp12MaxDiff(1);
//sgbm->setNumDisparities(1);
sgbm->setMode(cv::StereoSGBM::MODE_HH);
Mat disp,disp8U;
sgbm->compute(left_camera_calibration, right_camrera_calibration, disp);
disp.convertTo(disp, CV_32F, 1.0 / 16); //除以16得到真实视差值
disp8U = Mat(disp.rows, disp.cols, CV_8UC1); //显示
//normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);
disp.convertTo(disp8U,CV_8U,255/(numDisparities*16.));
reprojectImageTo3D(disp,xyz,Q);
xyz=xyz*16;
imshow("disparity",disp8U);
}
static void onMouse(int envent,int x,int y,int, void*)
{
if (selectObject)
{
selection.x=MIN(x,origin.x);
selection.y=MIN(y,origin.y);
selection.width=abs(x-origin.x);
selection.height=abs(y-origin.y);
}
switch (envent)
{
case EVENT_LBUTTONDOWN:
origin=Point(x,y);
selection=Rect(x,y,0,0);
selectObject= true;
cout<<origin<<"in the world coordinate is: "<<xyz.at<Vec3f>(origin)<<endl;
break;
case EVENT_LBUTTONUP:
selectObject= false;
if(selection.width>0 && selection.height>0)
break;
}
}
int main()
{
//T 矩阵参数
Mat T = Mat::zeros(3,1,CV_64F);
T.at<double>(0,0)= 119.5815;
T.at<double>(1,0)=-0.5328;
T.at<double>(2,0)=-1.7296;
//Mat rec = Mat::zeros(3,1,CV_64F);
//rec.at<double>(0,0)= 0.0191;
// rec.at<double>(1,0)=0.03125;
//rec.at<double>(2,0)=-0.00960;
//Mat R;
//R矩阵
// Rodrigues(rec,R);
// cout<<R<<endl;
Mat R=Mat::eye(3,3,CV_64F);
R.at<double>(0,1)= 0.000055498;
R.at<double>(0,2)= -0.0184;
R.at<double>(1,0)= -0.0001629;
R.at<double>(1,2)= 0.0118;
R.at<double>(2,0)= 0.0184;
R.at<double>(2,1)= -0.0118;
//*******************左相机参数**********************************
//*******************左相机参数**********************************
double fx=202.0386,fy=202.6377;
double cx=156.0528,cy=116.9724;
Size size(320,240);
Rect left(0,0,320,240);
Rect right(320,0,320,240);
Mat cameraMatrixL=Mat ::eye(3,3,CV_64F);
Mat cameraMatrixR=Mat ::eye(3,3,CV_64F);
//左边相机内参
cameraMatrixL.at<double>(0,0)=fx;
cameraMatrixL.at<double>(0,2)=cx;
cameraMatrixL.at<double>(1,1)=fy;
cameraMatrixL.at<double>(1,2)=cy;
//左边相机畸变
Mat distCoffsL=Mat::zeros(5,1,CV_64F);
distCoffsL.at<double>(0,0)=-0.0446;
distCoffsL.at<double>(1,0)=0.0597;
distCoffsL.at<double>(2,0)=0;
distCoffsL.at<double>(3,0)=0.;
distCoffsL.at<double>(4,0)=0;
//*******************做相机参数**********************************
//*******************右相机参数**********************************
cameraMatrixR.at<double>(0,0)=204.1203;
cameraMatrixR.at<double>(0,2)=164.9597;
cameraMatrixR.at<double>(1,1)=204.5797;
cameraMatrixR.at<double>(1,2)=117.9534;
Mat distCoffsR=Mat::zeros(5,1,CV_64F);
distCoffsR.at<double>(0,0)=-0.0352;
distCoffsR.at<double>(1,0)=0.0345;
distCoffsR.at<double>(2,0)=0.;
distCoffsR.at<double>(3,0)=0;
distCoffsR.at<double>(4,0)=0;
//*******************有相机参数**********************************
//*******************有相机参数**********************************
cout<<"calibration ......."<<endl;
//Mat new_cameraMatrix_L=getOptimalNewCameraMatrix(cameraMatrix1, distCoeffs1, image_size, 1, image_size, 0);
stereoRectify(cameraMatrixL,distCoffsL,cameraMatrixR,distCoffsR,size,R,T,RL,RR,PL,PR,Q,CALIB_ZERO_DISPARITY,0,size,&validROIL,&validROIR);
initUndistortRectifyMap(cameraMatrixL,distCoffsL,RL,PL ,size,CV_16SC2,mapL1,map_L2);
initUndistortRectifyMap(cameraMatrixR,distCoffsR,RR,PR ,size,CV_16SC2,mapR1,mapR2);
VideoCapture cap (0);
Mat rectifyL,rectifyR;
cap.set(CV_CAP_PROP_FRAME_WIDTH, 640);
cap.set(CV_CAP_PROP_FRAME_HEIGHT, 320);
while (cap.isOpened())
{
Mat frame,grayL,grayR;
cap>>frame;
left_camera=frame(left);
right_camera=frame(right);
imshow("left_org",left_camera);
imshow("right_org",right_camera);
imshow("org",frame);
cvtColor(left_camera,grayL,CV_RGB2GRAY);
cvtColor(right_camera,grayR,CV_RGB2GRAY);
remap(grayL,left_camera_calibration,mapL1,map_L2,INTER_LINEAR);
remap(grayR,right_camrera_calibration,mapR1,mapR2,INTER_LINEAR);
/*
Mat canvas;
double sf;
int w ,h;
sf=240. /320;
w=cvRound(240*sf);
h=cvRound(320*sf);
canvas.create(h,w*2,CV_8UC3);
Mat canvasPart=canvas(Rect(0,0,w,h));
resize(left_camera_calibration,canvasPart,canvasPart.size(),0,0,INTER_AREA);
Rect vroiL(cvRound(validROIL.x*sf),cvRound(validROIL.y*sf),cvRound(validROIL.width*sf),cvRound(validROIL.height*sf));
canvasPart=canvas(Rect(w,0,w,h));
resize(right_camrera_calibration,canvasPart,canvasPart.size(),0,0,INTER_AREA);
Rect vroiR(cvRound(validROIR.x*sf),cvRound(validROIR.y*sf),cvRound(validROIR.width*sf),cvRound(validROIR.height*sf));
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("rectify",canvas);
*/
namedWindow("disparity",CV_WINDOW_AUTOSIZE);
// createTrackbar("blocksize:\n","disparity",&blockSize,16,stereo_match);
// createTrackbar("UniquenessRatio:\n","disparity",&uniquenessRation,50,stereo_match);
createTrackbar("NumDisparities:\n","disparity",&numDisparities,16,stereo_SGBM_match);
setMouseCallback("disparity",onMouse,0);
// stereo_match(0,0);
stereo_SGBM_match(0, 0);
// Mat output;
//sgbm->compute(left_camera_calibration,right_camrera_calibration,output);
//imshow("SGBM",output);
//cvtColor(left_camera_calibration,left_camera_calibration,CV_GRAY2BGR);
// cvtColor(right_camrera_calibration,right_camrera_calibration,CV_GRAY2BGR);
imshow("cali_right",right_camrera_calibration);
imshow("cali_left",left_camera_calibration);
int key=waitKey(1);
if (key==27)
{
break;
}
}
return 0;
}
若测得距离为负数,可将T向量的三个值变换个符号即可,OK ,先讲这么多,觉得不错的点个赞哦。