From fc35c77f00219cf62eed63867240b10c6a7b1a26 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 4 Jun 2018 16:03:12 +0300 Subject: [PATCH] Merge pull request #11610 from savuor:fix/stereobm_simd_fixed_float * StereoBM: fixed SIMD processing for fixed-type output arrays * changed norm type and threshold, added assertion * fixed disp_shift --- modules/calib3d/src/stereobm.cpp | 68 ++++++++++++++++---- modules/calib3d/test/test_stereomatching.cpp | 6 ++ 2 files changed, 61 insertions(+), 13 deletions(-) diff --git a/modules/calib3d/src/stereobm.cpp b/modules/calib3d/src/stereobm.cpp index b3498c66d1..7aa721dd3d 100644 --- a/modules/calib3d/src/stereobm.cpp +++ b/modules/calib3d/src/stereobm.cpp @@ -284,7 +284,39 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero ) static const int DISPARITY_SHIFT_16S = 4; static const int DISPARITY_SHIFT_32S = 8; +template +struct dispShiftTemplate +{ }; + +template<> +struct dispShiftTemplate +{ + enum { value = DISPARITY_SHIFT_16S }; +}; + +template<> +struct dispShiftTemplate +{ + enum { value = DISPARITY_SHIFT_32S }; +}; + +template +inline T dispDescale(int /*v1*/, int /*v2*/, int /*d*/); + +template<> +inline short dispDescale(int v1, int v2, int d) +{ + return (short)((v1*256 + (d != 0 ? v2*256/d : 0) + 15) >> 4); +} + +template <> +inline int dispDescale(int v1, int v2, int d) +{ + return (int)(v1*256 + (d != 0 ? v2*256/d : 0)); // no need to add 127, this will be converted to float +} + #if CV_SIMD128 +template static void findStereoCorrespondenceBM_SIMD( const Mat& left, const Mat& right, Mat& disp, Mat& cost, StereoBMParams& state, uchar* buf, int _dy0, int _dy1 ) @@ -302,7 +334,8 @@ static void findStereoCorrespondenceBM_SIMD( const Mat& left, const Mat& right, int ftzero = state.preFilterCap; int textureThreshold = state.textureThreshold; int uniquenessRatio = state.uniquenessRatio; - short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT_16S); + const int disp_shift = dispShiftTemplate::value; + dType FILTERED = (dType)((mindisp - 1) << disp_shift); ushort *sad, *hsad0, *hsad, *hsad_sub; int *htext; @@ -310,7 +343,7 @@ static void findStereoCorrespondenceBM_SIMD( const Mat& left, const Mat& right, const uchar* lptr0 = left.ptr() + lofs; const uchar* rptr0 = right.ptr() + rofs; const uchar *lptr, *lptr_sub, *rptr; - short* dptr = disp.ptr(); + dType* dptr = disp.ptr(); int sstep = (int)left.step; int dstep = (int)(disp.step/sizeof(dptr[0])); int cstep = (height + dy0 + dy1)*ndisp; @@ -527,10 +560,10 @@ static void findStereoCorrespondenceBM_SIMD( const Mat& left, const Mat& right, { int p = sad[mind+1], n = sad[mind-1]; d = p + n - 2*sad[mind] + std::abs(p - n); - dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15) >> 4); + dptr[y*dstep] = dispDescale(ndisp - mind - 1 + mindisp, p-n, d); } else - dptr[y*dstep] = (short)((ndisp - mind - 1 + mindisp)*16); + dptr[y*dstep] = dispDescale(ndisp - mind - 1 + mindisp, 0, 0); costptr[y*coststep] = sad[mind]; } } @@ -540,8 +573,8 @@ static void findStereoCorrespondenceBM_SIMD( const Mat& left, const Mat& right, template static void findStereoCorrespondenceBM( const Mat& left, const Mat& right, - Mat& disp, Mat& cost, const StereoBMParams& state, - uchar* buf, int _dy0, int _dy1, const int disp_shift ) + Mat& disp, Mat& cost, const StereoBMParams& state, + uchar* buf, int _dy0, int _dy1 ) { const int ALIGN = 16; @@ -557,6 +590,7 @@ findStereoCorrespondenceBM( const Mat& left, const Mat& right, int ftzero = state.preFilterCap; int textureThreshold = state.textureThreshold; int uniquenessRatio = state.uniquenessRatio; + const int disp_shift = dispShiftTemplate::value; mType FILTERED = (mType)((mindisp - 1) << disp_shift); #if CV_SIMD128 @@ -849,8 +883,8 @@ findStereoCorrespondenceBM( const Mat& left, const Mat& right, sad[ndisp] = sad[ndisp-2]; int p = sad[mind+1], n = sad[mind-1]; d = p + n - 2*sad[mind] + std::abs(p - n); - dptr[y*dstep] = (mType)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15) - >> (DISPARITY_SHIFT_32S - disp_shift)); + dptr[y*dstep] = dispDescale(ndisp - mind - 1 + mindisp, p-n, d); + costptr[y*coststep] = sad[mind]; } } @@ -980,7 +1014,10 @@ struct FindStereoCorrespInvoker : public ParallelLoopBody int _row0 = std::min(cvRound(range.start * rows / nstripes), rows); int _row1 = std::min(cvRound(range.end * rows / nstripes), rows); uchar *ptr = slidingSumBuf->ptr() + range.start * stripeBufSize; - int FILTERED = (state->minDisparity - 1)*16; + + int dispShift = disp->type() == CV_16S ? DISPARITY_SHIFT_16S : + DISPARITY_SHIFT_32S; + int FILTERED = (state->minDisparity - 1) << dispShift; Rect roi = validDisparityRect & Rect(0, _row0, cols, _row1 - _row0); if( roi.height == 0 ) @@ -1008,15 +1045,18 @@ struct FindStereoCorrespInvoker : public ParallelLoopBody #if CV_SIMD128 if( useSIMD && useShorts ) { - findStereoCorrespondenceBM_SIMD( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 ); + if( disp_i.type() == CV_16S) + findStereoCorrespondenceBM_SIMD( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 ); + else + findStereoCorrespondenceBM_SIMD( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1); } else #endif { if( disp_i.type() == CV_16S ) - findStereoCorrespondenceBM( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1, DISPARITY_SHIFT_16S ); + findStereoCorrespondenceBM( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 ); else - findStereoCorrespondenceBM( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1, DISPARITY_SHIFT_32S ); + findStereoCorrespondenceBM( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 ); } if( state->disp12MaxDiff >= 0 ) @@ -1104,7 +1144,6 @@ public: else disp_shift = DISPARITY_SHIFT_32S; - int FILTERED = (params.minDisparity - 1) << disp_shift; #ifdef HAVE_OPENCL @@ -1115,6 +1154,9 @@ public: { if(ocl_stereobm(left, right, disparr, ¶ms)) { + disp_shift = DISPARITY_SHIFT_16S; + FILTERED = (params.minDisparity - 1) << disp_shift; + if( params.speckleRange >= 0 && params.speckleWindowSize > 0 ) filterSpeckles(disparr.getMat(), FILTERED, params.speckleWindowSize, params.speckleRange, slidingSumBuf); if (dtype == CV_32F) diff --git a/modules/calib3d/test/test_stereomatching.cpp b/modules/calib3d/test/test_stereomatching.cpp index c79f013ea5..94fc9718cc 100644 --- a/modules/calib3d/test/test_stereomatching.cpp +++ b/modules/calib3d/test/test_stereomatching.cpp @@ -791,6 +791,12 @@ protected: bm->compute( leftImg, rightImg, tempDisp ); tempDisp.convertTo(leftDisp, CV_32F, 1./StereoMatcher::DISP_SCALE); + //check for fixed-type disparity data type + Mat_ fixedFloatDisp; + bm->compute( leftImg, rightImg, fixedFloatDisp ); + EXPECT_LT(cvtest::norm(fixedFloatDisp, leftDisp, cv::NORM_L2 | cv::NORM_RELATIVE), + 0.005 + DBL_EPSILON); + if (params.mindisp != 0) for (int y = 0; y < leftDisp.rows; y++) for (int x = 0; x < leftDisp.cols; x++)