1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
42 #include "precomp.hpp"
44 /****************************************************************************************\
45 The code below is some modification of Stan Birchfield's algorithm described in:
47 Depth Discontinuities by Pixel-to-Pixel Stereo
48 Stan Birchfield and Carlo Tomasi
49 International Journal of Computer Vision,
50 35(3): 269-293, December 1999.
52 This implementation uses different cost function that results in
53 O(pixPerRow*maxDisparity) complexity of dynamic programming stage versus
54 O(pixPerRow*log(pixPerRow)*maxDisparity) in the above paper.
55 \****************************************************************************************/
57 /****************************************************************************************\
58 * Find stereo correspondence by dynamic programming algorithm *
59 \****************************************************************************************/
60 #define ICV_DP_STEP_LEFT 0
61 #define ICV_DP_STEP_UP 1
62 #define ICV_DP_STEP_DIAG 2
64 #define ICV_BIRCH_DIFF_LUM 5
66 #define ICV_MAX_DP_SUM_VAL (INT_MAX/4)
68 typedef struct _CvDPCell
70 uchar step; //local-optimal step
71 int sum; //current sum
74 typedef struct _CvRightImData
76 uchar min_val, max_val;
79 #define CV_IMAX3(a,b,c) (std::max(std::max((a), (b)), (c)))
80 #define CV_IMIN3(a,b,c) (std::min(std::min((a), (b)), (c)))
82 static void icvFindStereoCorrespondenceByBirchfieldDP( uchar* src1, uchar* src2,
84 CvSize size, int widthStep,
86 float _param1, float _param2,
87 float _param3, float _param4,
92 int dispH = maxDisparity + 3;
94 int imgW = size.width;
95 int imgH = size.height;
96 uchar val, prevval, prev, curr;
98 uchar* dest = disparities;
99 int param1 = cvRound(_param1);
100 int param2 = cvRound(_param2);
101 int param3 = cvRound(_param3);
102 int param4 = cvRound(_param4);
103 int param5 = cvRound(_param5);
105 #define CELL(d,x) cells[(d)+(x)*dispH]
107 uchar* dsi = (uchar*)cvAlloc(sizeof(uchar)*imgW*dispH);
108 uchar* edges = (uchar*)cvAlloc(sizeof(uchar)*imgW*imgH);
109 _CvDPCell* cells = (_CvDPCell*)cvAlloc(sizeof(_CvDPCell)*imgW*MAX(dispH,(imgH+1)/2));
110 _CvRightImData* rData = (_CvRightImData*)cvAlloc(sizeof(_CvRightImData)*imgW);
111 int* reliabilities = (int*)cells;
113 for( y = 0; y < imgH; y++ )
115 uchar* srcdata1 = src1 + widthStep * y;
116 uchar* srcdata2 = src2 + widthStep * y;
119 prevval = prev = srcdata2[0];
120 for( j = 1; j < imgW; j++ )
123 val = (uchar)((curr + prev)>>1);
124 rData[j-1].max_val = (uchar)CV_IMAX3( val, prevval, prev );
125 rData[j-1].min_val = (uchar)CV_IMIN3( val, prevval, prev );
129 rData[j-1] = rData[j-2];//last elem
131 // fill dissimularity space image
132 for( i = 1; i <= maxDisparity + 1; i++ )
136 for( j = i - 1; j < imgW - 1; j++ )
139 if( (t = srcdata1[j] - rData[j+1].max_val) >= 0 )
143 else if( (t = rData[j+1].min_val - srcdata1[j]) >= 0 )
153 dsi -= (maxDisparity+1)*imgW;
154 rData += maxDisparity+1;
156 //intensity gradients image construction
158 edges[y*imgW] = edges[y*imgW+1] = edges[y*imgW+2] = 2;
159 edges[y*imgW+imgW-1] = edges[y*imgW+imgW-2] = edges[y*imgW+imgW-3] = 1;
160 for( j = 3; j < imgW-4; j++ )
164 if( ( CV_IMAX3( srcdata1[j-3], srcdata1[j-2], srcdata1[j-1] ) -
165 CV_IMIN3( srcdata1[j-3], srcdata1[j-2], srcdata1[j-1] ) ) >= ICV_BIRCH_DIFF_LUM )
167 edges[y*imgW+j] |= 1;
169 if( ( CV_IMAX3( srcdata2[j+3], srcdata2[j+2], srcdata2[j+1] ) -
170 CV_IMIN3( srcdata2[j+3], srcdata2[j+2], srcdata2[j+1] ) ) >= ICV_BIRCH_DIFF_LUM )
172 edges[y*imgW+j] |= 2;
176 //find correspondence using dynamical programming
178 for( x = 0; x < imgW; x++ )
180 CELL(0,x).sum = CELL(dispH-1,x).sum = ICV_MAX_DP_SUM_VAL;
181 CELL(0,x).step = CELL(dispH-1,x).step = ICV_DP_STEP_LEFT;
183 for( d = 2; d < dispH; d++ )
185 CELL(d,d-2).sum = ICV_MAX_DP_SUM_VAL;
186 CELL(d,d-2).step = ICV_DP_STEP_UP;
189 CELL(1,0).step = ICV_DP_STEP_LEFT;
191 for( x = 1; x < imgW; x++ )
193 int dp = MIN( x + 1, maxDisparity + 1);
194 uchar* _edges = edges + y*imgW + x;
195 int e0 = _edges[0] & 1;
196 _CvDPCell* _cell = cells + x*dispH;
200 int _s = dsi[dp*imgW+x];
204 sum[0] = _cell[dp-dispH].sum - param2;
207 if( _cell[dp+1].step != ICV_DP_STEP_DIAG && e0 )
209 sum[1] = _cell[dp+1].sum + param1;
211 if( _cell[dp-1-dispH].step != ICV_DP_STEP_UP && (_edges[1-dp] & 2) )
215 sum[2] = _cell[dp-1-dispH].sum + param1;
219 //choose local-optimal pass
220 if( sum[t] <= sum[2] )
222 _cell[dp].step = (uchar)t;
223 _cell[dp].sum = sum[t] + _s;
227 _cell[dp].step = ICV_DP_STEP_DIAG;
228 _cell[dp].sum = sum[2] + _s;
233 if( sum[0] <= sum[1] )
235 _cell[dp].step = ICV_DP_STEP_LEFT;
236 _cell[dp].sum = sum[0] + _s;
240 _cell[dp].step = ICV_DP_STEP_UP;
241 _cell[dp].sum = sum[1] + _s;
245 else if( _cell[dp-1-dispH].step != ICV_DP_STEP_UP && (_edges[1-dp] & 2) )
247 sum[2] = _cell[dp-1-dispH].sum + param1;
248 if( sum[0] <= sum[2] )
250 _cell[dp].step = ICV_DP_STEP_LEFT;
251 _cell[dp].sum = sum[0] + _s;
255 _cell[dp].step = ICV_DP_STEP_DIAG;
256 _cell[dp].sum = sum[2] + _s;
261 _cell[dp].step = ICV_DP_STEP_LEFT;
262 _cell[dp].sum = sum[0] + _s;
268 //extract optimal way and fill disparity image
269 dispdata = dest + widthStep * y;
272 min_val = ICV_MAX_DP_SUM_VAL;
273 for( i = 1; i <= maxDisparity + 1; i++ )
275 if( min_val > CELL(i,imgW-1).sum )
278 min_val = CELL(i,imgW-1).sum;
283 for( x = imgW - 1; x > 0; x-- )
285 dispdata[x] = (uchar)(d - 1);
286 while( CELL(d,x).step == ICV_DP_STEP_UP ) d++;
287 if ( CELL(d,x).step == ICV_DP_STEP_DIAG )
290 while( CELL(d,x).step == ICV_DP_STEP_DIAG )
295 for( i = x; i < s; i++ )
297 dispdata[i] = (uchar)(d-1);
303 //Postprocessing the Disparity Map
305 //remove obvious errors in the disparity map
306 for( x = 0; x < imgW; x++ )
308 for( y = 1; y < imgH - 1; y++ )
310 if( dest[(y-1)*widthStep+x] == dest[(y+1)*widthStep+x] )
312 dest[y*widthStep+x] = dest[(y-1)*widthStep+x];
317 //compute intensity Y-gradients
318 for( x = 0; x < imgW; x++ )
320 for( y = 1; y < imgH - 1; y++ )
322 if( ( CV_IMAX3( src1[(y-1)*widthStep+x], src1[y*widthStep+x],
323 src1[(y+1)*widthStep+x] ) -
324 CV_IMIN3( src1[(y-1)*widthStep+x], src1[y*widthStep+x],
325 src1[(y+1)*widthStep+x] ) ) >= ICV_BIRCH_DIFF_LUM )
327 edges[y*imgW+x] |= 4;
328 edges[(y+1)*imgW+x] |= 4;
329 edges[(y-1)*imgW+x] |= 4;
335 //remove along any particular row, every gradient
336 //for which two adjacent columns do not agree.
337 for( y = 0; y < imgH; y++ )
339 prev = edges[y*imgW];
340 for( x = 1; x < imgW - 1; x++ )
342 curr = edges[y*imgW+x];
345 !( edges[y*imgW+x+1] & 4 ) ) )
347 edges[y*imgW+x] -= 4;
353 // define reliability
354 for( x = 0; x < imgW; x++ )
356 for( y = 1; y < imgH; y++ )
359 for( ; y < imgH && dest[y*widthStep+x] == dest[(y-1)*widthStep+x]; y++ )
364 reliabilities[i*imgW+x] = s;
369 //Y - propagate reliable regions
370 for( x = 0; x < imgW; x++ )
372 for( y = 0; y < imgH; y++ )
374 d = dest[y*widthStep+x];
375 if( reliabilities[y*imgW+x] >= param4 && !(edges[y*imgW+x] & 4) &&
376 d > 0 )//highly || moderately
378 disparities[y*widthStep+x] = (uchar)d;
380 for( i = y - 1; i >= 0; i-- )
382 if( ( edges[i*imgW+x] & 4 ) ||
383 ( dest[i*widthStep+x] < d &&
384 reliabilities[i*imgW+x] >= param3 ) ||
385 ( reliabilities[y*imgW+x] < param5 &&
386 dest[i*widthStep+x] - 1 == d ) ) break;
388 disparities[i*widthStep+x] = (uchar)d;
392 for( i = y + 1; i < imgH; i++ )
394 if( ( edges[i*imgW+x] & 4 ) ||
395 ( dest[i*widthStep+x] < d &&
396 reliabilities[i*imgW+x] >= param3 ) ||
397 ( reliabilities[y*imgW+x] < param5 &&
398 dest[i*widthStep+x] - 1 == d ) ) break;
400 disparities[i*widthStep+x] = (uchar)d;
406 disparities[y*widthStep+x] = (uchar)d;
411 // define reliability along X
412 for( y = 0; y < imgH; y++ )
414 for( x = 1; x < imgW; x++ )
417 for( ; x < imgW && dest[y*widthStep+x] == dest[y*widthStep+x-1]; x++ ) {}
421 reliabilities[y*imgW+i] = s;
426 //X - propagate reliable regions
427 for( y = 0; y < imgH; y++ )
429 for( x = 0; x < imgW; x++ )
431 d = dest[y*widthStep+x];
432 if( reliabilities[y*imgW+x] >= param4 && !(edges[y*imgW+x] & 1) &&
433 d > 0 )//highly || moderately
435 disparities[y*widthStep+x] = (uchar)d;
437 for( i = x - 1; i >= 0; i-- )
439 if( (edges[y*imgW+i] & 1) ||
440 ( dest[y*widthStep+i] < d &&
441 reliabilities[y*imgW+i] >= param3 ) ||
442 ( reliabilities[y*imgW+x] < param5 &&
443 dest[y*widthStep+i] - 1 == d ) ) break;
445 disparities[y*widthStep+i] = (uchar)d;
449 for( i = x + 1; i < imgW; i++ )
451 if( (edges[y*imgW+i] & 1) ||
452 ( dest[y*widthStep+i] < d &&
453 reliabilities[y*imgW+i] >= param3 ) ||
454 ( reliabilities[y*imgW+x] < param5 &&
455 dest[y*widthStep+i] - 1 == d ) ) break;
457 disparities[y*widthStep+i] = (uchar)d;
463 disparities[y*widthStep+x] = (uchar)d;
476 /*F///////////////////////////////////////////////////////////////////////////
478 // Name: cvFindStereoCorrespondence
479 // Purpose: find stereo correspondence on stereo-pair
482 // leftImage - left image of stereo-pair (format 8uC1).
483 // rightImage - right image of stereo-pair (format 8uC1).
484 // mode -mode of correspondance retrieval (now CV_RETR_DP_BIRCHFIELD only)
485 // dispImage - destination disparity image
486 // maxDisparity - maximal disparity
487 // param1, param2, param3, param4, param5 - parameters of algorithm
490 // Images must be rectified.
491 // All images must have format 8uC1.
494 cvFindStereoCorrespondence(
495 const CvArr* leftImage, const CvArr* rightImage,
499 double param1, double param2, double param3,
500 double param4, double param5 )
502 CV_FUNCNAME( "cvFindStereoCorrespondence" );
508 CvMat src1_stub, src2_stub, dst_stub;
511 CV_CALL( src1 = cvGetMat( leftImage, &src1_stub, &coi ));
512 if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" );
513 CV_CALL( src2 = cvGetMat( rightImage, &src2_stub, &coi ));
514 if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" );
515 CV_CALL( dst = cvGetMat( depthImage, &dst_stub, &coi ));
516 if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" );
519 if( CV_MAT_TYPE( src1->type ) != CV_8UC1 ||
520 CV_MAT_TYPE( src2->type ) != CV_8UC1 ||
521 CV_MAT_TYPE( dst->type ) != CV_8UC1) CV_ERROR(CV_StsUnsupportedFormat,
522 "All images must be single-channel and have 8u" );
524 if( !CV_ARE_SIZES_EQ( src1, src2 ) || !CV_ARE_SIZES_EQ( src1, dst ) )
525 CV_ERROR( CV_StsUnmatchedSizes, "" );
527 if( maxDisparity <= 0 || maxDisparity >= src1->width || maxDisparity > 255 )
528 CV_ERROR(CV_StsOutOfRange,
529 "parameter /maxDisparity/ is out of range");
531 if( mode == CV_DISPARITY_BIRCHFIELD )
533 if( param1 == CV_UNDEF_SC_PARAM ) param1 = CV_IDP_BIRCHFIELD_PARAM1;
534 if( param2 == CV_UNDEF_SC_PARAM ) param2 = CV_IDP_BIRCHFIELD_PARAM2;
535 if( param3 == CV_UNDEF_SC_PARAM ) param3 = CV_IDP_BIRCHFIELD_PARAM3;
536 if( param4 == CV_UNDEF_SC_PARAM ) param4 = CV_IDP_BIRCHFIELD_PARAM4;
537 if( param5 == CV_UNDEF_SC_PARAM ) param5 = CV_IDP_BIRCHFIELD_PARAM5;
539 CV_CALL( icvFindStereoCorrespondenceByBirchfieldDP( src1->data.ptr,
540 src2->data.ptr, dst->data.ptr,
541 cvGetMatSize( src1 ), src1->step,
542 maxDisparity, (float)param1, (float)param2, (float)param3,
543 (float)param4, (float)param5 ) );
547 CV_ERROR( CV_StsBadArg, "Unsupported mode of function" );