--- /dev/null
+//M*//////////////////////////////////////////////////////////////////////////////////////\r
+//\r
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
+//\r
+// By downloading, copying, installing or using the software you agree to this license.\r
+// If you do not agree to this license, do not download, install,\r
+// copy or use the software.\r
+//\r
+//\r
+// Intel License Agreement\r
+// For Open Source Computer Vision Library\r
+//\r
+// Copyright (C) 2000, Intel Corporation, all rights reserved.\r
+// Third party copyrights are property of their respective owners.\r
+//\r
+// Redistribution and use in source and binary forms, with or without modification,\r
+// are permitted provided that the following conditions are met:\r
+//\r
+// * Redistribution's of source code must retain the above copyright notice,\r
+// this list of conditions and the following disclaimer.\r
+//\r
+// * Redistribution's in binary form must reproduce the above copyright notice,\r
+// this list of conditions and the following disclaimer in the documentation\r
+// and/or other materials provided with the distribution.\r
+//\r
+// * The name of Intel Corporation may not be used to endorse or promote products\r
+// derived from this software without specific prior written permission.\r
+//\r
+// This software is provided by the copyright holders and contributors "as is" and\r
+// any express or implied warranties, including, but not limited to, the implied\r
+// warranties of merchantability and fitness for a particular purpose are disclaimed.\r
+// In no event shall the Intel Corporation or contributors be liable for any direct,\r
+// indirect, incidental, special, exemplary, or consequential damages\r
+// (including, but not limited to, procurement of substitute goods or services;\r
+// loss of use, data, or profits; or business interruption) however caused\r
+// and on any theory of liability, whether in contract, strict liability,\r
+// or tort (including negligence or otherwise) arising in any way out of\r
+// the use of this software, even if advised of the possibility of such damage.\r
+//\r
+//M*/\r
+\r
+/****************************************************************************************\\r
+* Very fast SAD-based (Sum-of-Absolute-Diffrences) stereo correspondence algorithm. *\r
+* Contributed by Kurt Konolige *\r
+\****************************************************************************************/\r
+\r
+#include "_cv.h"\r
+/*\r
+#undef CV_SSE2\r
+#define CV_SSE2 1\r
+#include "emmintrin.h"\r
+*/\r
+\r
+CV_IMPL CvStereoBMState*\r
+cvCreateStereoBMState( int /*preset*/, int numberOfDisparities )\r
+{\r
+ CvStereoBMState* state = 0;\r
+\r
+ //CV_FUNCNAME( "cvCreateStereoBMState" );\r
+\r
+ __BEGIN__;\r
+\r
+ state = (CvStereoBMState*)cvAlloc( sizeof(*state) );\r
+ if( !state )\r
+ EXIT;\r
+\r
+ state->preFilterType = CV_STEREO_BM_NORMALIZED_RESPONSE;\r
+ state->preFilterSize = 9;\r
+ state->preFilterCap = 31;\r
+ state->SADWindowSize = 15;\r
+ state->minDisparity = 0;\r
+ state->numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : 64;\r
+ state->textureThreshold = 10;\r
+ state->uniquenessRatio = 15;\r
+ state->speckleRange = state->speckleWindowSize = 0;\r
+ state->trySmallerWindows = 0;\r
+\r
+ state->preFilteredImg0 = state->preFilteredImg1 = state->slidingSumBuf = 0;\r
+ state->dbmin = state->dbmax = 0;\r
+\r
+ __END__;\r
+\r
+ if( cvGetErrStatus() < 0 )\r
+ cvReleaseStereoBMState( &state );\r
+ return state;\r
+}\r
+\r
+\r
+CV_IMPL void\r
+cvReleaseStereoBMState( CvStereoBMState** state )\r
+{\r
+ CV_FUNCNAME( "cvReleaseStereoBMState" );\r
+\r
+ __BEGIN__;\r
+\r
+ if( !state )\r
+ CV_ERROR( CV_StsNullPtr, "" );\r
+\r
+ if( !*state )\r
+ EXIT;\r
+\r
+ cvReleaseMat( &(*state)->preFilteredImg0 );\r
+ cvReleaseMat( &(*state)->preFilteredImg1 );\r
+ cvReleaseMat( &(*state)->slidingSumBuf );\r
+ cvReleaseMat( &(*state)->dbmin );\r
+ cvReleaseMat( &(*state)->dbmax );\r
+ cvFree( state );\r
+\r
+ __END__;\r
+}\r
+\r
+static void icvPrefilter( const CvMat* src, CvMat* dst, int winsize, int ftzero, uchar* buf )\r
+{\r
+ int x, y, wsz2 = winsize/2;\r
+ int* vsum = (int*)cvAlignPtr(buf + (wsz2 + 1)*sizeof(vsum[0]), 32);\r
+ int scale_g = winsize*winsize/8, scale_s = (1024 + scale_g)/(scale_g*2);\r
+ const int OFS = 256*5, TABSZ = OFS*2 + 256;\r
+ uchar tab[TABSZ];\r
+ const uchar* sptr = src->data.ptr;\r
+ int srcstep = src->step;\r
+ CvSize size = cvGetMatSize(src);\r
+\r
+ scale_g *= scale_s;\r
+\r
+ for( x = 0; x < TABSZ; x++ )\r
+ tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero*2 : x - OFS + ftzero);\r
+\r
+ for( x = 0; x < size.width; x++ )\r
+ vsum[x] = (ushort)(sptr[x]*(wsz2 + 2));\r
+\r
+ for( y = 1; y < wsz2; y++ )\r
+ {\r
+ for( x = 0; x < size.width; x++ )\r
+ vsum[x] = (ushort)(vsum[x] + sptr[srcstep*y + x]);\r
+ }\r
+\r
+ for( y = 0; y < size.height; y++ )\r
+ {\r
+ const uchar* top = sptr + srcstep*MAX(y-wsz2-1,0);\r
+ const uchar* bottom = sptr + srcstep*MIN(y+wsz2,size.height-1);\r
+ const uchar* prev = sptr + srcstep*MAX(y-1,0);\r
+ const uchar* curr = sptr + srcstep*y;\r
+ const uchar* next = sptr + srcstep*MIN(y+1,size.height-1);\r
+ uchar* dptr = dst->data.ptr + dst->step*y;\r
+ x = 0;\r
+\r
+ for( ; x < size.width; x++ )\r
+ vsum[x] = (ushort)(vsum[x] + bottom[x] - top[x]);\r
+\r
+ for( x = 0; x <= wsz2; x++ )\r
+ {\r
+ vsum[-x-1] = vsum[0];\r
+ vsum[size.width+x] = vsum[size.width-1];\r
+ }\r
+\r
+ int sum = vsum[0]*(wsz2 + 1);\r
+ for( x = 1; x <= wsz2; x++ )\r
+ sum += vsum[x];\r
+\r
+ int val = ((curr[0]*5 + curr[1] + prev[0] + next[0])*scale_g - sum*scale_s) >> 10;\r
+ dptr[0] = tab[val + OFS];\r
+\r
+ for( x = 1; x < size.width-1; x++ )\r
+ {\r
+ sum += vsum[x+wsz2] - vsum[x-wsz2-1];\r
+ val = ((curr[x]*4 + curr[x-1] + curr[x+1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;\r
+ dptr[x] = tab[val + OFS];\r
+ }\r
+ \r
+ sum += vsum[x+wsz2] - vsum[x-wsz2-1];\r
+ val = ((curr[x]*5 + curr[x-1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;\r
+ dptr[x] = tab[val + OFS];\r
+ }\r
+}\r
+\r
+\r
+static const int DISPARITY_SHIFT = 4;\r
+\r
+#if CV_SSE2\r
+static void\r
+icvFindStereoCorrespondenceBM_SSE2( const CvMat* left, const CvMat* right,\r
+ CvMat* disp, const CvMat* dbmin,\r
+ const CvMat* dbmax, CvStereoBMState* state,\r
+ int realSADWin, uchar* buf, int _dy0, int _dy1 )\r
+{\r
+ int x, y, d;\r
+ int wsz = realSADWin, wsz2 = wsz/2;\r
+ int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);\r
+ int ndisp = state->numberOfDisparities;\r
+ int mindisp = state->minDisparity;\r
+ int lofs = MAX(ndisp - 1 + mindisp, 0);\r
+ int rofs = -MIN(ndisp - 1 + mindisp, 0);\r
+ int width = left->cols, height = left->rows;\r
+ int width1 = width - rofs - ndisp + 1;\r
+ int ftzero = state->preFilterCap;\r
+ int textureThreshold = state->textureThreshold;\r
+ int uniquenessRatio = state->uniquenessRatio;\r
+ short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);\r
+ int DELTA = (state->numberOfDisparities - 1 - state->minDisparity) << DISPARITY_SHIFT;\r
+\r
+ ushort *sad, *hsad0, *hsad, *hsad_sub;\r
+ int *htext;\r
+ uchar *cbuf0, *cbuf;\r
+ const uchar* lptr0 = left->data.ptr + lofs;\r
+ const uchar* rptr0 = right->data.ptr + rofs;\r
+ const uchar *lptr, *lptr_sub, *rptr;\r
+ short* dptr = disp->data.s;\r
+ int sstep = left->step;\r
+ int dstep = disp->step/sizeof(dptr[0]);\r
+ int cstep = (height + dy0 + dy1)*ndisp;\r
+ const int TABSZ = 256;\r
+ uchar tab[TABSZ];\r
+ const __m128i d0_8 = _mm_setr_epi16(0,1,2,3,4,5,6,7), dd_8 = _mm_set1_epi16(8);\r
+\r
+ sad = (ushort*)cvAlignPtr(buf + sizeof(sad[0]));\r
+ hsad0 = (ushort*)cvAlignPtr(sad + ndisp + 1 + dy0*ndisp);\r
+ htext = (int*)cvAlignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2);\r
+ cbuf0 = (uchar*)cvAlignPtr(htext + height + wsz2 + 2 + dy0*ndisp);\r
+\r
+ for( x = 0; x < TABSZ; x++ )\r
+ tab[x] = (uchar)abs(x - ftzero);\r
+\r
+ // initialize buffers\r
+ memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) );\r
+ memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) );\r
+\r
+ for( x = -wsz2-1; x < wsz2; x++ )\r
+ {\r
+ hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp;\r
+ lptr = lptr0 + MIN(MAX(x, -lofs), width-lofs-1) - dy0*sstep;\r
+ rptr = rptr0 + MIN(MAX(x, -rofs), width-rofs-1) - dy0*sstep;\r
+\r
+ for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )\r
+ {\r
+ int lval = lptr[0];\r
+ for( d = 0; d < ndisp; d++ )\r
+ {\r
+ int diff = abs(lval - rptr[d]);\r
+ cbuf[d] = (uchar)diff;\r
+ hsad[d] = (ushort)(hsad[d] + diff);\r
+ }\r
+ htext[y] += tab[lval];\r
+ }\r
+ }\r
+\r
+ // initialize the left and right borders of the disparity map\r
+ for( y = 0; y < height; y++ )\r
+ {\r
+ for( x = 0; x < lofs; x++ )\r
+ dptr[y*dstep + x] = FILTERED;\r
+ for( x = lofs + width1; x < width; x++ )\r
+ dptr[y*dstep + x] = FILTERED;\r
+ }\r
+ dptr += lofs;\r
+\r
+ for( x = 0; x < width1; x++, dptr++ )\r
+ {\r
+ int x0 = x - wsz2 - 1, x1 = x + wsz2;\r
+ const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;\r
+ uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;\r
+ hsad = hsad0 - dy0*ndisp;\r
+ lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;\r
+ lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;\r
+ rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep;\r
+\r
+ for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp,\r
+ hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )\r
+ {\r
+ int lval = lptr[0];\r
+ __m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128();\r
+ for( d = 0; d < ndisp; d += 16 )\r
+ {\r
+ __m128i rv = _mm_loadu_si128((const __m128i*)(rptr + d));\r
+ __m128i hsad_l = _mm_load_si128((__m128i*)(hsad + d));\r
+ __m128i hsad_h = _mm_load_si128((__m128i*)(hsad + d + 8));\r
+ __m128i cbs = _mm_load_si128((const __m128i*)(cbuf_sub + d));\r
+ __m128i diff = _mm_adds_epu8(_mm_subs_epu8(lv, rv), _mm_subs_epu8(rv, lv));\r
+ __m128i diff_h = _mm_sub_epi16(_mm_unpackhi_epi8(diff, z), _mm_unpackhi_epi8(cbs, z));\r
+ _mm_store_si128((__m128i*)(cbuf + d), diff);\r
+ diff = _mm_sub_epi16(_mm_unpacklo_epi8(diff, z), _mm_unpacklo_epi8(cbs, z));\r
+ hsad_h = _mm_add_epi16(hsad_h, diff_h);\r
+ hsad_l = _mm_add_epi16(hsad_l, diff);\r
+ _mm_store_si128((__m128i*)(hsad + d), hsad_l);\r
+ _mm_store_si128((__m128i*)(hsad + d + 8), hsad_h);\r
+ }\r
+ htext[y] += tab[lval] - tab[lptr_sub[0]];\r
+ }\r
+\r
+ // fill borders\r
+ for( y = dy1; y <= wsz2; y++ )\r
+ htext[height+y] = htext[height+dy1-1];\r
+ for( y = -wsz2-1; y < -dy0; y++ )\r
+ htext[y] = htext[-dy0];\r
+\r
+ // initialize sums\r
+ for( d = 0; d < ndisp; d++ )\r
+ sad[d] = (ushort)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));\r
+ \r
+ hsad = hsad0 + (1 - dy0)*ndisp;\r
+ for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )\r
+ for( d = 0; d < ndisp; d++ )\r
+ sad[d] = (ushort)(sad[d] + hsad[d]);\r
+ int tsum = 0;\r
+ for( y = -wsz2-1; y < wsz2; y++ )\r
+ tsum += htext[y];\r
+\r
+ // finally, start the real processing\r
+ for( y = 0; y < height; y++ )\r
+ {\r
+ int minsad = INT_MAX, mind = -1;\r
+ hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;\r
+ hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;\r
+ __m128i minsad8 = _mm_set1_epi16(SHRT_MAX);\r
+ __m128i mind8 = _mm_set1_epi16(-1), d8 = d0_8, mask;\r
+\r
+ for( d = 0; d < ndisp; d += 8 )\r
+ {\r
+ __m128i v0 = _mm_load_si128((__m128i*)(hsad_sub + d));\r
+ __m128i v1 = _mm_load_si128((__m128i*)(hsad + d));\r
+ __m128i sad8 = _mm_load_si128((__m128i*)(sad + d));\r
+ sad8 = _mm_sub_epi16(sad8, v0);\r
+ sad8 = _mm_add_epi16(sad8, v1);\r
+\r
+ mask = _mm_cmpgt_epi16(minsad8, sad8);\r
+ _mm_store_si128((__m128i*)(sad + d), sad8);\r
+ minsad8 = _mm_min_epi16(minsad8, sad8);\r
+ mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(d8,mind8),mask));\r
+ d8 = _mm_add_epi16(d8, dd_8);\r
+ }\r
+\r
+ __m128i minsad82 = _mm_unpackhi_epi64(minsad8, minsad8);\r
+ __m128i mind82 = _mm_unpackhi_epi64(mind8, mind8);\r
+ mask = _mm_cmpgt_epi16(minsad8, minsad82);\r
+ mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));\r
+ minsad8 = _mm_min_epi16(minsad8, minsad82);\r
+\r
+ minsad82 = _mm_shufflelo_epi16(minsad8, _MM_SHUFFLE(3,2,3,2));\r
+ mind82 = _mm_shufflelo_epi16(mind8, _MM_SHUFFLE(3,2,3,2));\r
+ mask = _mm_cmpgt_epi16(minsad8, minsad82);\r
+ mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));\r
+ minsad8 = _mm_min_epi16(minsad8, minsad82);\r
+\r
+ minsad82 = _mm_shufflelo_epi16(minsad8, 1);\r
+ mind82 = _mm_shufflelo_epi16(mind8, 1);\r
+ mask = _mm_cmpgt_epi16(minsad8, minsad82);\r
+ mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask));\r
+ mind = (short)_mm_cvtsi128_si32(mind8);\r
+ minsad = sad[mind];\r
+ tsum += htext[y + wsz2] - htext[y - wsz2 - 1];\r
+ if( tsum < textureThreshold )\r
+ {\r
+ if( !dbmin )\r
+ dptr[y*dstep] = FILTERED;\r
+ continue;\r
+ }\r
+\r
+ if( uniquenessRatio > 0 )\r
+ {\r
+ int thresh = minsad + (minsad * uniquenessRatio/100);\r
+ __m128i thresh8 = _mm_set1_epi16((short)(thresh + 1));\r
+ __m128i d1 = _mm_set1_epi16((short)(mind-1)), d2 = _mm_set1_epi16((short)(mind+1));\r
+ __m128i d8 = d0_8;\r
+\r
+ for( d = 0; d < ndisp; d += 8 )\r
+ {\r
+ __m128i sad8 = _mm_load_si128((__m128i*)(sad + d));\r
+ __m128i mask = _mm_cmpgt_epi16( thresh8, sad8 );\r
+ mask = _mm_and_si128(mask, _mm_or_si128(_mm_cmpgt_epi16(d1,d8), _mm_cmpgt_epi16(d8,d2)));\r
+ if( _mm_movemask_epi8(mask) )\r
+ break;\r
+ d8 = _mm_add_epi16(d8, dd_8);\r
+ }\r
+ if( d < ndisp )\r
+ {\r
+ if( !dbmin )\r
+ dptr[y*dstep] = FILTERED;\r
+ continue;\r
+ }\r
+ }\r
+ \r
+ if( dbmin )\r
+ {\r
+ int maxval = ((const short*)(dbmin->data.ptr + dbmin->step*y))[x];\r
+ int minval = ((const short*)(dbmax->data.ptr + dbmax->step*y))[x];\r
+ minval = (DELTA - minval) >> DISPARITY_SHIFT;\r
+ maxval = (DELTA - maxval + (1<<DISPARITY_SHIFT)-1) >> DISPARITY_SHIFT;\r
+ if( d < minval || d > maxval )\r
+ continue;\r
+ }\r
+\r
+ {\r
+ sad[-1] = sad[1];\r
+ sad[ndisp] = sad[ndisp-2];\r
+ int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind];\r
+ dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);\r
+ }\r
+ }\r
+ }\r
+}\r
+#endif\r
+\r
+static void\r
+icvFindStereoCorrespondenceBM( const CvMat* left, const CvMat* right,\r
+ CvMat* disp, const CvMat* dbmin,\r
+ const CvMat* dbmax, CvStereoBMState* state,\r
+ int realSADWin, uchar* buf, int _dy0, int _dy1 )\r
+{\r
+ int x, y, d;\r
+ int wsz = realSADWin, wsz2 = wsz/2;\r
+ int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);\r
+ int ndisp = state->numberOfDisparities;\r
+ int mindisp = state->minDisparity;\r
+ int lofs = MAX(ndisp - 1 + mindisp, 0);\r
+ int rofs = -MIN(ndisp - 1 + mindisp, 0);\r
+ int width = left->cols, height = left->rows;\r
+ int width1 = width - rofs - ndisp + 1;\r
+ int ftzero = state->preFilterCap;\r
+ int textureThreshold = state->textureThreshold;\r
+ int uniquenessRatio = state->uniquenessRatio;\r
+ short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);\r
+ int DELTA = (state->numberOfDisparities - 1 - state->minDisparity) << DISPARITY_SHIFT;\r
+\r
+ int *sad, *hsad0, *hsad, *hsad_sub, *htext;\r
+ uchar *cbuf0, *cbuf;\r
+ const uchar* lptr0 = left->data.ptr + lofs;\r
+ const uchar* rptr0 = right->data.ptr + rofs;\r
+ const uchar *lptr, *lptr_sub, *rptr;\r
+ short* dptr = disp->data.s;\r
+ int sstep = left->step;\r
+ int dstep = disp->step/sizeof(dptr[0]);\r
+ int cstep = (height+dy0+dy1)*ndisp;\r
+ const int TABSZ = 256;\r
+ uchar tab[TABSZ];\r
+\r
+ sad = (int*)cvAlignPtr(buf + sizeof(sad[0]));\r
+ hsad0 = (int*)cvAlignPtr(sad + ndisp + 1 + dy0*ndisp);\r
+ htext = (int*)cvAlignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2);\r
+ cbuf0 = (uchar*)cvAlignPtr(htext + height + wsz2 + 2 + dy0*ndisp);\r
+\r
+ for( x = 0; x < TABSZ; x++ )\r
+ tab[x] = (uchar)abs(x - ftzero);\r
+\r
+ // initialize buffers\r
+ memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) );\r
+ memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) );\r
+\r
+ for( x = -wsz2-1; x < wsz2; x++ )\r
+ {\r
+ hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp;\r
+ lptr = lptr0 + MIN(MAX(x, -lofs), width-lofs-1) - dy0*sstep;\r
+ rptr = rptr0 + MIN(MAX(x, -rofs), width-rofs-1) - dy0*sstep;\r
+\r
+ for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )\r
+ {\r
+ int lval = lptr[0];\r
+ for( d = 0; d < ndisp; d++ )\r
+ {\r
+ int diff = abs(lval - rptr[d]);\r
+ cbuf[d] = (uchar)diff;\r
+ hsad[d] = (int)(hsad[d] + diff);\r
+ }\r
+ htext[y] += tab[lval];\r
+ }\r
+ }\r
+\r
+ // initialize the left and right borders of the disparity map\r
+ for( y = 0; y < height; y++ )\r
+ {\r
+ for( x = 0; x < lofs; x++ )\r
+ dptr[y*dstep + x] = FILTERED;\r
+ for( x = lofs + width1; x < width; x++ )\r
+ dptr[y*dstep + x] = FILTERED;\r
+ }\r
+ dptr += lofs;\r
+\r
+ for( x = 0; x < width1; x++, dptr++ )\r
+ {\r
+ int x0 = x - wsz2 - 1, x1 = x + wsz2;\r
+ const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;\r
+ uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;\r
+ hsad = hsad0 - dy0*ndisp;\r
+ lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;\r
+ lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;\r
+ rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep;\r
+\r
+ for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp,\r
+ hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )\r
+ {\r
+ int lval = lptr[0];\r
+ for( d = 0; d < ndisp; d++ )\r
+ {\r
+ int diff = abs(lval - rptr[d]);\r
+ cbuf[d] = (uchar)diff;\r
+ hsad[d] = hsad[d] + diff - cbuf_sub[d];\r
+ }\r
+ htext[y] += tab[lval] - tab[lptr_sub[0]];\r
+ }\r
+\r
+ // fill borders\r
+ for( y = dy1; y <= wsz2; y++ )\r
+ htext[height+y] = htext[height+dy1-1];\r
+ for( y = -wsz2-1; y < -dy0; y++ )\r
+ htext[y] = htext[-dy0];\r
+\r
+ // initialize sums\r
+ for( d = 0; d < ndisp; d++ )\r
+ sad[d] = (int)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));\r
+ \r
+ hsad = hsad0 + (1 - dy0)*ndisp;\r
+ for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )\r
+ for( d = 0; d < ndisp; d++ )\r
+ sad[d] = (int)(sad[d] + hsad[d]);\r
+ int tsum = 0;\r
+ for( y = -wsz2-1; y < wsz2; y++ )\r
+ tsum += htext[y];\r
+\r
+ // finally, start the real processing\r
+ for( y = 0; y < height; y++ )\r
+ {\r
+ int minsad = INT_MAX, mind = -1;\r
+ hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;\r
+ hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;\r
+\r
+ for( d = 0; d < ndisp; d++ )\r
+ {\r
+ int currsad = sad[d] + hsad[d] - hsad_sub[d];\r
+ sad[d] = currsad;\r
+ if( currsad < minsad )\r
+ {\r
+ minsad = currsad;\r
+ mind = d;\r
+ }\r
+ }\r
+ tsum += htext[y + wsz2] - htext[y - wsz2 - 1];\r
+ if( tsum < textureThreshold )\r
+ {\r
+ if( !dbmin )\r
+ dptr[y*dstep] = FILTERED;\r
+ continue;\r
+ }\r
+\r
+ if( uniquenessRatio > 0 )\r
+ {\r
+ int thresh = minsad + (minsad * uniquenessRatio/100);\r
+ for( d = 0; d < ndisp; d++ )\r
+ {\r
+ if( sad[d] <= thresh && (d < mind-1 || d > mind+1))\r
+ break;\r
+ }\r
+ if( d < ndisp )\r
+ {\r
+ if( !dbmin )\r
+ dptr[y*dstep] = FILTERED;\r
+ continue;\r
+ }\r
+ }\r
+\r
+ if( dbmin )\r
+ {\r
+ int maxval = ((const short*)(dbmin->data.ptr + dbmin->step*y))[x];\r
+ int minval = ((const short*)(dbmax->data.ptr + dbmax->step*y))[x];\r
+ minval = (DELTA - minval) >> DISPARITY_SHIFT;\r
+ maxval = (DELTA - maxval + (1<<DISPARITY_SHIFT)-1) >> DISPARITY_SHIFT;\r
+ if( d < minval || d > maxval )\r
+ continue;\r
+ }\r
+ \r
+ {\r
+ sad[-1] = sad[1];\r
+ sad[ndisp] = sad[ndisp-2];\r
+ int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind];\r
+ dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);\r
+ }\r
+ }\r
+ }\r
+}\r
+\r
+CV_IMPL void\r
+cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr,\r
+ CvArr* disparr, CvStereoBMState* state )\r
+{\r
+ CV_FUNCNAME( "cvFindStereoCorrespondenceBM" );\r
+\r
+ __BEGIN__;\r
+\r
+ CvMat lstub, *left0 = cvGetMat( leftarr, &lstub );\r
+ CvMat rstub, *right0 = cvGetMat( rightarr, &rstub );\r
+ CvMat left, right;\r
+ CvMat dstub, *disp = cvGetMat( disparr, &dstub );\r
+ int bufSize0, bufSize1, bufSize, width, width1, height;\r
+ int wsz, ndisp, mindisp, lofs, rofs;\r
+ int i, n = cvGetNumThreads();\r
+ int FILTERED, SMALL_WIN_SIZE;\r
+\r
+ if( !CV_ARE_SIZES_EQ(left0, right0) ||\r
+ !CV_ARE_SIZES_EQ(disp, left0) )\r
+ CV_ERROR( CV_StsUnmatchedSizes, "All the images must have the same size" );\r
+\r
+ if( CV_MAT_TYPE(left0->type) != CV_8UC1 ||\r
+ !CV_ARE_TYPES_EQ(left0, right0) ||\r
+ CV_MAT_TYPE(disp->type) != CV_16SC1 )\r
+ CV_ERROR( CV_StsUnsupportedFormat,\r
+ "Both input images must have 8uC1 format and the disparity image must have 16sC1 format" );\r
+\r
+ if( !state )\r
+ CV_ERROR( CV_StsNullPtr, "Stereo BM state is NULL." );\r
+\r
+ if( state->preFilterType != CV_STEREO_BM_NORMALIZED_RESPONSE )\r
+ CV_ERROR( CV_StsOutOfRange, "preFilterType must be =CV_STEREO_BM_NORMALIZED_RESPONSE" );\r
+\r
+ if( state->preFilterSize < 5 || state->preFilterSize > 255 || state->preFilterSize % 2 == 0 )\r
+ CV_ERROR( CV_StsOutOfRange, "preFilterSize must be odd and be within 5..255" );\r
+\r
+ if( state->preFilterCap < 1 || state->preFilterCap > 63 )\r
+ CV_ERROR( CV_StsOutOfRange, "preFilterCap must be within 1..63" );\r
+\r
+ if( state->SADWindowSize < 5 || state->SADWindowSize > 255 || state->SADWindowSize % 2 == 0 ||\r
+ state->SADWindowSize >= MIN(left0->cols, left0->rows) )\r
+ CV_ERROR( CV_StsOutOfRange, "SADWindowSize must be odd, be within 5..255 and "\r
+ "be not larger than image width or height" );\r
+\r
+ if( state->numberOfDisparities <= 0 || state->numberOfDisparities % 16 != 0 )\r
+ CV_ERROR( CV_StsOutOfRange, "numberOfDisparities must be positive and divisble by 16" );\r
+ if( state->textureThreshold < 0 )\r
+ CV_ERROR( CV_StsOutOfRange, "texture threshold must be non-negative" );\r
+ if( state->uniquenessRatio < 0 )\r
+ CV_ERROR( CV_StsOutOfRange, "uniqueness ratio must be non-negative" );\r
+\r
+ if( !state->preFilteredImg0 ||\r
+ state->preFilteredImg0->cols*state->preFilteredImg0->rows < left0->cols*left0->rows )\r
+ {\r
+ cvReleaseMat( &state->preFilteredImg0 );\r
+ cvReleaseMat( &state->preFilteredImg1 );\r
+\r
+ state->preFilteredImg0 = cvCreateMat( left0->rows, left0->cols, CV_8U );\r
+ state->preFilteredImg1 = cvCreateMat( left0->rows, left0->cols, CV_8U );\r
+ }\r
+ left = cvMat(left0->rows, left0->cols, CV_8U, state->preFilteredImg0->data.ptr);\r
+ right = cvMat(right0->rows, right0->cols, CV_8U, state->preFilteredImg1->data.ptr);\r
+ \r
+ mindisp = state->minDisparity;\r
+ ndisp = state->numberOfDisparities;\r
+\r
+ width = left0->cols;\r
+ height = left0->rows;\r
+ lofs = MAX(ndisp - 1 + mindisp, 0);\r
+ rofs = -MIN(ndisp - 1 + mindisp, 0);\r
+ width1 = width - rofs - ndisp + 1;\r
+ FILTERED = (short)((state->minDisparity - 1) << DISPARITY_SHIFT);\r
+\r
+ if( lofs >= width || rofs >= width || width1 < 1 )\r
+ {\r
+ cvSet( disp, cvScalarAll(FILTERED) );\r
+ EXIT;\r
+ }\r
+\r
+ wsz = state->SADWindowSize;\r
+ bufSize0 = (ndisp + 2)*sizeof(int) + (height+wsz+2)*ndisp*sizeof(int) +\r
+ (height + wsz + 2)*sizeof(int) + (height+wsz+2)*ndisp*(wsz+1)*sizeof(uchar) + 256;\r
+ bufSize1 = (width + state->preFilterSize + 2)*sizeof(int) + 256;\r
+ bufSize = MAX(bufSize0, bufSize1);\r
+ n = MAX(MIN(height/wsz, n), 1);\r
+\r
+ if( !state->slidingSumBuf || state->slidingSumBuf->cols < bufSize*n )\r
+ {\r
+ cvReleaseMat( &state->slidingSumBuf );\r
+ state->slidingSumBuf = cvCreateMat( 1, bufSize*n, CV_8U );\r
+ }\r
+\r
+#ifdef _OPENMP\r
+#pragma omp parallel sections num_threads(n)\r
+#endif\r
+ {\r
+ #ifdef _OPENMP\r
+ #pragma omp section\r
+ #endif\r
+ icvPrefilter( left0, &left, state->preFilterSize,\r
+ state->preFilterCap, state->slidingSumBuf->data.ptr );\r
+ #ifdef _OPENMP\r
+ #pragma omp section\r
+ #endif\r
+ icvPrefilter( right0, &right, state->preFilterSize,\r
+ state->preFilterCap, state->slidingSumBuf->data.ptr + bufSize1*(n>1) );\r
+ }\r
+\r
+#ifdef _OPENMP\r
+ #pragma omp parallel for num_threads(n) schedule(static)\r
+#endif\r
+ for( i = 0; i < n; i++ )\r
+ {\r
+ int thread_id = cvGetThreadNum();\r
+ CvMat left_i, right_i, disp_i;\r
+ int row0 = i*left.rows/n, row1 = (i+1)*left.rows/n;\r
+ cvGetRows( &left, &left_i, row0, row1 );\r
+ cvGetRows( &right, &right_i, row0, row1 );\r
+ cvGetRows( disp, &disp_i, row0, row1 );\r
+ #if CV_SSE2\r
+ if( state->preFilterCap <= 31 && state->SADWindowSize <= 21 )\r
+ {\r
+ icvFindStereoCorrespondenceBM_SSE2( &left_i, &right_i, &disp_i, 0, 0, state,\r
+ state->SADWindowSize, state->slidingSumBuf->data.ptr + thread_id*bufSize0,\r
+ row0, left.rows-row1 );\r
+ }\r
+ else\r
+ #endif\r
+ {\r
+ icvFindStereoCorrespondenceBM( &left_i, &right_i, &disp_i, 0, 0, state,\r
+ state->SADWindowSize, state->slidingSumBuf->data.ptr + thread_id*bufSize0,\r
+ row0, left.rows-row1 );\r
+ }\r
+ }\r
+\r
+ SMALL_WIN_SIZE = MAX((state->SADWindowSize/2)|1, 9);\r
+\r
+ if( !state->trySmallerWindows || SMALL_WIN_SIZE >= state->SADWindowSize )\r
+ EXIT;\r
+\r
+ if( !state->dbmin || !CV_ARE_SIZES_EQ(state->dbmin, disp) )\r
+ {\r
+ cvReleaseMat( &state->dbmin );\r
+ cvReleaseMat( &state->dbmax );\r
+ state->dbmin = cvCreateMat( disp->rows, disp->cols, CV_16SC1 );\r
+ state->dbmax = cvCreateMat( disp->rows, disp->cols, CV_16SC1 );\r
+ }\r
+ \r
+ for( i = 0; i < disp->rows; i++ )\r
+ {\r
+ int j;\r
+ short* minptr = (short*)(state->dbmin->data.ptr + state->dbmin->step*i);\r
+ short* maxptr = (short*)(state->dbmax->data.ptr + state->dbmax->step*i);\r
+\r
+ for( j = 0; j < disp->cols; j++ )\r
+ {\r
+ short dval = ((const short*)(disp->data.ptr + disp->step*i))[j];\r
+ if( dval < (state->minDisparity << DISPARITY_SHIFT) )\r
+ minptr[j] = maxptr[j] = dval;\r
+ else\r
+ {\r
+ minptr[j] = SHRT_MAX;\r
+ maxptr[j] = SHRT_MIN;\r
+ }\r
+ }\r
+ }\r
+\r
+ cvErode(state->dbmin, state->dbmin, 0, SMALL_WIN_SIZE );\r
+ cvDilate(state->dbmax, state->dbmax, 0, SMALL_WIN_SIZE );\r
+\r
+#ifdef _OPENMP\r
+ #pragma omp parallel for num_threads(n) schedule(static)\r
+#endif\r
+ for( i = 0; i < n; i++ )\r
+ {\r
+ int thread_id = cvGetThreadNum();\r
+ CvMat left_i, right_i, disp_i, dbmin_i, dbmax_i;\r
+ int row0 = i*left.rows/n, row1 = (i+1)*left.rows/n;\r
+ cvGetRows( &left, &left_i, row0, row1 );\r
+ cvGetRows( &right, &right_i, row0, row1 );\r
+ cvGetRows( disp, &disp_i, row0, row1 );\r
+ cvGetRows( state->dbmin, &dbmin_i, row0, row1 );\r
+ cvGetRows( state->dbmax, &dbmax_i, row0, row1 );\r
+ #if CV_SSE2\r
+ if( state->preFilterCap <= 31 && SMALL_WIN_SIZE <= 21 )\r
+ {\r
+ icvFindStereoCorrespondenceBM_SSE2( &left_i, &right_i,\r
+ &disp_i, &dbmin_i, &dbmax_i, state, SMALL_WIN_SIZE,\r
+ state->slidingSumBuf->data.ptr + thread_id*bufSize0,\r
+ row0, left.rows-row1 );\r
+ }\r
+ else\r
+ #endif\r
+ {\r
+ icvFindStereoCorrespondenceBM( &left_i, &right_i,\r
+ &disp_i, &dbmin_i, &dbmax_i, state, SMALL_WIN_SIZE,\r
+ state->slidingSumBuf->data.ptr + thread_id*bufSize0,\r
+ row0, left.rows-row1 );\r
+ }\r
+ }\r
+\r
+ __END__;\r
+}\r
+\r
+namespace cv\r
+{\r
+\r
+StereoBM::StereoBM()\r
+{ state = cvCreateStereoBMState(); }\r
+\r
+StereoBM::StereoBM(int _preset, int _ndisparities, int _SADWindowSize)\r
+{ init(_preset, _ndisparities, _SADWindowSize); }\r
+\r
+void StereoBM::init(int _preset, int _ndisparities, int _SADWindowSize)\r
+{\r
+ state = cvCreateStereoBMState(_preset, _ndisparities);\r
+ state->SADWindowSize = _SADWindowSize;\r
+}\r
+\r
+void StereoBM::operator()( const Mat& left, const Mat& right, Mat& disparity )\r
+{\r
+ disparity.create(left.size(), CV_16SC1);\r
+ CvMat _left = left, _right = right, _disparity = disparity;\r
+ cvFindStereoCorrespondenceBM(&_left, &_right, &_disparity, state);\r
+}\r
+\r
+}\r
+\r
+/* End of file. */\r
projects / opencv / blobdiff