| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // Intel License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2000, Intel Corporation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of Intel Corporation may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the Intel Corporation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| |
| #include "_cvaux.h" |
| |
| /****************************************************************************************\ |
| The code below is some modification of Stan Birchfield's algorithm described in: |
| |
| Depth Discontinuities by Pixel-to-Pixel Stereo |
| Stan Birchfield and Carlo Tomasi |
| International Journal of Computer Vision, |
| 35(3): 269-293, December 1999. |
| |
| This implementation uses different cost function that results in |
| O(pixPerRow*maxDisparity) complexity of dynamic programming stage versus |
| O(pixPerRow*log(pixPerRow)*maxDisparity) in the above paper. |
| \****************************************************************************************/ |
| |
| /****************************************************************************************\ |
| * Find stereo correspondence by dynamic programming algorithm * |
| \****************************************************************************************/ |
| #define ICV_DP_STEP_LEFT 0 |
| #define ICV_DP_STEP_UP 1 |
| #define ICV_DP_STEP_DIAG 2 |
| |
| #define ICV_BIRCH_DIFF_LUM 5 |
| |
| #define ICV_MAX_DP_SUM_VAL (INT_MAX/4) |
| |
| typedef struct _CvDPCell |
| { |
| uchar step; //local-optimal step |
| int sum; //current sum |
| }_CvDPCell; |
| |
| typedef struct _CvRightImData |
| { |
| uchar min_val, max_val; |
| } _CvRightImData; |
| |
| // When CV_IMAX3 and CV_IMIN3 are used in the same expression, the evulation |
| // order is undefined, and they should not assign to the same temporary variable. |
| #define CV_IMAX3(a,b,c) ((max3 = (a) >= (b) ? (a) : (b)),(max3 >= (c) ? max3 : (c))) |
| #define CV_IMIN3(a,b,c) ((min3 = (a) <= (b) ? (a) : (b)),(min3 <= (c) ? min3 : (c))) |
| |
| void icvFindStereoCorrespondenceByBirchfieldDP( uchar* src1, uchar* src2, |
| uchar* disparities, |
| CvSize size, int widthStep, |
| int maxDisparity, |
| float _param1, float _param2, |
| float _param3, float _param4, |
| float _param5 ) |
| { |
| int x, y, i, j, max3, min3; |
| int d, s; |
| int dispH = maxDisparity + 3; |
| uchar *dispdata; |
| int imgW = size.width; |
| int imgH = size.height; |
| uchar val, prevval, prev, curr; |
| int min_val; |
| uchar* dest = disparities; |
| int param1 = cvRound(_param1); |
| int param2 = cvRound(_param2); |
| int param3 = cvRound(_param3); |
| int param4 = cvRound(_param4); |
| int param5 = cvRound(_param5); |
| |
| #define CELL(d,x) cells[(d)+(x)*dispH] |
| |
| uchar* dsi = (uchar*)cvAlloc(sizeof(uchar)*imgW*dispH); |
| uchar* edges = (uchar*)cvAlloc(sizeof(uchar)*imgW*imgH); |
| _CvDPCell* cells = (_CvDPCell*)cvAlloc(sizeof(_CvDPCell)*imgW*MAX(dispH,(imgH+1)/2)); |
| _CvRightImData* rData = (_CvRightImData*)cvAlloc(sizeof(_CvRightImData)*imgW); |
| int* reliabilities = (int*)cells; |
| |
| for( y = 0; y < imgH; y++ ) |
| { |
| uchar* srcdata1 = src1 + widthStep * y; |
| uchar* srcdata2 = src2 + widthStep * y; |
| |
| //init rData |
| prevval = prev = srcdata2[0]; |
| for( j = 1; j < imgW; j++ ) |
| { |
| curr = srcdata2[j]; |
| val = (uchar)((curr + prev)>>1); |
| rData[j-1].max_val = (uchar)CV_IMAX3( val, prevval, prev ); |
| rData[j-1].min_val = (uchar)CV_IMIN3( val, prevval, prev ); |
| prevval = val; |
| prev = curr; |
| } |
| rData[j-1] = rData[j-2];//last elem |
| |
| // fill dissimularity space image |
| for( i = 1; i <= maxDisparity + 1; i++ ) |
| { |
| dsi += imgW; |
| rData--; |
| for( j = i - 1; j < imgW - 1; j++ ) |
| { |
| int t; |
| if( (t = srcdata1[j] - rData[j+1].max_val) >= 0 ) |
| { |
| dsi[j] = (uchar)t; |
| } |
| else if( (t = rData[j+1].min_val - srcdata1[j]) >= 0 ) |
| { |
| dsi[j] = (uchar)t; |
| } |
| else |
| { |
| dsi[j] = 0; |
| } |
| } |
| } |
| dsi -= (maxDisparity+1)*imgW; |
| rData += maxDisparity+1; |
| |
| //intensity gradients image construction |
| //left row |
| edges[y*imgW] = edges[y*imgW+1] = edges[y*imgW+2] = 2; |
| edges[y*imgW+imgW-1] = edges[y*imgW+imgW-2] = edges[y*imgW+imgW-3] = 1; |
| for( j = 3; j < imgW-4; j++ ) |
| { |
| edges[y*imgW+j] = 0; |
| |
| if( ( CV_IMAX3( srcdata1[j-3], srcdata1[j-2], srcdata1[j-1] ) - |
| CV_IMIN3( srcdata1[j-3], srcdata1[j-2], srcdata1[j-1] ) ) >= ICV_BIRCH_DIFF_LUM ) |
| { |
| edges[y*imgW+j] |= 1; |
| } |
| if( ( CV_IMAX3( srcdata2[j+3], srcdata2[j+2], srcdata2[j+1] ) - |
| CV_IMIN3( srcdata2[j+3], srcdata2[j+2], srcdata2[j+1] ) ) >= ICV_BIRCH_DIFF_LUM ) |
| { |
| edges[y*imgW+j] |= 2; |
| } |
| } |
| |
| //find correspondence using dynamical programming |
| //init DP table |
| for( x = 0; x < imgW; x++ ) |
| { |
| CELL(0,x).sum = CELL(dispH-1,x).sum = ICV_MAX_DP_SUM_VAL; |
| CELL(0,x).step = CELL(dispH-1,x).step = ICV_DP_STEP_LEFT; |
| } |
| for( d = 2; d < dispH; d++ ) |
| { |
| CELL(d,d-2).sum = ICV_MAX_DP_SUM_VAL; |
| CELL(d,d-2).step = ICV_DP_STEP_UP; |
| } |
| CELL(1,0).sum = 0; |
| CELL(1,0).step = ICV_DP_STEP_LEFT; |
| |
| for( x = 1; x < imgW; x++ ) |
| { |
| int d = MIN( x + 1, maxDisparity + 1); |
| uchar* _edges = edges + y*imgW + x; |
| int e0 = _edges[0] & 1; |
| _CvDPCell* _cell = cells + x*dispH; |
| |
| do |
| { |
| int s = dsi[d*imgW+x]; |
| int sum[3]; |
| |
| //check left step |
| sum[0] = _cell[d-dispH].sum - param2; |
| |
| //check up step |
| if( _cell[d+1].step != ICV_DP_STEP_DIAG && e0 ) |
| { |
| sum[1] = _cell[d+1].sum + param1; |
| |
| if( _cell[d-1-dispH].step != ICV_DP_STEP_UP && (_edges[1-d] & 2) ) |
| { |
| int t; |
| |
| sum[2] = _cell[d-1-dispH].sum + param1; |
| |
| t = sum[1] < sum[0]; |
| |
| //choose local-optimal pass |
| if( sum[t] <= sum[2] ) |
| { |
| _cell[d].step = (uchar)t; |
| _cell[d].sum = sum[t] + s; |
| } |
| else |
| { |
| _cell[d].step = ICV_DP_STEP_DIAG; |
| _cell[d].sum = sum[2] + s; |
| } |
| } |
| else |
| { |
| if( sum[0] <= sum[1] ) |
| { |
| _cell[d].step = ICV_DP_STEP_LEFT; |
| _cell[d].sum = sum[0] + s; |
| } |
| else |
| { |
| _cell[d].step = ICV_DP_STEP_UP; |
| _cell[d].sum = sum[1] + s; |
| } |
| } |
| } |
| else if( _cell[d-1-dispH].step != ICV_DP_STEP_UP && (_edges[1-d] & 2) ) |
| { |
| sum[2] = _cell[d-1-dispH].sum + param1; |
| if( sum[0] <= sum[2] ) |
| { |
| _cell[d].step = ICV_DP_STEP_LEFT; |
| _cell[d].sum = sum[0] + s; |
| } |
| else |
| { |
| _cell[d].step = ICV_DP_STEP_DIAG; |
| _cell[d].sum = sum[2] + s; |
| } |
| } |
| else |
| { |
| _cell[d].step = ICV_DP_STEP_LEFT; |
| _cell[d].sum = sum[0] + s; |
| } |
| } |
| while( --d ); |
| }// for x |
| |
| //extract optimal way and fill disparity image |
| dispdata = dest + widthStep * y; |
| |
| //find min_val |
| min_val = ICV_MAX_DP_SUM_VAL; |
| for( i = 1; i <= maxDisparity + 1; i++ ) |
| { |
| if( min_val > CELL(i,imgW-1).sum ) |
| { |
| d = i; |
| min_val = CELL(i,imgW-1).sum; |
| } |
| } |
| |
| //track optimal pass |
| for( x = imgW - 1; x > 0; x-- ) |
| { |
| dispdata[x] = (uchar)(d - 1); |
| while( CELL(d,x).step == ICV_DP_STEP_UP ) d++; |
| if ( CELL(d,x).step == ICV_DP_STEP_DIAG ) |
| { |
| s = x; |
| while( CELL(d,x).step == ICV_DP_STEP_DIAG ) |
| { |
| d--; |
| x--; |
| } |
| for( i = x; i < s; i++ ) |
| { |
| dispdata[i] = (uchar)(d-1); |
| } |
| } |
| }//for x |
| }// for y |
| |
| //Postprocessing the Disparity Map |
| |
| //remove obvious errors in the disparity map |
| for( x = 0; x < imgW; x++ ) |
| { |
| for( y = 1; y < imgH - 1; y++ ) |
| { |
| if( dest[(y-1)*widthStep+x] == dest[(y+1)*widthStep+x] ) |
| { |
| dest[y*widthStep+x] = dest[(y-1)*widthStep+x]; |
| } |
| } |
| } |
| |
| //compute intensity Y-gradients |
| for( x = 0; x < imgW; x++ ) |
| { |
| for( y = 1; y < imgH - 1; y++ ) |
| { |
| if( ( CV_IMAX3( src1[(y-1)*widthStep+x], src1[y*widthStep+x], |
| src1[(y+1)*widthStep+x] ) - |
| CV_IMIN3( src1[(y-1)*widthStep+x], src1[y*widthStep+x], |
| src1[(y+1)*widthStep+x] ) ) >= ICV_BIRCH_DIFF_LUM ) |
| { |
| edges[y*imgW+x] |= 4; |
| edges[(y+1)*imgW+x] |= 4; |
| edges[(y-1)*imgW+x] |= 4; |
| y++; |
| } |
| } |
| } |
| |
| //remove along any particular row, every gradient |
| //for which two adjacent columns do not agree. |
| for( y = 0; y < imgH; y++ ) |
| { |
| prev = edges[y*imgW]; |
| for( x = 1; x < imgW - 1; x++ ) |
| { |
| curr = edges[y*imgW+x]; |
| if( (curr & 4) && |
| ( !( prev & 4 ) || |
| !( edges[y*imgW+x+1] & 4 ) ) ) |
| { |
| edges[y*imgW+x] -= 4; |
| } |
| prev = curr; |
| } |
| } |
| |
| // define reliability |
| for( x = 0; x < imgW; x++ ) |
| { |
| for( y = 1; y < imgH; y++ ) |
| { |
| i = y - 1; |
| for( ; y < imgH && dest[y*widthStep+x] == dest[(y-1)*widthStep+x]; y++ ) |
| ; |
| s = y - i; |
| for( ; i < y; i++ ) |
| { |
| reliabilities[i*imgW+x] = s; |
| } |
| } |
| } |
| |
| //Y - propagate reliable regions |
| for( x = 0; x < imgW; x++ ) |
| { |
| for( y = 0; y < imgH; y++ ) |
| { |
| d = dest[y*widthStep+x]; |
| if( reliabilities[y*imgW+x] >= param4 && !(edges[y*imgW+x] & 4) && |
| d > 0 )//highly || moderately |
| { |
| disparities[y*widthStep+x] = (uchar)d; |
| //up propagation |
| for( i = y - 1; i >= 0; i-- ) |
| { |
| if( ( edges[i*imgW+x] & 4 ) || |
| ( dest[i*widthStep+x] < d && |
| reliabilities[i*imgW+x] >= param3 ) || |
| ( reliabilities[y*imgW+x] < param5 && |
| dest[i*widthStep+x] - 1 == d ) ) break; |
| |
| disparities[i*widthStep+x] = (uchar)d; |
| } |
| |
| //down propagation |
| for( i = y + 1; i < imgH; i++ ) |
| { |
| if( ( edges[i*imgW+x] & 4 ) || |
| ( dest[i*widthStep+x] < d && |
| reliabilities[i*imgW+x] >= param3 ) || |
| ( reliabilities[y*imgW+x] < param5 && |
| dest[i*widthStep+x] - 1 == d ) ) break; |
| |
| disparities[i*widthStep+x] = (uchar)d; |
| } |
| y = i - 1; |
| } |
| else |
| { |
| disparities[y*widthStep+x] = (uchar)d; |
| } |
| } |
| } |
| |
| // define reliability along X |
| for( y = 0; y < imgH; y++ ) |
| { |
| for( x = 1; x < imgW; x++ ) |
| { |
| i = x - 1; |
| for( ; x < imgW && dest[y*widthStep+x] == dest[y*widthStep+x-1]; x++ ); |
| s = x - i; |
| for( ; i < x; i++ ) |
| { |
| reliabilities[y*imgW+i] = s; |
| } |
| } |
| } |
| |
| //X - propagate reliable regions |
| for( y = 0; y < imgH; y++ ) |
| { |
| for( x = 0; x < imgW; x++ ) |
| { |
| d = dest[y*widthStep+x]; |
| if( reliabilities[y*imgW+x] >= param4 && !(edges[y*imgW+x] & 1) && |
| d > 0 )//highly || moderately |
| { |
| disparities[y*widthStep+x] = (uchar)d; |
| //up propagation |
| for( i = x - 1; i >= 0; i-- ) |
| { |
| if( (edges[y*imgW+i] & 1) || |
| ( dest[y*widthStep+i] < d && |
| reliabilities[y*imgW+i] >= param3 ) || |
| ( reliabilities[y*imgW+x] < param5 && |
| dest[y*widthStep+i] - 1 == d ) ) break; |
| |
| disparities[y*widthStep+i] = (uchar)d; |
| } |
| |
| //down propagation |
| for( i = x + 1; i < imgW; i++ ) |
| { |
| if( (edges[y*imgW+i] & 1) || |
| ( dest[y*widthStep+i] < d && |
| reliabilities[y*imgW+i] >= param3 ) || |
| ( reliabilities[y*imgW+x] < param5 && |
| dest[y*widthStep+i] - 1 == d ) ) break; |
| |
| disparities[y*widthStep+i] = (uchar)d; |
| } |
| x = i - 1; |
| } |
| else |
| { |
| disparities[y*widthStep+x] = (uchar)d; |
| } |
| } |
| } |
| |
| //release resources |
| cvFree( &dsi ); |
| cvFree( &edges ); |
| cvFree( &cells ); |
| cvFree( &rData ); |
| } |
| |
| |
| /*F/////////////////////////////////////////////////////////////////////////// |
| // |
| // Name: cvFindStereoCorrespondence |
| // Purpose: find stereo correspondence on stereo-pair |
| // Context: |
| // Parameters: |
| // leftImage - left image of stereo-pair (format 8uC1). |
| // rightImage - right image of stereo-pair (format 8uC1). |
| // mode -mode of correspondance retrieval (now CV_RETR_DP_BIRCHFIELD only) |
| // dispImage - destination disparity image |
| // maxDisparity - maximal disparity |
| // param1, param2, param3, param4, param5 - parameters of algorithm |
| // Returns: |
| // Notes: |
| // Images must be rectified. |
| // All images must have format 8uC1. |
| //F*/ |
| CV_IMPL void |
| cvFindStereoCorrespondence( |
| const CvArr* leftImage, const CvArr* rightImage, |
| int mode, |
| CvArr* depthImage, |
| int maxDisparity, |
| double param1, double param2, double param3, |
| double param4, double param5 ) |
| { |
| CV_FUNCNAME( "cvFindStereoCorrespondence" ); |
| |
| __BEGIN__; |
| |
| CvMat *src1, *src2; |
| CvMat *dst; |
| CvMat src1_stub, src2_stub, dst_stub; |
| int coi; |
| |
| CV_CALL( src1 = cvGetMat( leftImage, &src1_stub, &coi )); |
| if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" ); |
| CV_CALL( src2 = cvGetMat( rightImage, &src2_stub, &coi )); |
| if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" ); |
| CV_CALL( dst = cvGetMat( depthImage, &dst_stub, &coi )); |
| if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" ); |
| |
| // check args |
| if( CV_MAT_TYPE( src1->type ) != CV_8UC1 || |
| CV_MAT_TYPE( src2->type ) != CV_8UC1 || |
| CV_MAT_TYPE( dst->type ) != CV_8UC1) CV_ERROR(CV_StsUnsupportedFormat, |
| "All images must be single-channel and have 8u" ); |
| |
| if( !CV_ARE_SIZES_EQ( src1, src2 ) || !CV_ARE_SIZES_EQ( src1, dst ) ) |
| CV_ERROR( CV_StsUnmatchedSizes, "" ); |
| |
| if( maxDisparity <= 0 || maxDisparity >= src1->width || maxDisparity > 255 ) |
| CV_ERROR(CV_StsOutOfRange, |
| "parameter /maxDisparity/ is out of range"); |
| |
| if( mode == CV_DISPARITY_BIRCHFIELD ) |
| { |
| if( param1 == CV_UNDEF_SC_PARAM ) param1 = CV_IDP_BIRCHFIELD_PARAM1; |
| if( param2 == CV_UNDEF_SC_PARAM ) param2 = CV_IDP_BIRCHFIELD_PARAM2; |
| if( param3 == CV_UNDEF_SC_PARAM ) param3 = CV_IDP_BIRCHFIELD_PARAM3; |
| if( param4 == CV_UNDEF_SC_PARAM ) param4 = CV_IDP_BIRCHFIELD_PARAM4; |
| if( param5 == CV_UNDEF_SC_PARAM ) param5 = CV_IDP_BIRCHFIELD_PARAM5; |
| |
| CV_CALL( icvFindStereoCorrespondenceByBirchfieldDP( src1->data.ptr, |
| src2->data.ptr, dst->data.ptr, |
| cvGetMatSize( src1 ), src1->step, |
| maxDisparity, (float)param1, (float)param2, (float)param3, |
| (float)param4, (float)param5 ) ); |
| } |
| else |
| { |
| CV_ERROR( CV_StsBadArg, "Unsupported mode of function" ); |
| } |
| |
| __END__; |
| } |
| |
| /* End of file. */ |
| |
