cvdpstereo.cpp.svn-base

来自「非结构化路识别」· SVN-BASE 代码 · 共 555 行 · 第 1/2 页

            }
        }
        
        //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( (void**)&dsi );    
    cvFree( (void**)&edges );    
    cvFree( (void**)&cells );        
    cvFree( (void**)&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, 
            icvGetMatSize( 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. */