/*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. */