1 说明
怕以后忘了,现在总结一下前一段时间一直在弄的,有关双目视觉的东西。
双目视觉的原理网上有很多,我只简单记录一下我对于这个的理解。
具体的实现主要是参考大神的博客:
http://blog.csdn.net/chenyusiyuan/article/list/1
和这两篇博文:
http://blog.csdn.net/sunanger_wang/article/details/7744015
http://blog.csdn.net/scyscyao/article/details/5443341
运行环境:
1.windows10
2.opencv 2.4.9
3.visual studio 2013
4.两颗微软HD-3000摄像头
2 双目测距原理
先说一下自己对双目视觉实现原理的理解,不保证都是正确的:
首先就是这个经典的图:
这个图简单的说明了双目测距的基本原理,就是想方设法求出距离Z。
右下角Z的那个等式右边的参数中:
f是每个摄像头自己的焦距,也就是传感器到镜头之间的距离。
T是两个摄像头的镜头之间的距离,这些都是确定的。
d是不确定的,d是一个物体在分别两个传感器上所成的像,也就是xl和xr之间的距离,是个变量。
所以为了得出距离,每次就需要获得d的值,之后根据相似三角形原理就可以求出Z。
现在问题来了,为什么上面那个图,摄像头的传感器和镜头位置是反的呢?
看这个图:
为了数学上的处理方便,研究人员通常习惯采用虚拟平面V来替代成像平面I,其中虚拟平面V位于焦距平面F与物体之间,与成像平面关于焦点平面对称。
3 opencv实现双目测距的原理
上面就是双目视觉的简单原理介绍,公式什么的不想往上写了,跟实现什么的也没多大关系。
原理归原理,用opencv实现双目视觉的时候就是又一码事了。
在opencv上实现双目测距主要步骤是:
1.双目校正和标定,获得摄像头的参数矩阵:
进行标定得出俩摄像头的参数矩阵
cvStereoRectify 执行双目校正
initUndistortRectifyMap 分别生成两个图像校正所需的像素映射矩阵
cvremap 分别对两个图像进行校正
2.立体匹配,获得视差图:
stereoBM生成视差图
预处理: 图像归一化,减少亮度差别,增强纹理
匹配过程: 滑动sad窗口,沿着水平线进行匹配搜索,由于校正后左右图片平行,左图的特征可以在右图对应行找到最佳匹配
再过滤: 去除坏的匹配点 通过uniquenessratio
输出视差图disparity:如果左右匹配点比较稠密,匹配点多,得到的图像与原图相似度比较大, 如果匹配点比较稀疏,得到的点与原图 相似度比较小
3.得出测距:
把生成的视差图输入 reprojectImageTo3D()函数,生成3D点云,3D点云中保存有2D图像的三维坐标,再读出每帧图像的三维坐标中的z轴的值,就得出了距离数据。
4 双目测距代码说明
我的这个双目程序写是很简单和普通,部分关键代码借鉴了大神的代码,其实说是代码其实只不过是某些opencv函数的使用方式而已,在大体上还是有很大的不同:
首先大神的程序整个都是基于MFC的,这样做的话如果你想移植到arm板子上或是linux系统上就会很麻烦,所以我把整个程序包括图像显示,视差图显示,距离显示等都完全使用opencv的函数来实现。
再者,在大神的程序中输出距离的方式,我的理解是,首先得检测到最近的物体的轮廓,然后在三维点云中提取出这个轮廓的距离坐标。但是实现起来不是很理想,如果视差图的质量不高,根本检测不到轮廓,不会触发这个函数,更别提距离了。所以在我的程序中,得出距离的方式是用鼠标点视差图的某个点,就会显示这个点的距离,不过至今距离信息也不是很准确,有可能是一些参数没弄好。
需要说明的是,对于两个摄像头来说,他们之间参数矩阵只有一个(如果两摄像头间相对位置不变的话),所以定标过程只需要一次即可。所以我的程序并没有弄标定的东西(嫌麻烦),而是从外部读取calib_paras.xml这个参数文件,这个文件可以通过大神的代码来定标然后生成出来,还应该可以从matlab的标定工具箱来生成(下面的链接),不过我没弄(matlab生成出的参数数据不知道怎么用)。
http://www.vision.caltech.edu/bouguetj/calib_doc/
5 双目测距的代码和实现
我的这个程序只负责打开摄像头,显示图像,生成视差图,显示视差图,求出点云,得出距离,显示距离。
也就是说他是在标定过程之后开始的,程序里没有摄像头定标的过程和函数,所以要正确运行是需要calib_paras.xml这个文件的,也就是标定后得出的参数文件,可以通过运行大神的代码定标后生成。
而且为了保证你能正确成功的运行,最好确保你的电脑能运行的了大神的程序(地址如下)。
https://github.com/yuhuazou/StereoVision
我的程序:
#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <cxmisc.h>
#include <highgui.h>
#include <cvaux.h>
#include <iostream>
#include <ctype.h>
#include <stdlib.h>
#include <vector>
#include <string>
#include <algorithm>
#include <ctype.h>
#include <stdarg.h>
#include <string.h>
#include <stdio.h>
using namespace cv;
using namespace std;
bool left_mouse = false;
Point2f point;
int pic_info[4];
Mat gray, prevGray, image, image1;
const Scalar GREEN = Scalar(0, 255, 0);
int rect_width = 0, rect_height = 0;
Point tmpPoint;
int num = 0;
int m_frameWidth = 640;
int m_frameHeight = 480;
bool m_Calib_Data_Loaded;
cv::Mat m_Calib_Mat_Q;
cv::Mat m_Calib_Mat_Remap_X_L;
cv::Mat m_Calib_Mat_Remap_Y_L;
cv::Mat m_Calib_Mat_Remap_X_R;
cv::Mat m_Calib_Mat_Remap_Y_R;
cv::Mat m_Calib_Mat_Mask_Roi;
cv::Rect m_Calib_Roi_L;
cv::Rect m_Calib_Roi_R;
double m_FL;
int m_numberOfDisparies;
cv::StereoBM m_BM;
CvMat* vdisp = cvCreateMat(480, 640, CV_8U);
cv::Mat img1, img2, img1p, img2p, disp, disp8u, pointClouds, imageLeft, imageRight, disparityImage, imaget1;
static IplImage *framet1 = NULL;
static IplImage *framet2 = NULL;
static IplImage *framet3 = NULL;
static IplImage *framet = NULL;
static void onMouse(int event, int x, int y, int , void* ){
Mat mouse_show;
image.copyTo(mouse_show);
if (event == CV_EVENT_LBUTTONDOWN)
{
pic_info[0] = x;
pic_info[1] = y;
cout << "x:" << pic_info[0] << "y:" << pic_info[1] << endl;
left_mouse = true;
}
else if (event == CV_EVENT_LBUTTONUP)
{
left_mouse = false;
}
else if ((event == CV_EVENT_MOUSEMOVE) && (left_mouse == true))
{
}
}
int loadCalibData()
{
try
{
cv::FileStorage fs("calib_paras.xml", cv::FileStorage::READ);
cout << fs.isOpened() << endl;
if (!fs.isOpened())
{
return (0);
}
cv::Size imageSize;
cv::FileNodeIterator it = fs["imageSize"].begin();
it >> imageSize.width >> imageSize.height;
vector<int> roiVal1;
vector<int> roiVal2;
fs["leftValidArea"] >> roiVal1;
m_Calib_Roi_L.x = roiVal1[0];
m_Calib_Roi_L.y = roiVal1[1];
m_Calib_Roi_L.width = roiVal1[2];
m_Calib_Roi_L.height = roiVal1[3];
fs["rightValidArea"] >> roiVal2;
m_Calib_Roi_R.x = roiVal2[0];
m_Calib_Roi_R.y = roiVal2[1];
m_Calib_Roi_R.width = roiVal2[2];
m_Calib_Roi_R.height = roiVal2[3];
fs["QMatrix"] >> m_Calib_Mat_Q;
fs["remapX1"] >> m_Calib_Mat_Remap_X_L;
fs["remapY1"] >> m_Calib_Mat_Remap_Y_L;
fs["remapX2"] >> m_Calib_Mat_Remap_X_R;
fs["remapY2"] >> m_Calib_Mat_Remap_Y_R;
cv::Mat lfCamMat;
fs["leftCameraMatrix"] >> lfCamMat;
m_FL = lfCamMat.at<double>(0, 0);
m_Calib_Mat_Q.at<double>(3, 2) = -m_Calib_Mat_Q.at<double>(3, 2);
m_Calib_Mat_Mask_Roi = cv::Mat::zeros(m_frameHeight, m_frameWidth, CV_8UC1);
cv::rectangle(m_Calib_Mat_Mask_Roi, m_Calib_Roi_L, cv::Scalar(255), -1);
m_BM.state->roi1 = m_Calib_Roi_L;
m_BM.state->roi2 = m_Calib_Roi_R;
m_Calib_Data_Loaded = true;
string method;
fs["rectifyMethod"] >> method;
if (method != "BOUGUET")
{
return (-2);
}
}
catch (std::exception& e)
{
m_Calib_Data_Loaded = false;
return (-99);
}
return 1;
}
void updatebm()
{
m_BM.state->preFilterCap = 31;
m_BM.state->SADWindowSize = 19;
m_BM.state->minDisparity = 0;
m_BM.state->numberOfDisparities = 96;
m_BM.state->textureThreshold = 10;
m_BM.state->uniquenessRatio = 25;
m_BM.state->speckleWindowSize = 100;
m_BM.state->speckleRange = 32;
m_BM.state->disp12MaxDiff = -1;
}
int bmMatch(cv::Mat& frameLeft, cv::Mat& frameRight, cv::Mat& disparity, cv::Mat& imageLeft, cv::Mat& imageRight)
{
if (frameLeft.empty() || frameRight.empty())
{
disparity = cv::Scalar(0);
return 0;
}
if (m_frameWidth == 0 || m_frameHeight == 0)
{
return 0;
}
cv::Mat img1proc, img2proc;
cvtColor(frameLeft, img1proc, CV_BGR2GRAY);
cvtColor(frameRight, img2proc, CV_BGR2GRAY);
cv::Mat img1remap, img2remap;
if (m_Calib_Data_Loaded)
{
remap(img1proc, img1remap, m_Calib_Mat_Remap_X_L, m_Calib_Mat_Remap_Y_L, cv::INTER_LINEAR);
remap(img2proc, img2remap, m_Calib_Mat_Remap_X_R, m_Calib_Mat_Remap_Y_R, cv::INTER_LINEAR);
}
else
{
img1remap = img1proc;
img2remap = img2proc;
}
cv::Mat img1border, img2border;
if (m_numberOfDisparies != m_BM.state->numberOfDisparities)
m_numberOfDisparies = m_BM.state->numberOfDisparities;
copyMakeBorder(img1remap, img1border, 0, 0, m_BM.state->numberOfDisparities, 0, IPL_BORDER_REPLICATE);
copyMakeBorder(img2remap, img2border, 0, 0, m_BM.state->numberOfDisparities, 0, IPL_BORDER_REPLICATE);
cv::Mat dispBorder;
m_BM(img1border, img2border, dispBorder);
cv::Mat disp;
disp = dispBorder.colRange(m_BM.state->numberOfDisparities, img1border.cols);
disp.copyTo(disparity, m_Calib_Mat_Mask_Roi);
if (m_Calib_Data_Loaded)
{
remap(frameLeft, imageLeft, m_Calib_Mat_Remap_X_L, m_Calib_Mat_Remap_Y_L, cv::INTER_LINEAR);
rectangle(imageLeft, m_Calib_Roi_L, CV_RGB(0, 0, 255), 3);
}
else
frameLeft.copyTo(imageLeft);
if (m_Calib_Data_Loaded)
remap(frameRight, imageRight, m_Calib_Mat_Remap_X_R, m_Calib_Mat_Remap_Y_R, cv::INTER_LINEAR);
else
frameRight.copyTo(imageRight);
rectangle(imageRight, m_Calib_Roi_R, CV_RGB(0, 0, 255), 3);
return 1;
}
int getDisparityImage(cv::Mat& disparity, cv::Mat& disparityImage, bool isColor)
{
cv::Mat disp8u;
if (disparity.depth() != CV_8U)
{
if (disparity.depth() == CV_8S)
{
disparity.convertTo(disp8u, CV_8U);
}
else
{
disparity.convertTo(disp8u, CV_8U, 255 / (m_numberOfDisparies*16.));
}
}
else
{
disp8u = disparity;
}
if (isColor)
{
if (disparityImage.empty() || disparityImage.type() != CV_8UC3 || disparityImage.size() != disparity.size())
{
disparityImage = cv::Mat::zeros(disparity.rows, disparity.cols, CV_8UC3);
}
for (int y = 0; y<disparity.rows; y++)
{
for (int x = 0; x<disparity.cols; x++)
{
uchar val = disp8u.at<uchar>(y, x);
uchar r, g, b;
if (val == 0)
r = g = b = 0;
else
{
r = 255 - val;
g = val < 128 ? val * 2 : (uchar)((255 - val) * 2);
b = val;
}
disparityImage.at<cv::Vec3b>(y, x) = cv::Vec3b(r, g, b);
}
}
}
else
{
disp8u.copyTo(disparityImage);
}
return 1;
}
int getPointClouds(cv::Mat& disparity, cv::Mat& pointClouds)
{
if (disparity.empty())
{
return 0;
}
reprojectImageTo3D(disparity, pointClouds, m_Calib_Mat_Q, true);
pointClouds *= 1.6;
for (int y = 0; y < pointClouds.rows; ++y)
{
for (int x = 0; x < pointClouds.cols; ++x)
{
cv::Point3f point = pointClouds.at<cv::Point3f>(y, x);
point.y = -point.y;
pointClouds.at<cv::Point3f>(y, x) = point;
}
}
return 1;
}
void detectDistance(cv::Mat& pointCloud)
{
if (pointCloud.empty())
{
return;
}
vector<cv::Mat> xyzSet;
split(pointCloud, xyzSet);
cv::Mat depth;
xyzSet[2].copyTo(depth);
double maxVal = 0, minVal = 0;
cv::Mat depthThresh = cv::Mat::zeros(depth.rows, depth.cols, CV_8UC1);
cv::minMaxLoc(depth, &minVal, &maxVal);
double thrVal = minVal * 1.5;
threshold(depth, depthThresh, thrVal, 255, CV_THRESH_BINARY_INV);
depthThresh.convertTo(depthThresh, CV_8UC1);
double distance = depth.at<float>(pic_info[0], pic_info[1]);
cout << "distance:" << distance << endl;
}
int main(int argc, char** argv)
{
VideoCapture cap(0);
VideoCapture cap1(1);
if (!cap.isOpened())
{
cout << "error happened while open cam 1"<<endl;
return -1;
}
if (!cap1.isOpened())
{
cout << "error happened while open cam 2" << endl;
return -1;
}
namedWindow("left", 1);
namedWindow("right", 1);
namedWindow("disparitycolor", 1);
setMouseCallback("disparitycolor", onMouse, 0);
loadCalibData();
cout << m_Calib_Data_Loaded << endl;
while (true)
{
Mat frame;
Mat frame1;
cap.read(frame);
cap1.read(frame1);
if (frame.empty()) break;
if (frame1.empty()) break;
frame.copyTo(image);
frame1.copyTo(image1);
updatebm();
bmMatch(frame, frame1, disp, imageLeft, imageRight);
imshow("left", imageLeft);
imshow("right", imageRight);
getDisparityImage(disp, disparityImage, true);
getPointClouds(disp, pointClouds);
imshow("disparitycolor", disparityImage);
detectDistance(pointClouds);
waitKey(100);
}
return 0;
}