git.maemo.org Git - opencv/blob - src/cv/cvstereobm.cpp

   1 //M*//////////////////////////////////////////////////////////////////////////////////////\r
   2 //\r
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
   4 //\r
   5 //  By downloading, copying, installing or using the software you agree to this license.\r
   6 //  If you do not agree to this license, do not download, install,\r
   7 //  copy or use the software.\r
   8 //\r
   9 //\r
  10 //                        Intel License Agreement\r
  11 //                For Open Source Computer Vision Library\r
  12 //\r
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.\r
  14 // Third party copyrights are property of their respective owners.\r
  15 //\r
  16 // Redistribution and use in source and binary forms, with or without modification,\r
  17 // are permitted provided that the following conditions are met:\r
  18 //\r
  19 //   * Redistribution's of source code must retain the above copyright notice,\r
  20 //     this list of conditions and the following disclaimer.\r
  21 //\r
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,\r
  23 //     this list of conditions and the following disclaimer in the documentation\r
  24 //     and/or other materials provided with the distribution.\r
  25 //\r
  26 //   * The name of Intel Corporation may not be used to endorse or promote products\r
  27 //     derived from this software without specific prior written permission.\r
  28 //\r
  29 // This software is provided by the copyright holders and contributors "as is" and\r
  30 // any express or implied warranties, including, but not limited to, the implied\r
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.\r
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,\r
  33 // indirect, incidental, special, exemplary, or consequential damages\r
  34 // (including, but not limited to, procurement of substitute goods or services;\r
  35 // loss of use, data, or profits; or business interruption) however caused\r
  36 // and on any theory of liability, whether in contract, strict liability,\r
  37 // or tort (including negligence or otherwise) arising in any way out of\r
  38 // the use of this software, even if advised of the possibility of such damage.\r
  39 //\r
  40 //M*/\r
  41 \r
  42 /****************************************************************************************\\r
  43 *    Very fast SAD-based (Sum-of-Absolute-Diffrences) stereo correspondence algorithm.   *\r
  44 *    Contributed by Kurt Konolige                                                        *\r
  45 \****************************************************************************************/\r
  46 \r
  47 #include "_cv.h"\r
  48 /*\r
  49 #undef CV_SSE2\r
  50 #define CV_SSE2 1\r
  51 #include "emmintrin.h"\r
  52 */\r
  53 \r
  54 CV_IMPL CvStereoBMState*\r
  55 cvCreateStereoBMState( int /*preset*/, int numberOfDisparities )\r
  56 {\r
  57     CvStereoBMState* state = 0;\r
  58 \r
  59     //CV_FUNCNAME( "cvCreateStereoBMState" );\r
  60 \r
  61     __BEGIN__;\r
  62 \r
  63     state = (CvStereoBMState*)cvAlloc( sizeof(*state) );\r
  64     if( !state )\r
  65         EXIT;\r
  66 \r
  67     state->preFilterType = CV_STEREO_BM_NORMALIZED_RESPONSE;\r
  68     state->preFilterSize = 9;\r
  69     state->preFilterCap = 31;\r
  70     state->SADWindowSize = 15;\r
  71     state->minDisparity = 0;\r
  72     state->numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : 64;\r
  73     state->textureThreshold = 10;\r
  74     state->uniquenessRatio = 15;\r
  75     state->speckleRange = state->speckleWindowSize = 0;\r
  76     state->trySmallerWindows = 0;\r
  77 \r
  78     state->preFilteredImg0 = state->preFilteredImg1 = state->slidingSumBuf = 0;\r
  79     state->dbmin = state->dbmax = 0;\r
  80 \r
  81     __END__;\r
  82 \r
  83     if( cvGetErrStatus() < 0 )\r
  84         cvReleaseStereoBMState( &state );\r
  85     return state;\r
  86 }\r
  87 \r
  88 \r
  89 CV_IMPL void\r
  90 cvReleaseStereoBMState( CvStereoBMState** state )\r
  91 {\r
  92     CV_FUNCNAME( "cvReleaseStereoBMState" );\r
  93 \r
  94     __BEGIN__;\r
  95 \r
  96     if( !state )\r
  97         CV_ERROR( CV_StsNullPtr, "" );\r
  98 \r
  99     if( !*state )\r
 100         EXIT;\r
 101 \r
 102     cvReleaseMat( &(*state)->preFilteredImg0 );\r
 103     cvReleaseMat( &(*state)->preFilteredImg1 );\r
 104     cvReleaseMat( &(*state)->slidingSumBuf );\r
 105     cvReleaseMat( &(*state)->dbmin );\r
 106     cvReleaseMat( &(*state)->dbmax );\r
 107     cvFree( state );\r
 108 \r
 109     __END__;\r
 110 }\r
 111 \r
 112 static void icvPrefilter( const CvMat* src, CvMat* dst, int winsize, int ftzero, uchar* buf )\r
 113 {\r
 114     int x, y, wsz2 = winsize/2;\r
 115     int* vsum = (int*)cvAlignPtr(buf + (wsz2 + 1)*sizeof(vsum[0]), 32);\r
 116     int scale_g = winsize*winsize/8, scale_s = (1024 + scale_g)/(scale_g*2);\r
 117     const int OFS = 256*5, TABSZ = OFS*2 + 256;\r
 118     uchar tab[TABSZ];\r
 119     const uchar* sptr = src->data.ptr;\r
 120     int srcstep = src->step;\r
 121     CvSize size = cvGetMatSize(src);\r
 122 \r
 123     scale_g *= scale_s;\r
 124 \r
 125     for( x = 0; x < TABSZ; x++ )\r
 126         tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero*2 : x - OFS + ftzero);\r
 127 \r
 128     for( x = 0; x < size.width; x++ )\r
 129         vsum[x] = (ushort)(sptr[x]*(wsz2 + 2));\r
 130 \r
 131     for( y = 1; y < wsz2; y++ )\r
 132     {\r
 133         for( x = 0; x < size.width; x++ )\r
 134             vsum[x] = (ushort)(vsum[x] + sptr[srcstep*y + x]);\r
 135     }\r
 136 \r
 137     for( y = 0; y < size.height; y++ )\r
 138     {\r
 139         const uchar* top = sptr + srcstep*MAX(y-wsz2-1,0);\r
 140         const uchar* bottom = sptr + srcstep*MIN(y+wsz2,size.height-1);\r
 141         const uchar* prev = sptr + srcstep*MAX(y-1,0);\r
 142         const uchar* curr = sptr + srcstep*y;\r
 143         const uchar* next = sptr + srcstep*MIN(y+1,size.height-1);\r
 144         uchar* dptr = dst->data.ptr + dst->step*y;\r
 145         x = 0;\r
 146 \r
 147         for( ; x < size.width; x++ )\r
 148             vsum[x] = (ushort)(vsum[x] + bottom[x] - top[x]);\r
 149 \r
 150         for( x = 0; x <= wsz2; x++ )\r
 151         {\r
 152             vsum[-x-1] = vsum[0];\r
 153             vsum[size.width+x] = vsum[size.width-1];\r
 154         }\r
 155 \r
 156         int sum = vsum[0]*(wsz2 + 1);\r
 157         for( x = 1; x <= wsz2; x++ )\r
 158             sum += vsum[x];\r
 159 \r
 160         int val = ((curr[0]*5 + curr[1] + prev[0] + next[0])*scale_g - sum*scale_s) >> 10;\r
 161         dptr[0] = tab[val + OFS];\r
 162 \r
 163         for( x = 1; x < size.width-1; x++ )\r
 164         {\r
 165             sum += vsum[x+wsz2] - vsum[x-wsz2-1];\r
 166             val = ((curr[x]*4 + curr[x-1] + curr[x+1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;\r
 167             dptr[x] = tab[val + OFS];\r
 168         }\r
 169         \r
 170         sum += vsum[x+wsz2] - vsum[x-wsz2-1];\r
 171         val = ((curr[x]*5 + curr[x-1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;\r
 172         dptr[x] = tab[val + OFS];\r
 173     }\r
 174 }\r
 175 \r
 176 \r
 177 static const int DISPARITY_SHIFT = 4;\r
 178 \r
 179 #if CV_SSE2\r
 180 static void\r
 181 icvFindStereoCorrespondenceBM_SSE2( const CvMat* left, const CvMat* right,\r
 182                                     CvMat* disp, const CvMat* dbmin,\r
 183                                     const CvMat* dbmax, CvStereoBMState* state,\r
 184                                     int realSADWin, uchar* buf, int _dy0, int _dy1 )\r
 185 {\r
 186     int x, y, d;\r
 187     int wsz = realSADWin, wsz2 = wsz/2;\r
 188     int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);\r
 189     int ndisp = state->numberOfDisparities;\r
 190     int mindisp = state->minDisparity;\r
 191     int lofs = MAX(ndisp - 1 + mindisp, 0);\r
 192     int rofs = -MIN(ndisp - 1 + mindisp, 0);\r
 193     int width = left->cols, height = left->rows;\r
 194     int width1 = width - rofs - ndisp + 1;\r
 195     int ftzero = state->preFilterCap;\r
 196     int textureThreshold = state->textureThreshold;\r
 197     int uniquenessRatio = state->uniquenessRatio;\r
 198     short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);\r
 199     int DELTA = (state->numberOfDisparities - 1 - state->minDisparity) << DISPARITY_SHIFT;\r
 200 \r
 201     ushort *sad, *hsad0, *hsad, *hsad_sub;\r
 202     int *htext;\r
 203     uchar *cbuf0, *cbuf;\r
 204     const uchar* lptr0 = left->data.ptr + lofs;\r
 205     const uchar* rptr0 = right->data.ptr + rofs;\r
 206     const uchar *lptr, *lptr_sub, *rptr;\r
 207     short* dptr = disp->data.s;\r
 208     int sstep = left->step;\r
 209     int dstep = disp->step/sizeof(dptr[0]);\r
 210     int cstep = (height + dy0 + dy1)*ndisp;\r
 211     const int TABSZ = 256;\r
 212     uchar tab[TABSZ];\r
 213     const __m128i d0_8 = _mm_setr_epi16(0,1,2,3,4,5,6,7), dd_8 = _mm_set1_epi16(8);\r
 214 \r
 215     sad = (ushort*)cvAlignPtr(buf + sizeof(sad[0]));\r
 216     hsad0 = (ushort*)cvAlignPtr(sad + ndisp + 1 + dy0*ndisp);\r
 217     htext = (int*)cvAlignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2);\r
 218     cbuf0 = (uchar*)cvAlignPtr(htext + height + wsz2 + 2 + dy0*ndisp);\r
 219 \r
 220     for( x = 0; x < TABSZ; x++ )\r
 221         tab[x] = (uchar)abs(x - ftzero);\r
 222 \r
 223     // initialize buffers\r
 224     memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) );\r
 225     memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) );\r
 226 \r
 227     for( x = -wsz2-1; x < wsz2; x++ )\r
 228     {\r
 229         hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp;\r
 230         lptr = lptr0 + MIN(MAX(x, -lofs), width-lofs-1) - dy0*sstep;\r
 231         rptr = rptr0 + MIN(MAX(x, -rofs), width-rofs-1) - dy0*sstep;\r
 232 \r
 233         for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )\r
 234         {\r
 235             int lval = lptr[0];\r
 236             for( d = 0; d < ndisp; d++ )\r
 237             {\r
 238                 int diff = abs(lval - rptr[d]);\r
 239                 cbuf[d] = (uchar)diff;\r
 240                 hsad[d] = (ushort)(hsad[d] + diff);\r
 241             }\r
 242             htext[y] += tab[lval];\r
 243         }\r
 244     }\r
 245 \r
 246     // initialize the left and right borders of the disparity map\r
 247     for( y = 0; y < height; y++ )\r
 248     {\r
 249         for( x = 0; x < lofs; x++ )\r
 250             dptr[y*dstep + x] = FILTERED;\r
 251         for( x = lofs + width1; x < width; x++ )\r
 252             dptr[y*dstep + x] = FILTERED;\r
 253     }\r
 254     dptr += lofs;\r
 255 \r
 256     for( x = 0; x < width1; x++, dptr++ )\r
 257     {\r
 258         int x0 = x - wsz2 - 1, x1 = x + wsz2;\r
 259         const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;\r
 260         uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;\r
 261         hsad = hsad0 - dy0*ndisp;\r
 262         lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;\r
 263         lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;\r
 264         rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep;\r
 265 \r
 266         for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp,\r
 267              hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )\r
 268         {\r
 269             int lval = lptr[0];\r
 270             __m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128();\r
 271             for( d = 0; d < ndisp; d += 16 )\r
 272             {\r
 273                 __m128i rv = _mm_loadu_si128((const __m128i*)(rptr + d));\r
 274                 __m128i hsad_l = _mm_load_si128((__m128i*)(hsad + d));\r
 275                 __m128i hsad_h = _mm_load_si128((__m128i*)(hsad + d + 8));\r
 276                 __m128i cbs = _mm_load_si128((const __m128i*)(cbuf_sub + d));\r
 277                 __m128i diff = _mm_adds_epu8(_mm_subs_epu8(lv, rv), _mm_subs_epu8(rv, lv));\r
 278                 __m128i diff_h = _mm_sub_epi16(_mm_unpackhi_epi8(diff, z), _mm_unpackhi_epi8(cbs, z));\r
 279                 _mm_store_si128((__m128i*)(cbuf + d), diff);\r
 280                 diff = _mm_sub_epi16(_mm_unpacklo_epi8(diff, z), _mm_unpacklo_epi8(cbs, z));\r
 281                 hsad_h = _mm_add_epi16(hsad_h, diff_h);\r
 282                 hsad_l = _mm_add_epi16(hsad_l, diff);\r
 283                 _mm_store_si128((__m128i*)(hsad + d), hsad_l);\r
 284                 _mm_store_si128((__m128i*)(hsad + d + 8), hsad_h);\r
 285             }\r
 286             htext[y] += tab[lval] - tab[lptr_sub[0]];\r
 287         }\r
 288 \r
 289         // fill borders\r
 290         for( y = dy1; y <= wsz2; y++ )\r
 291             htext[height+y] = htext[height+dy1-1];\r
 292         for( y = -wsz2-1; y < -dy0; y++ )\r
 293             htext[y] = htext[-dy0];\r
 294 \r
 295         // initialize sums\r
 296         for( d = 0; d < ndisp; d++ )\r
 297             sad[d] = (ushort)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));\r
 298         \r
 299         hsad = hsad0 + (1 - dy0)*ndisp;\r
 300         for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )\r
 301             for( d = 0; d < ndisp; d++ )\r
 302                 sad[d] = (ushort)(sad[d] + hsad[d]);\r
 303         int tsum = 0;\r
 304         for( y = -wsz2-1; y < wsz2; y++ )\r
 305             tsum += htext[y];\r
 306 \r
 307         // finally, start the real processing\r
 308         for( y = 0; y < height; y++ )\r
 309         {\r
 310             int minsad = INT_MAX, mind = -1;\r
 311             hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;\r
 312             hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;\r
 313             __m128i minsad8 = _mm_set1_epi16(SHRT_MAX);\r
 314             __m128i mind8 = _mm_set1_epi16(-1), d8 = d0_8, mask;\r
 315 \r
 316             for( d = 0; d < ndisp; d += 8 )\r
 317             {\r
 318                 __m128i v0 = _mm_load_si128((__m128i*)(hsad_sub + d));\r
 319                 __m128i v1 = _mm_load_si128((__m128i*)(hsad + d));\r
 320                 __m128i sad8 = _mm_load_si128((__m128i*)(sad + d));\r
 321                 sad8 = _mm_sub_epi16(sad8, v0);\r
 322                 sad8 = _mm_add_epi16(sad8, v1);\r
 323 \r
 324                 mask = _mm_cmpgt_epi16(minsad8, sad8);\r
 325                 _mm_store_si128((__m128i*)(sad + d), sad8);\r
 326                 minsad8 = _mm_min_epi16(minsad8, sad8);\r
 327                 mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(d8,mind8),mask));\r
 328                 d8 = _mm_add_epi16(d8, dd_8);\r
 329             }\r
 330 \r
 331             __m128i minsad82 = _mm_unpackhi_epi64(minsad8, minsad8);\r
 332             __m128i mind82 = _mm_unpackhi_epi64(mind8, mind8);\r
 333             mask = _mm_cmpgt_epi16(minsad8, minsad82);\r
 334             mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));\r
 335             minsad8 = _mm_min_epi16(minsad8, minsad82);\r
 336 \r
 337             minsad82 = _mm_shufflelo_epi16(minsad8, _MM_SHUFFLE(3,2,3,2));\r
 338             mind82 = _mm_shufflelo_epi16(mind8, _MM_SHUFFLE(3,2,3,2));\r
 339             mask = _mm_cmpgt_epi16(minsad8, minsad82);\r
 340             mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));\r
 341             minsad8 = _mm_min_epi16(minsad8, minsad82);\r
 342 \r
 343             minsad82 = _mm_shufflelo_epi16(minsad8, 1);\r
 344             mind82 = _mm_shufflelo_epi16(mind8, 1);\r
 345             mask = _mm_cmpgt_epi16(minsad8, minsad82);\r
 346             mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));\r
 347             mind = (short)_mm_cvtsi128_si32(mind8);\r
 348             minsad = sad[mind];\r
 349             tsum += htext[y + wsz2] - htext[y - wsz2 - 1];\r
 350             if( tsum < textureThreshold )\r
 351             {\r
 352                 if( !dbmin )\r
 353                     dptr[y*dstep] = FILTERED;\r
 354                 continue;\r
 355             }\r
 356 \r
 357             if( uniquenessRatio > 0 )\r
 358             {\r
 359                 int thresh = minsad + (minsad * uniquenessRatio/100);\r
 360                 __m128i thresh8 = _mm_set1_epi16((short)(thresh + 1));\r
 361                 __m128i d1 = _mm_set1_epi16((short)(mind-1)), d2 = _mm_set1_epi16((short)(mind+1));\r
 362                 __m128i d8 = d0_8;\r
 363 \r
 364                 for( d = 0; d < ndisp; d += 8 )\r
 365                 {\r
 366                     __m128i sad8 = _mm_load_si128((__m128i*)(sad + d));\r
 367                     __m128i mask = _mm_cmpgt_epi16( thresh8, sad8 );\r
 368                     mask = _mm_and_si128(mask, _mm_or_si128(_mm_cmpgt_epi16(d1,d8), _mm_cmpgt_epi16(d8,d2)));\r
 369                     if( _mm_movemask_epi8(mask) )\r
 370                         break;\r
 371                     d8 = _mm_add_epi16(d8, dd_8);\r
 372                 }\r
 373                 if( d < ndisp )\r
 374                 {\r
 375                     if( !dbmin )\r
 376                         dptr[y*dstep] = FILTERED;\r
 377                     continue;\r
 378                 }\r
 379             }\r
 380             \r
 381             if( dbmin )\r
 382             {\r
 383                 int maxval = ((const short*)(dbmin->data.ptr + dbmin->step*y))[x];\r
 384                 int minval = ((const short*)(dbmax->data.ptr + dbmax->step*y))[x];\r
 385                 minval = (DELTA - minval) >> DISPARITY_SHIFT;\r
 386                 maxval = (DELTA - maxval + (1<<DISPARITY_SHIFT)-1) >> DISPARITY_SHIFT;\r
 387                 if( d < minval || d > maxval )\r
 388                     continue;\r
 389             }\r
 390 \r
 391             {\r
 392             sad[-1] = sad[1];\r
 393             sad[ndisp] = sad[ndisp-2];\r
 394             int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind];\r
 395             dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);\r
 396             }\r
 397         }\r
 398     }\r
 399 }\r
 400 #endif\r
 401 \r
 402 static void\r
 403 icvFindStereoCorrespondenceBM( const CvMat* left, const CvMat* right,\r
 404                                CvMat* disp, const CvMat* dbmin,\r
 405                                const CvMat* dbmax, CvStereoBMState* state,\r
 406                                int realSADWin, uchar* buf, int _dy0, int _dy1 )\r
 407 {\r
 408     int x, y, d;\r
 409     int wsz = realSADWin, wsz2 = wsz/2;\r
 410     int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);\r
 411     int ndisp = state->numberOfDisparities;\r
 412     int mindisp = state->minDisparity;\r
 413     int lofs = MAX(ndisp - 1 + mindisp, 0);\r
 414     int rofs = -MIN(ndisp - 1 + mindisp, 0);\r
 415     int width = left->cols, height = left->rows;\r
 416     int width1 = width - rofs - ndisp + 1;\r
 417     int ftzero = state->preFilterCap;\r
 418     int textureThreshold = state->textureThreshold;\r
 419     int uniquenessRatio = state->uniquenessRatio;\r
 420     short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);\r
 421     int DELTA = (state->numberOfDisparities - 1 - state->minDisparity) << DISPARITY_SHIFT;\r
 422 \r
 423     int *sad, *hsad0, *hsad, *hsad_sub, *htext;\r
 424     uchar *cbuf0, *cbuf;\r
 425     const uchar* lptr0 = left->data.ptr + lofs;\r
 426     const uchar* rptr0 = right->data.ptr + rofs;\r
 427     const uchar *lptr, *lptr_sub, *rptr;\r
 428     short* dptr = disp->data.s;\r
 429     int sstep = left->step;\r
 430     int dstep = disp->step/sizeof(dptr[0]);\r
 431     int cstep = (height+dy0+dy1)*ndisp;\r
 432     const int TABSZ = 256;\r
 433     uchar tab[TABSZ];\r
 434 \r
 435     sad = (int*)cvAlignPtr(buf + sizeof(sad[0]));\r
 436     hsad0 = (int*)cvAlignPtr(sad + ndisp + 1 + dy0*ndisp);\r
 437     htext = (int*)cvAlignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2);\r
 438     cbuf0 = (uchar*)cvAlignPtr(htext + height + wsz2 + 2 + dy0*ndisp);\r
 439 \r
 440     for( x = 0; x < TABSZ; x++ )\r
 441         tab[x] = (uchar)abs(x - ftzero);\r
 442 \r
 443     // initialize buffers\r
 444     memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) );\r
 445     memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) );\r
 446 \r
 447     for( x = -wsz2-1; x < wsz2; x++ )\r
 448     {\r
 449         hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp;\r
 450         lptr = lptr0 + MIN(MAX(x, -lofs), width-lofs-1) - dy0*sstep;\r
 451         rptr = rptr0 + MIN(MAX(x, -rofs), width-rofs-1) - dy0*sstep;\r
 452 \r
 453         for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )\r
 454         {\r
 455             int lval = lptr[0];\r
 456             for( d = 0; d < ndisp; d++ )\r
 457             {\r
 458                 int diff = abs(lval - rptr[d]);\r
 459                 cbuf[d] = (uchar)diff;\r
 460                 hsad[d] = (int)(hsad[d] + diff);\r
 461             }\r
 462             htext[y] += tab[lval];\r
 463         }\r
 464     }\r
 465 \r
 466     // initialize the left and right borders of the disparity map\r
 467     for( y = 0; y < height; y++ )\r
 468     {\r
 469         for( x = 0; x < lofs; x++ )\r
 470             dptr[y*dstep + x] = FILTERED;\r
 471         for( x = lofs + width1; x < width; x++ )\r
 472             dptr[y*dstep + x] = FILTERED;\r
 473     }\r
 474     dptr += lofs;\r
 475 \r
 476     for( x = 0; x < width1; x++, dptr++ )\r
 477     {\r
 478         int x0 = x - wsz2 - 1, x1 = x + wsz2;\r
 479         const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;\r
 480         uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;\r
 481         hsad = hsad0 - dy0*ndisp;\r
 482         lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;\r
 483         lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;\r
 484         rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep;\r
 485 \r
 486         for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp,\r
 487              hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )\r
 488         {\r
 489             int lval = lptr[0];\r
 490             for( d = 0; d < ndisp; d++ )\r
 491             {\r
 492                 int diff = abs(lval - rptr[d]);\r
 493                 cbuf[d] = (uchar)diff;\r
 494                 hsad[d] = hsad[d] + diff - cbuf_sub[d];\r
 495             }\r
 496             htext[y] += tab[lval] - tab[lptr_sub[0]];\r
 497         }\r
 498 \r
 499         // fill borders\r
 500         for( y = dy1; y <= wsz2; y++ )\r
 501             htext[height+y] = htext[height+dy1-1];\r
 502         for( y = -wsz2-1; y < -dy0; y++ )\r
 503             htext[y] = htext[-dy0];\r
 504 \r
 505         // initialize sums\r
 506         for( d = 0; d < ndisp; d++ )\r
 507             sad[d] = (int)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));\r
 508         \r
 509         hsad = hsad0 + (1 - dy0)*ndisp;\r
 510         for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )\r
 511             for( d = 0; d < ndisp; d++ )\r
 512                 sad[d] = (int)(sad[d] + hsad[d]);\r
 513         int tsum = 0;\r
 514         for( y = -wsz2-1; y < wsz2; y++ )\r
 515             tsum += htext[y];\r
 516 \r
 517         // finally, start the real processing\r
 518         for( y = 0; y < height; y++ )\r
 519         {\r
 520             int minsad = INT_MAX, mind = -1;\r
 521             hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;\r
 522             hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;\r
 523 \r
 524             for( d = 0; d < ndisp; d++ )\r
 525             {\r
 526                 int currsad = sad[d] + hsad[d] - hsad_sub[d];\r
 527                 sad[d] = currsad;\r
 528                 if( currsad < minsad )\r
 529                 {\r
 530                     minsad = currsad;\r
 531                     mind = d;\r
 532                 }\r
 533             }\r
 534             tsum += htext[y + wsz2] - htext[y - wsz2 - 1];\r
 535             if( tsum < textureThreshold )\r
 536             {\r
 537                 if( !dbmin )\r
 538                     dptr[y*dstep] = FILTERED;\r
 539                 continue;\r
 540             }\r
 541 \r
 542             if( uniquenessRatio > 0 )\r
 543             {\r
 544                 int thresh = minsad + (minsad * uniquenessRatio/100);\r
 545                 for( d = 0; d < ndisp; d++ )\r
 546                 {\r
 547                     if( sad[d] <= thresh && (d < mind-1 || d > mind+1))\r
 548                         break;\r
 549                 }\r
 550                 if( d < ndisp )\r
 551                 {\r
 552                     if( !dbmin )\r
 553                         dptr[y*dstep] = FILTERED;\r
 554                     continue;\r
 555                 }\r
 556             }\r
 557 \r
 558             if( dbmin )\r
 559             {\r
 560                 int maxval = ((const short*)(dbmin->data.ptr + dbmin->step*y))[x];\r
 561                 int minval = ((const short*)(dbmax->data.ptr + dbmax->step*y))[x];\r
 562                 minval = (DELTA - minval) >> DISPARITY_SHIFT;\r
 563                 maxval = (DELTA - maxval + (1<<DISPARITY_SHIFT)-1) >> DISPARITY_SHIFT;\r
 564                 if( d < minval || d > maxval )\r
 565                     continue;\r
 566             }\r
 567             \r
 568             {\r
 569             sad[-1] = sad[1];\r
 570             sad[ndisp] = sad[ndisp-2];\r
 571             int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind];\r
 572             dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);\r
 573             }\r
 574         }\r
 575     }\r
 576 }\r
 577 \r
 578 CV_IMPL void\r
 579 cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr,\r
 580                               CvArr* disparr, CvStereoBMState* state )\r
 581 {\r
 582     CV_FUNCNAME( "cvFindStereoCorrespondenceBM" );\r
 583 \r
 584     __BEGIN__;\r
 585 \r
 586     CvMat lstub, *left0 = cvGetMat( leftarr, &lstub );\r
 587     CvMat rstub, *right0 = cvGetMat( rightarr, &rstub );\r
 588     CvMat left, right;\r
 589     CvMat dstub, *disp = cvGetMat( disparr, &dstub );\r
 590     int bufSize0, bufSize1, bufSize, width, width1, height;\r
 591     int wsz, ndisp, mindisp, lofs, rofs;\r
 592     int i, n = cvGetNumThreads();\r
 593     int FILTERED, SMALL_WIN_SIZE;\r
 594 \r
 595     if( !CV_ARE_SIZES_EQ(left0, right0) ||\r
 596         !CV_ARE_SIZES_EQ(disp, left0) )\r
 597         CV_ERROR( CV_StsUnmatchedSizes, "All the images must have the same size" );\r
 598 \r
 599     if( CV_MAT_TYPE(left0->type) != CV_8UC1 ||\r
 600         !CV_ARE_TYPES_EQ(left0, right0) ||\r
 601         CV_MAT_TYPE(disp->type) != CV_16SC1 )\r
 602         CV_ERROR( CV_StsUnsupportedFormat,\r
 603         "Both input images must have 8uC1 format and the disparity image must have 16sC1 format" );\r
 604 \r
 605     if( !state )\r
 606         CV_ERROR( CV_StsNullPtr, "Stereo BM state is NULL." );\r
 607 \r
 608     if( state->preFilterType != CV_STEREO_BM_NORMALIZED_RESPONSE )\r
 609         CV_ERROR( CV_StsOutOfRange, "preFilterType must be =CV_STEREO_BM_NORMALIZED_RESPONSE" );\r
 610 \r
 611     if( state->preFilterSize < 5 || state->preFilterSize > 255 || state->preFilterSize % 2 == 0 )\r
 612         CV_ERROR( CV_StsOutOfRange, "preFilterSize must be odd and be within 5..255" );\r
 613 \r
 614     if( state->preFilterCap < 1 || state->preFilterCap > 63 )\r
 615         CV_ERROR( CV_StsOutOfRange, "preFilterCap must be within 1..63" );\r
 616 \r
 617     if( state->SADWindowSize < 5 || state->SADWindowSize > 255 || state->SADWindowSize % 2 == 0 ||\r
 618         state->SADWindowSize >= MIN(left0->cols, left0->rows) )\r
 619         CV_ERROR( CV_StsOutOfRange, "SADWindowSize must be odd, be within 5..255 and "\r
 620                                     "be not larger than image width or height" );\r
 621 \r
 622     if( state->numberOfDisparities <= 0 || state->numberOfDisparities % 16 != 0 )\r
 623         CV_ERROR( CV_StsOutOfRange, "numberOfDisparities must be positive and divisble by 16" );\r
 624     if( state->textureThreshold < 0 )\r
 625         CV_ERROR( CV_StsOutOfRange, "texture threshold must be non-negative" );\r
 626     if( state->uniquenessRatio < 0 )\r
 627         CV_ERROR( CV_StsOutOfRange, "uniqueness ratio must be non-negative" );\r
 628 \r
 629     if( !state->preFilteredImg0 ||\r
 630         state->preFilteredImg0->cols*state->preFilteredImg0->rows < left0->cols*left0->rows )\r
 631     {\r
 632         cvReleaseMat( &state->preFilteredImg0 );\r
 633         cvReleaseMat( &state->preFilteredImg1 );\r
 634 \r
 635         state->preFilteredImg0 = cvCreateMat( left0->rows, left0->cols, CV_8U );\r
 636         state->preFilteredImg1 = cvCreateMat( left0->rows, left0->cols, CV_8U );\r
 637     }\r
 638     left = cvMat(left0->rows, left0->cols, CV_8U, state->preFilteredImg0->data.ptr);\r
 639     right = cvMat(right0->rows, right0->cols, CV_8U, state->preFilteredImg1->data.ptr);\r
 640     \r
 641     mindisp = state->minDisparity;\r
 642     ndisp = state->numberOfDisparities;\r
 643 \r
 644     width = left0->cols;\r
 645     height = left0->rows;\r
 646     lofs = MAX(ndisp - 1 + mindisp, 0);\r
 647     rofs = -MIN(ndisp - 1 + mindisp, 0);\r
 648     width1 = width - rofs - ndisp + 1;\r
 649     FILTERED = (short)((state->minDisparity - 1) << DISPARITY_SHIFT);\r
 650 \r
 651     if( lofs >= width || rofs >= width || width1 < 1 )\r
 652     {\r
 653         cvSet( disp, cvScalarAll(FILTERED) );\r
 654         EXIT;\r
 655     }\r
 656 \r
 657     wsz = state->SADWindowSize;\r
 658     bufSize0 = (ndisp + 2)*sizeof(int) + (height+wsz+2)*ndisp*sizeof(int) +\r
 659         (height + wsz + 2)*sizeof(int) + (height+wsz+2)*ndisp*(wsz+1)*sizeof(uchar) + 256;\r
 660     bufSize1 = (width + state->preFilterSize + 2)*sizeof(int) + 256;\r
 661     bufSize = MAX(bufSize0, bufSize1);\r
 662     n = MAX(MIN(height/wsz, n), 1);\r
 663 \r
 664     if( !state->slidingSumBuf || state->slidingSumBuf->cols < bufSize*n )\r
 665     {\r
 666         cvReleaseMat( &state->slidingSumBuf );\r
 667         state->slidingSumBuf = cvCreateMat( 1, bufSize*n, CV_8U );\r
 668     }\r
 669 \r
 670 #ifdef _OPENMP\r
 671 #pragma omp parallel sections num_threads(n)\r
 672 #endif\r
 673     {\r
 674     #ifdef _OPENMP\r
 675     #pragma omp section\r
 676     #endif\r
 677         icvPrefilter( left0, &left, state->preFilterSize,\r
 678             state->preFilterCap, state->slidingSumBuf->data.ptr );\r
 679     #ifdef _OPENMP\r
 680     #pragma omp section\r
 681     #endif\r
 682         icvPrefilter( right0, &right, state->preFilterSize,\r
 683             state->preFilterCap, state->slidingSumBuf->data.ptr + bufSize1*(n>1) );\r
 684     }\r
 685 \r
 686 #ifdef _OPENMP\r
 687     #pragma omp parallel for num_threads(n) schedule(static)\r
 688 #endif\r
 689     for( i = 0; i < n; i++ )\r
 690     {\r
 691         int thread_id = cvGetThreadNum();\r
 692         CvMat left_i, right_i, disp_i;\r
 693         int row0 = i*left.rows/n, row1 = (i+1)*left.rows/n;\r
 694         cvGetRows( &left, &left_i, row0, row1 );\r
 695         cvGetRows( &right, &right_i, row0, row1 );\r
 696         cvGetRows( disp, &disp_i, row0, row1 );\r
 697     #if CV_SSE2\r
 698         if( state->preFilterCap <= 31 && state->SADWindowSize <= 21 )\r
 699         {\r
 700             icvFindStereoCorrespondenceBM_SSE2( &left_i, &right_i, &disp_i, 0, 0, state,\r
 701                 state->SADWindowSize, state->slidingSumBuf->data.ptr + thread_id*bufSize0,\r
 702                 row0, left.rows-row1 );\r
 703         }\r
 704         else\r
 705     #endif\r
 706         {\r
 707             icvFindStereoCorrespondenceBM( &left_i, &right_i, &disp_i, 0, 0, state,\r
 708                 state->SADWindowSize, state->slidingSumBuf->data.ptr + thread_id*bufSize0,\r
 709                 row0, left.rows-row1 );\r
 710         }\r
 711     }\r
 712 \r
 713     SMALL_WIN_SIZE = MAX((state->SADWindowSize/2)|1, 9);\r
 714 \r
 715     if( !state->trySmallerWindows || SMALL_WIN_SIZE >= state->SADWindowSize )\r
 716         EXIT;\r
 717 \r
 718     if( !state->dbmin || !CV_ARE_SIZES_EQ(state->dbmin, disp) )\r
 719     {\r
 720         cvReleaseMat( &state->dbmin );\r
 721         cvReleaseMat( &state->dbmax );\r
 722         state->dbmin = cvCreateMat( disp->rows, disp->cols, CV_16SC1 );\r
 723         state->dbmax = cvCreateMat( disp->rows, disp->cols, CV_16SC1 );\r
 724     }\r
 725     \r
 726     for( i = 0; i < disp->rows; i++ )\r
 727     {\r
 728         int j;\r
 729         short* minptr = (short*)(state->dbmin->data.ptr + state->dbmin->step*i);\r
 730         short* maxptr = (short*)(state->dbmax->data.ptr + state->dbmax->step*i);\r
 731 \r
 732         for( j = 0; j < disp->cols; j++ )\r
 733         {\r
 734             short dval = ((const short*)(disp->data.ptr + disp->step*i))[j];\r
 735             if( dval < (state->minDisparity << DISPARITY_SHIFT) )\r
 736                 minptr[j] = maxptr[j] = dval;\r
 737             else\r
 738             {\r
 739                 minptr[j] = SHRT_MAX;\r
 740                 maxptr[j] = SHRT_MIN;\r
 741             }\r
 742         }\r
 743     }\r
 744 \r
 745     cvErode(state->dbmin, state->dbmin, 0, SMALL_WIN_SIZE );\r
 746     cvDilate(state->dbmax, state->dbmax, 0, SMALL_WIN_SIZE );\r
 747 \r
 748 #ifdef _OPENMP\r
 749     #pragma omp parallel for num_threads(n) schedule(static)\r
 750 #endif\r
 751     for( i = 0; i < n; i++ )\r
 752     {\r
 753         int thread_id = cvGetThreadNum();\r
 754         CvMat left_i, right_i, disp_i, dbmin_i, dbmax_i;\r
 755         int row0 = i*left.rows/n, row1 = (i+1)*left.rows/n;\r
 756         cvGetRows( &left, &left_i, row0, row1 );\r
 757         cvGetRows( &right, &right_i, row0, row1 );\r
 758         cvGetRows( disp, &disp_i, row0, row1 );\r
 759         cvGetRows( state->dbmin, &dbmin_i, row0, row1 );\r
 760         cvGetRows( state->dbmax, &dbmax_i, row0, row1 );\r
 761     #if CV_SSE2\r
 762         if( state->preFilterCap <= 31 && SMALL_WIN_SIZE <= 21 )\r
 763         {\r
 764             icvFindStereoCorrespondenceBM_SSE2( &left_i, &right_i,\r
 765                 &disp_i, &dbmin_i, &dbmax_i, state, SMALL_WIN_SIZE,\r
 766                 state->slidingSumBuf->data.ptr + thread_id*bufSize0,\r
 767                 row0, left.rows-row1 );\r
 768         }\r
 769         else\r
 770     #endif\r
 771         {\r
 772             icvFindStereoCorrespondenceBM( &left_i, &right_i,\r
 773                 &disp_i, &dbmin_i, &dbmax_i, state, SMALL_WIN_SIZE,\r
 774                 state->slidingSumBuf->data.ptr + thread_id*bufSize0,\r
 775                 row0, left.rows-row1 );\r
 776         }\r
 777     }\r
 778 \r
 779     __END__;\r
 780 }\r
 781 \r
 782 namespace cv\r
 783 {\r
 784 \r
 785 StereoBM::StereoBM()\r
 786 { state = cvCreateStereoBMState(); }\r
 787 \r
 788 StereoBM::StereoBM(int _preset, int _ndisparities, int _SADWindowSize)\r
 789 { init(_preset, _ndisparities, _SADWindowSize); }\r
 790 \r
 791 void StereoBM::init(int _preset, int _ndisparities, int _SADWindowSize)\r
 792 {\r
 793     state = cvCreateStereoBMState(_preset, _ndisparities);\r
 794     state->SADWindowSize = _SADWindowSize;\r
 795 }\r
 796 \r
 797 void StereoBM::operator()( const Mat& left, const Mat& right, Mat& disparity )\r
 798 {\r
 799     disparity.create(left.size(), CV_16SC1);\r
 800     CvMat _left = left, _right = right, _disparity = disparity;\r
 801     cvFindStereoCorrespondenceBM(&_left, &_right, &_disparity, state);\r
 802 }\r
 803 \r
 804 }\r
 805 \r
 806 /* End of file. */\r