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

@@ -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 <typename T>
+struct dispShiftTemplate
+{ };
+
+template<>
+struct dispShiftTemplate<short>
+{
+    enum { value = DISPARITY_SHIFT_16S };
+};
+
+template<>
+struct dispShiftTemplate<int>
+{
+    enum { value = DISPARITY_SHIFT_32S };
+};
+
+template <typename T>
+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 <typename dType>
 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<dType>::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<short>();
+    dType* dptr = disp.ptr<dType>();
     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<dType>(ndisp - mind - 1 + mindisp, p-n, d);
             }
             else
-                dptr[y*dstep] = (short)((ndisp - mind - 1 + mindisp)*16);
+                dptr[y*dstep] = dispDescale<dType>(ndisp - mind - 1 + mindisp, 0, 0);
             costptr[y*coststep] = sad[mind];
         }
     }
@@ -541,7 +574,7 @@ template <typename mType>
 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 )
+                            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<mType>::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<mType>(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<short>( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 );
+            else
+                findStereoCorrespondenceBM_SIMD<int>( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1);
         }
         else
 #endif
         {
             if( disp_i.type() == CV_16S )
-                findStereoCorrespondenceBM<short>( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1, DISPARITY_SHIFT_16S );
+                findStereoCorrespondenceBM<short>( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 );
             else
-                findStereoCorrespondenceBM<int>( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1, DISPARITY_SHIFT_32S );
+                findStereoCorrespondenceBM<int>( 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, &params))
                 {
+                    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)

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_<float> 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++)