git.maemo.org Git - opencv/blob - cvaux/src/cvdpstereo.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "_cvaux.h"
  43
  44 /****************************************************************************************\
  45     The code below is some modification of Stan Birchfield's algorithm described in:
  46
  47     Depth Discontinuities by Pixel-to-Pixel Stereo
  48     Stan Birchfield and Carlo Tomasi
  49     International Journal of Computer Vision,
  50     35(3): 269-293, December 1999.
  51
  52     This implementation uses different cost function that results in
  53     O(pixPerRow*maxDisparity) complexity of dynamic programming stage versus
  54     O(pixPerRow*log(pixPerRow)*maxDisparity) in the above paper.
  55 \****************************************************************************************/
  56
  57 /****************************************************************************************\
  58 *       Find stereo correspondence by dynamic programming algorithm                      *
  59 \****************************************************************************************/
  60 #define ICV_DP_STEP_LEFT  0
  61 #define ICV_DP_STEP_UP    1
  62 #define ICV_DP_STEP_DIAG  2
  63
  64 #define ICV_BIRCH_DIFF_LUM 5
  65
  66 #define ICV_MAX_DP_SUM_VAL (INT_MAX/4)
  67
  68 typedef struct _CvDPCell
  69 {
  70     uchar  step; //local-optimal step
  71     int    sum;  //current sum
  72 }_CvDPCell;
  73
  74 typedef struct _CvRightImData
  75 {
  76     uchar min_val, max_val;
  77 } _CvRightImData;
  78
  79 #define CV_IMAX3(a,b,c) ((temp3 = (a) >= (b) ? (a) : (b)),(temp3 >= (c) ? temp3 : (c)))
  80 #define CV_IMIN3(a,b,c) ((temp3 = (a) <= (b) ? (a) : (b)),(temp3 <= (c) ? temp3 : (c)))
  81
  82 void icvFindStereoCorrespondenceByBirchfieldDP( uchar* src1, uchar* src2,
  83                                                 uchar* disparities,
  84                                                 CvSize size, int widthStep,
  85                                                 int    maxDisparity,
  86                                                 float  _param1, float _param2,
  87                                                 float  _param3, float _param4,
  88                                                 float  _param5 )
  89 {
  90     int     x, y, i, j, temp3;
  91     int     d, s;
  92     int     dispH =  maxDisparity + 3;
  93     uchar  *dispdata;
  94     int     imgW = size.width;
  95     int     imgH = size.height;
  96     uchar   val, prevval, prev, curr;
  97     int     min_val;
  98     uchar*  dest = disparities;
  99     int param1 = cvRound(_param1);
 100     int param2 = cvRound(_param2);
 101     int param3 = cvRound(_param3);
 102     int param4 = cvRound(_param4);
 103     int param5 = cvRound(_param5);
 104
 105     #define CELL(d,x)   cells[(d)+(x)*dispH]
 106
 107     uchar*              dsi = (uchar*)cvAlloc(sizeof(uchar)*imgW*dispH);
 108     uchar*              edges = (uchar*)cvAlloc(sizeof(uchar)*imgW*imgH);
 109     _CvDPCell*          cells = (_CvDPCell*)cvAlloc(sizeof(_CvDPCell)*imgW*MAX(dispH,(imgH+1)/2));
 110     _CvRightImData*     rData = (_CvRightImData*)cvAlloc(sizeof(_CvRightImData)*imgW);
 111     int*                reliabilities = (int*)cells;
 112
 113     for( y = 0; y < imgH; y++ )
 114     {
 115         uchar* srcdata1 = src1 + widthStep * y;
 116         uchar* srcdata2 = src2 + widthStep * y;
 117
 118         //init rData
 119         prevval = prev = srcdata2[0];
 120         for( j = 1; j < imgW; j++ )
 121         {
 122             curr = srcdata2[j];
 123             val = (uchar)((curr + prev)>>1);
 124             rData[j-1].max_val = (uchar)CV_IMAX3( val, prevval, prev );
 125             rData[j-1].min_val = (uchar)CV_IMIN3( val, prevval, prev );
 126             prevval = val;
 127             prev = curr;
 128         }
 129         rData[j-1] = rData[j-2];//last elem
 130
 131         // fill dissimularity space image
 132         for( i = 1; i <= maxDisparity + 1; i++ )
 133         {
 134             dsi += imgW;
 135             rData--;
 136             for( j = i - 1; j < imgW - 1; j++ )
 137             {
 138                 int t;
 139                 if( (t = srcdata1[j] - rData[j+1].max_val) >= 0 )
 140                 {
 141                     dsi[j] = (uchar)t;
 142                 }
 143                 else if( (t = rData[j+1].min_val - srcdata1[j]) >= 0 )
 144                 {
 145                     dsi[j] = (uchar)t;
 146                 }
 147                 else
 148                 {
 149                     dsi[j] = 0;
 150                 }
 151             }
 152         }
 153         dsi -= (maxDisparity+1)*imgW;
 154         rData += maxDisparity+1;
 155
 156         //intensity gradients image construction
 157         //left row
 158         edges[y*imgW] = edges[y*imgW+1] = edges[y*imgW+2] = 2;
 159         edges[y*imgW+imgW-1] = edges[y*imgW+imgW-2] = edges[y*imgW+imgW-3] = 1;
 160         for( j = 3; j < imgW-4; j++ )
 161         {
 162             edges[y*imgW+j] = 0;
 163
 164             if( ( CV_IMAX3( srcdata1[j-3], srcdata1[j-2], srcdata1[j-1] ) -
 165                   CV_IMIN3( srcdata1[j-3], srcdata1[j-2], srcdata1[j-1] ) ) >= ICV_BIRCH_DIFF_LUM )
 166             {
 167                 edges[y*imgW+j] |= 1;
 168             }
 169             if( ( CV_IMAX3( srcdata2[j+3], srcdata2[j+2], srcdata2[j+1] ) -
 170                   CV_IMIN3( srcdata2[j+3], srcdata2[j+2], srcdata2[j+1] ) ) >= ICV_BIRCH_DIFF_LUM )
 171             {
 172                 edges[y*imgW+j] |= 2;
 173             }
 174         }
 175
 176         //find correspondence using dynamical programming
 177         //init DP table
 178         for( x = 0; x < imgW; x++ )
 179         {
 180             CELL(0,x).sum = CELL(dispH-1,x).sum = ICV_MAX_DP_SUM_VAL;
 181             CELL(0,x).step = CELL(dispH-1,x).step = ICV_DP_STEP_LEFT;
 182         }
 183         for( d = 2; d < dispH; d++ )
 184         {
 185             CELL(d,d-2).sum = ICV_MAX_DP_SUM_VAL;
 186             CELL(d,d-2).step = ICV_DP_STEP_UP;
 187         }
 188         CELL(1,0).sum  = 0;
 189         CELL(1,0).step = ICV_DP_STEP_LEFT;
 190
 191         for( x = 1; x < imgW; x++ )
 192         {
 193             int d = MIN( x + 1, maxDisparity + 1);
 194             uchar* _edges = edges + y*imgW + x;
 195             int e0 = _edges[0] & 1;
 196             _CvDPCell* _cell = cells + x*dispH;
 197
 198             do
 199             {
 200                 int s = dsi[d*imgW+x];
 201                 int sum[3];
 202
 203                 //check left step
 204                 sum[0] = _cell[d-dispH].sum - param2;
 205
 206                 //check up step
 207                 if( _cell[d+1].step != ICV_DP_STEP_DIAG && e0 )
 208                 {
 209                     sum[1] = _cell[d+1].sum + param1;
 210
 211                     if( _cell[d-1-dispH].step != ICV_DP_STEP_UP && (_edges[1-d] & 2) )
 212                     {
 213                         int t;
 214
 215                         sum[2] = _cell[d-1-dispH].sum + param1;
 216
 217                         t = sum[1] < sum[0];
 218
 219                         //choose local-optimal pass
 220                         if( sum[t] <= sum[2] )
 221                         {
 222                             _cell[d].step = (uchar)t;
 223                             _cell[d].sum = sum[t] + s;
 224                         }
 225                         else
 226                         {
 227                             _cell[d].step = ICV_DP_STEP_DIAG;
 228                             _cell[d].sum = sum[2] + s;
 229                         }
 230                     }
 231                     else
 232                     {
 233                         if( sum[0] <= sum[1] )
 234                         {
 235                             _cell[d].step = ICV_DP_STEP_LEFT;
 236                             _cell[d].sum = sum[0] + s;
 237                         }
 238                         else
 239                         {
 240                             _cell[d].step = ICV_DP_STEP_UP;
 241                             _cell[d].sum = sum[1] + s;
 242                         }
 243                     }
 244                 }
 245                 else if( _cell[d-1-dispH].step != ICV_DP_STEP_UP && (_edges[1-d] & 2) )
 246                 {
 247                     sum[2] = _cell[d-1-dispH].sum + param1;
 248                     if( sum[0] <= sum[2] )
 249                     {
 250                         _cell[d].step = ICV_DP_STEP_LEFT;
 251                         _cell[d].sum = sum[0] + s;
 252                     }
 253                     else
 254                     {
 255                         _cell[d].step = ICV_DP_STEP_DIAG;
 256                         _cell[d].sum = sum[2] + s;
 257                     }
 258                 }
 259                 else
 260                 {
 261                     _cell[d].step = ICV_DP_STEP_LEFT;
 262                     _cell[d].sum = sum[0] + s;
 263                 }
 264             }
 265             while( --d );
 266         }// for x
 267
 268         //extract optimal way and fill disparity image
 269         dispdata = dest + widthStep * y;
 270
 271         //find min_val
 272         min_val = ICV_MAX_DP_SUM_VAL;
 273         for( i = 1; i <= maxDisparity + 1; i++ )
 274         {
 275             if( min_val > CELL(i,imgW-1).sum )
 276             {
 277                 d = i;
 278                 min_val = CELL(i,imgW-1).sum;
 279             }
 280         }
 281
 282         //track optimal pass
 283         for( x = imgW - 1; x > 0; x-- )
 284         {
 285             dispdata[x] = (uchar)(d - 1);
 286             while( CELL(d,x).step == ICV_DP_STEP_UP ) d++;
 287             if ( CELL(d,x).step == ICV_DP_STEP_DIAG )
 288             {
 289                 s = x;
 290                 while( CELL(d,x).step == ICV_DP_STEP_DIAG )
 291                 {
 292                     d--;
 293                     x--;
 294                 }
 295                 for( i = x; i < s; i++ )
 296                 {
 297                     dispdata[i] = (uchar)(d-1);
 298                 }
 299             }
 300         }//for x
 301     }// for y
 302
 303     //Postprocessing the Disparity Map
 304
 305     //remove obvious errors in the disparity map
 306     for( x = 0; x < imgW; x++ )
 307     {
 308         for( y = 1; y < imgH - 1; y++ )
 309         {
 310             if( dest[(y-1)*widthStep+x] == dest[(y+1)*widthStep+x] )
 311             {
 312                 dest[y*widthStep+x] = dest[(y-1)*widthStep+x];
 313             }
 314         }
 315     }
 316
 317     //compute intensity Y-gradients
 318     for( x = 0; x < imgW; x++ )
 319     {
 320         for( y = 1; y < imgH - 1; y++ )
 321         {
 322             if( ( CV_IMAX3( src1[(y-1)*widthStep+x], src1[y*widthStep+x],
 323                         src1[(y+1)*widthStep+x] ) -
 324                   CV_IMIN3( src1[(y-1)*widthStep+x], src1[y*widthStep+x],
 325                         src1[(y+1)*widthStep+x] ) ) >= ICV_BIRCH_DIFF_LUM )
 326             {
 327                 edges[y*imgW+x] |= 4;
 328                 edges[(y+1)*imgW+x] |= 4;
 329                 edges[(y-1)*imgW+x] |= 4;
 330                 y++;
 331             }
 332         }
 333     }
 334
 335     //remove along any particular row, every gradient
 336     //for which two adjacent columns do not agree.
 337     for( y = 0; y < imgH; y++ )
 338     {
 339         prev = edges[y*imgW];
 340         for( x = 1; x < imgW - 1; x++ )
 341         {
 342             curr = edges[y*imgW+x];
 343             if( (curr & 4) &&
 344                 ( !( prev & 4 ) ||
 345                   !( edges[y*imgW+x+1] & 4 ) ) )
 346             {
 347                 edges[y*imgW+x] -= 4;
 348             }
 349             prev = curr;
 350         }
 351     }
 352
 353     // define reliability
 354     for( x = 0; x < imgW; x++ )
 355     {
 356         for( y = 1; y < imgH; y++ )
 357         {
 358             i = y - 1;
 359             for( ; y < imgH && dest[y*widthStep+x] == dest[(y-1)*widthStep+x]; y++ )
 360                 ;
 361             s = y - i;
 362             for( ; i < y; i++ )
 363             {
 364                 reliabilities[i*imgW+x] = s;
 365             }
 366         }
 367     }
 368
 369     //Y - propagate reliable regions
 370     for( x = 0; x < imgW; x++ )
 371     {
 372         for( y = 0; y < imgH; y++ )
 373         {
 374             d = dest[y*widthStep+x];
 375             if( reliabilities[y*imgW+x] >= param4 && !(edges[y*imgW+x] & 4) &&
 376                 d > 0 )//highly || moderately
 377             {
 378                 disparities[y*widthStep+x] = (uchar)d;
 379                 //up propagation
 380                 for( i = y - 1; i >= 0; i-- )
 381                 {
 382                     if(  ( edges[i*imgW+x] & 4 ) ||
 383                          ( dest[i*widthStep+x] < d &&
 384                            reliabilities[i*imgW+x] >= param3 ) ||
 385                          ( reliabilities[y*imgW+x] < param5 &&
 386                            dest[i*widthStep+x] - 1 == d ) ) break;
 387
 388                     disparities[i*widthStep+x] = (uchar)d;
 389                 }
 390
 391                 //down propagation
 392                 for( i = y + 1; i < imgH; i++ )
 393                 {
 394                     if(  ( edges[i*imgW+x] & 4 ) ||
 395                          ( dest[i*widthStep+x] < d &&
 396                            reliabilities[i*imgW+x] >= param3 ) ||
 397                          ( reliabilities[y*imgW+x] < param5 &&
 398                            dest[i*widthStep+x] - 1 == d ) ) break;
 399
 400                     disparities[i*widthStep+x] = (uchar)d;
 401                 }
 402                 y = i - 1;
 403             }
 404             else
 405             {
 406                 disparities[y*widthStep+x] = (uchar)d;
 407             }
 408         }
 409     }
 410
 411     // define reliability along X
 412     for( y = 0; y < imgH; y++ )
 413     {
 414         for( x = 1; x < imgW; x++ )
 415         {
 416             i = x - 1;
 417             for( ; x < imgW && dest[y*widthStep+x] == dest[y*widthStep+x-1]; x++ );
 418             s = x - i;
 419             for( ; i < x; i++ )
 420             {
 421                 reliabilities[y*imgW+i] = s;
 422             }
 423         }
 424     }
 425
 426     //X - propagate reliable regions
 427     for( y = 0; y < imgH; y++ )
 428     {
 429         for( x = 0; x < imgW; x++ )
 430         {
 431             d = dest[y*widthStep+x];
 432             if( reliabilities[y*imgW+x] >= param4 && !(edges[y*imgW+x] & 1) &&
 433                 d > 0 )//highly || moderately
 434             {
 435                 disparities[y*widthStep+x] = (uchar)d;
 436                 //up propagation
 437                 for( i = x - 1; i >= 0; i-- )
 438                 {
 439                     if(  (edges[y*imgW+i] & 1) ||
 440                          ( dest[y*widthStep+i] < d &&
 441                            reliabilities[y*imgW+i] >= param3 ) ||
 442                          ( reliabilities[y*imgW+x] < param5 &&
 443                            dest[y*widthStep+i] - 1 == d ) ) break;
 444
 445                     disparities[y*widthStep+i] = (uchar)d;
 446                 }
 447
 448                 //down propagation
 449                 for( i = x + 1; i < imgW; i++ )
 450                 {
 451                     if(  (edges[y*imgW+i] & 1) ||
 452                          ( dest[y*widthStep+i] < d &&
 453                            reliabilities[y*imgW+i] >= param3 ) ||
 454                          ( reliabilities[y*imgW+x] < param5 &&
 455                            dest[y*widthStep+i] - 1 == d ) ) break;
 456
 457                     disparities[y*widthStep+i] = (uchar)d;
 458                 }
 459                 x = i - 1;
 460             }
 461             else
 462             {
 463                 disparities[y*widthStep+x] = (uchar)d;
 464             }
 465         }
 466     }
 467
 468     //release resources
 469     cvFree( &dsi );
 470     cvFree( &edges );
 471     cvFree( &cells );
 472     cvFree( &rData );
 473 }
 474
 475
 476 /*F///////////////////////////////////////////////////////////////////////////
 477 //
 478 //    Name:    cvFindStereoCorrespondence
 479 //    Purpose: find stereo correspondence on stereo-pair
 480 //    Context:
 481 //    Parameters:
 482 //      leftImage - left image of stereo-pair (format 8uC1).
 483 //      rightImage - right image of stereo-pair (format 8uC1).
 484 //      mode -mode of correspondance retrieval (now CV_RETR_DP_BIRCHFIELD only)
 485 //      dispImage - destination disparity image
 486 //      maxDisparity - maximal disparity
 487 //      param1, param2, param3, param4, param5 - parameters of algorithm
 488 //    Returns:
 489 //    Notes:
 490 //      Images must be rectified.
 491 //      All images must have format 8uC1.
 492 //F*/
 493 CV_IMPL void
 494 cvFindStereoCorrespondence(
 495                    const  CvArr* leftImage, const  CvArr* rightImage,
 496                    int     mode,
 497                    CvArr*  depthImage,
 498                    int     maxDisparity,
 499                    double  param1, double  param2, double  param3,
 500                    double  param4, double  param5  )
 501 {
 502     CV_FUNCNAME( "cvFindStereoCorrespondence" );
 503
 504     __BEGIN__;
 505
 506     CvMat  *src1, *src2;
 507     CvMat  *dst;
 508     CvMat  src1_stub, src2_stub, dst_stub;
 509     int    coi;
 510
 511     CV_CALL( src1 = cvGetMat( leftImage, &src1_stub, &coi ));
 512     if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" );
 513     CV_CALL( src2 = cvGetMat( rightImage, &src2_stub, &coi ));
 514     if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" );
 515     CV_CALL( dst = cvGetMat( depthImage, &dst_stub, &coi ));
 516     if( coi ) CV_ERROR( CV_BadCOI, "COI is not supported by the function" );
 517
 518     // check args
 519     if( CV_MAT_TYPE( src1->type ) != CV_8UC1 ||
 520         CV_MAT_TYPE( src2->type ) != CV_8UC1 ||
 521         CV_MAT_TYPE( dst->type ) != CV_8UC1) CV_ERROR(CV_StsUnsupportedFormat,
 522                         "All images must be single-channel and have 8u" );
 523
 524     if( !CV_ARE_SIZES_EQ( src1, src2 ) || !CV_ARE_SIZES_EQ( src1, dst ) )
 525             CV_ERROR( CV_StsUnmatchedSizes, "" );
 526
 527     if( maxDisparity <= 0 || maxDisparity >= src1->width || maxDisparity > 255 )
 528         CV_ERROR(CV_StsOutOfRange,
 529                  "parameter /maxDisparity/ is out of range");
 530
 531     if( mode == CV_DISPARITY_BIRCHFIELD )
 532     {
 533         if( param1 == CV_UNDEF_SC_PARAM ) param1 = CV_IDP_BIRCHFIELD_PARAM1;
 534         if( param2 == CV_UNDEF_SC_PARAM ) param2 = CV_IDP_BIRCHFIELD_PARAM2;
 535         if( param3 == CV_UNDEF_SC_PARAM ) param3 = CV_IDP_BIRCHFIELD_PARAM3;
 536         if( param4 == CV_UNDEF_SC_PARAM ) param4 = CV_IDP_BIRCHFIELD_PARAM4;
 537         if( param5 == CV_UNDEF_SC_PARAM ) param5 = CV_IDP_BIRCHFIELD_PARAM5;
 538
 539         CV_CALL( icvFindStereoCorrespondenceByBirchfieldDP( src1->data.ptr,
 540             src2->data.ptr, dst->data.ptr,
 541             cvGetMatSize( src1 ), src1->step,
 542             maxDisparity, (float)param1, (float)param2, (float)param3,
 543             (float)param4, (float)param5 ) );
 544     }
 545     else
 546     {
 547         CV_ERROR( CV_StsBadArg, "Unsupported mode of function" );
 548     }
 549
 550     __END__;
 551 }
 552
 553 /* End of file. */
 554