mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-11-29 05:29:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #8061 from tomoaki0705:universalIntrinsicStereoCalib3d

Browse Source
This commit is contained in:
Alexander Alekhin 2017-01-25 16:08:02 +00:00
commit 3413c23b34
1 changed files with 265 additions and 269 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

534
modules/calib3d/src/stereobm.cpp
View File

@ -49,6 +49,7 @@
#include <stdio.h> #include <stdio.h>
#include <limits> #include <limits>
#include "opencl_kernels_calib3d.hpp" #include "opencl_kernels_calib3d.hpp"
#include "opencv2/core/hal/intrin.hpp"
namespace cv namespace cv
{ {
@ -203,8 +204,8 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero )
tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero*2 : x - OFS + ftzero); tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero*2 : x - OFS + ftzero);
uchar val0 = tab[0 + OFS]; uchar val0 = tab[0 + OFS];
#if CV_SSE2 #if CV_SIMD128
volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE2); bool useSIMD = hasSIMD128();
#endif #endif
for( y = 0; y < size.height-1; y += 2 ) for( y = 0; y < size.height-1; y += 2 )
@ -219,71 +220,34 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero )
dptr0[0] = dptr0[size.width-1] = dptr1[0] = dptr1[size.width-1] = val0; dptr0[0] = dptr0[size.width-1] = dptr1[0] = dptr1[size.width-1] = val0;
x = 1; x = 1;
#if CV_NEON #if CV_SIMD128
int16x8_t ftz = vdupq_n_s16 ((short) ftzero);
uint8x8_t ftz2 = vdup_n_u8 (cv::saturate_cast<uchar>(ftzero*2));
for(; x <=size.width-9; x += 8 )
{
uint8x8_t c0 = vld1_u8 (srow0 + x - 1);
uint8x8_t c1 = vld1_u8 (srow1 + x - 1);
uint8x8_t d0 = vld1_u8 (srow0 + x + 1);
uint8x8_t d1 = vld1_u8 (srow1 + x + 1);
int16x8_t t0 = vreinterpretq_s16_u16 (vsubl_u8 (d0, c0));
int16x8_t t1 = vreinterpretq_s16_u16 (vsubl_u8 (d1, c1));
uint8x8_t c2 = vld1_u8 (srow2 + x - 1);
uint8x8_t c3 = vld1_u8 (srow3 + x - 1);
uint8x8_t d2 = vld1_u8 (srow2 + x + 1);
uint8x8_t d3 = vld1_u8 (srow3 + x + 1);
int16x8_t t2 = vreinterpretq_s16_u16 (vsubl_u8 (d2, c2));
int16x8_t t3 = vreinterpretq_s16_u16 (vsubl_u8 (d3, c3));
int16x8_t v0 = vaddq_s16 (vaddq_s16 (t2, t0), vaddq_s16 (t1, t1));
int16x8_t v1 = vaddq_s16 (vaddq_s16 (t3, t1), vaddq_s16 (t2, t2));
uint8x8_t v0_u8 = vqmovun_s16 (vaddq_s16 (v0, ftz));
uint8x8_t v1_u8 = vqmovun_s16 (vaddq_s16 (v1, ftz));
v0_u8 = vmin_u8 (v0_u8, ftz2);
v1_u8 = vmin_u8 (v1_u8, ftz2);
vqmovun_s16 (vaddq_s16 (v1, ftz));
vst1_u8 (dptr0 + x, v0_u8);
vst1_u8 (dptr1 + x, v1_u8);
}
#elif CV_SSE2
if( useSIMD ) if( useSIMD )
{ {
__m128i z = _mm_setzero_si128(), ftz = _mm_set1_epi16((short)ftzero), v_int16x8 ftz = v_setall_s16((short) ftzero);
ftz2 = _mm_set1_epi8(cv::saturate_cast<uchar>(ftzero*2)); v_int16x8 ftz2 = v_setall_s16((short)(ftzero*2));
for( ; x <= size.width-9; x += 8 ) v_int16x8 z = v_setzero_s16();
for(; x <= size.width-8; x += 8 )
{ {
__m128i c0 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow0 + x - 1)), z); v_int16x8 s00 = v_reinterpret_as_s16(v_load_expand(srow0 + x + 1));
__m128i c1 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow1 + x - 1)), z); v_int16x8 s01 = v_reinterpret_as_s16(v_load_expand(srow0 + x - 1));
__m128i d0 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow0 + x + 1)), z); v_int16x8 s10 = v_reinterpret_as_s16(v_load_expand(srow1 + x + 1));
__m128i d1 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow1 + x + 1)), z); v_int16x8 s11 = v_reinterpret_as_s16(v_load_expand(srow1 + x - 1));
v_int16x8 s20 = v_reinterpret_as_s16(v_load_expand(srow2 + x + 1));
v_int16x8 s21 = v_reinterpret_as_s16(v_load_expand(srow2 + x - 1));
v_int16x8 s30 = v_reinterpret_as_s16(v_load_expand(srow3 + x + 1));
v_int16x8 s31 = v_reinterpret_as_s16(v_load_expand(srow3 + x - 1));
d0 = _mm_sub_epi16(d0, c0); v_int16x8 d0 = s00 - s01;
d1 = _mm_sub_epi16(d1, c1); v_int16x8 d1 = s10 - s11;
v_int16x8 d2 = s20 - s21;
v_int16x8 d3 = s30 - s31;
__m128i c2 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x - 1)), z); v_uint16x8 v0 = v_reinterpret_as_u16(v_max(v_min(d0 + d1 + d1 + d2 + ftz, ftz2), z));
__m128i c3 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow3 + x - 1)), z); v_uint16x8 v1 = v_reinterpret_as_u16(v_max(v_min(d1 + d2 + d2 + d3 + ftz, ftz2), z));
__m128i d2 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x + 1)), z);
__m128i d3 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow3 + x + 1)), z);
d2 = _mm_sub_epi16(d2, c2); v_pack_store(dptr0 + x, v0);
d3 = _mm_sub_epi16(d3, c3); v_pack_store(dptr1 + x, v1);
__m128i v0 = _mm_add_epi16(d0, _mm_add_epi16(d2, _mm_add_epi16(d1, d1)));
__m128i v1 = _mm_add_epi16(d1, _mm_add_epi16(d3, _mm_add_epi16(d2, d2)));
v0 = _mm_packus_epi16(_mm_add_epi16(v0, ftz), _mm_add_epi16(v1, ftz));
v0 = _mm_min_epu8(v0, ftz2);
_mm_storel_epi64((__m128i*)(dptr0 + x), v0);
_mm_storel_epi64((__m128i*)(dptr1 + x), _mm_unpackhi_epi64(v0, v0));
} }
} }
#endif #endif
@ -299,18 +263,18 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero )
} }
} }
#if CV_NEON
uint8x16_t val0_16 = vdupq_n_u8 (val0);
#endif
for( ; y < size.height; y++ ) for( ; y < size.height; y++ )
{ {
uchar* dptr = dst.ptr<uchar>(y); uchar* dptr = dst.ptr<uchar>(y);
x = 0; x = 0;
#if CV_NEON #if CV_SIMD128
for(; x <= size.width-16; x+=16 ) if( useSIMD )
vst1q_u8 (dptr + x, val0_16); {
#endif v_uint8x16 val0_16 = v_setall_u8(val0);
for(; x <= size.width-16; x+=16 )
v_store(dptr + x, val0_16);
}
#endif
for(; x < size.width; x++ ) for(; x < size.width; x++ )
dptr[x] = val0; dptr[x] = val0;
} }
@ -320,8 +284,8 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero )
static const int DISPARITY_SHIFT_16S = 4; static const int DISPARITY_SHIFT_16S = 4;
static const int DISPARITY_SHIFT_32S = 8; static const int DISPARITY_SHIFT_32S = 8;
#if CV_SSE2 #if CV_SIMD128
static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right, static void findStereoCorrespondenceBM_SIMD( const Mat& left, const Mat& right,
Mat& disp, Mat& cost, StereoBMParams& state, Mat& disp, Mat& cost, StereoBMParams& state,
uchar* buf, int _dy0, int _dy1 ) uchar* buf, int _dy0, int _dy1 )
{ {
@ -354,7 +318,7 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
int coststep = cost.data ? (int)(cost.step/sizeof(costbuf)) : 0; int coststep = cost.data ? (int)(cost.step/sizeof(costbuf)) : 0;
const int TABSZ = 256; const int TABSZ = 256;
uchar tab[TABSZ]; uchar tab[TABSZ];
const __m128i d0_8 = _mm_setr_epi16(0,1,2,3,4,5,6,7), dd_8 = _mm_set1_epi16(8); const v_int16x8 d0_8 = v_int16x8(0,1,2,3,4,5,6,7), dd_8 = v_setall_s16(8);
sad = (ushort*)alignPtr(buf + sizeof(sad[0]), ALIGN); sad = (ushort*)alignPtr(buf + sizeof(sad[0]), ALIGN);
hsad0 = (ushort*)alignPtr(sad + ndisp + 1 + dy0*ndisp, ALIGN); hsad0 = (ushort*)alignPtr(sad + ndisp + 1 + dy0*ndisp, ALIGN);
@ -377,18 +341,20 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep ) for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )
{ {
int lval = lptr[0]; int lval = lptr[0];
__m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128(); v_uint8x16 lv = v_setall_u8((uchar)lval);
for( d = 0; d < ndisp; d += 16 ) for( d = 0; d < ndisp; d += 16 )
{ {
__m128i rv = _mm_loadu_si128((const __m128i*)(rptr + d)); v_uint8x16 rv = v_load(rptr + d);
__m128i hsad_l = _mm_load_si128((__m128i*)(hsad + d)); v_uint16x8 hsad_l = v_load(hsad + d);
__m128i hsad_h = _mm_load_si128((__m128i*)(hsad + d + 8)); v_uint16x8 hsad_h = v_load(hsad + d + 8);
__m128i diff = _mm_adds_epu8(_mm_subs_epu8(lv, rv), _mm_subs_epu8(rv, lv)); v_uint8x16 diff = v_absdiff(lv, rv);
_mm_store_si128((__m128i*)(cbuf + d), diff); v_store(cbuf + d, diff);
hsad_l = _mm_add_epi16(hsad_l, _mm_unpacklo_epi8(diff,z)); v_uint16x8 diff0, diff1;
hsad_h = _mm_add_epi16(hsad_h, _mm_unpackhi_epi8(diff,z)); v_expand(diff, diff0, diff1);
_mm_store_si128((__m128i*)(hsad + d), hsad_l); hsad_l += diff0;
_mm_store_si128((__m128i*)(hsad + d + 8), hsad_h); hsad_h += diff1;
v_store(hsad + d, hsad_l);
v_store(hsad + d + 8, hsad_h);
} }
htext[y] += tab[lval]; htext[y] += tab[lval];
} }
@ -419,21 +385,24 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep ) hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )
{ {
int lval = lptr[0]; int lval = lptr[0];
__m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128(); v_uint8x16 lv = v_setall_u8((uchar)lval);
for( d = 0; d < ndisp; d += 16 ) for( d = 0; d < ndisp; d += 16 )
{ {
__m128i rv = _mm_loadu_si128((const __m128i*)(rptr + d)); v_uint8x16 rv = v_load(rptr + d);
__m128i hsad_l = _mm_load_si128((__m128i*)(hsad + d)); v_uint16x8 hsad_l = v_load(hsad + d);
__m128i hsad_h = _mm_load_si128((__m128i*)(hsad + d + 8)); v_uint16x8 hsad_h = v_load(hsad + d + 8);
__m128i cbs = _mm_load_si128((const __m128i*)(cbuf_sub + d)); v_uint8x16 cbs = v_load(cbuf_sub + d);
__m128i diff = _mm_adds_epu8(_mm_subs_epu8(lv, rv), _mm_subs_epu8(rv, lv)); v_uint8x16 diff = v_absdiff(lv, rv);
__m128i diff_h = _mm_sub_epi16(_mm_unpackhi_epi8(diff, z), _mm_unpackhi_epi8(cbs, z)); v_int16x8 diff_l, diff_h, cbs_l, cbs_h;
_mm_store_si128((__m128i*)(cbuf + d), diff); v_store(cbuf + d, diff);
diff = _mm_sub_epi16(_mm_unpacklo_epi8(diff, z), _mm_unpacklo_epi8(cbs, z)); v_expand(v_reinterpret_as_s8(diff), diff_l, diff_h);
hsad_h = _mm_add_epi16(hsad_h, diff_h); v_expand(v_reinterpret_as_s8(cbs), cbs_l, cbs_h);
hsad_l = _mm_add_epi16(hsad_l, diff); diff_l -= cbs_l;
_mm_store_si128((__m128i*)(hsad + d), hsad_l); diff_h -= cbs_h;
_mm_store_si128((__m128i*)(hsad + d + 8), hsad_h); hsad_h = v_reinterpret_as_u16(v_reinterpret_as_s16(hsad_h) + diff_h);
hsad_l = v_reinterpret_as_u16(v_reinterpret_as_s16(hsad_l) + diff_l);
v_store(hsad + d, hsad_l);
v_store(hsad + d + 8, hsad_h);
} }
htext[y] += tab[lval] - tab[lptr_sub[0]]; htext[y] += tab[lval] - tab[lptr_sub[0]];
} }
@ -450,16 +419,16 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
hsad = hsad0 + (1 - dy0)*ndisp; hsad = hsad0 + (1 - dy0)*ndisp;
for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp ) for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )
for( d = 0; d < ndisp; d += 16 ) for( d = 0; d <= ndisp-16; d += 16 )
{ {
__m128i s0 = _mm_load_si128((__m128i*)(sad + d)); v_uint16x8 s0 = v_load(sad + d);
__m128i s1 = _mm_load_si128((__m128i*)(sad + d + 8)); v_uint16x8 s1 = v_load(sad + d + 8);
__m128i t0 = _mm_load_si128((__m128i*)(hsad + d)); v_uint16x8 t0 = v_load(hsad + d);
__m128i t1 = _mm_load_si128((__m128i*)(hsad + d + 8)); v_uint16x8 t1 = v_load(hsad + d + 8);
s0 = _mm_add_epi16(s0, t0); s0 = s0 + t0;
s1 = _mm_add_epi16(s1, t1); s1 = s1 + t1;
_mm_store_si128((__m128i*)(sad + d), s0); v_store(sad + d, s0);
_mm_store_si128((__m128i*)(sad + d + 8), s1); v_store(sad + d + 8, s1);
} }
int tsum = 0; int tsum = 0;
for( y = -wsz2-1; y < wsz2; y++ ) for( y = -wsz2-1; y < wsz2; y++ )
@ -471,38 +440,38 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
int minsad = INT_MAX, mind = -1; int minsad = INT_MAX, mind = -1;
hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp; hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;
hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp; hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;
__m128i minsad8 = _mm_set1_epi16(SHRT_MAX); v_int16x8 minsad8 = v_setall_s16(SHRT_MAX);
__m128i mind8 = _mm_set1_epi16(0), d8 = d0_8, mask; v_int16x8 mind8 = v_setall_s16(0), d8 = d0_8;
for( d = 0; d < ndisp; d += 16 ) for( d = 0; d < ndisp; d += 16 )
{ {
__m128i u0 = _mm_load_si128((__m128i*)(hsad_sub + d)); v_int16x8 u0 = v_reinterpret_as_s16(v_load(hsad_sub + d));
__m128i u1 = _mm_load_si128((__m128i*)(hsad + d)); v_int16x8 u1 = v_reinterpret_as_s16(v_load(hsad + d));
__m128i v0 = _mm_load_si128((__m128i*)(hsad_sub + d + 8)); v_int16x8 v0 = v_reinterpret_as_s16(v_load(hsad_sub + d + 8));
__m128i v1 = _mm_load_si128((__m128i*)(hsad + d + 8)); v_int16x8 v1 = v_reinterpret_as_s16(v_load(hsad + d + 8));
__m128i usad8 = _mm_load_si128((__m128i*)(sad + d)); v_int16x8 usad8 = v_reinterpret_as_s16(v_load(sad + d));
__m128i vsad8 = _mm_load_si128((__m128i*)(sad + d + 8)); v_int16x8 vsad8 = v_reinterpret_as_s16(v_load(sad + d + 8));
u1 = _mm_sub_epi16(u1, u0); u1 -= u0;
v1 = _mm_sub_epi16(v1, v0); v1 -= v0;
usad8 = _mm_add_epi16(usad8, u1); usad8 += u1;
vsad8 = _mm_add_epi16(vsad8, v1); vsad8 += v1;
mask = _mm_cmpgt_epi16(minsad8, usad8); v_int16x8 mask = minsad8 > usad8;
minsad8 = _mm_min_epi16(minsad8, usad8); minsad8 = v_min(minsad8, usad8);
mind8 = _mm_max_epi16(mind8, _mm_and_si128(mask, d8)); mind8 = v_max(mind8, (mask& d8));
_mm_store_si128((__m128i*)(sad + d), usad8); v_store(sad + d, v_reinterpret_as_u16(usad8));
_mm_store_si128((__m128i*)(sad + d + 8), vsad8); v_store(sad + d + 8, v_reinterpret_as_u16(vsad8));
mask = _mm_cmpgt_epi16(minsad8, vsad8); mask = minsad8 > vsad8;
minsad8 = _mm_min_epi16(minsad8, vsad8); minsad8 = v_min(minsad8, vsad8);
d8 = _mm_add_epi16(d8, dd_8); d8 = d8 + dd_8;
mind8 = _mm_max_epi16(mind8, _mm_and_si128(mask, d8)); mind8 = v_max(mind8, (mask & d8));
d8 = _mm_add_epi16(d8, dd_8); d8 = d8 + dd_8;
} }
tsum += htext[y + wsz2] - htext[y - wsz2 - 1]; tsum += htext[y + wsz2] - htext[y - wsz2 - 1];
@ -513,8 +482,8 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
} }
ushort CV_DECL_ALIGNED(16) minsad_buf[8], mind_buf[8]; ushort CV_DECL_ALIGNED(16) minsad_buf[8], mind_buf[8];
_mm_store_si128((__m128i*)minsad_buf, minsad8); v_store(minsad_buf, v_reinterpret_as_u16(minsad8));
_mm_store_si128((__m128i*)mind_buf, mind8); v_store(mind_buf, v_reinterpret_as_u16(mind8));
for( d = 0; d < 8; d++ ) for( d = 0; d < 8; d++ )
if(minsad > (int)minsad_buf[d] || (minsad == (int)minsad_buf[d] && mind > mind_buf[d])) if(minsad > (int)minsad_buf[d] || (minsad == (int)minsad_buf[d] && mind > mind_buf[d]))
{ {
@ -525,27 +494,27 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
if( uniquenessRatio > 0 ) if( uniquenessRatio > 0 )
{ {
int thresh = minsad + (minsad * uniquenessRatio/100); int thresh = minsad + (minsad * uniquenessRatio/100);
__m128i thresh4 = _mm_set1_epi32(thresh + 1); v_int32x4 thresh4 = v_setall_s32(thresh + 1);
__m128i d1 = _mm_set1_epi32(mind-1), d2 = _mm_set1_epi32(mind+1); v_int32x4 d1 = v_setall_s32(mind-1), d2 = v_setall_s32(mind+1);
__m128i dd_4 = _mm_set1_epi32(4); v_int32x4 dd_4 = v_setall_s32(4);
__m128i d4 = _mm_set_epi32(3,2,1,0); v_int32x4 d4 = v_int32x4(0,1,2,3);
__m128i z = _mm_setzero_si128(); v_int32x4 mask4;
for( d = 0; d < ndisp; d += 8 ) for( d = 0; d < ndisp; d += 8 )
{ {
__m128i usad4 = _mm_loadu_si128((__m128i*)(sad + d)); v_int16x8 sad8 = v_reinterpret_as_s16(v_load(sad + d));
__m128i vsad4 = _mm_unpackhi_epi16(usad4, z); v_int32x4 sad4_l, sad4_h;
usad4 = _mm_unpacklo_epi16(usad4, z); v_expand(sad8, sad4_l, sad4_h);
mask = _mm_cmpgt_epi32( thresh4, usad4); mask4 = thresh4 > sad4_l;
mask = _mm_and_si128(mask, _mm_or_si128(_mm_cmpgt_epi32(d1,d4), _mm_cmpgt_epi32(d4,d2))); mask4 = mask4 & ((d1 > d4) | (d4 > d2));
if( _mm_movemask_epi8(mask) ) if( v_signmask(mask4) )
break; break;
d4 = _mm_add_epi16(d4, dd_4); d4 += dd_4;
mask = _mm_cmpgt_epi32( thresh4, vsad4); mask4 = thresh4 > sad4_h;
mask = _mm_and_si128(mask, _mm_or_si128(_mm_cmpgt_epi32(d1,d4), _mm_cmpgt_epi32(d4,d2))); mask4 = mask4 & ((d1 > d4) | (d4 > d2));
if( _mm_movemask_epi8(mask) ) if( v_signmask(mask4) )
break; break;
d4 = _mm_add_epi16(d4, dd_4); d4 += dd_4;
} }
if( d < ndisp ) if( d < ndisp )
{ {
@ -570,7 +539,7 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
template <typename mType> template <typename mType>
static void static void
findStereoCorrespondenceBM_( const Mat& left, const Mat& right, findStereoCorrespondenceBM( const Mat& left, const Mat& right,
Mat& disp, Mat& cost, const StereoBMParams& state, Mat& disp, Mat& cost, const StereoBMParams& state,
uchar* buf, int _dy0, int _dy1, const int disp_shift ) uchar* buf, int _dy0, int _dy1, const int disp_shift )
{ {
@ -590,13 +559,12 @@ findStereoCorrespondenceBM_( const Mat& left, const Mat& right,
int uniquenessRatio = state.uniquenessRatio; int uniquenessRatio = state.uniquenessRatio;
mType FILTERED = (mType)((mindisp - 1) << disp_shift); mType FILTERED = (mType)((mindisp - 1) << disp_shift);
#if CV_NEON #if CV_SIMD128
CV_Assert (ndisp % 8 == 0); bool useSIMD = hasSIMD128();
int32_t d0_4_temp [4]; if( useSIMD )
for (int i = 0; i < 4; i ++) {
d0_4_temp[i] = i; CV_Assert (ndisp % 8 == 0);
int32x4_t d0_4 = vld1q_s32 (d0_4_temp); }
int32x4_t dd_4 = vdupq_n_s32 (4);
#endif #endif
int *sad, *hsad0, *hsad, *hsad_sub, *htext; int *sad, *hsad0, *hsad, *hsad_sub, *htext;
@ -633,29 +601,46 @@ findStereoCorrespondenceBM_( const Mat& left, const Mat& right,
for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep ) for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )
{ {
int lval = lptr[0]; int lval = lptr[0];
#if CV_NEON d = 0;
int16x8_t lv = vdupq_n_s16 ((int16_t)lval); #if CV_SIMD128
if( useSIMD )
for( d = 0; d < ndisp; d += 8 )
{ {
int16x8_t rv = vreinterpretq_s16_u16 (vmovl_u8 (vld1_u8 (rptr + d))); v_uint8x16 lv = v_setall_u8((uchar)lval);
int32x4_t hsad_l = vld1q_s32 (hsad + d);
int32x4_t hsad_h = vld1q_s32 (hsad + d + 4); for( ; d <= ndisp - 16; d += 16 )
int16x8_t diff = vabdq_s16 (lv, rv); {
vst1_u8 (cbuf + d, vmovn_u16(vreinterpretq_u16_s16(diff))); v_uint8x16 rv = v_load(rptr + d);
hsad_l = vaddq_s32 (hsad_l, vmovl_s16(vget_low_s16 (diff))); v_int32x4 hsad_0 = v_load(hsad + d);
hsad_h = vaddq_s32 (hsad_h, vmovl_s16(vget_high_s16 (diff))); v_int32x4 hsad_1 = v_load(hsad + d + 4);
vst1q_s32 ((hsad + d), hsad_l); v_int32x4 hsad_2 = v_load(hsad + d + 8);
vst1q_s32 ((hsad + d + 4), hsad_h); v_int32x4 hsad_3 = v_load(hsad + d + 12);
v_uint8x16 diff = v_absdiff(lv, rv);
v_store(cbuf + d, diff);
v_uint16x8 diff0, diff1;
v_uint32x4 diff00, diff01, diff10, diff11;
v_expand(diff, diff0, diff1);
v_expand(diff0, diff00, diff01);
v_expand(diff1, diff10, diff11);
hsad_0 += v_reinterpret_as_s32(diff00);
hsad_1 += v_reinterpret_as_s32(diff01);
hsad_2 += v_reinterpret_as_s32(diff10);
hsad_3 += v_reinterpret_as_s32(diff11);
v_store(hsad + d, hsad_0);
v_store(hsad + d + 4, hsad_1);
v_store(hsad + d + 8, hsad_2);
v_store(hsad + d + 12, hsad_3);
}
} }
#else #endif
for( d = 0; d < ndisp; d++ ) for( ; d < ndisp; d++ )
{ {
int diff = std::abs(lval - rptr[d]); int diff = std::abs(lval - rptr[d]);
cbuf[d] = (uchar)diff; cbuf[d] = (uchar)diff;
hsad[d] = (int)(hsad[d] + diff); hsad[d] = (int)(hsad[d] + diff);
} }
#endif
htext[y] += tab[lval]; htext[y] += tab[lval];
} }
} }
@ -685,31 +670,53 @@ findStereoCorrespondenceBM_( const Mat& left, const Mat& right,
hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep ) hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )
{ {
int lval = lptr[0]; int lval = lptr[0];
#if CV_NEON d = 0;
int16x8_t lv = vdupq_n_s16 ((int16_t)lval); #if CV_SIMD128
for( d = 0; d < ndisp; d += 8 ) if( useSIMD )
{ {
int16x8_t rv = vreinterpretq_s16_u16 (vmovl_u8 (vld1_u8 (rptr + d))); v_uint8x16 lv = v_setall_u8((uchar)lval);
int32x4_t hsad_l = vld1q_s32 (hsad + d); for( ; d <= ndisp - 16; d += 16 )
int32x4_t hsad_h = vld1q_s32 (hsad + d + 4); {
int16x8_t cbs = vreinterpretq_s16_u16 (vmovl_u8 (vld1_u8 (cbuf_sub + d))); v_uint8x16 rv = v_load(rptr + d);
int16x8_t diff = vabdq_s16 (lv, rv); v_int32x4 hsad_0 = v_load(hsad + d);
int32x4_t diff_h = vsubl_s16 (vget_high_s16 (diff), vget_high_s16 (cbs)); v_int32x4 hsad_1 = v_load(hsad + d + 4);
int32x4_t diff_l = vsubl_s16 (vget_low_s16 (diff), vget_low_s16 (cbs)); v_int32x4 hsad_2 = v_load(hsad + d + 8);
vst1_u8 (cbuf + d, vmovn_u16(vreinterpretq_u16_s16(diff))); v_int32x4 hsad_3 = v_load(hsad + d + 12);
hsad_h = vaddq_s32 (hsad_h, diff_h); v_uint8x16 cbs = v_load(cbuf_sub + d);
hsad_l = vaddq_s32 (hsad_l, diff_l); v_uint8x16 diff = v_absdiff(lv, rv);
vst1q_s32 ((hsad + d), hsad_l); v_store(cbuf + d, diff);
vst1q_s32 ((hsad + d + 4), hsad_h);
v_uint16x8 diff0, diff1, cbs0, cbs1;
v_int32x4 diff00, diff01, diff10, diff11, cbs00, cbs01, cbs10, cbs11;
v_expand(diff, diff0, diff1);
v_expand(cbs, cbs0, cbs1);
v_expand(v_reinterpret_as_s16(diff0), diff00, diff01);
v_expand(v_reinterpret_as_s16(diff1), diff10, diff11);
v_expand(v_reinterpret_as_s16(cbs0), cbs00, cbs01);
v_expand(v_reinterpret_as_s16(cbs1), cbs10, cbs11);
v_int32x4 diff_0 = diff00 - cbs00;
v_int32x4 diff_1 = diff01 - cbs01;
v_int32x4 diff_2 = diff10 - cbs10;
v_int32x4 diff_3 = diff11 - cbs11;
hsad_0 += diff_0;
hsad_1 += diff_1;
hsad_2 += diff_2;
hsad_3 += diff_3;
v_store(hsad + d, hsad_0);
v_store(hsad + d + 4, hsad_1);
v_store(hsad + d + 8, hsad_2);
v_store(hsad + d + 12, hsad_3);
}
} }
#else #endif
for( d = 0; d < ndisp; d++ ) for( ; d < ndisp; d++ )
{ {
int diff = std::abs(lval - rptr[d]); int diff = std::abs(lval - rptr[d]);
cbuf[d] = (uchar)diff; cbuf[d] = (uchar)diff;
hsad[d] = hsad[d] + diff - cbuf_sub[d]; hsad[d] = hsad[d] + diff - cbuf_sub[d];
} }
#endif
htext[y] += tab[lval] - tab[lptr_sub[0]]; htext[y] += tab[lval] - tab[lptr_sub[0]];
} }
@ -726,22 +733,25 @@ findStereoCorrespondenceBM_( const Mat& left, const Mat& right,
hsad = hsad0 + (1 - dy0)*ndisp; hsad = hsad0 + (1 - dy0)*ndisp;
for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp ) for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )
{ {
#if CV_NEON d = 0;
for( d = 0; d <= ndisp-8; d += 8 ) #if CV_SIMD128
if( useSIMD )
{ {
int32x4_t s0 = vld1q_s32 (sad + d); for( d = 0; d <= ndisp-8; d += 8 )
int32x4_t s1 = vld1q_s32 (sad + d + 4); {
int32x4_t t0 = vld1q_s32 (hsad + d); v_int32x4 s0 = v_load(sad + d);
int32x4_t t1 = vld1q_s32 (hsad + d + 4); v_int32x4 s1 = v_load(sad + d + 4);
s0 = vaddq_s32 (s0, t0); v_int32x4 t0 = v_load(hsad + d);
s1 = vaddq_s32 (s1, t1); v_int32x4 t1 = v_load(hsad + d + 4);
vst1q_s32 (sad + d, s0); s0 += t0;
vst1q_s32 (sad + d + 4, s1); s1 += t1;
v_store(sad + d, s0);
v_store(sad + d + 4, s1);
}
} }
#else #endif
for( d = 0; d < ndisp; d++ ) for( ; d < ndisp; d++ )
sad[d] = (int)(sad[d] + hsad[d]); sad[d] = (int)(sad[d] + hsad[d]);
#endif
} }
int tsum = 0; int tsum = 0;
for( y = -wsz2-1; y < wsz2; y++ ) for( y = -wsz2-1; y < wsz2; y++ )
@ -753,62 +763,55 @@ findStereoCorrespondenceBM_( const Mat& left, const Mat& right,
int minsad = INT_MAX, mind = -1; int minsad = INT_MAX, mind = -1;
hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp; hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp;
hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp; hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp;
#if CV_NEON d = 0;
int32x4_t minsad4 = vdupq_n_s32 (INT_MAX); #if CV_SIMD128
int32x4_t mind4 = vdupq_n_s32(0), d4 = d0_4; if( useSIMD )
for( d = 0; d <= ndisp-8; d += 8 )
{ {
int32x4_t u0 = vld1q_s32 (hsad_sub + d); v_int32x4 d0_4 = v_int32x4(0, 1, 2, 3);
int32x4_t u1 = vld1q_s32 (hsad + d); v_int32x4 dd_4 = v_setall_s32(4);
v_int32x4 minsad4 = v_setall_s32(INT_MAX);
v_int32x4 mind4 = v_setall_s32(0), d4 = d0_4;
int32x4_t v0 = vld1q_s32 (hsad_sub + d + 4); for( ; d <= ndisp - 8; d += 8 )
int32x4_t v1 = vld1q_s32 (hsad + d + 4); {
v_int32x4 u0 = v_load(hsad_sub + d);
v_int32x4 u1 = v_load(hsad + d);
int32x4_t usad4 = vld1q_s32(sad + d); v_int32x4 v0 = v_load(hsad_sub + d + 4);
int32x4_t vsad4 = vld1q_s32(sad + d + 4); v_int32x4 v1 = v_load(hsad + d + 4);
u1 = vsubq_s32 (u1, u0); v_int32x4 usad4 = v_load(sad + d);
v1 = vsubq_s32 (v1, v0); v_int32x4 vsad4 = v_load(sad + d + 4);
usad4 = vaddq_s32 (usad4, u1);
vsad4 = vaddq_s32 (vsad4, v1);
uint32x4_t mask = vcgtq_s32 (minsad4, usad4); u1 -= u0;
minsad4 = vminq_s32 (minsad4, usad4); v1 -= v0;
mind4 = vbslq_s32(mask, d4, mind4); usad4 += u1;
vsad4 += v1;
vst1q_s32 (sad + d, usad4); v_store(sad + d, usad4);
vst1q_s32 (sad + d + 4, vsad4); v_store(sad + d + 4, vsad4);
d4 = vaddq_s32 (d4, dd_4);
mask = vcgtq_s32 (minsad4, vsad4); v_int32x4 mask = minsad4 > usad4;
minsad4 = vminq_s32 (minsad4, vsad4); minsad4 = v_min(minsad4, usad4);
mind4 = vbslq_s32(mask, d4, mind4); mind4 = v_select(mask, d4, mind4);
d4 += dd_4;
d4 = vaddq_s32 (d4, dd_4); mask = minsad4 > vsad4;
minsad4 = v_min(minsad4, vsad4);
mind4 = v_select(mask, d4, mind4);
d4 += dd_4;
}
int CV_DECL_ALIGNED(16) minsad_buf[4], mind_buf[4];
v_store(minsad_buf, minsad4);
v_store(mind_buf, mind4);
if(minsad_buf[0] < minsad || (minsad == minsad_buf[0] && mind_buf[0] < mind)) { minsad = minsad_buf[0]; mind = mind_buf[0]; }
if(minsad_buf[1] < minsad || (minsad == minsad_buf[1] && mind_buf[1] < mind)) { minsad = minsad_buf[1]; mind = mind_buf[1]; }
if(minsad_buf[2] < minsad || (minsad == minsad_buf[2] && mind_buf[2] < mind)) { minsad = minsad_buf[2]; mind = mind_buf[2]; }
if(minsad_buf[3] < minsad || (minsad == minsad_buf[3] && mind_buf[3] < mind)) { minsad = minsad_buf[3]; mind = mind_buf[3]; }
} }
int32x2_t mind4_h = vget_high_s32 (mind4); #endif
int32x2_t mind4_l = vget_low_s32 (mind4); for( ; d < ndisp; d++ )
int32x2_t minsad4_h = vget_high_s32 (minsad4);
int32x2_t minsad4_l = vget_low_s32 (minsad4);
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)));
mind4_h = vbsl_s32 (mask, mind4_h, mind4_l);
minsad4_h = vbsl_s32 (mask, minsad4_h, minsad4_l);
mind4_l = vext_s32 (mind4_h,mind4_h,1);
minsad4_l = vext_s32 (minsad4_h,minsad4_h,1);
mask = vorr_u32 (vclt_s32 (minsad4_h, minsad4_l), vand_u32 (vceq_s32 (minsad4_h, minsad4_l), vclt_s32 (mind4_h, mind4_l)));
mind4_h = vbsl_s32 (mask, mind4_h, mind4_l);
minsad4_h = vbsl_s32 (mask, minsad4_h, minsad4_l);
mind = (int) vget_lane_s32 (mind4_h, 0);
minsad = sad[mind];
#else
for( d = 0; d < ndisp; d++ )
{ {
int currsad = sad[d] + hsad[d] - hsad_sub[d]; int currsad = sad[d] + hsad[d] - hsad_sub[d];
sad[d] = currsad; sad[d] = currsad;
@ -818,7 +821,6 @@ findStereoCorrespondenceBM_( const Mat& left, const Mat& right,
mind = d; mind = d;
} }
} }
#endif
tsum += htext[y + wsz2] - htext[y - wsz2 - 1]; tsum += htext[y + wsz2] - htext[y - wsz2 - 1];
if( tsum < textureThreshold ) if( tsum < textureThreshold )
@ -855,19 +857,6 @@ findStereoCorrespondenceBM_( const Mat& left, const Mat& right,
} }
} }
static void
findStereoCorrespondenceBM( const Mat& left, const Mat& right,
Mat& disp, Mat& cost, const StereoBMParams& state,
uchar* buf, int _dy0, int _dy1 )
{
if(disp.type() == CV_16S)
findStereoCorrespondenceBM_<short>(left, right, disp, cost, state,
buf, _dy0, _dy1, DISPARITY_SHIFT_16S );
else
findStereoCorrespondenceBM_<int>(left, right, disp, cost, state,
buf, _dy0, _dy1, DISPARITY_SHIFT_32S );
}
#ifdef HAVE_OPENCL #ifdef HAVE_OPENCL
static bool ocl_prefiltering(InputArray left0, InputArray right0, OutputArray left, OutputArray right, StereoBMParams* state) static bool ocl_prefiltering(InputArray left0, InputArray right0, OutputArray left, OutputArray right, StereoBMParams* state)
{ {
@ -972,6 +961,7 @@ struct FindStereoCorrespInvoker : public ParallelLoopBody
bool _useShorts, Rect _validDisparityRect, bool _useShorts, Rect _validDisparityRect,
Mat& _slidingSumBuf, Mat& _cost ) Mat& _slidingSumBuf, Mat& _cost )
{ {
CV_Assert( _disp.type() == CV_16S || _disp.type() == CV_32S );
left = &_left; right = &_right; left = &_left; right = &_right;
disp = &_disp; state = _state; disp = &_disp; state = _state;
nstripes = _nstripes; stripeBufSize = _stripeBufSize; nstripes = _nstripes; stripeBufSize = _stripeBufSize;
@ -979,6 +969,9 @@ struct FindStereoCorrespInvoker : public ParallelLoopBody
validDisparityRect = _validDisparityRect; validDisparityRect = _validDisparityRect;
slidingSumBuf = &_slidingSumBuf; slidingSumBuf = &_slidingSumBuf;
cost = &_cost; cost = &_cost;
#if CV_SIMD128
useSIMD = hasSIMD128();
#endif
} }
void operator()( const Range& range ) const void operator()( const Range& range ) const
@ -1012,12 +1005,19 @@ struct FindStereoCorrespInvoker : public ParallelLoopBody
Mat disp_i = disp->rowRange(row0, row1); Mat disp_i = disp->rowRange(row0, row1);
Mat cost_i = state->disp12MaxDiff >= 0 ? cost->rowRange(row0, row1) : Mat(); Mat cost_i = state->disp12MaxDiff >= 0 ? cost->rowRange(row0, row1) : Mat();
#if CV_SSE2 #if CV_SIMD128
if( useShorts ) if( useSIMD && useShorts )
findStereoCorrespondenceBM_SSE2( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 ); {
findStereoCorrespondenceBM_SIMD( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 );
}
else else
#endif #endif
findStereoCorrespondenceBM( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 ); {
if( disp_i.type() == CV_16S )
findStereoCorrespondenceBM<short>( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1, DISPARITY_SHIFT_16S );
else
findStereoCorrespondenceBM<int>( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1, DISPARITY_SHIFT_32S );
}
if( state->disp12MaxDiff >= 0 ) if( state->disp12MaxDiff >= 0 )
validateDisparity( disp_i, cost_i, state->minDisparity, state->numDisparities, state->disp12MaxDiff ); validateDisparity( disp_i, cost_i, state->minDisparity, state->numDisparities, state->disp12MaxDiff );
@ -1043,6 +1043,7 @@ protected:
size_t stripeBufSize; size_t stripeBufSize;
bool useShorts; bool useShorts;
Rect validDisparityRect; Rect validDisparityRect;
bool useSIMD;
}; };
class StereoBMImpl : public StereoBM class StereoBMImpl : public StereoBM
@ -1168,12 +1169,7 @@ public:
if( params.speckleRange >= 0 && params.speckleWindowSize > 0 ) if( params.speckleRange >= 0 && params.speckleWindowSize > 0 )
bufSize2 = width*height*(sizeof(Point_<short>) + sizeof(int) + sizeof(uchar)); bufSize2 = width*height*(sizeof(Point_<short>) + sizeof(int) + sizeof(uchar));
#if CV_SSE2 bool useShorts = params.preFilterCap <= 31 && params.SADWindowSize <= 21;
bool useShorts = params.preFilterCap <= 31 && params.SADWindowSize <= 21 && checkHardwareSupport(CV_CPU_SSE2);
#else
const bool useShorts = false;
#endif
const double SAD_overhead_coeff = 10.0; const double SAD_overhead_coeff = 10.0;
double N0 = 8000000 / (useShorts ? 1 : 4); // approx tbb's min number instructions reasonable for one thread double N0 = 8000000 / (useShorts ? 1 : 4); // approx tbb's min number instructions reasonable for one thread
double maxStripeSize = std::min(std::max(N0 / (width * ndisp), (wsz-1) * SAD_overhead_coeff), (double)height); double maxStripeSize = std::min(std::max(N0 / (width * ndisp), (wsz-1) * SAD_overhead_coeff), (double)height);