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