gpu::StereoConstantSpaceBP:

  fixed some bugs in init_data_cost on first level (added non-reduction version for first level)
  optimized compute_data_cost like init_data_cost (used reduction scheme)
  avoid temp matrix

diff --git a/modules/gpu/include/opencv2/gpu/gpu.hpp b/modules/gpu/include/opencv2/gpu/gpu.hpp
index 2ad3a44..5734709 100644 (file)
--- a/modules/gpu/include/opencv2/gpu/gpu.hpp
+++ b/modules/gpu/include/opencv2/gpu/gpu.hpp
@@ -473,7 +473,7 @@ namespace cv
             GpuMat data_cost;\r
             GpuMat data_cost_selected;\r
 \r
-            GpuMat temp1, temp2;\r
+            GpuMat temp;\r
 \r
             GpuMat out;\r
         };\r

diff --git a/modules/gpu/src/constantspacebp_gpu.cpp b/modules/gpu/src/constantspacebp_gpu.cpp
index 09b8e89..2656ed9 100644 (file)
--- a/modules/gpu/src/constantspacebp_gpu.cpp
+++ b/modules/gpu/src/constantspacebp_gpu.cpp
@@ -59,14 +59,14 @@ void cv::gpu::StereoConstantSpaceBP::operator()(const GpuMat&, const GpuMat&, Gp
 namespace cv { namespace gpu { namespace csbp \r
 {   \r
     void load_constants(int ndisp, float max_data_term, float data_weight, float max_disc_term, float disc_single_jump, \r
-                        const DevMem2D& left, const DevMem2D& right, const DevMem2D& temp1, const DevMem2D& temp2);\r
+                        const DevMem2D& left, const DevMem2D& right, const DevMem2D& temp/*, const DevMem2D& temp2*/);\r
 \r
     void init_data_cost(int rows, int cols, const DevMem2D& disp_selected_pyr, const DevMem2D& data_cost_selected,\r
                         size_t msg_step, int msg_type, int h, int w, int level, int nr_plane, int ndisp, int channels, \r
                         const cudaStream_t& stream);\r
     \r
     void compute_data_cost(const DevMem2D& disp_selected_pyr, const DevMem2D& data_cost, size_t msg_step1, size_t msg_step2, int msg_type,\r
-                           int h, int w, int h2, int level, int nr_plane, int channels, const cudaStream_t& stream);\r
+                           int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, const cudaStream_t& stream);\r
 \r
     void init_message(const DevMem2D& u_new, const DevMem2D& d_new, const DevMem2D& l_new, const DevMem2D& r_new, \r
                       const DevMem2D& u_cur, const DevMem2D& d_cur, const DevMem2D& l_cur, const DevMem2D& r_cur, \r
@@ -116,7 +116,7 @@ static void stereo_csbp_gpu_operator(int& ndisp, int& iters, int& levels, int& n
                                      int& msg_type,\r
                                      GpuMat u[2], GpuMat d[2], GpuMat l[2], GpuMat r[2],\r
                                      GpuMat disp_selected_pyr[2], GpuMat& data_cost, GpuMat& data_cost_selected,\r
-                                     GpuMat& temp1, GpuMat& temp2, GpuMat& out,\r
+                                     GpuMat& temp, GpuMat& out,\r
                                      const GpuMat& left, const GpuMat& right, GpuMat& disp,\r
                                      const cudaStream_t& stream)\r
 {\r
@@ -190,14 +190,13 @@ static void stereo_csbp_gpu_operator(int& ndisp, int& iters, int& levels, int& n
         temp_size = Size(step_pyr[levels - 1], rows_pyr[levels - 1] * ndisp);\r
     }\r
 \r
-    temp1.create(temp_size, msg_type);\r
-    temp2.create(temp_size, msg_type);\r
+    temp.create(temp_size, msg_type);\r
 \r
     ////////////////////////////////////////////////////////////////////////////\r
     // Compute\r
 \r
     csbp::load_constants(ndisp, max_data_term, scale * data_weight, scale * max_disc_term, scale * disc_single_jump, \r
-        left, right, temp1, temp2);\r
+        left, right, temp);\r
 \r
     l[0] = zero;\r
     d[0] = zero;\r
@@ -224,7 +223,7 @@ static void stereo_csbp_gpu_operator(int& ndisp, int& iters, int& levels, int& n
         else\r
         {\r
             csbp::compute_data_cost(disp_selected_pyr[cur_idx], data_cost, step_pyr[i], step_pyr[i+1], msg_type, \r
-                rows_pyr[i], cols_pyr[i], rows_pyr[i+1], i, nr_plane_pyr[i+1], left.channels(), stream);\r
+                left.rows, left.cols, rows_pyr[i], cols_pyr[i], rows_pyr[i+1], i, nr_plane_pyr[i+1], left.channels(), stream);\r
 \r
             int new_idx = (cur_idx + 1) & 1;\r
 \r
@@ -259,13 +258,13 @@ static void stereo_csbp_gpu_operator(int& ndisp, int& iters, int& levels, int& n
 void cv::gpu::StereoConstantSpaceBP::operator()(const GpuMat& left, const GpuMat& right, GpuMat& disp)\r
 {\r
     ::stereo_csbp_gpu_operator(ndisp, iters, levels, nr_plane, max_data_term, data_weight, max_disc_term, disc_single_jump, msg_type,\r
-                               u, d, l, r, disp_selected_pyr, data_cost, data_cost_selected, temp1, temp2, out, left, right, disp, 0);\r
+                               u, d, l, r, disp_selected_pyr, data_cost, data_cost_selected, temp/*, temp2*/, out, left, right, disp, 0);\r
 }\r
 \r
 void cv::gpu::StereoConstantSpaceBP::operator()(const GpuMat& left, const GpuMat& right, GpuMat& disp, const Stream& stream)\r
 {\r
     ::stereo_csbp_gpu_operator(ndisp, iters, levels, nr_plane, max_data_term, data_weight, max_disc_term, disc_single_jump, msg_type,\r
-                               u, d, l, r, disp_selected_pyr, data_cost, data_cost_selected, temp1, temp2, out, left, right, disp, \r
+                               u, d, l, r, disp_selected_pyr, data_cost, data_cost_selected, temp/*, temp2*/, out, left, right, disp, \r
                                StreamAccessor::getStream(stream));\r
 }\r
 \r

diff --git a/modules/gpu/src/cuda/constantspacebp.cu b/modules/gpu/src/cuda/constantspacebp.cu
index a94d374..2bdcb4a 100644 (file)
--- a/modules/gpu/src/cuda/constantspacebp.cu
+++ b/modules/gpu/src/cuda/constantspacebp.cu
@@ -48,7 +48,7 @@ using namespace cv::gpu;
 using namespace cv::gpu::impl;\r
 \r
 #ifndef FLT_MAX\r
-#define FLT_MAX 3.402823466e+38F\r
+#define FLT_MAX 3.402823466e+30F\r
 #endif\r
 \r
 #ifndef SHRT_MAX\r
@@ -77,6 +77,7 @@ struct TypeLimits<float>
 namespace csbp_kernels\r
 {\r
     __constant__ int cndisp;\r
+    __constant__ int cth;\r
 \r
     __constant__ float cmax_data_term;\r
     __constant__ float cdata_weight;\r
@@ -91,16 +92,18 @@ namespace csbp_kernels
 \r
     __constant__ uchar* cleft;\r
     __constant__ uchar* cright;\r
-    __constant__ uchar* ctemp1;\r
-    __constant__ uchar* ctemp2;\r
+    __constant__ uchar* ctemp;\r
 }\r
 \r
 namespace cv { namespace gpu { namespace csbp \r
 {\r
     void load_constants(int ndisp, float max_data_term, float data_weight, float max_disc_term, float disc_single_jump, \r
-                        const DevMem2D& left, const DevMem2D& right, const DevMem2D& temp1, const DevMem2D& temp2)\r
+                        const DevMem2D& left, const DevMem2D& right, const DevMem2D& temp)\r
     {\r
+        int th = (int)(ndisp * 0.2); \r
+\r
         cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::cndisp, &ndisp, sizeof(int)) );\r
+        cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::cth,    &th,    sizeof(int)) );\r
 \r
         cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::cmax_data_term,    &max_data_term,    sizeof(float)) );\r
         cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::cdata_weight,      &data_weight,      sizeof(float)) );\r
@@ -111,8 +114,7 @@ namespace cv { namespace gpu { namespace csbp
 \r
         cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::cleft,  &left.ptr,  sizeof(left.ptr)) );\r
         cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::cright, &right.ptr, sizeof(right.ptr)) );\r
-        cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::ctemp1, &temp1.ptr, sizeof(temp1.ptr)) );\r
-        cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::ctemp2, &temp2.ptr, sizeof(temp2.ptr)) );\r
+        cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::ctemp, &temp.ptr, sizeof(temp.ptr)) );\r
     }\r
 }}}\r
 \r
@@ -154,7 +156,7 @@ namespace csbp_kernels
         {\r
             T* selected_disparity = selected_disp_pyr + y * cmsg_step1 + x;\r
             T* data_cost_selected = data_cost_selected_ + y * cmsg_step1 + x;\r
-            T* data_cost = (T*)ctemp1 + y * cmsg_step1 + x;\r
+            T* data_cost = (T*)ctemp + y * cmsg_step1 + x;\r
 \r
             int nr_local_minimum = 0;\r
 \r
@@ -200,8 +202,48 @@ namespace csbp_kernels
         }\r
     }\r
 \r
+    template <typename T, int channels>\r
+    __global__ void init_data_cost(int h, int w, int level) \r
+    {\r
+        int x = blockIdx.x * blockDim.x + threadIdx.x;\r
+        int y = blockIdx.y * blockDim.y + threadIdx.y;\r
+        \r
+        if (y < h && x < w)\r
+        {\r
+            int y0 = y << level;\r
+            int yt = (y + 1) << level;\r
+\r
+            int x0 = x << level;\r
+            int xt = (x + 1) << level;\r
+\r
+            T* data_cost = (T*)ctemp + y * cmsg_step1 + x;\r
+\r
+            for(int d = 0; d < cndisp; ++d)\r
+            {\r
+                float val = 0.0f;\r
+                for(int yi = y0; yi < yt; yi++)\r
+                {\r
+                    for(int xi = x0; xi < xt; xi++)\r
+                    {                    \r
+                        int xr = xi - d;\r
+                        if(d < cth || xr < 0) \r
+                            val += cdata_weight * cmax_data_term;\r
+                        else \r
+                        {                            \r
+                            const uchar* lle = cleft + yi * cimg_step + xi * channels;\r
+                            const uchar* lri = cright + yi * cimg_step + xr * channels;\r
+\r
+                            val += DataCostPerPixel<channels>::compute(lle, lri);\r
+                        }\r
+                    }                  \r
+                }\r
+                data_cost[cdisp_step1 * d] = saturate_cast<T>(val);\r
+            }\r
+        }\r
+    }\r
+\r
     template <typename T, int winsz, int channels> \r
-    __global__ void data_init(int level, int rows, int cols, int h)\r
+    __global__ void init_data_cost_reduce(int level, int rows, int cols, int h)\r
     {\r
         int x_out = blockIdx.x;        \r
         int y_out = blockIdx.y % h;\r
@@ -219,7 +261,7 @@ namespace csbp_kernels
             float val = 0.0f;\r
             if (x0 + tid < cols)\r
             {\r
-                if (x0 + tid - d < 0)\r
+                if (x0 + tid - d < 0 || d < cth)\r
                     val = cdata_weight * cmax_data_term * len;\r
                 else\r
                 {\r
@@ -253,7 +295,7 @@ namespace csbp_kernels
             if (winsz >=  4) if (tid <  2) dline[tid] += dline[tid + 2];            \r
             if (winsz >=  2) if (tid <  1) dline[tid] += dline[tid + 1];\r
 \r
-            T* data_cost = (T*)ctemp1 + y_out * cmsg_step1 + x_out;\r
+            T* data_cost = (T*)ctemp + y_out * cmsg_step1 + x_out;\r
 \r
             if (tid == 0)                 \r
                 data_cost[cdisp_step1 * d] = saturate_cast<T>(dline[0]);\r
@@ -263,8 +305,25 @@ namespace csbp_kernels
 \r
 namespace cv { namespace gpu { namespace csbp \r
 {\r
+    template <typename T> \r
+    void init_data_cost_caller_(int /*rows*/, int /*cols*/, int h, int w, int level, int /*ndisp*/, int channels, const cudaStream_t& stream)\r
+    {\r
+        dim3 threads(32, 8, 1);\r
+        dim3 grid(1, 1, 1);\r
+\r
+        grid.x = divUp(w, threads.x);\r
+        grid.y = divUp(h, threads.y);       \r
+        \r
+        switch (channels)\r
+        {\r
+        case 1: csbp_kernels::init_data_cost<T, 1><<<grid, threads, 0, stream>>>(h, w, level); break;\r
+        case 3: csbp_kernels::init_data_cost<T, 3><<<grid, threads, 0, stream>>>(h, w, level); break;\r
+        default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__);\r
+        }\r
+    }\r
+\r
     template <typename T, int winsz> \r
-    void data_init_caller(int rows, int cols, int h, int w, int level, int ndisp, int channels, const cudaStream_t& stream)\r
+    void init_data_cost_reduce_caller_(int rows, int cols, int h, int w, int level, int ndisp, int channels, const cudaStream_t& stream)\r
     {\r
         const int threadsNum = 256;\r
         const size_t smem_size = threadsNum * sizeof(float);\r
@@ -275,16 +334,16 @@ namespace cv { namespace gpu { namespace csbp
         \r
         switch (channels)\r
         {\r
-        case 1: csbp_kernels::data_init<T, winsz, 1><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
-        case 3: csbp_kernels::data_init<T, winsz, 3><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
+        case 1: csbp_kernels::init_data_cost_reduce<T, winsz, 1><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
+        case 3: csbp_kernels::init_data_cost_reduce<T, winsz, 3><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
         default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__);\r
         }\r
     }\r
       \r
-    typedef void (*DataInitCaller)(int cols, int rows, int w, int h, int level, int ndisp, int channels, const cudaStream_t& stream);\r
+    typedef void (*InitDataCostCaller)(int cols, int rows, int w, int h, int level, int ndisp, int channels, const cudaStream_t& stream);\r
 \r
     template <typename T>\r
-    void get_first_k_initial_local_caller(const DevMem2D& disp_selected_pyr, const DevMem2D& data_cost_selected, int h, int w, int nr_plane, const cudaStream_t& stream)\r
+    void get_first_k_initial_local_caller_(const DevMem2D& disp_selected_pyr, const DevMem2D& data_cost_selected, int h, int w, int nr_plane, const cudaStream_t& stream)\r
     {\r
         dim3 threads(32, 8, 1);\r
         dim3 grid(1, 1, 1);\r
@@ -301,18 +360,18 @@ namespace cv { namespace gpu { namespace csbp
                         size_t msg_step, int msg_type, int h, int w, int level, int nr_plane, int ndisp, int channels, const cudaStream_t& stream)\r
     {\r
 \r
-        static const DataInitCaller data_init_callers[8][9] = \r
+        static const InitDataCostCaller init_data_cost_callers[8][9] = \r
             {\r
-                {0, 0, 0, 0, 0, 0, 0, 0, 0}, \r
+                {0, 0, 0, 0, 0, 0, 0, 0, 0},\r
                 {0, 0, 0, 0, 0, 0, 0, 0, 0}, \r
                 {0, 0, 0, 0, 0, 0, 0, 0, 0},\r
-                {data_init_caller<short, 1>, data_init_caller<short, 2>, data_init_caller<short, 4>, data_init_caller<short, 8>, \r
-                 data_init_caller<short, 16>, data_init_caller<short, 32>, data_init_caller<short, 64>, data_init_caller<short, 128>, \r
-                 data_init_caller<short, 256>},\r
+                {init_data_cost_caller_<short>, init_data_cost_caller_<short>, init_data_cost_reduce_caller_<short, 4>, \r
+                 init_data_cost_reduce_caller_<short, 8>, init_data_cost_reduce_caller_<short, 16>, init_data_cost_reduce_caller_<short, 32>, \r
+                 init_data_cost_reduce_caller_<short, 64>, init_data_cost_reduce_caller_<short, 128>, init_data_cost_reduce_caller_<short, 256>},\r
                 {0, 0, 0, 0, 0, 0, 0, 0, 0},\r
-                {data_init_caller<float, 1>, data_init_caller<float, 2>, data_init_caller<float, 4>, data_init_caller<float, 8>, \r
-                 data_init_caller<float, 16>, data_init_caller<float, 32>, data_init_caller<float, 64>, data_init_caller<float, 128>, \r
-                 data_init_caller<float, 256>},\r
+                {init_data_cost_caller_<float>, init_data_cost_caller_<float>, init_data_cost_reduce_caller_<float, 4>, \r
+                 init_data_cost_reduce_caller_<float, 8>, init_data_cost_reduce_caller_<float, 16>, init_data_cost_reduce_caller_<float, 32>, \r
+                 init_data_cost_reduce_caller_<float, 64>, init_data_cost_reduce_caller_<float, 128>, init_data_cost_reduce_caller_<float, 256>},\r
                 {0, 0, 0, 0, 0, 0, 0, 0, 0}, \r
                 {0, 0, 0, 0, 0, 0, 0, 0, 0}\r
             };\r
@@ -320,22 +379,22 @@ namespace cv { namespace gpu { namespace csbp
         static const GetFirstKInitialLocalCaller get_first_k_initial_local_callers[8] = \r
             {\r
                 0, 0, 0,\r
-                get_first_k_initial_local_caller<short>,\r
+                get_first_k_initial_local_caller_<short>,\r
                 0,\r
-                get_first_k_initial_local_caller<float>,\r
+                get_first_k_initial_local_caller_<float>,\r
                 0, 0\r
             };\r
                 \r
-        DataInitCaller data_init_caller = data_init_callers[msg_type][level];\r
+        InitDataCostCaller init_data_cost_caller = init_data_cost_callers[msg_type][level];\r
         GetFirstKInitialLocalCaller get_first_k_initial_local_caller = get_first_k_initial_local_callers[msg_type];\r
-        if (!data_init_caller || !get_first_k_initial_local_caller)\r
+        if (!init_data_cost_caller || !get_first_k_initial_local_caller)\r
             cv::gpu::error("Unsupported message type or levels count", __FILE__, __LINE__);\r
         \r
         size_t disp_step = msg_step * h;\r
         cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::cdisp_step1, &disp_step, sizeof(size_t)) );\r
         cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::cmsg_step1,  &msg_step,  sizeof(size_t)) );\r
 \r
-        data_init_caller(rows, cols, h, w, level, ndisp, channels, stream);\r
+        init_data_cost_caller(rows, cols, h, w, level, ndisp, channels, stream);\r
 \r
         if (stream == 0)\r
             cudaSafeCall( cudaThreadSynchronize() );\r
@@ -354,7 +413,7 @@ namespace cv { namespace gpu { namespace csbp
 namespace csbp_kernels\r
 {\r
     template <typename T, int channels>\r
-    __global__ void compute_data_cost(T* selected_disp_pyr, T* data_cost_, int h, int w, int level, int nr_plane)\r
+    __global__ void compute_data_cost(const T* selected_disp_pyr, T* data_cost_, int h, int w, int level, int nr_plane)\r
     {\r
         int x = blockIdx.x * blockDim.x + threadIdx.x;\r
         int y = blockIdx.y * blockDim.y + threadIdx.y;        \r
@@ -367,7 +426,7 @@ namespace csbp_kernels
             int x0 = x << level;\r
             int xt = (x + 1) << level;\r
 \r
-            T* selected_disparity = selected_disp_pyr + y/2 * cmsg_step2 + x/2;\r
+            const T* selected_disparity = selected_disp_pyr + y/2 * cmsg_step2 + x/2;\r
             T* data_cost = data_cost_ + y * cmsg_step1 + x;\r
 \r
             for(int d = 0; d < nr_plane; d++)\r
@@ -376,11 +435,11 @@ namespace csbp_kernels
                 for(int yi = y0; yi < yt; yi++)\r
                 {\r
                     for(int xi = x0; xi < xt; xi++)\r
-                    {                    \r
+                    {\r
                         int sel_disp = selected_disparity[d * cdisp_step2];\r
                         int xr = xi - sel_disp;\r
 \r
-                        if (xr < 0) \r
+                        if (xr < 0 || sel_disp < cth) \r
                             val += cdata_weight * cmax_data_term;\r
                         else \r
                         {\r
@@ -395,12 +454,75 @@ namespace csbp_kernels
             }\r
         }\r
     }\r
+\r
+    template <typename T, int winsz, int channels> \r
+    __global__ void compute_data_cost_reduce(const T* selected_disp_pyr, T* data_cost_, int level, int rows, int cols, int h, int nr_plane)\r
+    {\r
+        int x_out = blockIdx.x;        \r
+        int y_out = blockIdx.y % h;\r
+        int d = (blockIdx.y / h) * blockDim.z + threadIdx.z;\r
+\r
+        int tid = threadIdx.x;\r
+\r
+        const T* selected_disparity = selected_disp_pyr + y_out/2 * cmsg_step2 + x_out/2;\r
+        T* data_cost = data_cost_ + y_out * cmsg_step1 + x_out;        \r
+\r
+        if (d < nr_plane)\r
+        {\r
+            int sel_disp = selected_disparity[d * cdisp_step2];\r
+\r
+            int x0 = x_out << level;\r
+            int y0 = y_out << level;\r
+\r
+            int len = min(y0 + winsz, rows) - y0;\r
+\r
+            float val = 0.0f;\r
+            if (x0 + tid < cols)\r
+            {\r
+                if (x0 + tid - sel_disp < 0 || sel_disp < cth)\r
+                    val = cdata_weight * cmax_data_term * len;\r
+                else\r
+                {\r
+                    const uchar* lle =  cleft + y0 * cimg_step + channels * (x0 + tid    );\r
+                    const uchar* lri = cright + y0 * cimg_step + channels * (x0 + tid - sel_disp);\r
+\r
+                    for(int y = 0; y < len; ++y)\r
+                    {                                                \r
+                        val += DataCostPerPixel<channels>::compute(lle, lri);\r
+\r
+                        lle += cimg_step;\r
+                        lri += cimg_step;\r
+                    }\r
+                }\r
+            }\r
+\r
+            extern __shared__ float smem[];\r
+            float* dline = smem + winsz * threadIdx.z;\r
+\r
+            dline[tid] = val;\r
+\r
+            __syncthreads();\r
+\r
+            if (winsz >= 256) { if (tid < 128) { dline[tid] += dline[tid + 128]; } __syncthreads(); }\r
+            if (winsz >= 128) { if (tid <  64) { dline[tid] += dline[tid + 64]; } __syncthreads(); }\r
+\r
+            if (winsz >= 64) if (tid < 32) dline[tid] += dline[tid + 32];\r
+            if (winsz >= 32) if (tid < 16) dline[tid] += dline[tid + 16];\r
+            if (winsz >= 16) if (tid <  8) dline[tid] += dline[tid + 8];\r
+            if (winsz >=  8) if (tid <  4) dline[tid] += dline[tid + 4];\r
+            if (winsz >=  4) if (tid <  2) dline[tid] += dline[tid + 2];            \r
+            if (winsz >=  2) if (tid <  1) dline[tid] += dline[tid + 1];\r
+\r
+            if (tid == 0)                 \r
+                data_cost[cdisp_step1 * d] = saturate_cast<T>(dline[0]);\r
+        }\r
+    }\r
 }\r
 \r
 namespace cv { namespace gpu { namespace csbp \r
 {\r
     template <typename T> \r
-    void compute_data_cost_caller(const DevMem2D& disp_selected_pyr, const DevMem2D& data_cost, \r
+    void compute_data_cost_caller_(const DevMem2D& disp_selected_pyr, const DevMem2D& data_cost, int /*rows*/, int /*cols*/,\r
                                   int h, int w, int level, int nr_plane, int channels, const cudaStream_t& stream)\r
     {\r
         dim3 threads(32, 8, 1);\r
@@ -411,25 +533,51 @@ namespace cv { namespace gpu { namespace csbp
 \r
         switch(channels)\r
         {\r
-        case 1: csbp_kernels::compute_data_cost<T, 1><<<grid, threads, 0, stream>>>((T*)disp_selected_pyr.ptr, (T*)data_cost.ptr, h, w, level, nr_plane); break;\r
-        case 3: csbp_kernels::compute_data_cost<T, 3><<<grid, threads, 0, stream>>>((T*)disp_selected_pyr.ptr, (T*)data_cost.ptr, h, w, level, nr_plane); break;\r
+        case 1: csbp_kernels::compute_data_cost<T, 1><<<grid, threads, 0, stream>>>((const T*)disp_selected_pyr.ptr, (T*)data_cost.ptr, h, w, level, nr_plane); break;\r
+        case 3: csbp_kernels::compute_data_cost<T, 3><<<grid, threads, 0, stream>>>((const T*)disp_selected_pyr.ptr, (T*)data_cost.ptr, h, w, level, nr_plane); break;\r
         default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__);\r
         }           \r
     }\r
+\r
+    template <typename T, int winsz> \r
+    void compute_data_cost_reduce_caller_(const DevMem2D& disp_selected_pyr, const DevMem2D& data_cost, int rows, int cols,\r
+                                  int h, int w, int level, int nr_plane, int channels, const cudaStream_t& stream)\r
+    {\r
+        const int threadsNum = 256;\r
+        const size_t smem_size = threadsNum * sizeof(float);\r
+        \r
+        dim3 threads(winsz, 1, threadsNum / winsz);\r
+        dim3 grid(w, h, 1);        \r
+        grid.y *= divUp(nr_plane, threads.z);\r
+        \r
+        switch (channels)\r
+        {\r
+        case 1: csbp_kernels::compute_data_cost_reduce<T, winsz, 1><<<grid, threads, smem_size, stream>>>((const T*)disp_selected_pyr.ptr, (T*)data_cost.ptr, level, rows, cols, h, nr_plane); break;\r
+        case 3: csbp_kernels::compute_data_cost_reduce<T, winsz, 3><<<grid, threads, smem_size, stream>>>((const T*)disp_selected_pyr.ptr, (T*)data_cost.ptr, level, rows, cols, h, nr_plane); break;\r
+        default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__);\r
+        }\r
+    }\r
       \r
-    typedef void (*ComputeDataCostCaller)(const DevMem2D& disp_selected_pyr, const DevMem2D& data_cost, \r
+    typedef void (*ComputeDataCostCaller)(const DevMem2D& disp_selected_pyr, const DevMem2D& data_cost, int rows, int cols,\r
                            int h, int w, int level, int nr_plane, int channels, const cudaStream_t& stream);\r
     \r
     void compute_data_cost(const DevMem2D& disp_selected_pyr, const DevMem2D& data_cost, size_t msg_step1, size_t msg_step2, int msg_type,\r
-                           int h, int w, int h2, int level, int nr_plane, int channels, const cudaStream_t& stream)\r
+                           int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, const cudaStream_t& stream)\r
     {\r
-        static const ComputeDataCostCaller callers[8] = \r
+        static const ComputeDataCostCaller callers[8][9] = \r
             {\r
-                0, 0, 0,\r
-                compute_data_cost_caller<short>,\r
-                0,\r
-                compute_data_cost_caller<float>,\r
-                0, 0\r
+                {0, 0, 0, 0, 0, 0, 0, 0, 0},\r
+                {0, 0, 0, 0, 0, 0, 0, 0, 0}, \r
+                {0, 0, 0, 0, 0, 0, 0, 0, 0},\r
+                {compute_data_cost_caller_<short>, compute_data_cost_caller_<short>, compute_data_cost_reduce_caller_<short, 4>, \r
+                 compute_data_cost_reduce_caller_<short, 8>, compute_data_cost_reduce_caller_<short, 16>, compute_data_cost_reduce_caller_<short, 32>, \r
+                 compute_data_cost_reduce_caller_<short, 64>, compute_data_cost_reduce_caller_<short, 128>, compute_data_cost_reduce_caller_<short, 256>},\r
+                {0, 0, 0, 0, 0, 0, 0, 0, 0},\r
+                {compute_data_cost_caller_<float>, compute_data_cost_caller_<float>, compute_data_cost_reduce_caller_<float, 4>, \r
+                 compute_data_cost_reduce_caller_<float, 8>, compute_data_cost_reduce_caller_<float, 16>, compute_data_cost_reduce_caller_<float, 32>, \r
+                 compute_data_cost_reduce_caller_<float, 64>, compute_data_cost_reduce_caller_<float, 128>, compute_data_cost_reduce_caller_<float, 256>},\r
+                {0, 0, 0, 0, 0, 0, 0, 0, 0}, \r
+                {0, 0, 0, 0, 0, 0, 0, 0, 0}\r
             };\r
 \r
         size_t disp_step1 = msg_step1 * h;\r
@@ -439,11 +587,11 @@ namespace cv { namespace gpu { namespace csbp
         cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::cmsg_step1,  &msg_step1,  sizeof(size_t)) );\r
         cudaSafeCall( cudaMemcpyToSymbol(csbp_kernels::cmsg_step2,  &msg_step2,  sizeof(size_t)) );\r
 \r
-        ComputeDataCostCaller caller = callers[msg_type];\r
+        ComputeDataCostCaller caller = callers[msg_type][level];\r
         if (!caller)\r
             cv::gpu::error("Unsopported message type", __FILE__, __LINE__);\r
         \r
-        caller(disp_selected_pyr, data_cost, h, w, level, nr_plane, channels, stream);\r
+        caller(disp_selected_pyr, data_cost, rows, cols, h, w, level, nr_plane, channels, stream);\r
         \r
         if (stream == 0)\r
             cudaSafeCall( cudaThreadSynchronize() ); \r
@@ -478,7 +626,7 @@ namespace csbp_kernels
             }\r
 \r
             data_cost_selected[i * cdisp_step1] = data_cost_cur[id * cdisp_step1];\r
-            disparity_selected_new[i * cdisp_step1] = disparity_selected_cur[id * cdisp_step1];\r
+            disparity_selected_new[i * cdisp_step1] = disparity_selected_cur[id * cdisp_step2];\r
 \r
             u_new[i * cdisp_step1] = u_cur[id * cdisp_step2];\r
             d_new[i * cdisp_step1] = d_cur[id * cdisp_step2];\r
@@ -506,8 +654,7 @@ namespace csbp_kernels
             const T* l_cur = l_cur_ + y/2                * cmsg_step2 + min(w2-1, x/2 + 1);\r
             const T* r_cur = r_cur_ + y/2                * cmsg_step2 + max(0, x/2 - 1);\r
 \r
-            T* disparity_selected_cur_backup = (T*)ctemp2 + y * cmsg_step1 + x;\r
-            T* data_cost_new = (T*)ctemp1 + y * cmsg_step1 + x;\r
+            T* data_cost_new = (T*)ctemp + y * cmsg_step1 + x;\r
 \r
             const T* disparity_selected_cur = selected_disp_pyr_cur + y/2 * cmsg_step2 + x/2;\r
             T* data_cost = data_cost_ + y * cmsg_step1 + x;\r
@@ -515,8 +662,7 @@ namespace csbp_kernels
             for(int d = 0; d < nr_plane2; d++)\r
             {\r
                 int idx2 = d * cdisp_step2;\r
-\r
-                disparity_selected_cur_backup[d * cdisp_step1] = disparity_selected_cur[idx2];                \r
+            \r
                 T val  = data_cost[d * cdisp_step1] + u_cur[idx2] + d_cur[idx2] + l_cur[idx2] + r_cur[idx2];\r
                 data_cost_new[d * cdisp_step1] = val;\r
             }\r
@@ -536,7 +682,7 @@ namespace csbp_kernels
 \r
             get_first_k_element_increase(u_new, d_new, l_new, r_new, u_cur, d_cur, l_cur, r_cur,\r
                                          data_cost_selected, disparity_selected_new, data_cost_new,\r
-                                         data_cost, disparity_selected_cur_backup, nr_plane, nr_plane2);\r
+                                         data_cost, disparity_selected_cur, nr_plane, nr_plane2);\r
         }\r
     }\r
 }\r
@@ -544,7 +690,7 @@ namespace csbp_kernels
 namespace cv { namespace gpu { namespace csbp \r
 {\r
     template <typename T>\r
-    void init_message_caller(const DevMem2D& u_new, const DevMem2D& d_new, const DevMem2D& l_new, const DevMem2D& r_new, \r
+    void init_message_caller_(const DevMem2D& u_new, const DevMem2D& d_new, const DevMem2D& l_new, const DevMem2D& r_new, \r
                       const DevMem2D& u_cur, const DevMem2D& d_cur, const DevMem2D& l_cur, const DevMem2D& r_cur, \r
                       const DevMem2D& selected_disp_pyr_new, const DevMem2D& selected_disp_pyr_cur, \r
                       const DevMem2D& data_cost_selected, const DevMem2D& data_cost, \r
@@ -578,9 +724,9 @@ namespace cv { namespace gpu { namespace csbp
         static const InitMessageCaller callers[8] = \r
             {\r
                 0, 0, 0,\r
-                init_message_caller<short>,\r
+                init_message_caller_<short>,\r
                 0,\r
-                init_message_caller<float>,\r
+                init_message_caller_<float>,\r
                 0, 0\r
             };\r
         \r
@@ -663,7 +809,7 @@ namespace csbp_kernels
             \r
             const T* disp = selected_disp_pyr_cur + y * cmsg_step1 + x;\r
             \r
-            T* temp = (T*)ctemp1 + y * cmsg_step1 + x;\r
+            T* temp = (T*)ctemp + y * cmsg_step1 + x;\r
 \r
             message_per_pixel(data, u, r - 1, u + cmsg_step1, l + 1, disp, disp - cmsg_step1, nr_plane, temp);\r
             message_per_pixel(data, d, d - cmsg_step1, r - 1, l + 1, disp, disp + cmsg_step1, nr_plane, temp);\r
@@ -676,7 +822,7 @@ namespace csbp_kernels
 namespace cv { namespace gpu { namespace csbp \r
 {\r
     template <typename T>\r
-    void compute_message_caller(const DevMem2D& u, const DevMem2D& d, const DevMem2D& l, const DevMem2D& r, const DevMem2D& data_cost_selected, \r
+    void compute_message_caller_(const DevMem2D& u, const DevMem2D& d, const DevMem2D& l, const DevMem2D& r, const DevMem2D& data_cost_selected, \r
                                 const DevMem2D& selected_disp_pyr_cur, int h, int w, int nr_plane, int t, const cudaStream_t& stream)\r
     {            \r
         dim3 threads(32, 8, 1);\r
@@ -699,9 +845,9 @@ namespace cv { namespace gpu { namespace csbp
         static const ComputeMessageCaller callers[8] = \r
             {\r
                 0, 0, 0,\r
-                compute_message_caller<short>,\r
+                compute_message_caller_<short>,\r
                 0,\r
-                compute_message_caller<float>,\r
+                compute_message_caller_<float>,\r
                 0, 0\r
             };\r
         \r
@@ -769,7 +915,7 @@ namespace csbp_kernels
 namespace cv { namespace gpu { namespace csbp \r
 {\r
     template <typename T>\r
-    void compute_disp_caller(const DevMem2D& u, const DevMem2D& d, const DevMem2D& l, const DevMem2D& r, const DevMem2D& data_cost_selected, \r
+    void compute_disp_caller_(const DevMem2D& u, const DevMem2D& d, const DevMem2D& l, const DevMem2D& r, const DevMem2D& data_cost_selected, \r
                       const DevMem2D& disp_selected, const DevMem2D& disp, int nr_plane, const cudaStream_t& stream)\r
     {\r
         dim3 threads(32, 8, 1);\r
@@ -792,9 +938,9 @@ namespace cv { namespace gpu { namespace csbp
         static const ComputeDispCaller callers[8] = \r
             {\r
                 0, 0, 0,\r
-                compute_disp_caller<short>,\r
+                compute_disp_caller_<short>,\r
                 0,\r
-                compute_disp_caller<float>,\r
+                compute_disp_caller_<float>,\r
                 0, 0\r
             };\r
         \r