← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/modules/calib3d/src/stereobm.cpp

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/modules/calib3d/src/stereobm.cpp
 
1
//M*//////////////////////////////////////////////////////////////////////////////////////
 
2
//
 
3
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
 
4
//
 
5
//  By downloading, copying, installing or using the software you agree to this license.
 
6
//  If you do not agree to this license, do not download, install,
 
7
//  copy or use the software.
 
8
//
 
9
//
 
10
//                          License Agreement
 
11
//                For Open Source Computer Vision Library
 
12
//
 
13
// Copyright (C) 2000, Intel Corporation, all rights reserved.
 
14
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
 
15
// Third party copyrights are property of their respective owners.
 
16
//
 
17
// Redistribution and use in source and binary forms, with or without modification,
 
18
// are permitted provided that the following conditions are met:
 
19
//
 
20
//   * Redistribution's of source code must retain the above copyright notice,
 
21
//     this list of conditions and the following disclaimer.
 
22
//
 
23
//   * Redistribution's in binary form must reproduce the above copyright notice,
 
24
//     this list of conditions and the following disclaimer in the documentation
 
25
//     and/or other materials provided with the distribution.
 
26
//
 
27
//   * The name of the copyright holders may not be used to endorse or promote products
 
28
//     derived from this software without specific prior written permission.
 
29
//
 
30
// This software is provided by the copyright holders and contributors "as is" and
 
31
// any express or implied warranties, including, but not limited to, the implied
 
32
// warranties of merchantability and fitness for a particular purpose are disclaimed.
 
33
// In no event shall the Intel Corporation or contributors be liable for any direct,
 
34
// indirect, incidental, special, exemplary, or consequential damages
 
35
// (including, but not limited to, procurement of substitute goods or services;
 
36
// loss of use, data, or profits; or business interruption) however caused
 
37
// and on any theory of liability, whether in contract, strict liability,
 
38
// or tort (including negligence or otherwise) arising in any way out of
 
39
// the use of this software, even if advised of the possibility of such damage.
 
40
//
 
41
//M*/
 
42
 
 
43
/****************************************************************************************\
 
44
*    Very fast SAD-based (Sum-of-Absolute-Diffrences) stereo correspondence algorithm.   *
 
45
*    Contributed by Kurt Konolige                                                        *
 
46
\****************************************************************************************/
 
47
 
 
48
#include "precomp.hpp"
 
49
#include <stdio.h>
 
50
#include <limits>
 
51
#include "opencl_kernels_calib3d.hpp"
 
52
 
 
53
namespace cv
 
54
{
 
55
 
 
56
struct StereoBMParams
 
57
{
 
58
    StereoBMParams(int _numDisparities=64, int _SADWindowSize=21)
 
59
    {
 
60
        preFilterType = StereoBM::PREFILTER_XSOBEL;
 
61
        preFilterSize = 9;
 
62
        preFilterCap = 31;
 
63
        SADWindowSize = _SADWindowSize;
 
64
        minDisparity = 0;
 
65
        numDisparities = _numDisparities > 0 ? _numDisparities : 64;
 
66
        textureThreshold = 10;
 
67
        uniquenessRatio = 15;
 
68
        speckleRange = speckleWindowSize = 0;
 
69
        roi1 = roi2 = Rect(0,0,0,0);
 
70
        disp12MaxDiff = -1;
 
71
        dispType = CV_16S;
 
72
    }
 
73
 
 
74
    int preFilterType;
 
75
    int preFilterSize;
 
76
    int preFilterCap;
 
77
    int SADWindowSize;
 
78
    int minDisparity;
 
79
    int numDisparities;
 
80
    int textureThreshold;
 
81
    int uniquenessRatio;
 
82
    int speckleRange;
 
83
    int speckleWindowSize;
 
84
    Rect roi1, roi2;
 
85
    int disp12MaxDiff;
 
86
    int dispType;
 
87
};
 
88
 
 
89
#ifdef HAVE_OPENCL
 
90
static bool ocl_prefilter_norm(InputArray _input, OutputArray _output, int winsize, int prefilterCap)
 
91
{
 
92
    ocl::Kernel k("prefilter_norm", ocl::calib3d::stereobm_oclsrc, cv::format("-D WSZ=%d", winsize));
 
93
    if(k.empty())
 
94
        return false;
 
95
 
 
96
    int scale_g = winsize*winsize/8, scale_s = (1024 + scale_g)/(scale_g*2);
 
97
    scale_g *= scale_s;
 
98
 
 
99
    UMat input = _input.getUMat(), output;
 
100
    _output.create(input.size(), input.type());
 
101
    output = _output.getUMat();
 
102
 
 
103
    size_t globalThreads[3] = { (size_t)input.cols, (size_t)input.rows, 1 };
 
104
 
 
105
    k.args(ocl::KernelArg::PtrReadOnly(input), ocl::KernelArg::PtrWriteOnly(output), input.rows, input.cols,
 
106
        prefilterCap, scale_g, scale_s);
 
107
 
 
108
    return k.run(2, globalThreads, NULL, false);
 
109
}
 
110
#endif
 
111
 
 
112
static void prefilterNorm( const Mat& src, Mat& dst, int winsize, int ftzero, uchar* buf )
 
113
{
 
114
    int x, y, wsz2 = winsize/2;
 
115
    int* vsum = (int*)alignPtr(buf + (wsz2 + 1)*sizeof(vsum[0]), 32);
 
116
    int scale_g = winsize*winsize/8, scale_s = (1024 + scale_g)/(scale_g*2);
 
117
    const int OFS = 256*5, TABSZ = OFS*2 + 256;
 
118
    uchar tab[TABSZ];
 
119
    const uchar* sptr = src.ptr();
 
120
    int srcstep = (int)src.step;
 
121
    Size size = src.size();
 
122
 
 
123
    scale_g *= scale_s;
 
124
 
 
125
    for( x = 0; x < TABSZ; x++ )
 
126
        tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero*2 : x - OFS + ftzero);
 
127
 
 
128
    for( x = 0; x < size.width; x++ )
 
129
        vsum[x] = (ushort)(sptr[x]*(wsz2 + 2));
 
130
 
 
131
    for( y = 1; y < wsz2; y++ )
 
132
    {
 
133
        for( x = 0; x < size.width; x++ )
 
134
            vsum[x] = (ushort)(vsum[x] + sptr[srcstep*y + x]);
 
135
    }
 
136
 
 
137
    for( y = 0; y < size.height; y++ )
 
138
    {
 
139
        const uchar* top = sptr + srcstep*MAX(y-wsz2-1,0);
 
140
        const uchar* bottom = sptr + srcstep*MIN(y+wsz2,size.height-1);
 
141
        const uchar* prev = sptr + srcstep*MAX(y-1,0);
 
142
        const uchar* curr = sptr + srcstep*y;
 
143
        const uchar* next = sptr + srcstep*MIN(y+1,size.height-1);
 
144
        uchar* dptr = dst.ptr<uchar>(y);
 
145
 
 
146
        for( x = 0; x < size.width; x++ )
 
147
            vsum[x] = (ushort)(vsum[x] + bottom[x] - top[x]);
 
148
 
 
149
        for( x = 0; x <= wsz2; x++ )
 
150
        {
 
151
            vsum[-x-1] = vsum[0];
 
152
            vsum[size.width+x] = vsum[size.width-1];
 
153
        }
 
154
 
 
155
        int sum = vsum[0]*(wsz2 + 1);
 
156
        for( x = 1; x <= wsz2; x++ )
 
157
            sum += vsum[x];
 
158
 
 
159
        int val = ((curr[0]*5 + curr[1] + prev[0] + next[0])*scale_g - sum*scale_s) >> 10;
 
160
        dptr[0] = tab[val + OFS];
 
161
 
 
162
        for( x = 1; x < size.width-1; x++ )
 
163
        {
 
164
            sum += vsum[x+wsz2] - vsum[x-wsz2-1];
 
165
            val = ((curr[x]*4 + curr[x-1] + curr[x+1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;
 
166
            dptr[x] = tab[val + OFS];
 
167
        }
 
168
 
 
169
        sum += vsum[x+wsz2] - vsum[x-wsz2-1];
 
170
        val = ((curr[x]*5 + curr[x-1] + prev[x] + next[x])*scale_g - sum*scale_s) >> 10;
 
171
        dptr[x] = tab[val + OFS];
 
172
    }
 
173
}
 
174
 
 
175
#ifdef HAVE_OPENCL
 
176
static bool ocl_prefilter_xsobel(InputArray _input, OutputArray _output, int prefilterCap)
 
177
{
 
178
    ocl::Kernel k("prefilter_xsobel", ocl::calib3d::stereobm_oclsrc);
 
179
    if(k.empty())
 
180
        return false;
 
181
 
 
182
    UMat input = _input.getUMat(), output;
 
183
    _output.create(input.size(), input.type());
 
184
    output = _output.getUMat();
 
185
 
 
186
    size_t globalThreads[3] = { (size_t)input.cols, (size_t)input.rows, 1 };
 
187
 
 
188
    k.args(ocl::KernelArg::PtrReadOnly(input), ocl::KernelArg::PtrWriteOnly(output), input.rows, input.cols, prefilterCap);
 
189
 
 
190
    return k.run(2, globalThreads, NULL, false);
 
191
}
 
192
#endif
 
193
 
 
194
static void
 
195
prefilterXSobel( const Mat& src, Mat& dst, int ftzero )
 
196
{
 
197
    int x, y;
 
198
    const int OFS = 256*4, TABSZ = OFS*2 + 256;
 
199
    uchar tab[TABSZ];
 
200
    Size size = src.size();
 
201
 
 
202
    for( x = 0; x < TABSZ; x++ )
 
203
        tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero*2 : x - OFS + ftzero);
 
204
    uchar val0 = tab[0 + OFS];
 
205
 
 
206
#if CV_SSE2
 
207
    volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE2);
 
208
#endif
 
209
 
 
210
    for( y = 0; y < size.height-1; y += 2 )
 
211
    {
 
212
        const uchar* srow1 = src.ptr<uchar>(y);
 
213
        const uchar* srow0 = y > 0 ? srow1 - src.step : size.height > 1 ? srow1 + src.step : srow1;
 
214
        const uchar* srow2 = y < size.height-1 ? srow1 + src.step : size.height > 1 ? srow1 - src.step : srow1;
 
215
        const uchar* srow3 = y < size.height-2 ? srow1 + src.step*2 : srow1;
 
216
        uchar* dptr0 = dst.ptr<uchar>(y);
 
217
        uchar* dptr1 = dptr0 + dst.step;
 
218
 
 
219
        dptr0[0] = dptr0[size.width-1] = dptr1[0] = dptr1[size.width-1] = val0;
 
220
        x = 1;
 
221
 
 
222
#if CV_NEON
 
223
        int16x8_t ftz = vdupq_n_s16 ((short) ftzero);
 
224
        uint8x8_t ftz2 = vdup_n_u8 (cv::saturate_cast<uchar>(ftzero*2));
 
225
 
 
226
        for(; x <=size.width-9; x += 8 )
 
227
        {
 
228
            uint8x8_t c0 = vld1_u8 (srow0 + x - 1);
 
229
            uint8x8_t c1 = vld1_u8 (srow1 + x - 1);
 
230
            uint8x8_t d0 = vld1_u8 (srow0 + x + 1);
 
231
            uint8x8_t d1 = vld1_u8 (srow1 + x + 1);
 
232
 
 
233
            int16x8_t t0 = vreinterpretq_s16_u16 (vsubl_u8 (d0, c0));
 
234
            int16x8_t t1 = vreinterpretq_s16_u16 (vsubl_u8 (d1, c1));
 
235
 
 
236
            uint8x8_t c2 = vld1_u8 (srow2 + x - 1);
 
237
            uint8x8_t c3 = vld1_u8 (srow3 + x - 1);
 
238
            uint8x8_t d2 = vld1_u8 (srow2 + x + 1);
 
239
            uint8x8_t d3 = vld1_u8 (srow3 + x + 1);
 
240
 
 
241
            int16x8_t t2 = vreinterpretq_s16_u16 (vsubl_u8 (d2, c2));
 
242
            int16x8_t t3 = vreinterpretq_s16_u16 (vsubl_u8 (d3, c3));
 
243
 
 
244
            int16x8_t v0 = vaddq_s16 (vaddq_s16 (t2, t0), vaddq_s16 (t1, t1));
 
245
            int16x8_t v1 = vaddq_s16 (vaddq_s16 (t3, t1), vaddq_s16 (t2, t2));
 
246
 
 
247
 
 
248
            uint8x8_t v0_u8 = vqmovun_s16 (vaddq_s16 (v0, ftz));
 
249
            uint8x8_t v1_u8 = vqmovun_s16 (vaddq_s16 (v1, ftz));
 
250
            v0_u8 =  vmin_u8 (v0_u8, ftz2);
 
251
            v1_u8 =  vmin_u8 (v1_u8, ftz2);
 
252
            vqmovun_s16 (vaddq_s16 (v1, ftz));
 
253
 
 
254
            vst1_u8 (dptr0 + x, v0_u8);
 
255
            vst1_u8 (dptr1 + x, v1_u8);
 
256
        }
 
257
#elif CV_SSE2
 
258
        if( useSIMD )
 
259
        {
 
260
            __m128i z = _mm_setzero_si128(), ftz = _mm_set1_epi16((short)ftzero),
 
261
            ftz2 = _mm_set1_epi8(cv::saturate_cast<uchar>(ftzero*2));
 
262
            for( ; x <= size.width-9; x += 8 )
 
263
            {
 
264
                __m128i c0 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow0 + x - 1)), z);
 
265
                __m128i c1 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow1 + x - 1)), z);
 
266
                __m128i d0 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow0 + x + 1)), z);
 
267
                __m128i d1 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow1 + x + 1)), z);
 
268
 
 
269
                d0 = _mm_sub_epi16(d0, c0);
 
270
                d1 = _mm_sub_epi16(d1, c1);
 
271
 
 
272
                __m128i c2 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x - 1)), z);
 
273
                __m128i c3 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow3 + x - 1)), z);
 
274
                __m128i d2 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x + 1)), z);
 
275
                __m128i d3 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow3 + x + 1)), z);
 
276
 
 
277
                d2 = _mm_sub_epi16(d2, c2);
 
278
                d3 = _mm_sub_epi16(d3, c3);
 
279
 
 
280
                __m128i v0 = _mm_add_epi16(d0, _mm_add_epi16(d2, _mm_add_epi16(d1, d1)));
 
281
                __m128i v1 = _mm_add_epi16(d1, _mm_add_epi16(d3, _mm_add_epi16(d2, d2)));
 
282
                v0 = _mm_packus_epi16(_mm_add_epi16(v0, ftz), _mm_add_epi16(v1, ftz));
 
283
                v0 = _mm_min_epu8(v0, ftz2);
 
284
 
 
285
                _mm_storel_epi64((__m128i*)(dptr0 + x), v0);
 
286
                _mm_storel_epi64((__m128i*)(dptr1 + x), _mm_unpackhi_epi64(v0, v0));
 
287
            }
 
288
        }
 
289
#endif
 
290
 
 
291
        for( ; x < size.width-1; x++ )
 
292
        {
 
293
            int d0 = srow0[x+1] - srow0[x-1], d1 = srow1[x+1] - srow1[x-1],
 
294
            d2 = srow2[x+1] - srow2[x-1], d3 = srow3[x+1] - srow3[x-1];
 
295
            int v0 = tab[d0 + d1*2 + d2 + OFS];
 
296
            int v1 = tab[d1 + d2*2 + d3 + OFS];
 
297
            dptr0[x] = (uchar)v0;
 
298
            dptr1[x] = (uchar)v1;
 
299
        }
 
300
    }
 
301
 
 
302
#if CV_NEON
 
303
    uint8x16_t val0_16 = vdupq_n_u8 (val0);
 
304
#endif
 
305
 
 
306
    for( ; y < size.height; y++ )
 
307
    {
 
308
        uchar* dptr = dst.ptr<uchar>(y);
 
309
        x = 0;
 
310
    #if CV_NEON
 
311
        for(; x <= size.width-16; x+=16 )
 
312
            vst1q_u8 (dptr + x, val0_16);
 
313
    #endif
 
314
        for(; x < size.width; x++ )
 
315
            dptr[x] = val0;
 
316
    }
 
317
}
 
318
 
 
319
 
 
320
static const int DISPARITY_SHIFT = 4;
 
321
 
 
322
#if CV_SSE2
 
323
static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
 
324
                                            Mat& disp, Mat& cost, StereoBMParams& state,
 
325
                                            uchar* buf, int _dy0, int _dy1 )
 
326
{
 
327
    const int ALIGN = 16;
 
328
    int x, y, d;
 
329
    int wsz = state.SADWindowSize, wsz2 = wsz/2;
 
330
    int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);
 
331
    int ndisp = state.numDisparities;
 
332
    int mindisp = state.minDisparity;
 
333
    int lofs = MAX(ndisp - 1 + mindisp, 0);
 
334
    int rofs = -MIN(ndisp - 1 + mindisp, 0);
 
335
    int width = left.cols, height = left.rows;
 
336
    int width1 = width - rofs - ndisp + 1;
 
337
    int ftzero = state.preFilterCap;
 
338
    int textureThreshold = state.textureThreshold;
 
339
    int uniquenessRatio = state.uniquenessRatio;
 
340
    short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);
 
341
 
 
342
    ushort *sad, *hsad0, *hsad, *hsad_sub;
 
343
    int *htext;
 
344
    uchar *cbuf0, *cbuf;
 
345
    const uchar* lptr0 = left.ptr() + lofs;
 
346
    const uchar* rptr0 = right.ptr() + rofs;
 
347
    const uchar *lptr, *lptr_sub, *rptr;
 
348
    short* dptr = disp.ptr<short>();
 
349
    int sstep = (int)left.step;
 
350
    int dstep = (int)(disp.step/sizeof(dptr[0]));
 
351
    int cstep = (height + dy0 + dy1)*ndisp;
 
352
    short costbuf = 0;
 
353
    int coststep = cost.data ? (int)(cost.step/sizeof(costbuf)) : 0;
 
354
    const int TABSZ = 256;
 
355
    uchar tab[TABSZ];
 
356
    const __m128i d0_8 = _mm_setr_epi16(0,1,2,3,4,5,6,7), dd_8 = _mm_set1_epi16(8);
 
357
 
 
358
    sad = (ushort*)alignPtr(buf + sizeof(sad[0]), ALIGN);
 
359
    hsad0 = (ushort*)alignPtr(sad + ndisp + 1 + dy0*ndisp, ALIGN);
 
360
    htext = (int*)alignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2, ALIGN);
 
361
    cbuf0 = (uchar*)alignPtr((uchar*)(htext + height + wsz2 + 2) + dy0*ndisp, ALIGN);
 
362
 
 
363
    for( x = 0; x < TABSZ; x++ )
 
364
        tab[x] = (uchar)std::abs(x - ftzero);
 
365
 
 
366
    // initialize buffers
 
367
    memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) );
 
368
    memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) );
 
369
 
 
370
    for( x = -wsz2-1; x < wsz2; x++ )
 
371
    {
 
372
        hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp;
 
373
        lptr = lptr0 + MIN(MAX(x, -lofs), width-lofs-1) - dy0*sstep;
 
374
        rptr = rptr0 + MIN(MAX(x, -rofs), width-rofs-ndisp) - dy0*sstep;
 
375
 
 
376
        for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )
 
377
        {
 
378
            int lval = lptr[0];
 
379
            __m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128();
 
380
            for( d = 0; d < ndisp; d += 16 )
 
381
            {
 
382
                __m128i rv = _mm_loadu_si128((const __m128i*)(rptr + d));
 
383
                __m128i hsad_l = _mm_load_si128((__m128i*)(hsad + d));
 
384
                __m128i hsad_h = _mm_load_si128((__m128i*)(hsad + d + 8));
 
385
                __m128i diff = _mm_adds_epu8(_mm_subs_epu8(lv, rv), _mm_subs_epu8(rv, lv));
 
386
                _mm_store_si128((__m128i*)(cbuf + d), diff);
 
387
                hsad_l = _mm_add_epi16(hsad_l, _mm_unpacklo_epi8(diff,z));
 
388
                hsad_h = _mm_add_epi16(hsad_h, _mm_unpackhi_epi8(diff,z));
 
389
                _mm_store_si128((__m128i*)(hsad + d), hsad_l);
 
390
                _mm_store_si128((__m128i*)(hsad + d + 8), hsad_h);
 
391
            }
 
392
            htext[y] += tab[lval];
 
393
        }
 
394
    }
 
395
 
 
396
    // initialize the left and right borders of the disparity map
 
397
    for( y = 0; y < height; y++ )
 
398
    {
 
399
        for( x = 0; x < lofs; x++ )
 
400
            dptr[y*dstep + x] = FILTERED;
 
401
        for( x = lofs + width1; x < width; x++ )
 
402
            dptr[y*dstep + x] = FILTERED;
 
403
    }
 
404
    dptr += lofs;
 
405
 
 
406
    for( x = 0; x < width1; x++, dptr++ )
 
407
    {
 
408
        short* costptr = cost.data ? cost.ptr<short>() + lofs + x : &costbuf;
 
409
        int x0 = x - wsz2 - 1, x1 = x + wsz2;
 
410
        const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
 
411
        cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
 
412
        hsad = hsad0 - dy0*ndisp;
 
413
        lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;
 
414
        lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;
 
415
        rptr = rptr0 + MIN(MAX(x1, -rofs), width-ndisp-rofs) - dy0*sstep;
 
416
 
 
417
        for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp,
 
418
            hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )
 
419
        {
 
420
            int lval = lptr[0];
 
421
            __m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128();
 
422
            for( d = 0; d < ndisp; d += 16 )
 
423
            {
 
424
                __m128i rv = _mm_loadu_si128((const __m128i*)(rptr + d));
 
425
                __m128i hsad_l = _mm_load_si128((__m128i*)(hsad + d));
 
426
                __m128i hsad_h = _mm_load_si128((__m128i*)(hsad + d + 8));
 
427
                __m128i cbs = _mm_load_si128((const __m128i*)(cbuf_sub + d));
 
428
                __m128i diff = _mm_adds_epu8(_mm_subs_epu8(lv, rv), _mm_subs_epu8(rv, lv));
 
429
                __m128i diff_h = _mm_sub_epi16(_mm_unpackhi_epi8(diff, z), _mm_unpackhi_epi8(cbs, z));
 
430
                _mm_store_si128((__m128i*)(cbuf + d), diff);
 
431
                diff = _mm_sub_epi16(_mm_unpacklo_epi8(diff, z), _mm_unpacklo_epi8(cbs, z));
 
432
                hsad_h = _mm_add_epi16(hsad_h, diff_h);
 
433
                hsad_l = _mm_add_epi16(hsad_l, diff);
 
434
                _mm_store_si128((__m128i*)(hsad + d), hsad_l);
 
435
                _mm_store_si128((__m128i*)(hsad + d + 8), hsad_h);
 
436
            }
 
437
            htext[y] += tab[lval] - tab[lptr_sub[0]];
 
438
        }
 
439
 
 
440
        // fill borders
 
441
        for( y = dy1; y <= wsz2; y++ )
 
442
            htext[height+y] = htext[height+dy1-1];
 
443
        for( y = -wsz2-1; y < -dy0; y++ )
 
444
            htext[y] = htext[-dy0];
 
445
 
 
446
        // initialize sums
 
447
        for( d = 0; d < ndisp; d++ )
 
448
            sad[d] = (ushort)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));
 
449
 
 
450
        hsad = hsad0 + (1 - dy0)*ndisp;
 
451
        for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )
 
452
            for( d = 0; d < ndisp; d += 16 )
 
453
            {
 
454
                __m128i s0 = _mm_load_si128((__m128i*)(sad + d));
 
455
                __m128i s1 = _mm_load_si128((__m128i*)(sad + d + 8));
 
456
                __m128i t0 = _mm_load_si128((__m128i*)(hsad + d));
 
457
                __m128i t1 = _mm_load_si128((__m128i*)(hsad + d + 8));
 
458
                s0 = _mm_add_epi16(s0, t0);
 
459
                s1 = _mm_add_epi16(s1, t1);
 
460
                _mm_store_si128((__m128i*)(sad + d), s0);
 
461
                _mm_store_si128((__m128i*)(sad + d + 8), s1);
 
462
            }
 
463
        int tsum = 0;
 
464
        for( y = -wsz2-1; y < wsz2; y++ )
 
465
            tsum += htext[y];
 
466
 
 
467
        // finally, start the real processing
 
468
        for( y = 0; y < height; y++ )
 
469
        {
 
470
            int minsad = INT_MAX, mind = -1;
 
471
            hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;
 
472
            hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;
 
473
            __m128i minsad8 = _mm_set1_epi16(SHRT_MAX);
 
474
            __m128i mind8 = _mm_set1_epi16(0), d8 = d0_8, mask;
 
475
 
 
476
            for( d = 0; d < ndisp; d += 16 )
 
477
            {
 
478
                __m128i u0 = _mm_load_si128((__m128i*)(hsad_sub + d));
 
479
                __m128i u1 = _mm_load_si128((__m128i*)(hsad + d));
 
480
 
 
481
                __m128i v0 = _mm_load_si128((__m128i*)(hsad_sub + d + 8));
 
482
                __m128i v1 = _mm_load_si128((__m128i*)(hsad + d + 8));
 
483
 
 
484
                __m128i usad8 = _mm_load_si128((__m128i*)(sad + d));
 
485
                __m128i vsad8 = _mm_load_si128((__m128i*)(sad + d + 8));
 
486
 
 
487
                u1 = _mm_sub_epi16(u1, u0);
 
488
                v1 = _mm_sub_epi16(v1, v0);
 
489
                usad8 = _mm_add_epi16(usad8, u1);
 
490
                vsad8 = _mm_add_epi16(vsad8, v1);
 
491
 
 
492
                mask = _mm_cmpgt_epi16(minsad8, usad8);
 
493
                minsad8 = _mm_min_epi16(minsad8, usad8);
 
494
                mind8 = _mm_max_epi16(mind8, _mm_and_si128(mask, d8));
 
495
 
 
496
                _mm_store_si128((__m128i*)(sad + d), usad8);
 
497
                _mm_store_si128((__m128i*)(sad + d + 8), vsad8);
 
498
 
 
499
                mask = _mm_cmpgt_epi16(minsad8, vsad8);
 
500
                minsad8 = _mm_min_epi16(minsad8, vsad8);
 
501
 
 
502
                d8 = _mm_add_epi16(d8, dd_8);
 
503
                mind8 = _mm_max_epi16(mind8, _mm_and_si128(mask, d8));
 
504
                d8 = _mm_add_epi16(d8, dd_8);
 
505
            }
 
506
 
 
507
            tsum += htext[y + wsz2] - htext[y - wsz2 - 1];
 
508
            if( tsum < textureThreshold )
 
509
            {
 
510
                dptr[y*dstep] = FILTERED;
 
511
                continue;
 
512
            }
 
513
 
 
514
            ushort CV_DECL_ALIGNED(16) minsad_buf[8], mind_buf[8];
 
515
            _mm_store_si128((__m128i*)minsad_buf, minsad8);
 
516
            _mm_store_si128((__m128i*)mind_buf, mind8);
 
517
            for( d = 0; d < 8; d++ )
 
518
                if(minsad > (int)minsad_buf[d] || (minsad == (int)minsad_buf[d] && mind > mind_buf[d]))
 
519
                {
 
520
                    minsad = minsad_buf[d];
 
521
                    mind = mind_buf[d];
 
522
                }
 
523
 
 
524
            if( uniquenessRatio > 0 )
 
525
            {
 
526
                int thresh = minsad + (minsad * uniquenessRatio/100);
 
527
                __m128i thresh8 = _mm_set1_epi16((short)(thresh + 1));
 
528
                __m128i d1 = _mm_set1_epi16((short)(mind-1)), d2 = _mm_set1_epi16((short)(mind+1));
 
529
                __m128i dd_16 = _mm_add_epi16(dd_8, dd_8);
 
530
                d8 = _mm_sub_epi16(d0_8, dd_16);
 
531
 
 
532
                for( d = 0; d < ndisp; d += 16 )
 
533
                {
 
534
                    __m128i usad8 = _mm_load_si128((__m128i*)(sad + d));
 
535
                    __m128i vsad8 = _mm_load_si128((__m128i*)(sad + d + 8));
 
536
                    mask = _mm_cmpgt_epi16( thresh8, _mm_min_epi16(usad8,vsad8));
 
537
                    d8 = _mm_add_epi16(d8, dd_16);
 
538
                    if( !_mm_movemask_epi8(mask) )
 
539
                        continue;
 
540
                    mask = _mm_cmpgt_epi16( thresh8, usad8);
 
541
                    mask = _mm_and_si128(mask, _mm_or_si128(_mm_cmpgt_epi16(d1,d8), _mm_cmpgt_epi16(d8,d2)));
 
542
                    if( _mm_movemask_epi8(mask) )
 
543
                        break;
 
544
                    __m128i t8 = _mm_add_epi16(d8, dd_8);
 
545
                    mask = _mm_cmpgt_epi16( thresh8, vsad8);
 
546
                    mask = _mm_and_si128(mask, _mm_or_si128(_mm_cmpgt_epi16(d1,t8), _mm_cmpgt_epi16(t8,d2)));
 
547
                    if( _mm_movemask_epi8(mask) )
 
548
                        break;
 
549
                }
 
550
                if( d < ndisp )
 
551
                {
 
552
                    dptr[y*dstep] = FILTERED;
 
553
                    continue;
 
554
                }
 
555
            }
 
556
 
 
557
            if( 0 < mind && mind < ndisp - 1 )
 
558
            {
 
559
                int p = sad[mind+1], n = sad[mind-1];
 
560
                d = p + n - 2*sad[mind] + std::abs(p - n);
 
561
                dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15) >> 4);
 
562
            }
 
563
            else
 
564
                dptr[y*dstep] = (short)((ndisp - mind - 1 + mindisp)*16);
 
565
            costptr[y*coststep] = sad[mind];
 
566
        }
 
567
    }
 
568
}
 
569
#endif
 
570
 
 
571
static void
 
572
findStereoCorrespondenceBM( const Mat& left, const Mat& right,
 
573
                           Mat& disp, Mat& cost, const StereoBMParams& state,
 
574
                           uchar* buf, int _dy0, int _dy1 )
 
575
{
 
576
 
 
577
    const int ALIGN = 16;
 
578
    int x, y, d;
 
579
    int wsz = state.SADWindowSize, wsz2 = wsz/2;
 
580
    int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);
 
581
    int ndisp = state.numDisparities;
 
582
    int mindisp = state.minDisparity;
 
583
    int lofs = MAX(ndisp - 1 + mindisp, 0);
 
584
    int rofs = -MIN(ndisp - 1 + mindisp, 0);
 
585
    int width = left.cols, height = left.rows;
 
586
    int width1 = width - rofs - ndisp + 1;
 
587
    int ftzero = state.preFilterCap;
 
588
    int textureThreshold = state.textureThreshold;
 
589
    int uniquenessRatio = state.uniquenessRatio;
 
590
    short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);
 
591
 
 
592
#if CV_NEON
 
593
    CV_Assert (ndisp % 8 == 0);
 
594
    int32_t d0_4_temp [4];
 
595
    for (int i = 0; i < 4; i ++)
 
596
        d0_4_temp[i] = i;
 
597
    int32x4_t d0_4 = vld1q_s32 (d0_4_temp);
 
598
    int32x4_t dd_4 = vdupq_n_s32 (4);
 
599
#endif
 
600
 
 
601
    int *sad, *hsad0, *hsad, *hsad_sub, *htext;
 
602
    uchar *cbuf0, *cbuf;
 
603
    const uchar* lptr0 = left.ptr() + lofs;
 
604
    const uchar* rptr0 = right.ptr() + rofs;
 
605
    const uchar *lptr, *lptr_sub, *rptr;
 
606
    short* dptr = disp.ptr<short>();
 
607
    int sstep = (int)left.step;
 
608
    int dstep = (int)(disp.step/sizeof(dptr[0]));
 
609
    int cstep = (height+dy0+dy1)*ndisp;
 
610
    int costbuf = 0;
 
611
    int coststep = cost.data ? (int)(cost.step/sizeof(costbuf)) : 0;
 
612
    const int TABSZ = 256;
 
613
    uchar tab[TABSZ];
 
614
 
 
615
    sad = (int*)alignPtr(buf + sizeof(sad[0]), ALIGN);
 
616
    hsad0 = (int*)alignPtr(sad + ndisp + 1 + dy0*ndisp, ALIGN);
 
617
    htext = (int*)alignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2, ALIGN);
 
618
    cbuf0 = (uchar*)alignPtr((uchar*)(htext + height + wsz2 + 2) + dy0*ndisp, ALIGN);
 
619
 
 
620
    for( x = 0; x < TABSZ; x++ )
 
621
        tab[x] = (uchar)std::abs(x - ftzero);
 
622
 
 
623
    // initialize buffers
 
624
    memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) );
 
625
    memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) );
 
626
 
 
627
    for( x = -wsz2-1; x < wsz2; x++ )
 
628
    {
 
629
        hsad = hsad0 - dy0*ndisp; cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp;
 
630
        lptr = lptr0 + std::min(std::max(x, -lofs), width-lofs-1) - dy0*sstep;
 
631
        rptr = rptr0 + std::min(std::max(x, -rofs), width-rofs-ndisp) - dy0*sstep;
 
632
        for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )
 
633
        {
 
634
            int lval = lptr[0];
 
635
        #if CV_NEON
 
636
            int16x8_t lv = vdupq_n_s16 ((int16_t)lval);
 
637
 
 
638
            for( d = 0; d < ndisp; d += 8 )
 
639
            {
 
640
                int16x8_t rv = vreinterpretq_s16_u16 (vmovl_u8 (vld1_u8 (rptr + d)));
 
641
                int32x4_t hsad_l = vld1q_s32 (hsad + d);
 
642
                int32x4_t hsad_h = vld1q_s32 (hsad + d + 4);
 
643
                int16x8_t diff = vabdq_s16 (lv, rv);
 
644
                vst1_u8 (cbuf + d, vmovn_u16(vreinterpretq_u16_s16(diff)));
 
645
                hsad_l = vaddq_s32 (hsad_l, vmovl_s16(vget_low_s16 (diff)));
 
646
                hsad_h = vaddq_s32 (hsad_h, vmovl_s16(vget_high_s16 (diff)));
 
647
                vst1q_s32 ((hsad + d), hsad_l);
 
648
                vst1q_s32 ((hsad + d + 4), hsad_h);
 
649
            }
 
650
        #else
 
651
            for( d = 0; d < ndisp; d++ )
 
652
            {
 
653
                int diff = std::abs(lval - rptr[d]);
 
654
                cbuf[d] = (uchar)diff;
 
655
                hsad[d] = (int)(hsad[d] + diff);
 
656
            }
 
657
        #endif
 
658
            htext[y] += tab[lval];
 
659
        }
 
660
    }
 
661
 
 
662
    // initialize the left and right borders of the disparity map
 
663
    for( y = 0; y < height; y++ )
 
664
    {
 
665
        for( x = 0; x < lofs; x++ )
 
666
            dptr[y*dstep + x] = FILTERED;
 
667
        for( x = lofs + width1; x < width; x++ )
 
668
            dptr[y*dstep + x] = FILTERED;
 
669
    }
 
670
    dptr += lofs;
 
671
 
 
672
    for( x = 0; x < width1; x++, dptr++ )
 
673
    {
 
674
        int* costptr = cost.data ? cost.ptr<int>() + lofs + x : &costbuf;
 
675
        int x0 = x - wsz2 - 1, x1 = x + wsz2;
 
676
        const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
 
677
        cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
 
678
        hsad = hsad0 - dy0*ndisp;
 
679
        lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;
 
680
        lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;
 
681
        rptr = rptr0 + MIN(MAX(x1, -rofs), width-ndisp-rofs) - dy0*sstep;
 
682
 
 
683
        for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp,
 
684
            hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )
 
685
        {
 
686
            int lval = lptr[0];
 
687
        #if CV_NEON
 
688
            int16x8_t lv = vdupq_n_s16 ((int16_t)lval);
 
689
            for( d = 0; d < ndisp; d += 8 )
 
690
            {
 
691
                int16x8_t rv = vreinterpretq_s16_u16 (vmovl_u8 (vld1_u8 (rptr + d)));
 
692
                int32x4_t hsad_l = vld1q_s32 (hsad + d);
 
693
                int32x4_t hsad_h = vld1q_s32 (hsad + d + 4);
 
694
                int16x8_t cbs = vreinterpretq_s16_u16 (vmovl_u8 (vld1_u8 (cbuf_sub + d)));
 
695
                int16x8_t diff = vabdq_s16 (lv, rv);
 
696
                int32x4_t diff_h = vsubl_s16 (vget_high_s16 (diff), vget_high_s16 (cbs));
 
697
                int32x4_t diff_l = vsubl_s16 (vget_low_s16 (diff), vget_low_s16 (cbs));
 
698
                vst1_u8 (cbuf + d, vmovn_u16(vreinterpretq_u16_s16(diff)));
 
699
                hsad_h = vaddq_s32 (hsad_h, diff_h);
 
700
                hsad_l = vaddq_s32 (hsad_l, diff_l);
 
701
                vst1q_s32 ((hsad + d), hsad_l);
 
702
                vst1q_s32 ((hsad + d + 4), hsad_h);
 
703
            }
 
704
        #else
 
705
            for( d = 0; d < ndisp; d++ )
 
706
            {
 
707
                int diff = std::abs(lval - rptr[d]);
 
708
                cbuf[d] = (uchar)diff;
 
709
                hsad[d] = hsad[d] + diff - cbuf_sub[d];
 
710
            }
 
711
        #endif
 
712
            htext[y] += tab[lval] - tab[lptr_sub[0]];
 
713
        }
 
714
 
 
715
        // fill borders
 
716
        for( y = dy1; y <= wsz2; y++ )
 
717
            htext[height+y] = htext[height+dy1-1];
 
718
        for( y = -wsz2-1; y < -dy0; y++ )
 
719
            htext[y] = htext[-dy0];
 
720
 
 
721
        // initialize sums
 
722
        for( d = 0; d < ndisp; d++ )
 
723
            sad[d] = (int)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));
 
724
 
 
725
        hsad = hsad0 + (1 - dy0)*ndisp;
 
726
        for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )
 
727
        {
 
728
        #if CV_NEON
 
729
            for( d = 0; d <= ndisp-8; d += 8 )
 
730
            {
 
731
                int32x4_t s0 = vld1q_s32 (sad + d);
 
732
                int32x4_t s1 = vld1q_s32 (sad + d + 4);
 
733
                int32x4_t t0 = vld1q_s32 (hsad + d);
 
734
                int32x4_t t1 = vld1q_s32 (hsad + d + 4);
 
735
                s0 = vaddq_s32 (s0, t0);
 
736
                s1 = vaddq_s32 (s1, t1);
 
737
                vst1q_s32 (sad + d, s0);
 
738
                vst1q_s32 (sad + d + 4, s1);
 
739
            }
 
740
        #else
 
741
            for( d = 0; d < ndisp; d++ )
 
742
                sad[d] = (int)(sad[d] + hsad[d]);
 
743
        #endif
 
744
        }
 
745
        int tsum = 0;
 
746
        for( y = -wsz2-1; y < wsz2; y++ )
 
747
            tsum += htext[y];
 
748
 
 
749
        // finally, start the real processing
 
750
        for( y = 0; y < height; y++ )
 
751
        {
 
752
            int minsad = INT_MAX, mind = -1;
 
753
            hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;
 
754
            hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;
 
755
        #if CV_NEON
 
756
            int32x4_t minsad4 = vdupq_n_s32 (INT_MAX);
 
757
            int32x4_t mind4 = vdupq_n_s32(0), d4 = d0_4;
 
758
 
 
759
            for( d = 0; d <= ndisp-8; d += 8 )
 
760
            {
 
761
                int32x4_t u0 = vld1q_s32 (hsad_sub + d);
 
762
                int32x4_t u1 = vld1q_s32 (hsad + d);
 
763
 
 
764
                int32x4_t v0 = vld1q_s32 (hsad_sub + d + 4);
 
765
                int32x4_t v1 = vld1q_s32 (hsad + d + 4);
 
766
 
 
767
                int32x4_t usad4 = vld1q_s32(sad + d);
 
768
                int32x4_t vsad4 = vld1q_s32(sad + d + 4);
 
769
 
 
770
                u1 = vsubq_s32 (u1, u0);
 
771
                v1 = vsubq_s32 (v1, v0);
 
772
                usad4 = vaddq_s32 (usad4, u1);
 
773
                vsad4 = vaddq_s32 (vsad4, v1);
 
774
 
 
775
                uint32x4_t mask = vcgtq_s32 (minsad4, usad4);
 
776
                minsad4 = vminq_s32 (minsad4, usad4);
 
777
                mind4 = vbslq_s32(mask, d4, mind4);
 
778
 
 
779
                vst1q_s32 (sad + d, usad4);
 
780
                vst1q_s32 (sad + d + 4, vsad4);
 
781
                d4 = vaddq_s32 (d4, dd_4);
 
782
 
 
783
                mask = vcgtq_s32 (minsad4, vsad4);
 
784
                minsad4 = vminq_s32 (minsad4, vsad4);
 
785
                mind4 = vbslq_s32(mask, d4, mind4);
 
786
 
 
787
                d4 = vaddq_s32 (d4, dd_4);
 
788
 
 
789
            }
 
790
            int32x2_t mind4_h = vget_high_s32 (mind4);
 
791
            int32x2_t mind4_l = vget_low_s32 (mind4);
 
792
            int32x2_t minsad4_h = vget_high_s32 (minsad4);
 
793
            int32x2_t minsad4_l = vget_low_s32 (minsad4);
 
794
 
 
795
            uint32x2_t mask = vorr_u32 (vclt_s32 (minsad4_h, minsad4_l), vand_u32 (vceq_s32 (minsad4_h, minsad4_l), vclt_s32 (mind4_h, mind4_l)));
 
796
            mind4_h = vbsl_s32 (mask, mind4_h, mind4_l);
 
797
            minsad4_h = vbsl_s32 (mask, minsad4_h, minsad4_l);
 
798
 
 
799
            mind4_l = vext_s32 (mind4_h,mind4_h,1);
 
800
            minsad4_l = vext_s32 (minsad4_h,minsad4_h,1);
 
801
 
 
802
            mask = vorr_u32 (vclt_s32 (minsad4_h, minsad4_l), vand_u32 (vceq_s32 (minsad4_h, minsad4_l), vclt_s32 (mind4_h, mind4_l)));
 
803
            mind4_h = vbsl_s32 (mask, mind4_h, mind4_l);
 
804
            minsad4_h = vbsl_s32 (mask, minsad4_h, minsad4_l);
 
805
 
 
806
            mind = (int) vget_lane_s32 (mind4_h, 0);
 
807
            minsad = sad[mind];
 
808
 
 
809
        #else
 
810
            for( d = 0; d < ndisp; d++ )
 
811
            {
 
812
                int currsad = sad[d] + hsad[d] - hsad_sub[d];
 
813
                sad[d] = currsad;
 
814
                if( currsad < minsad )
 
815
                {
 
816
                    minsad = currsad;
 
817
                    mind = d;
 
818
                }
 
819
            }
 
820
        #endif
 
821
 
 
822
            tsum += htext[y + wsz2] - htext[y - wsz2 - 1];
 
823
            if( tsum < textureThreshold )
 
824
            {
 
825
                dptr[y*dstep] = FILTERED;
 
826
                continue;
 
827
            }
 
828
 
 
829
            if( uniquenessRatio > 0 )
 
830
            {
 
831
                int thresh = minsad + (minsad * uniquenessRatio/100);
 
832
                for( d = 0; d < ndisp; d++ )
 
833
                {
 
834
                    if( (d < mind-1 || d > mind+1) && sad[d] <= thresh)
 
835
                        break;
 
836
                }
 
837
                if( d < ndisp )
 
838
                {
 
839
                    dptr[y*dstep] = FILTERED;
 
840
                    continue;
 
841
                }
 
842
            }
 
843
 
 
844
            {
 
845
                sad[-1] = sad[1];
 
846
                sad[ndisp] = sad[ndisp-2];
 
847
                int p = sad[mind+1], n = sad[mind-1];
 
848
                d = p + n - 2*sad[mind] + std::abs(p - n);
 
849
                dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15) >> 4);
 
850
                costptr[y*coststep] = sad[mind];
 
851
            }
 
852
        }
 
853
    }
 
854
}
 
855
 
 
856
#ifdef HAVE_OPENCL
 
857
static bool ocl_prefiltering(InputArray left0, InputArray right0, OutputArray left, OutputArray right, StereoBMParams* state)
 
858
{
 
859
    if( state->preFilterType == StereoBM::PREFILTER_NORMALIZED_RESPONSE )
 
860
    {
 
861
        if(!ocl_prefilter_norm( left0, left, state->preFilterSize, state->preFilterCap))
 
862
            return false;
 
863
        if(!ocl_prefilter_norm( right0, right, state->preFilterSize, state->preFilterCap))
 
864
            return false;
 
865
    }
 
866
    else
 
867
    {
 
868
        if(!ocl_prefilter_xsobel( left0, left, state->preFilterCap ))
 
869
            return false;
 
870
        if(!ocl_prefilter_xsobel( right0, right, state->preFilterCap))
 
871
            return false;
 
872
    }
 
873
    return true;
 
874
}
 
875
#endif
 
876
 
 
877
struct PrefilterInvoker : public ParallelLoopBody
 
878
{
 
879
    PrefilterInvoker(const Mat& left0, const Mat& right0, Mat& left, Mat& right,
 
880
                     uchar* buf0, uchar* buf1, StereoBMParams* _state)
 
881
    {
 
882
        imgs0[0] = &left0; imgs0[1] = &right0;
 
883
        imgs[0] = &left; imgs[1] = &right;
 
884
        buf[0] = buf0; buf[1] = buf1;
 
885
        state = _state;
 
886
    }
 
887
 
 
888
    void operator()( const Range& range ) const
 
889
    {
 
890
        for( int i = range.start; i < range.end; i++ )
 
891
        {
 
892
            if( state->preFilterType == StereoBM::PREFILTER_NORMALIZED_RESPONSE )
 
893
                prefilterNorm( *imgs0[i], *imgs[i], state->preFilterSize, state->preFilterCap, buf[i] );
 
894
            else
 
895
                prefilterXSobel( *imgs0[i], *imgs[i], state->preFilterCap );
 
896
        }
 
897
    }
 
898
 
 
899
    const Mat* imgs0[2];
 
900
    Mat* imgs[2];
 
901
    uchar* buf[2];
 
902
    StereoBMParams* state;
 
903
};
 
904
 
 
905
#ifdef HAVE_OPENCL
 
906
static bool ocl_stereobm( InputArray _left, InputArray _right,
 
907
                       OutputArray _disp, StereoBMParams* state)
 
908
{
 
909
    int ndisp = state->numDisparities;
 
910
    int mindisp = state->minDisparity;
 
911
    int wsz = state->SADWindowSize;
 
912
    int wsz2 = wsz/2;
 
913
 
 
914
    ocl::Device devDef = ocl::Device::getDefault();
 
915
    int sizeX = devDef.isIntel() ? 32 : std::max(11, 27 - devDef.maxComputeUnits()),
 
916
        sizeY = sizeX - 1,
 
917
        N = ndisp * 2;
 
918
 
 
919
    cv::String opt = cv::format("-D DEFINE_KERNEL_STEREOBM -D MIN_DISP=%d -D NUM_DISP=%d"
 
920
                                " -D BLOCK_SIZE_X=%d -D BLOCK_SIZE_Y=%d -D WSZ=%d",
 
921
                                mindisp, ndisp,
 
922
                                sizeX, sizeY, wsz);
 
923
    ocl::Kernel k("stereoBM", ocl::calib3d::stereobm_oclsrc, opt);
 
924
    if(k.empty())
 
925
        return false;
 
926
 
 
927
    UMat left = _left.getUMat(), right = _right.getUMat();
 
928
    int cols = left.cols, rows = left.rows;
 
929
 
 
930
    _disp.create(_left.size(), CV_16S);
 
931
    _disp.setTo((mindisp - 1) << 4);
 
932
    Rect roi = Rect(Point(wsz2 + mindisp + ndisp - 1, wsz2), Point(cols-wsz2-mindisp, rows-wsz2) );
 
933
    UMat disp = (_disp.getUMat())(roi);
 
934
 
 
935
    int globalX = (disp.cols + sizeX - 1) / sizeX,
 
936
        globalY = (disp.rows + sizeY - 1) / sizeY;
 
937
    size_t globalThreads[3] = {(size_t)N, (size_t)globalX, (size_t)globalY};
 
938
    size_t localThreads[3]  = {(size_t)N, 1, 1};
 
939
 
 
940
    int idx = 0;
 
941
    idx = k.set(idx, ocl::KernelArg::PtrReadOnly(left));
 
942
    idx = k.set(idx, ocl::KernelArg::PtrReadOnly(right));
 
943
    idx = k.set(idx, ocl::KernelArg::WriteOnlyNoSize(disp));
 
944
    idx = k.set(idx, rows);
 
945
    idx = k.set(idx, cols);
 
946
    idx = k.set(idx, state->textureThreshold);
 
947
    idx = k.set(idx, state->uniquenessRatio);
 
948
    return k.run(3, globalThreads, localThreads, false);
 
949
}
 
950
#endif
 
951
 
 
952
struct FindStereoCorrespInvoker : public ParallelLoopBody
 
953
{
 
954
    FindStereoCorrespInvoker( const Mat& _left, const Mat& _right,
 
955
                             Mat& _disp, StereoBMParams* _state,
 
956
                             int _nstripes, size_t _stripeBufSize,
 
957
                             bool _useShorts, Rect _validDisparityRect,
 
958
                             Mat& _slidingSumBuf, Mat& _cost )
 
959
    {
 
960
        left = &_left; right = &_right;
 
961
        disp = &_disp; state = _state;
 
962
        nstripes = _nstripes; stripeBufSize = _stripeBufSize;
 
963
        useShorts = _useShorts;
 
964
        validDisparityRect = _validDisparityRect;
 
965
        slidingSumBuf = &_slidingSumBuf;
 
966
        cost = &_cost;
 
967
    }
 
968
 
 
969
    void operator()( const Range& range ) const
 
970
    {
 
971
        int cols = left->cols, rows = left->rows;
 
972
        int _row0 = std::min(cvRound(range.start * rows / nstripes), rows);
 
973
        int _row1 = std::min(cvRound(range.end * rows / nstripes), rows);
 
974
        uchar *ptr = slidingSumBuf->ptr() + range.start * stripeBufSize;
 
975
        int FILTERED = (state->minDisparity - 1)*16;
 
976
 
 
977
        Rect roi = validDisparityRect & Rect(0, _row0, cols, _row1 - _row0);
 
978
        if( roi.height == 0 )
 
979
            return;
 
980
        int row0 = roi.y;
 
981
        int row1 = roi.y + roi.height;
 
982
 
 
983
        Mat part;
 
984
        if( row0 > _row0 )
 
985
        {
 
986
            part = disp->rowRange(_row0, row0);
 
987
            part = Scalar::all(FILTERED);
 
988
        }
 
989
        if( _row1 > row1 )
 
990
        {
 
991
            part = disp->rowRange(row1, _row1);
 
992
            part = Scalar::all(FILTERED);
 
993
        }
 
994
 
 
995
        Mat left_i = left->rowRange(row0, row1);
 
996
        Mat right_i = right->rowRange(row0, row1);
 
997
        Mat disp_i = disp->rowRange(row0, row1);
 
998
        Mat cost_i = state->disp12MaxDiff >= 0 ? cost->rowRange(row0, row1) : Mat();
 
999
 
 
1000
#if CV_SSE2
 
1001
        if( useShorts )
 
1002
            findStereoCorrespondenceBM_SSE2( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 );
 
1003
        else
 
1004
#endif
 
1005
            findStereoCorrespondenceBM( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 );
 
1006
 
 
1007
        if( state->disp12MaxDiff >= 0 )
 
1008
            validateDisparity( disp_i, cost_i, state->minDisparity, state->numDisparities, state->disp12MaxDiff );
 
1009
 
 
1010
        if( roi.x > 0 )
 
1011
        {
 
1012
            part = disp_i.colRange(0, roi.x);
 
1013
            part = Scalar::all(FILTERED);
 
1014
        }
 
1015
        if( roi.x + roi.width < cols )
 
1016
        {
 
1017
            part = disp_i.colRange(roi.x + roi.width, cols);
 
1018
            part = Scalar::all(FILTERED);
 
1019
        }
 
1020
    }
 
1021
 
 
1022
protected:
 
1023
    const Mat *left, *right;
 
1024
    Mat* disp, *slidingSumBuf, *cost;
 
1025
    StereoBMParams *state;
 
1026
 
 
1027
    int nstripes;
 
1028
    size_t stripeBufSize;
 
1029
    bool useShorts;
 
1030
    Rect validDisparityRect;
 
1031
};
 
1032
 
 
1033
class StereoBMImpl : public StereoBM
 
1034
{
 
1035
public:
 
1036
    StereoBMImpl()
 
1037
    {
 
1038
        params = StereoBMParams();
 
1039
    }
 
1040
 
 
1041
    StereoBMImpl( int _numDisparities, int _SADWindowSize )
 
1042
    {
 
1043
        params = StereoBMParams(_numDisparities, _SADWindowSize);
 
1044
    }
 
1045
 
 
1046
    void compute( InputArray leftarr, InputArray rightarr, OutputArray disparr )
 
1047
    {
 
1048
        int dtype = disparr.fixedType() ? disparr.type() : params.dispType;
 
1049
        Size leftsize = leftarr.size();
 
1050
 
 
1051
        if (leftarr.size() != rightarr.size())
 
1052
            CV_Error( Error::StsUnmatchedSizes, "All the images must have the same size" );
 
1053
 
 
1054
        if (leftarr.type() != CV_8UC1 || rightarr.type() != CV_8UC1)
 
1055
            CV_Error( Error::StsUnsupportedFormat, "Both input images must have CV_8UC1" );
 
1056
 
 
1057
        if (dtype != CV_16SC1 && dtype != CV_32FC1)
 
1058
            CV_Error( Error::StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format" );
 
1059
 
 
1060
        if( params.preFilterType != PREFILTER_NORMALIZED_RESPONSE &&
 
1061
            params.preFilterType != PREFILTER_XSOBEL )
 
1062
            CV_Error( Error::StsOutOfRange, "preFilterType must be = CV_STEREO_BM_NORMALIZED_RESPONSE" );
 
1063
 
 
1064
        if( params.preFilterSize < 5 || params.preFilterSize > 255 || params.preFilterSize % 2 == 0 )
 
1065
            CV_Error( Error::StsOutOfRange, "preFilterSize must be odd and be within 5..255" );
 
1066
 
 
1067
        if( params.preFilterCap < 1 || params.preFilterCap > 63 )
 
1068
            CV_Error( Error::StsOutOfRange, "preFilterCap must be within 1..63" );
 
1069
 
 
1070
        if( params.SADWindowSize < 5 || params.SADWindowSize > 255 || params.SADWindowSize % 2 == 0 ||
 
1071
            params.SADWindowSize >= std::min(leftsize.width, leftsize.height) )
 
1072
            CV_Error( Error::StsOutOfRange, "SADWindowSize must be odd, be within 5..255 and be not larger than image width or height" );
 
1073
 
 
1074
        if( params.numDisparities <= 0 || params.numDisparities % 16 != 0 )
 
1075
            CV_Error( Error::StsOutOfRange, "numDisparities must be positive and divisble by 16" );
 
1076
 
 
1077
        if( params.textureThreshold < 0 )
 
1078
            CV_Error( Error::StsOutOfRange, "texture threshold must be non-negative" );
 
1079
 
 
1080
        if( params.uniquenessRatio < 0 )
 
1081
            CV_Error( Error::StsOutOfRange, "uniqueness ratio must be non-negative" );
 
1082
 
 
1083
        int FILTERED = (params.minDisparity - 1) << DISPARITY_SHIFT;
 
1084
 
 
1085
#ifdef HAVE_OPENCL
 
1086
        if(ocl::useOpenCL() && disparr.isUMat() && params.textureThreshold == 0)
 
1087
        {
 
1088
            UMat left, right;
 
1089
            if(ocl_prefiltering(leftarr, rightarr, left, right, &params))
 
1090
            {
 
1091
                if(ocl_stereobm(left, right, disparr, &params))
 
1092
                {
 
1093
                    if( params.speckleRange >= 0 && params.speckleWindowSize > 0 )
 
1094
                        filterSpeckles(disparr.getMat(), FILTERED, params.speckleWindowSize, params.speckleRange, slidingSumBuf);
 
1095
                    if (dtype == CV_32F)
 
1096
                        disparr.getUMat().convertTo(disparr, CV_32FC1, 1./(1 << DISPARITY_SHIFT), 0);
 
1097
                    CV_IMPL_ADD(CV_IMPL_OCL);
 
1098
                    return;
 
1099
                }
 
1100
            }
 
1101
        }
 
1102
#endif
 
1103
 
 
1104
        Mat left0 = leftarr.getMat(), right0 = rightarr.getMat();
 
1105
        disparr.create(left0.size(), dtype);
 
1106
        Mat disp0 = disparr.getMat();
 
1107
 
 
1108
        preFilteredImg0.create( left0.size(), CV_8U );
 
1109
        preFilteredImg1.create( left0.size(), CV_8U );
 
1110
        cost.create( left0.size(), CV_16S );
 
1111
 
 
1112
        Mat left = preFilteredImg0, right = preFilteredImg1;
 
1113
 
 
1114
        int mindisp = params.minDisparity;
 
1115
        int ndisp = params.numDisparities;
 
1116
 
 
1117
        int width = left0.cols;
 
1118
        int height = left0.rows;
 
1119
        int lofs = std::max(ndisp - 1 + mindisp, 0);
 
1120
        int rofs = -std::min(ndisp - 1 + mindisp, 0);
 
1121
        int width1 = width - rofs - ndisp + 1;
 
1122
 
 
1123
        if( lofs >= width || rofs >= width || width1 < 1 )
 
1124
        {
 
1125
            disp0 = Scalar::all( FILTERED * ( disp0.type() < CV_32F ? 1 : 1./(1 << DISPARITY_SHIFT) ) );
 
1126
            return;
 
1127
        }
 
1128
 
 
1129
        Mat disp = disp0;
 
1130
        if( dtype == CV_32F )
 
1131
        {
 
1132
            dispbuf.create(disp0.size(), CV_16S);
 
1133
            disp = dispbuf;
 
1134
        }
 
1135
 
 
1136
        int wsz = params.SADWindowSize;
 
1137
        int bufSize0 = (int)((ndisp + 2)*sizeof(int));
 
1138
        bufSize0 += (int)((height+wsz+2)*ndisp*sizeof(int));
 
1139
        bufSize0 += (int)((height + wsz + 2)*sizeof(int));
 
1140
        bufSize0 += (int)((height+wsz+2)*ndisp*(wsz+2)*sizeof(uchar) + 256);
 
1141
 
 
1142
        int bufSize1 = (int)((width + params.preFilterSize + 2) * sizeof(int) + 256);
 
1143
        int bufSize2 = 0;
 
1144
        if( params.speckleRange >= 0 && params.speckleWindowSize > 0 )
 
1145
            bufSize2 = width*height*(sizeof(Point_<short>) + sizeof(int) + sizeof(uchar));
 
1146
 
 
1147
#if CV_SSE2
 
1148
        bool useShorts = params.preFilterCap <= 31 && params.SADWindowSize <= 21 && checkHardwareSupport(CV_CPU_SSE2);
 
1149
#else
 
1150
        const bool useShorts = false;
 
1151
#endif
 
1152
 
 
1153
        const double SAD_overhead_coeff = 10.0;
 
1154
        double N0 = 8000000 / (useShorts ? 1 : 4);  // approx tbb's min number instructions reasonable for one thread
 
1155
        double maxStripeSize = std::min(std::max(N0 / (width * ndisp), (wsz-1) * SAD_overhead_coeff), (double)height);
 
1156
        int nstripes = cvCeil(height / maxStripeSize);
 
1157
        int bufSize = std::max(bufSize0 * nstripes, std::max(bufSize1 * 2, bufSize2));
 
1158
 
 
1159
        if( slidingSumBuf.cols < bufSize )
 
1160
            slidingSumBuf.create( 1, bufSize, CV_8U );
 
1161
 
 
1162
        uchar *_buf = slidingSumBuf.ptr();
 
1163
 
 
1164
        parallel_for_(Range(0, 2), PrefilterInvoker(left0, right0, left, right, _buf, _buf + bufSize1, &params), 1);
 
1165
 
 
1166
        Rect validDisparityRect(0, 0, width, height), R1 = params.roi1, R2 = params.roi2;
 
1167
        validDisparityRect = getValidDisparityROI(R1.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect,
 
1168
                                                  R2.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect,
 
1169
                                                  params.minDisparity, params.numDisparities,
 
1170
                                                  params.SADWindowSize);
 
1171
 
 
1172
        parallel_for_(Range(0, nstripes),
 
1173
                      FindStereoCorrespInvoker(left, right, disp, &params, nstripes,
 
1174
                                               bufSize0, useShorts, validDisparityRect,
 
1175
                                               slidingSumBuf, cost));
 
1176
 
 
1177
        if( params.speckleRange >= 0 && params.speckleWindowSize > 0 )
 
1178
            filterSpeckles(disp, FILTERED, params.speckleWindowSize, params.speckleRange, slidingSumBuf);
 
1179
 
 
1180
        if (disp0.data != disp.data)
 
1181
            disp.convertTo(disp0, disp0.type(), 1./(1 << DISPARITY_SHIFT), 0);
 
1182
    }
 
1183
 
 
1184
    int getMinDisparity() const { return params.minDisparity; }
 
1185
    void setMinDisparity(int minDisparity) { params.minDisparity = minDisparity; }
 
1186
 
 
1187
    int getNumDisparities() const { return params.numDisparities; }
 
1188
    void setNumDisparities(int numDisparities) { params.numDisparities = numDisparities; }
 
1189
 
 
1190
    int getBlockSize() const { return params.SADWindowSize; }
 
1191
    void setBlockSize(int blockSize) { params.SADWindowSize = blockSize; }
 
1192
 
 
1193
    int getSpeckleWindowSize() const { return params.speckleWindowSize; }
 
1194
    void setSpeckleWindowSize(int speckleWindowSize) { params.speckleWindowSize = speckleWindowSize; }
 
1195
 
 
1196
    int getSpeckleRange() const { return params.speckleRange; }
 
1197
    void setSpeckleRange(int speckleRange) { params.speckleRange = speckleRange; }
 
1198
 
 
1199
    int getDisp12MaxDiff() const { return params.disp12MaxDiff; }
 
1200
    void setDisp12MaxDiff(int disp12MaxDiff) { params.disp12MaxDiff = disp12MaxDiff; }
 
1201
 
 
1202
    int getPreFilterType() const { return params.preFilterType; }
 
1203
    void setPreFilterType(int preFilterType) { params.preFilterType = preFilterType; }
 
1204
 
 
1205
    int getPreFilterSize() const { return params.preFilterSize; }
 
1206
    void setPreFilterSize(int preFilterSize) { params.preFilterSize = preFilterSize; }
 
1207
 
 
1208
    int getPreFilterCap() const { return params.preFilterCap; }
 
1209
    void setPreFilterCap(int preFilterCap) { params.preFilterCap = preFilterCap; }
 
1210
 
 
1211
    int getTextureThreshold() const { return params.textureThreshold; }
 
1212
    void setTextureThreshold(int textureThreshold) { params.textureThreshold = textureThreshold; }
 
1213
 
 
1214
    int getUniquenessRatio() const { return params.uniquenessRatio; }
 
1215
    void setUniquenessRatio(int uniquenessRatio) { params.uniquenessRatio = uniquenessRatio; }
 
1216
 
 
1217
    int getSmallerBlockSize() const { return 0; }
 
1218
    void setSmallerBlockSize(int) {}
 
1219
 
 
1220
    Rect getROI1() const { return params.roi1; }
 
1221
    void setROI1(Rect roi1) { params.roi1 = roi1; }
 
1222
 
 
1223
    Rect getROI2() const { return params.roi2; }
 
1224
    void setROI2(Rect roi2) { params.roi2 = roi2; }
 
1225
 
 
1226
    void write(FileStorage& fs) const
 
1227
    {
 
1228
        fs << "name" << name_
 
1229
        << "minDisparity" << params.minDisparity
 
1230
        << "numDisparities" << params.numDisparities
 
1231
        << "blockSize" << params.SADWindowSize
 
1232
        << "speckleWindowSize" << params.speckleWindowSize
 
1233
        << "speckleRange" << params.speckleRange
 
1234
        << "disp12MaxDiff" << params.disp12MaxDiff
 
1235
        << "preFilterType" << params.preFilterType
 
1236
        << "preFilterSize" << params.preFilterSize
 
1237
        << "preFilterCap" << params.preFilterCap
 
1238
        << "textureThreshold" << params.textureThreshold
 
1239
        << "uniquenessRatio" << params.uniquenessRatio;
 
1240
    }
 
1241
 
 
1242
    void read(const FileNode& fn)
 
1243
    {
 
1244
        FileNode n = fn["name"];
 
1245
        CV_Assert( n.isString() && String(n) == name_ );
 
1246
        params.minDisparity = (int)fn["minDisparity"];
 
1247
        params.numDisparities = (int)fn["numDisparities"];
 
1248
        params.SADWindowSize = (int)fn["blockSize"];
 
1249
        params.speckleWindowSize = (int)fn["speckleWindowSize"];
 
1250
        params.speckleRange = (int)fn["speckleRange"];
 
1251
        params.disp12MaxDiff = (int)fn["disp12MaxDiff"];
 
1252
        params.preFilterType = (int)fn["preFilterType"];
 
1253
        params.preFilterSize = (int)fn["preFilterSize"];
 
1254
        params.preFilterCap = (int)fn["preFilterCap"];
 
1255
        params.textureThreshold = (int)fn["textureThreshold"];
 
1256
        params.uniquenessRatio = (int)fn["uniquenessRatio"];
 
1257
        params.roi1 = params.roi2 = Rect();
 
1258
    }
 
1259
 
 
1260
    StereoBMParams params;
 
1261
    Mat preFilteredImg0, preFilteredImg1, cost, dispbuf;
 
1262
    Mat slidingSumBuf;
 
1263
 
 
1264
    static const char* name_;
 
1265
};
 
1266
 
 
1267
const char* StereoBMImpl::name_ = "StereoMatcher.BM";
 
1268
 
 
1269
Ptr<StereoBM> StereoBM::create(int _numDisparities, int _SADWindowSize)
 
1270
{
 
1271
    return makePtr<StereoBMImpl>(_numDisparities, _SADWindowSize);
 
1272
}
 
1273
 
 
1274
}
 
1275
 
 
1276
/* End of file. */