+++ /dev/null
-/*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;
-
-#define CV_IMAX3(a,b,c) ((temp3 = (a) >= (b) ? (a) : (b)),(temp3 >= (c) ? temp3 : (c)))
-#define CV_IMIN3(a,b,c) ((temp3 = (a) <= (b) ? (a) : (b)),(temp3 <= (c) ? temp3 : (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, temp3;
- 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. */
-
projects / opencv / blobdiff