cvdpstereo.cpp.svn-base

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

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