16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Very fast SAD-based (Sum-of-Absolute-Diffrences) stereo correspondence algorithm. * 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Contributed by Kurt Konolige * 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cv.h" 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#undef CV_SSE2 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define CV_SSE2 1 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "emmintrin.h" 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn*/ 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL CvStereoBMState* 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCreateStereoBMState( int /*preset*/, int numberOfDisparities ) 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvStereoBMState* state = 0; 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //CV_FUNCNAME( "cvCreateStereoBMState" ); 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = (CvStereoBMState*)cvAlloc( sizeof(*state) ); 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !state ) 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->preFilterType = CV_STEREO_BM_NORMALIZED_RESPONSE; 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->preFilterSize = 9; 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->preFilterCap = 31; 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->SADWindowSize = 15; 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->minDisparity = 0; 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : 64; 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->textureThreshold = 10; 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->uniquenessRatio = 15; 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->speckleRange = state->speckleWindowSize = 0; 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->preFilteredImg0 = state->preFilteredImg1 = state->slidingSumBuf = 0; 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvGetErrStatus() < 0 ) 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseStereoBMState( &state ); 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return state; 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvReleaseStereoBMState( CvStereoBMState** state ) 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvReleaseStereoBMState" ); 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !state ) 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !*state ) 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &(*state)->preFilteredImg0 ); 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &(*state)->preFilteredImg1 ); 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &(*state)->slidingSumBuf ); 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( state ); 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void icvPrefilter( const CvMat* src, CvMat* dst, int winsize, int ftzero, uchar* buf ) 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, y, wsz2 = winsize/2; 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* vsum = (int*)cvAlignPtr(buf + (wsz2 + 1)*sizeof(vsum[0]), 32); 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int scale_g = winsize*winsize/8, scale_s = (1024 + scale_g)/(scale_g*2); 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int OFS = 256*5, TABSZ = OFS*2 + 256; 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar tab[TABSZ]; 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* sptr = src->data.ptr; 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int srcstep = src->step; 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size = cvGetMatSize(src); 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale_g *= scale_s; 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < TABSZ; x++ ) 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero*2 : x - OFS + ftzero); 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < size.width; x++ ) 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vsum[x] = (ushort)(sptr[x]*(wsz2 + 2)); 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 1; y < wsz2; y++ ) 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < size.width; x++ ) 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vsum[x] = (ushort)(vsum[x] + sptr[srcstep*y + x]); 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < size.height; y++ ) 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* top = sptr + srcstep*MAX(y-wsz2-1,0); 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* bottom = sptr + srcstep*MIN(y+wsz2,size.height-1); 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* prev = sptr + srcstep*MAX(y-1,0); 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* curr = sptr + srcstep*y; 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* next = sptr + srcstep*MIN(y+1,size.height-1); 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* dptr = dst->data.ptr + dst->step*y; 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = 0; 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; x < size.width; x++ ) 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vsum[x] = (ushort)(vsum[x] + bottom[x] - top[x]); 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x <= wsz2; x++ ) 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vsum[-x-1] = vsum[0]; 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vsum[size.width+x] = vsum[size.width-1]; 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int sum = vsum[0]*(wsz2 + 1); 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 1; x <= wsz2; x++ ) 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += vsum[x]; 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int val = ((curr[0]*5 + curr[1] + prev[0] + next[0])*scale_g - sum*scale_s) >> 10; 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[0] = tab[val + OFS]; 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 1; x < size.width-1; x++ ) 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += vsum[x+wsz2] - vsum[x-wsz2-1]; 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn val = ((curr[x]*4 + curr[x-1] + curr[x+1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[x] = tab[val + OFS]; 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += vsum[x+wsz2] - vsum[x-wsz2-1]; 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn val = ((curr[x]*5 + curr[x-1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10; 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[x] = tab[val + OFS]; 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic const int DISPARITY_SHIFT = 4; 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if CV_SSE2 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvFindStereoCorrespondenceBM_SSE2( const CvMat* left, const CvMat* right, 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* disp, CvStereoBMState* state, 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* buf, int _dy0, int _dy1 ) 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, y, d; 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int wsz = state->SADWindowSize, wsz2 = wsz/2; 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1); 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ndisp = state->numberOfDisparities; 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int mindisp = state->minDisparity; 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int lofs = MAX(ndisp - 1 + mindisp, 0); 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int rofs = -MIN(ndisp - 1 + mindisp, 0); 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int width = left->cols, height = left->rows; 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int width1 = width - rofs - ndisp + 1; 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ftzero = state->preFilterCap; 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int textureThreshold = state->textureThreshold; 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int uniquenessRatio = state->uniquenessRatio; 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT); 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ushort *sad, *hsad0, *hsad, *hsad_sub; 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int *htext; 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *cbuf0, *cbuf; 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* lptr0 = left->data.ptr + lofs; 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* rptr0 = right->data.ptr + rofs; 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar *lptr, *lptr_sub, *rptr; 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn short* dptr = disp->data.s; 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int sstep = left->step; 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dstep = disp->step/sizeof(dptr[0]); 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int cstep = (height + dy0 + dy1)*ndisp; 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int TABSZ = 256; 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar tab[TABSZ]; 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const __m128i d0_8 = _mm_setr_epi16(0,1,2,3,4,5,6,7), dd_8 = _mm_set1_epi16(8); 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad = (ushort*)cvAlignPtr(buf + sizeof(sad[0])); 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad0 = (ushort*)cvAlignPtr(sad + ndisp + 1 + dy0*ndisp); 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn htext = (int*)cvAlignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2); 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cbuf0 = (uchar*)cvAlignPtr(htext + height + wsz2 + 2 + dy0*ndisp); 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < TABSZ; x++ ) 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tab[x] = (uchar)abs(x - ftzero); 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // initialize buffers 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) ); 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) ); 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = -wsz2-1; x < wsz2; x++ ) 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp; 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lptr = lptr0 + MIN(MAX(x, -lofs), width-lofs-1) - dy0*sstep; 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rptr = rptr0 + MIN(MAX(x, -rofs), width-rofs-1) - dy0*sstep; 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep ) 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int lval = lptr[0]; 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d++ ) 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int diff = abs(lval - rptr[d]); 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cbuf[d] = (uchar)diff; 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad[d] = (ushort)(hsad[d] + diff); 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn htext[y] += tab[lval]; 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // initialize the left and right borders of the disparity map 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < height; y++ ) 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < lofs; x++ ) 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[y*dstep + x] = FILTERED; 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = lofs + width1; x < width; x++ ) 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[y*dstep + x] = FILTERED; 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr += lofs; 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < width1; x++, dptr++ ) 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x0 = x - wsz2 - 1, x1 = x + wsz2; 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp; 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp; 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad = hsad0 - dy0*ndisp; 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep; 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep; 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep; 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp, 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep ) 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int lval = lptr[0]; 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128(); 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d += 16 ) 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i rv = _mm_loadu_si128((const __m128i*)(rptr + d)); 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i hsad_l = _mm_load_si128((__m128i*)(hsad + d)); 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i hsad_h = _mm_load_si128((__m128i*)(hsad + d + 8)); 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i cbs = _mm_load_si128((const __m128i*)(cbuf_sub + d)); 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i diff = _mm_adds_epu8(_mm_subs_epu8(lv, rv), _mm_subs_epu8(rv, lv)); 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i diff_h = _mm_sub_epi16(_mm_unpackhi_epi8(diff, z), _mm_unpackhi_epi8(cbs, z)); 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _mm_store_si128((__m128i*)(cbuf + d), diff); 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn diff = _mm_sub_epi16(_mm_unpacklo_epi8(diff, z), _mm_unpacklo_epi8(cbs, z)); 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad_h = _mm_add_epi16(hsad_h, diff_h); 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad_l = _mm_add_epi16(hsad_l, diff); 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _mm_store_si128((__m128i*)(hsad + d), hsad_l); 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _mm_store_si128((__m128i*)(hsad + d + 8), hsad_h); 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn htext[y] += tab[lval] - tab[lptr_sub[0]]; 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // fill borders 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = dy1; y <= wsz2; y++ ) 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn htext[height+y] = htext[height+dy1-1]; 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = -wsz2-1; y < -dy0; y++ ) 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn htext[y] = htext[-dy0]; 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // initialize sums 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d++ ) 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad[d] = (ushort)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0)); 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad = hsad0 + (1 - dy0)*ndisp; 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp ) 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d++ ) 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad[d] = (ushort)(sad[d] + hsad[d]); 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int tsum = 0; 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = -wsz2-1; y < wsz2; y++ ) 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tsum += htext[y]; 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // finally, start the real processing 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < height; y++ ) 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int minsad = INT_MAX, mind = -1; 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp; 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp; 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i minsad8 = _mm_set1_epi16(SHRT_MAX); 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i mind8 = _mm_set1_epi16(-1), d8 = d0_8, mask; 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d += 8 ) 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i v0 = _mm_load_si128((__m128i*)(hsad_sub + d)); 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i v1 = _mm_load_si128((__m128i*)(hsad + d)); 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i sad8 = _mm_load_si128((__m128i*)(sad + d)); 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad8 = _mm_sub_epi16(sad8, v0); 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad8 = _mm_add_epi16(sad8, v1); 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask = _mm_cmpgt_epi16(minsad8, sad8); 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _mm_store_si128((__m128i*)(sad + d), sad8); 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minsad8 = _mm_min_epi16(minsad8, sad8); 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(d8,mind8),mask)); 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d8 = _mm_add_epi16(d8, dd_8); 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i minsad82 = _mm_unpackhi_epi64(minsad8, minsad8); 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i mind82 = _mm_unpackhi_epi64(mind8, mind8); 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask = _mm_cmpgt_epi16(minsad8, minsad82); 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask)); 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minsad8 = _mm_min_epi16(minsad8, minsad82); 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minsad82 = _mm_shufflelo_epi16(minsad8, _MM_SHUFFLE(3,2,3,2)); 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mind82 = _mm_shufflelo_epi16(mind8, _MM_SHUFFLE(3,2,3,2)); 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask = _mm_cmpgt_epi16(minsad8, minsad82); 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask)); 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minsad8 = _mm_min_epi16(minsad8, minsad82); 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minsad82 = _mm_shufflelo_epi16(minsad8, 1); 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mind82 = _mm_shufflelo_epi16(mind8, 1); 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask = _mm_cmpgt_epi16(minsad8, minsad82); 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mind8 = _mm_xor_si128(mind8,_mm_and_si128(_mm_xor_si128(mind82,mind8),mask)); 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mind = (short)_mm_cvtsi128_si32(mind8); 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minsad = sad[mind]; 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tsum += htext[y + wsz2] - htext[y - wsz2 - 1]; 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tsum < textureThreshold ) 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[y*dstep] = FILTERED; 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( uniquenessRatio > 0 ) 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int thresh = minsad + (minsad * uniquenessRatio/100); 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i thresh8 = _mm_set1_epi16((short)(thresh + 1)); 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i d1 = _mm_set1_epi16((short)(mind-1)), d2 = _mm_set1_epi16((short)(mind+1)); 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i d8 = d0_8; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d += 8 ) 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i sad8 = _mm_load_si128((__m128i*)(sad + d)); 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __m128i mask = _mm_cmpgt_epi16( thresh8, sad8 ); 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask = _mm_and_si128(mask, _mm_or_si128(_mm_cmpgt_epi16(d1,d8), _mm_cmpgt_epi16(d8,d2))); 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _mm_movemask_epi8(mask) ) 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d8 = _mm_add_epi16(d8, dd_8); 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d < ndisp ) 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[y*dstep] = FILTERED; 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad[-1] = sad[1]; 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad[ndisp] = sad[ndisp-2]; 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind]; 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4); 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvFindStereoCorrespondenceBM( const CvMat* left, const CvMat* right, 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* disp, CvStereoBMState* state, 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* buf, int _dy0, int _dy1 ) 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, y, d; 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int wsz = state->SADWindowSize, wsz2 = wsz/2; 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1); 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ndisp = state->numberOfDisparities; 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int mindisp = state->minDisparity; 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int lofs = MAX(ndisp - 1 + mindisp, 0); 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int rofs = -MIN(ndisp - 1 + mindisp, 0); 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int width = left->cols, height = left->rows; 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int width1 = width - rofs - ndisp + 1; 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ftzero = state->preFilterCap; 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int textureThreshold = state->textureThreshold; 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int uniquenessRatio = state->uniquenessRatio; 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT); 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int *sad, *hsad0, *hsad, *hsad_sub, *htext; 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *cbuf0, *cbuf; 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* lptr0 = left->data.ptr + lofs; 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* rptr0 = right->data.ptr + rofs; 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar *lptr, *lptr_sub, *rptr; 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn short* dptr = disp->data.s; 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int sstep = left->step; 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dstep = disp->step/sizeof(dptr[0]); 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int cstep = (height+dy0+dy1)*ndisp; 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int TABSZ = 256; 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar tab[TABSZ]; 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad = (int*)cvAlignPtr(buf + sizeof(sad[0])); 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad0 = (int*)cvAlignPtr(sad + ndisp + 1 + dy0*ndisp); 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn htext = (int*)cvAlignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2); 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cbuf0 = (uchar*)cvAlignPtr(htext + height + wsz2 + 2 + dy0*ndisp); 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < TABSZ; x++ ) 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tab[x] = (uchar)abs(x - ftzero); 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // initialize buffers 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) ); 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) ); 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = -wsz2-1; x < wsz2; x++ ) 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp; 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lptr = lptr0 + MIN(MAX(x, -lofs), width-lofs-1) - dy0*sstep; 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rptr = rptr0 + MIN(MAX(x, -rofs), width-rofs-1) - dy0*sstep; 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep ) 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int lval = lptr[0]; 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d++ ) 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int diff = abs(lval - rptr[d]); 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cbuf[d] = (uchar)diff; 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad[d] = (int)(hsad[d] + diff); 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn htext[y] += tab[lval]; 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // initialize the left and right borders of the disparity map 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < height; y++ ) 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < lofs; x++ ) 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[y*dstep + x] = FILTERED; 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = lofs + width1; x < width; x++ ) 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[y*dstep + x] = FILTERED; 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr += lofs; 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < width1; x++, dptr++ ) 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x0 = x - wsz2 - 1, x1 = x + wsz2; 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp; 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp; 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad = hsad0 - dy0*ndisp; 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep; 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep; 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep; 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp, 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep ) 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int lval = lptr[0]; 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d++ ) 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int diff = abs(lval - rptr[d]); 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cbuf[d] = (uchar)diff; 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad[d] = hsad[d] + diff - cbuf_sub[d]; 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn htext[y] += tab[lval] - tab[lptr_sub[0]]; 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // fill borders 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = dy1; y <= wsz2; y++ ) 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn htext[height+y] = htext[height+dy1-1]; 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = -wsz2-1; y < -dy0; y++ ) 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn htext[y] = htext[-dy0]; 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // initialize sums 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d++ ) 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad[d] = (int)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0)); 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad = hsad0 + (1 - dy0)*ndisp; 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp ) 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d++ ) 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad[d] = (int)(sad[d] + hsad[d]); 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int tsum = 0; 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = -wsz2-1; y < wsz2; y++ ) 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tsum += htext[y]; 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // finally, start the real processing 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < height; y++ ) 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int minsad = INT_MAX, mind = -1; 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp; 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp; 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d++ ) 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currsad = sad[d] + hsad[d] - hsad_sub[d]; 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad[d] = currsad; 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( currsad < minsad ) 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minsad = currsad; 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mind = d; 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tsum += htext[y + wsz2] - htext[y - wsz2 - 1]; 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tsum < textureThreshold ) 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[y*dstep] = FILTERED; 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( uniquenessRatio > 0 ) 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int thresh = minsad + (minsad * uniquenessRatio/100); 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( d = 0; d < ndisp; d++ ) 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( sad[d] <= thresh && (d < mind-1 || d > mind+1)) 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d < ndisp ) 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[y*dstep] = FILTERED; 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad[-1] = sad[1]; 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sad[ndisp] = sad[ndisp-2]; 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int p = sad[mind+1], n = sad[mind-1], d = p + n - 2*sad[mind]; 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4); 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr, 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvArr* disparr, CvStereoBMState* state ) 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvFindStereoCorrespondenceBM" ); 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat lstub, *left0 = cvGetMat( leftarr, &lstub ); 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat rstub, *right0 = cvGetMat( rightarr, &rstub ); 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat left, right; 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dstub, *disp = cvGetMat( disparr, &dstub ); 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int bufSize0, bufSize1, bufSize, width, width1, height; 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int wsz, ndisp, mindisp, lofs, rofs; 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, n = cvGetNumThreads(); 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ(left0, right0) || 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_SIZES_EQ(disp, left0) ) 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "All the images must have the same size" ); 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(left0->type) != CV_8UC1 || 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_TYPES_EQ(left0, right0) || 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(disp->type) != CV_16SC1 ) 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Both input images must have 8uC1 format and the disparity image must have 16sC1 format" ); 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !state ) 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Stereo BM state is NULL." ); 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state->preFilterType != CV_STEREO_BM_NORMALIZED_RESPONSE ) 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "preFilterType must be =CV_STEREO_BM_NORMALIZED_RESPONSE" ); 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state->preFilterSize < 5 || state->preFilterSize > 255 || state->preFilterSize % 2 == 0 ) 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "preFilterSize must be odd and be within 5..255" ); 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state->preFilterCap < 1 || state->preFilterCap > 63 ) 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "preFilterCap must be within 1..63" ); 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state->SADWindowSize < 5 || state->SADWindowSize > 255 || state->SADWindowSize % 2 == 0 || 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->SADWindowSize >= MIN(left0->cols, left0->rows) ) 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "SADWindowSize must be odd, be within 5..255 and " 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "be not larger than image width or height" ); 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state->numberOfDisparities <= 0 || state->numberOfDisparities % 16 != 0 ) 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "numberOfDisparities must be positive and divisble by 16" ); 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state->textureThreshold < 0 ) 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "texture threshold must be non-negative" ); 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state->uniquenessRatio < 0 ) 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "uniqueness ratio must be non-negative" ); 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !state->preFilteredImg0 || 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->preFilteredImg0->cols*state->preFilteredImg0->rows < left0->cols*left0->rows ) 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->preFilteredImg0 ); 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->preFilteredImg1 ); 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->preFilteredImg0 = cvCreateMat( left0->rows, left0->cols, CV_8U ); 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->preFilteredImg1 = cvCreateMat( left0->rows, left0->cols, CV_8U ); 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn left = cvMat(left0->rows, left0->cols, CV_8U, state->preFilteredImg0->data.ptr); 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn right = cvMat(right0->rows, right0->cols, CV_8U, state->preFilteredImg1->data.ptr); 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mindisp = state->minDisparity; 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ndisp = state->numberOfDisparities; 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn width = left0->cols; 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn height = left0->rows; 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lofs = MAX(ndisp - 1 + mindisp, 0); 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rofs = -MIN(ndisp - 1 + mindisp, 0); 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn width1 = width - rofs - ndisp + 1; 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( lofs >= width || rofs >= width || width1 < 1 ) 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int FILTERED = (short)((state->minDisparity - 1) << DISPARITY_SHIFT); 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSet( disp, cvScalarAll(FILTERED) ); 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn wsz = state->SADWindowSize; 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bufSize0 = (ndisp + 2)*sizeof(int) + (height+wsz+2)*ndisp*sizeof(int) + 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (height + wsz + 2)*sizeof(int) + (height+wsz+2)*ndisp*(wsz+1)*sizeof(uchar) + 256; 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bufSize1 = (width + state->preFilterSize + 2)*sizeof(int) + 256; 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bufSize = MAX(bufSize0, bufSize1); 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n = MAX(MIN(height/wsz, n), 1); 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !state->slidingSumBuf || state->slidingSumBuf->cols < bufSize*n ) 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &state->slidingSumBuf ); 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->slidingSumBuf = cvCreateMat( 1, bufSize*n, CV_8U ); 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef _OPENMP 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#pragma omp parallel sections num_threads(n) 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #ifdef _OPENMP 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #pragma omp section 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #endif 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvPrefilter( left0, &left, state->preFilterSize, 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->preFilterCap, state->slidingSumBuf->data.ptr ); 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #ifdef _OPENMP 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #pragma omp section 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #endif 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvPrefilter( right0, &right, state->preFilterSize, 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->preFilterCap, state->slidingSumBuf->data.ptr + bufSize1*(n>1) ); 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef _OPENMP 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #pragma omp parallel for num_threads(n) schedule(static) 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < n; i++ ) 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int thread_id = cvGetThreadNum(); 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat left_i, right_i, disp_i; 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int row0 = i*left.rows/n, row1 = (i+1)*left.rows/n; 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( &left, &left_i, row0, row1 ); 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( &right, &right_i, row0, row1 ); 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( disp, &disp_i, row0, row1 ); 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #if CV_SSE2 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state->preFilterCap <= 31 && state->SADWindowSize <= 21 ) 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFindStereoCorrespondenceBM_SSE2( &left_i, &right_i, &disp_i, state, 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->slidingSumBuf->data.ptr + thread_id*bufSize0, row0, left.rows-row1 ); 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #endif 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFindStereoCorrespondenceBM( &left_i, &right_i, &disp_i, state, 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state->slidingSumBuf->data.ptr + thread_id*bufSize0, row0, left.rows-row1 ); 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 682