【计算机视觉】 完全基于opencv的双目景深与测距的实现

转载 Joe_quan的

http://blog.csdn.net/hysteric314/article/details/50456570?locationNum=3&fps=1

目录

• 目录
• 说明
• 双目测距原理
• opencv实现双目测距的原理
• 双目测距代码说明
• 双目测距的代码和实现
• 接下来

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 <cv.h>#include <cxmisc.h>#include <highgui.h>#include <cvaux.h>#include <iostream>#include <ctype.h>//#include <unistd.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;//vector<Point2f> point1, point2;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;              // Q 矩阵cv::Mat m_Calib_Mat_Remap_X_L;      // 左视图畸变校正像素坐标映射矩阵 Xcv::Mat m_Calib_Mat_Remap_Y_L;      // 左视图畸变校正像素坐标映射矩阵 Ycv::Mat m_Calib_Mat_Remap_X_R;      // 右视图畸变校正像素坐标映射矩阵 Xcv::Mat m_Calib_Mat_Remap_Y_R;      // 右视图畸变校正像素坐标映射矩阵 Ycv::Mat m_Calib_Mat_Mask_Roi;       // 左视图校正后的有效区域cv::Rect m_Calib_Roi_L;             // 左视图校正后的有效区域矩形cv::Rect m_Calib_Roi_R;             // 右视图校正后的有效区域矩形double          m_FL;//CvStereoBMState *BMState = cvCreateStereoBMState();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 /*flags*/, void* /*param*/){    Mat mouse_show;    image.copyTo(mouse_show);    //char buffer[100];    //sprintf(buffer, "D:\\l%d.jpg", num);    //string t1(buffer);    //sprintf(buffer, "D:\\r%d.jpg", num);    //string t(buffer);    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;        //用于存储打印图片        //imwrite(t, image);    //  imwrite(t1, image1);    //  num = num++;    }    else if (event == CV_EVENT_LBUTTONUP)    {        left_mouse = false;    }    else if ((event == CV_EVENT_MOUSEMOVE) && (left_mouse == true))    {    }}int loadCalibData(){    // 读入摄像头定标参数 Q roi1 roi2 mapx1 mapy1 mapx2 mapy2    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;    //  if (imageSize.width != m_frameWidth || imageSize.height != m_frameHeight)   {           return (-1);        }        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)    {        //if (init(frameLeft.cols, frameLeft.rows, "calib_paras.xml"/*待改为由本地设置文件确定*/) == 0) //执行类初始化        //  {        return 0;        //}    }    // 转换为灰度图    cv::Mat img1proc, img2proc;    cvtColor(frameLeft, img1proc, CV_BGR2GRAY);    cvtColor(frameRight, img2proc, CV_BGR2GRAY);    // 校正图像，使左右视图行对齐        cv::Mat img1remap, img2remap;    //cout << m_Calib_Data_Loaded << endl;    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);    //cvFindStereoCorrespondenceBM(img1border, img2border, dispBorder,BMState);    // 截取与原始画面对应的视差区域（舍去加宽的部分）    cv::Mat disp;    disp = dispBorder.colRange(m_BM.state->numberOfDisparities, img1border.cols);    disp.copyTo(disparity, m_Calib_Mat_Mask_Roi);    //reprojectImageTo3D(dispBorder, pointClouds, m_Calib_Mat_Q, true);    // 输出处理后的图像    //cout << m_Calib_Data_Loaded << endl;    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){    // 将原始视差数据的位深转换为 8 位    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;    }    //计算生成三维点云//  cv::reprojectImageTo3D(disparity, pointClouds, m_Calib_Mat_Q, true);    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);    //imageDenoising(depthThresh, 3);    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);            //  framet2 = &IplImage(disparityImage);            //  cvShowImage("disparitycolor", framet2);                getPointClouds(disp, pointClouds);            imshow("disparitycolor", disparityImage);            detectDistance(pointClouds);            waitKey(100);    }    //std::swap(point2, point1);    //  cv::swap(prevGray, gray);    return 0;}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440

我的使用的双目摄像头 

正面照片 

程序跑出来的效果截图 

截图中分别是左右摄像头的图像，距离显示，和视差图显示。

能看出视差图不是很准，首先是因为参数没时间仔细调，再关键是总是有人来动我的摄像头，花了半天时间来标定好，别人拿手一掰一碰，俩摄像头相对位置就又变了，前面标定的工作全白费，后来索性就不管了。

视差图不准，距离信息也肯定不是很准，大多数情况是16000，上面的图里用鼠标点视差图中红色那一条，距离显示中首先会输出该点的x，y坐标，然后就是该点的距离坐标，也就是这个点z轴的值。

总体来说就是还得调。

6 接下来

以上就差不多是前段时间弄的东西，本打算把双目测距移植到NVIDIA jetson tk1上看看效果，不过没弄完，目前进程是：上面的代码已经可以在tk1上编译成功了，就是死活运行不了，我怀疑是tk那个板子是不是不支持打开和显示俩摄像头，这个问题花点时间我觉得应该是可以解决的，不过现在弄别的东西了，也懒得查了。