From 0e9c8e2dd5f20ffbedbe0af1b9aaa6b104f7ffca Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Leonardo=20Br=C3=83=C2=A1s?= <leonardob@server2.localnet>
Date: Fri, 15 Jul 2016 19:55:19 -0300
Subject: [PATCH] 'resolves' 6898 I have seen that you can input a Mat_<float>
 on StereoBM, but the value seems the same as CV_16S. I changed it so, only if
 you input a Mat_<float> it makes use of a previously truncated 4 bits, giving
 more resolution to Disparity Matrix. (The algorithm stays the same, it's not
 more precise). If any other input Mat is given, it changes nothing.

---
 modules/calib3d/src/stereobm.cpp | 32 ++++++++++++++++++++++++++------
 1 file changed, 26 insertions(+), 6 deletions(-)

diff --git a/modules/calib3d/src/stereobm.cpp b/modules/calib3d/src/stereobm.cpp
index 83ed9ae0c..9df510080 100644
--- a/modules/calib3d/src/stereobm.cpp
+++ b/modules/calib3d/src/stereobm.cpp
@@ -317,7 +317,9 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero )
 }
 
 
-static const int DISPARITY_SHIFT = 4;
+static int DISPARITY_SHIFT;
+static const int DISPARITY_SHIFT_16S = 4;
+static const int DISPARITY_SHIFT_32S = 8;
 
 #if CV_SSE2
 static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
@@ -568,8 +570,9 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
 }
 #endif
 
+template <typename mType>
 static void
-findStereoCorrespondenceBM( const Mat& left, const Mat& right,
+findStereoCorrespondenceBM_( const Mat& left, const Mat& right,
                            Mat& disp, Mat& cost, const StereoBMParams& state,
                            uchar* buf, int _dy0, int _dy1 )
 {
@@ -587,7 +590,7 @@ findStereoCorrespondenceBM( 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);
+    mType FILTERED = (mType)((mindisp - 1) << DISPARITY_SHIFT);
 
 #if CV_NEON
     CV_Assert (ndisp % 8 == 0);
@@ -603,7 +606,7 @@ findStereoCorrespondenceBM( 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>();
+    mType* dptr = disp.ptr<mType>();
     int sstep = (int)left.step;
     int dstep = (int)(disp.step/sizeof(dptr[0]));
     int cstep = (height+dy0+dy1)*ndisp;
@@ -846,13 +849,30 @@ 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] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15) >> 4);
+                dptr[y*dstep] = (mType)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15)
+                                 >> (DISPARITY_SHIFT_32S - DISPARITY_SHIFT));
                 costptr[y*coststep] = sad[mind];
             }
         }
     }
 }
 
+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){
+        DISPARITY_SHIFT = DISPARITY_SHIFT_16S;
+        findStereoCorrespondenceBM_<short>(left, right, disp, cost, state,
+                                           buf, _dy0, _dy1 );
+    } else {
+        DISPARITY_SHIFT = DISPARITY_SHIFT_32S;
+        findStereoCorrespondenceBM_<int>(left, right, disp, cost, state,
+                                         buf, _dy0, _dy1 );
+    }
+}
+
 #ifdef HAVE_OPENCL
 static bool ocl_prefiltering(InputArray left0, InputArray right0, OutputArray left, OutputArray right, StereoBMParams* state)
 {
@@ -1129,7 +1149,7 @@ public:
         Mat disp = disp0;
         if( dtype == CV_32F )
         {
-            dispbuf.create(disp0.size(), CV_16S);
+            dispbuf.create(disp0.size(), CV_32S);
             disp = dispbuf;
         }