modules/gpu/src/cuda/stereocsbp.cu

   1 /*M///////////////////////////////////////////////////////////////////////////////////////\r
   2 //\r
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
   4 //\r
   5 //  By downloading, copying, installing or using the software you agree to this license.\r
   6 //  If you do not agree to this license, do not download, install,\r
   7 //  copy or use the software.\r
   8 //\r
   9 //\r
  10 //                           License Agreement\r
  11 //                For Open Source Computer Vision Library\r
  12 //\r
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.\r
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.\r
  15 // Third party copyrights are property of their respective owners.\r
  16 //\r
  17 // Redistribution and use in source and binary forms, with or without modification,\r
  18 // are permitted provided that the following conditions are met:\r
  19 //\r
  20 //   * Redistribution's of source code must retain the above copyright notice,\r
  21 //     this list of conditions and the following disclaimer.\r
  22 //\r
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,\r
  24 //     this list of conditions and the following disclaimer in the documentation\r
  25 //     and/or other materials provided with the distribution.\r
  26 //\r
  27 //   * The name of the copyright holders may not be used to endorse or promote products\r
  28 //     derived from this software without specific prior written permission.\r
  29 //\r
  30 // This software is provided by the copyright holders and contributors "as is" and\r
  31 // any express or implied warranties, including, but not limited to, the implied\r
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.\r
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,\r
  34 // indirect, incidental, special, exemplary, or consequential damages\r
  35 // (including, but not limited to, procurement of substitute goods or services;\r
  36 // loss of use, data, or profits; or business interruption) however caused\r
  37 // and on any theory of liability, whether in contract, strict liability,\r
  38 // or tort (including negligence or otherwise) arising in any way out of\r
  39 // the use of this software, even if advised of the possibility of such damage.\r
  40 //\r
  41 //M*/\r
  42 \r
  43 #include "internal_shared.hpp"\r
  44 #include "opencv2/gpu/device/saturate_cast.hpp"\r
  45 #include "opencv2/gpu/device/limits.hpp"\r
  46 \r
  47 namespace cv { namespace gpu { namespace device \r
  48 {\r
  49     namespace stereocsbp \r
  50     {\r
  51         ///////////////////////////////////////////////////////////////\r
  52         /////////////////////// load constants ////////////////////////\r
  53         ///////////////////////////////////////////////////////////////\r
  54 \r
  55         __constant__ int cndisp;\r
  56 \r
  57         __constant__ float cmax_data_term;\r
  58         __constant__ float cdata_weight;\r
  59         __constant__ float cmax_disc_term;\r
  60         __constant__ float cdisc_single_jump;\r
  61 \r
  62         __constant__ int cth;\r
  63 \r
  64         __constant__ size_t cimg_step;\r
  65         __constant__ size_t cmsg_step;        \r
  66         __constant__ size_t cdisp_step1;\r
  67         __constant__ size_t cdisp_step2;\r
  68 \r
  69         __constant__ uchar* cleft;\r
  70         __constant__ uchar* cright;\r
  71         __constant__ uchar* ctemp;\r
  72 \r
  73 \r
  74         void load_constants(int ndisp, float max_data_term, float data_weight, float max_disc_term, float disc_single_jump, int min_disp_th,\r
  75                             const DevMem2Db& left, const DevMem2Db& right, const DevMem2Db& temp)\r
  76         {\r
  77             cudaSafeCall( cudaMemcpyToSymbol(cndisp, &ndisp, sizeof(int)) );\r
  78 \r
  79             cudaSafeCall( cudaMemcpyToSymbol(cmax_data_term,    &max_data_term,    sizeof(float)) );\r
  80             cudaSafeCall( cudaMemcpyToSymbol(cdata_weight,      &data_weight,      sizeof(float)) );\r
  81             cudaSafeCall( cudaMemcpyToSymbol(cmax_disc_term,    &max_disc_term,    sizeof(float)) );\r
  82             cudaSafeCall( cudaMemcpyToSymbol(cdisc_single_jump, &disc_single_jump, sizeof(float)) );\r
  83 \r
  84             cudaSafeCall( cudaMemcpyToSymbol(cth, &min_disp_th, sizeof(int)) );\r
  85 \r
  86             cudaSafeCall( cudaMemcpyToSymbol(cimg_step, &left.step, sizeof(size_t)) );\r
  87 \r
  88             cudaSafeCall( cudaMemcpyToSymbol(cleft,  &left.data,  sizeof(left.data)) );\r
  89             cudaSafeCall( cudaMemcpyToSymbol(cright, &right.data, sizeof(right.data)) );\r
  90             cudaSafeCall( cudaMemcpyToSymbol(ctemp, &temp.data, sizeof(temp.data)) );\r
  91         }\r
  92 \r
  93         ///////////////////////////////////////////////////////////////\r
  94         /////////////////////// init data cost ////////////////////////\r
  95         ///////////////////////////////////////////////////////////////\r
  96 \r
  97         template <int channels> struct DataCostPerPixel;\r
  98         template <> struct DataCostPerPixel<1>\r
  99         {\r
 100             static __device__ __forceinline__ float compute(const uchar* left, const uchar* right)\r
 101             {\r
 102                 return fmin(cdata_weight * ::abs((int)*left - *right), cdata_weight * cmax_data_term);\r
 103             }\r
 104         };\r
 105         template <> struct DataCostPerPixel<3>\r
 106         {\r
 107             static __device__ __forceinline__ float compute(const uchar* left, const uchar* right)\r
 108             {\r
 109                 float tb = 0.114f * ::abs((int)left[0] - right[0]);\r
 110                 float tg = 0.587f * ::abs((int)left[1] - right[1]);\r
 111                 float tr = 0.299f * ::abs((int)left[2] - right[2]);\r
 112 \r
 113                 return fmin(cdata_weight * (tr + tg + tb), cdata_weight * cmax_data_term);\r
 114             }\r
 115         };\r
 116         template <> struct DataCostPerPixel<4>\r
 117         {\r
 118             static __device__ __forceinline__ float compute(const uchar* left, const uchar* right)\r
 119             {\r
 120                 uchar4 l = *((const uchar4*)left);\r
 121                 uchar4 r = *((const uchar4*)right);\r
 122 \r
 123                 float tb = 0.114f * ::abs((int)l.x - r.x);\r
 124                 float tg = 0.587f * ::abs((int)l.y - r.y);\r
 125                 float tr = 0.299f * ::abs((int)l.z - r.z);\r
 126 \r
 127                 return fmin(cdata_weight * (tr + tg + tb), cdata_weight * cmax_data_term);\r
 128             }\r
 129         };\r
 130 \r
 131         template <typename T>\r
 132         __global__ void get_first_k_initial_global(T* data_cost_selected_, T *selected_disp_pyr, int h, int w, int nr_plane)\r
 133         {\r
 134             int x = blockIdx.x * blockDim.x + threadIdx.x;\r
 135             int y = blockIdx.y * blockDim.y + threadIdx.y;\r
 136 \r
 137             if (y < h && x < w)\r
 138             {\r
 139                 T* selected_disparity = selected_disp_pyr + y * cmsg_step + x;\r
 140                 T* data_cost_selected = data_cost_selected_ + y * cmsg_step + x;\r
 141                 T* data_cost = (T*)ctemp + y * cmsg_step + x;\r
 142 \r
 143                 for(int i = 0; i < nr_plane; i++)\r
 144                 {\r
 145                     T minimum = device::numeric_limits<T>::max();\r
 146                     int id = 0;\r
 147                     for(int d = 0; d < cndisp; d++)\r
 148                     {\r
 149                         T cur = data_cost[d * cdisp_step1];\r
 150                         if(cur < minimum)\r
 151                         {\r
 152                             minimum = cur;\r
 153                             id = d;\r
 154                         }\r
 155                     }\r
 156 \r
 157                     data_cost_selected[i  * cdisp_step1] = minimum;\r
 158                     selected_disparity[i  * cdisp_step1] = id;\r
 159                     data_cost         [id * cdisp_step1] = numeric_limits<T>::max();\r
 160                 }\r
 161             }\r
 162         }\r
 163 \r
 164 \r
 165         template <typename T>\r
 166         __global__ void get_first_k_initial_local(T* data_cost_selected_, T* selected_disp_pyr, int h, int w, int nr_plane)\r
 167         {\r
 168             int x = blockIdx.x * blockDim.x + threadIdx.x;\r
 169             int y = blockIdx.y * blockDim.y + threadIdx.y;\r
 170 \r
 171             if (y < h && x < w)\r
 172             {\r
 173                 T* selected_disparity = selected_disp_pyr + y * cmsg_step + x;\r
 174                 T* data_cost_selected = data_cost_selected_ + y * cmsg_step + x;\r
 175                 T* data_cost = (T*)ctemp + y * cmsg_step + x;\r
 176 \r
 177                 int nr_local_minimum = 0;\r
 178 \r
 179                 T prev = data_cost[0 * cdisp_step1];\r
 180                 T cur  = data_cost[1 * cdisp_step1];\r
 181                 T next = data_cost[2 * cdisp_step1];\r
 182 \r
 183                 for (int d = 1; d < cndisp - 1 && nr_local_minimum < nr_plane; d++)\r
 184                 {\r
 185                     if (cur < prev && cur < next)\r
 186                     {\r
 187                         data_cost_selected[nr_local_minimum * cdisp_step1] = cur;\r
 188                         selected_disparity[nr_local_minimum * cdisp_step1] = d;\r
 189 \r
 190                         data_cost[d * cdisp_step1] = numeric_limits<T>::max();\r
 191 \r
 192                         nr_local_minimum++;\r
 193                     }\r
 194                     prev = cur;\r
 195                     cur = next;\r
 196                     next = data_cost[(d + 1) * cdisp_step1];\r
 197                 }\r
 198 \r
 199                 for (int i = nr_local_minimum; i < nr_plane; i++)\r
 200                 {\r
 201                     T minimum = numeric_limits<T>::max();\r
 202                     int id = 0;\r
 203 \r
 204                     for (int d = 0; d < cndisp; d++)\r
 205                     {\r
 206                         cur = data_cost[d * cdisp_step1];\r
 207                         if (cur < minimum)\r
 208                         {\r
 209                             minimum = cur;\r
 210                             id = d;\r
 211                         }\r
 212                     }\r
 213                     data_cost_selected[i * cdisp_step1] = minimum;\r
 214                     selected_disparity[i * cdisp_step1] = id;\r
 215 \r
 216                     data_cost[id * cdisp_step1] = numeric_limits<T>::max();\r
 217                 }\r
 218             }\r
 219         }\r
 220 \r
 221         template <typename T, int channels>\r
 222         __global__ void init_data_cost(int h, int w, int level)\r
 223         {\r
 224             int x = blockIdx.x * blockDim.x + threadIdx.x;\r
 225             int y = blockIdx.y * blockDim.y + threadIdx.y;\r
 226 \r
 227             if (y < h && x < w)\r
 228             {\r
 229                 int y0 = y << level;\r
 230                 int yt = (y + 1) << level;\r
 231 \r
 232                 int x0 = x << level;\r
 233                 int xt = (x + 1) << level;\r
 234 \r
 235                 T* data_cost = (T*)ctemp + y * cmsg_step + x;\r
 236 \r
 237                 for(int d = 0; d < cndisp; ++d)\r
 238                 {\r
 239                     float val = 0.0f;\r
 240                     for(int yi = y0; yi < yt; yi++)\r
 241                     {\r
 242                         for(int xi = x0; xi < xt; xi++)\r
 243                         {\r
 244                             int xr = xi - d;\r
 245                             if(d < cth || xr < 0)\r
 246                                 val += cdata_weight * cmax_data_term;\r
 247                             else\r
 248                             {\r
 249                                 const uchar* lle = cleft + yi * cimg_step + xi * channels;\r
 250                                 const uchar* lri = cright + yi * cimg_step + xr * channels;\r
 251 \r
 252                                 val += DataCostPerPixel<channels>::compute(lle, lri);\r
 253                             }\r
 254                         }\r
 255                     }\r
 256                     data_cost[cdisp_step1 * d] = saturate_cast<T>(val);\r
 257                 }\r
 258             }\r
 259         }\r
 260 \r
 261         template <typename T, int winsz, int channels>\r
 262         __global__ void init_data_cost_reduce(int level, int rows, int cols, int h)\r
 263         {\r
 264             int x_out = blockIdx.x;\r
 265             int y_out = blockIdx.y % h;\r
 266             int d = (blockIdx.y / h) * blockDim.z + threadIdx.z;\r
 267 \r
 268             int tid = threadIdx.x;\r
 269 \r
 270             if (d < cndisp)\r
 271             {\r
 272                 int x0 = x_out << level;\r
 273                 int y0 = y_out << level;\r
 274 \r
 275                 int len = ::min(y0 + winsz, rows) - y0;\r
 276 \r
 277                 float val = 0.0f;\r
 278                 if (x0 + tid < cols)\r
 279                 {\r
 280                     if (x0 + tid - d < 0 || d < cth)\r
 281                         val = cdata_weight * cmax_data_term * len;\r
 282                     else\r
 283                     {\r
 284                         const uchar* lle =  cleft + y0 * cimg_step + channels * (x0 + tid    );\r
 285                         const uchar* lri = cright + y0 * cimg_step + channels * (x0 + tid - d);\r
 286 \r
 287                         for(int y = 0; y < len; ++y)\r
 288                         {\r
 289                             val += DataCostPerPixel<channels>::compute(lle, lri);\r
 290 \r
 291                             lle += cimg_step;\r
 292                             lri += cimg_step;\r
 293                         }\r
 294                     }\r
 295                 }\r
 296 \r
 297                 extern __shared__ float smem[];\r
 298                 float* dline = smem + winsz * threadIdx.z;\r
 299 \r
 300                 dline[tid] = val;\r
 301 \r
 302                 __syncthreads();\r
 303 \r
 304                 if (winsz >= 256) { if (tid < 128) { dline[tid] += dline[tid + 128]; } __syncthreads(); }\r
 305                 if (winsz >= 128) { if (tid <  64) { dline[tid] += dline[tid + 64]; } __syncthreads(); }\r
 306 \r
 307                         volatile float* vdline = smem + winsz * threadIdx.z;\r
 308 \r
 309                 if (winsz >= 64) if (tid < 32) vdline[tid] += vdline[tid + 32];\r
 310                 if (winsz >= 32) if (tid < 16) vdline[tid] += vdline[tid + 16];\r
 311                 if (winsz >= 16) if (tid <  8) vdline[tid] += vdline[tid + 8];\r
 312                 if (winsz >=  8) if (tid <  4) vdline[tid] += vdline[tid + 4];\r
 313                 if (winsz >=  4) if (tid <  2) vdline[tid] += vdline[tid + 2];\r
 314                 if (winsz >=  2) if (tid <  1) vdline[tid] += vdline[tid + 1];\r
 315 \r
 316                 T* data_cost = (T*)ctemp + y_out * cmsg_step + x_out;\r
 317 \r
 318                 if (tid == 0)\r
 319                     data_cost[cdisp_step1 * d] = saturate_cast<T>(dline[0]);\r
 320             }\r
 321         }\r
 322 \r
 323 \r
 324         template <typename T>\r
 325         void init_data_cost_caller_(int /*rows*/, int /*cols*/, int h, int w, int level, int /*ndisp*/, int channels, cudaStream_t stream)\r
 326         {\r
 327             dim3 threads(32, 8, 1);\r
 328             dim3 grid(1, 1, 1);\r
 329 \r
 330             grid.x = divUp(w, threads.x);\r
 331             grid.y = divUp(h, threads.y);\r
 332 \r
 333             switch (channels)\r
 334             {\r
 335             case 1: init_data_cost<T, 1><<<grid, threads, 0, stream>>>(h, w, level); break;\r
 336             case 3: init_data_cost<T, 3><<<grid, threads, 0, stream>>>(h, w, level); break;\r
 337             case 4: init_data_cost<T, 4><<<grid, threads, 0, stream>>>(h, w, level); break;\r
 338             default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__, "init_data_cost_caller_");\r
 339             }\r
 340         }\r
 341 \r
 342         template <typename T, int winsz>\r
 343         void init_data_cost_reduce_caller_(int rows, int cols, int h, int w, int level, int ndisp, int channels, cudaStream_t stream)\r
 344         {\r
 345             const int threadsNum = 256;\r
 346             const size_t smem_size = threadsNum * sizeof(float);\r
 347 \r
 348             dim3 threads(winsz, 1, threadsNum / winsz);\r
 349             dim3 grid(w, h, 1);\r
 350             grid.y *= divUp(ndisp, threads.z);\r
 351 \r
 352             switch (channels)\r
 353             {\r
 354             case 1: init_data_cost_reduce<T, winsz, 1><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
 355             case 3: init_data_cost_reduce<T, winsz, 3><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
 356             case 4: init_data_cost_reduce<T, winsz, 4><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
 357             default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__, "init_data_cost_reduce_caller_");\r
 358             }\r
 359         }\r
 360 \r
 361         template<class T>\r
 362         void init_data_cost(int rows, int cols, T* disp_selected_pyr, T* data_cost_selected, size_t msg_step,\r
 363                     int h, int w, int level, int nr_plane, int ndisp, int channels, bool use_local_init_data_cost, cudaStream_t stream)\r
 364         {\r
 365 \r
 366             typedef void (*InitDataCostCaller)(int cols, int rows, int w, int h, int level, int ndisp, int channels, cudaStream_t stream);\r
 367 \r
 368             static const InitDataCostCaller init_data_cost_callers[] =\r
 369             {\r
 370                 init_data_cost_caller_<T>, init_data_cost_caller_<T>, init_data_cost_reduce_caller_<T, 4>,\r
 371                 init_data_cost_reduce_caller_<T, 8>, init_data_cost_reduce_caller_<T, 16>, init_data_cost_reduce_caller_<T, 32>,\r
 372                 init_data_cost_reduce_caller_<T, 64>, init_data_cost_reduce_caller_<T, 128>, init_data_cost_reduce_caller_<T, 256>\r
 373             };\r
 374 \r
 375             size_t disp_step = msg_step * h;\r
 376             cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step, sizeof(size_t)) );\r
 377             cudaSafeCall( cudaMemcpyToSymbol(cmsg_step,  &msg_step,  sizeof(size_t)) );\r
 378 \r
 379             init_data_cost_callers[level](rows, cols, h, w, level, ndisp, channels, stream);\r
 380             cudaSafeCall( cudaGetLastError() );\r
 381 \r
 382             if (stream == 0)\r
 383                 cudaSafeCall( cudaDeviceSynchronize() );\r
 384 \r
 385             dim3 threads(32, 8, 1);\r
 386             dim3 grid(1, 1, 1);\r
 387 \r
 388             grid.x = divUp(w, threads.x);\r
 389             grid.y = divUp(h, threads.y);\r
 390 \r
 391             if (use_local_init_data_cost == true)\r
 392                 get_first_k_initial_local<<<grid, threads, 0, stream>>> (data_cost_selected, disp_selected_pyr, h, w, nr_plane);\r
 393             else\r
 394                 get_first_k_initial_global<<<grid, threads, 0, stream>>>(data_cost_selected, disp_selected_pyr, h, w, nr_plane);\r
 395             \r
 396             cudaSafeCall( cudaGetLastError() );\r
 397 \r
 398             if (stream == 0)\r
 399                 cudaSafeCall( cudaDeviceSynchronize() );\r
 400         }\r
 401 \r
 402         template void init_data_cost(int rows, int cols, short* disp_selected_pyr, short* data_cost_selected, size_t msg_step,\r
 403                     int h, int w, int level, int nr_plane, int ndisp, int channels, bool use_local_init_data_cost, cudaStream_t stream);\r
 404 \r
 405         template void init_data_cost(int rows, int cols, float* disp_selected_pyr, float* data_cost_selected, size_t msg_step,\r
 406                     int h, int w, int level, int nr_plane, int ndisp, int channels, bool use_local_init_data_cost, cudaStream_t stream);\r
 407 \r
 408         ///////////////////////////////////////////////////////////////\r
 409         ////////////////////// compute data cost //////////////////////\r
 410         ///////////////////////////////////////////////////////////////\r
 411 \r
 412         template <typename T, int channels>\r
 413         __global__ void compute_data_cost(const T* selected_disp_pyr, T* data_cost_, int h, int w, int level, int nr_plane)\r
 414         {\r
 415             int x = blockIdx.x * blockDim.x + threadIdx.x;\r
 416             int y = blockIdx.y * blockDim.y + threadIdx.y;\r
 417 \r
 418             if (y < h && x < w)\r
 419             {\r
 420                 int y0 = y << level;\r
 421                 int yt = (y + 1) << level;\r
 422 \r
 423                 int x0 = x << level;\r
 424                 int xt = (x + 1) << level;\r
 425 \r
 426                 const T* selected_disparity = selected_disp_pyr + y/2 * cmsg_step + x/2;\r
 427                 T* data_cost = data_cost_ + y * cmsg_step + x;\r
 428 \r
 429                 for(int d = 0; d < nr_plane; d++)\r
 430                 {\r
 431                     float val = 0.0f;\r
 432                     for(int yi = y0; yi < yt; yi++)\r
 433                     {\r
 434                         for(int xi = x0; xi < xt; xi++)\r
 435                         {\r
 436                             int sel_disp = selected_disparity[d * cdisp_step2];\r
 437                             int xr = xi - sel_disp;\r
 438 \r
 439                             if (xr < 0 || sel_disp < cth)\r
 440                                 val += cdata_weight * cmax_data_term;\r
 441                             else\r
 442                             {\r
 443                                 const uchar* left_x = cleft + yi * cimg_step + xi * channels;\r
 444                                 const uchar* right_x = cright + yi * cimg_step + xr * channels;\r
 445 \r
 446                                 val += DataCostPerPixel<channels>::compute(left_x, right_x);\r
 447                             }\r
 448                         }\r
 449                     }\r
 450                     data_cost[cdisp_step1 * d] = saturate_cast<T>(val);\r
 451                 }\r
 452             }\r
 453         }\r
 454 \r
 455         template <typename T, int winsz, int channels>\r
 456         __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
 457         {\r
 458             int x_out = blockIdx.x;\r
 459             int y_out = blockIdx.y % h;\r
 460             int d = (blockIdx.y / h) * blockDim.z + threadIdx.z;\r
 461 \r
 462             int tid = threadIdx.x;\r
 463 \r
 464             const T* selected_disparity = selected_disp_pyr + y_out/2 * cmsg_step + x_out/2;\r
 465             T* data_cost = data_cost_ + y_out * cmsg_step + x_out;\r
 466 \r
 467             if (d < nr_plane)\r
 468             {\r
 469                 int sel_disp = selected_disparity[d * cdisp_step2];\r
 470 \r
 471                 int x0 = x_out << level;\r
 472                 int y0 = y_out << level;\r
 473 \r
 474                 int len = ::min(y0 + winsz, rows) - y0;\r
 475 \r
 476                 float val = 0.0f;\r
 477                 if (x0 + tid < cols)\r
 478                 {\r
 479                     if (x0 + tid - sel_disp < 0 || sel_disp < cth)\r
 480                         val = cdata_weight * cmax_data_term * len;\r
 481                     else\r
 482                     {\r
 483                         const uchar* lle =  cleft + y0 * cimg_step + channels * (x0 + tid    );\r
 484                         const uchar* lri = cright + y0 * cimg_step + channels * (x0 + tid - sel_disp);\r
 485 \r
 486                         for(int y = 0; y < len; ++y)\r
 487                         {\r
 488                             val += DataCostPerPixel<channels>::compute(lle, lri);\r
 489 \r
 490                             lle += cimg_step;\r
 491                             lri += cimg_step;\r
 492                         }\r
 493                     }\r
 494                 }\r
 495 \r
 496                 extern __shared__ float smem[];\r
 497                 float* dline = smem + winsz * threadIdx.z;\r
 498 \r
 499                 dline[tid] = val;\r
 500 \r
 501                 __syncthreads();\r
 502 \r
 503                 if (winsz >= 256) { if (tid < 128) { dline[tid] += dline[tid + 128]; } __syncthreads(); }\r
 504                 if (winsz >= 128) { if (tid <  64) { dline[tid] += dline[tid +  64]; } __syncthreads(); }\r
 505 \r
 506                         volatile float* vdline = smem + winsz * threadIdx.z;\r
 507 \r
 508                 if (winsz >= 64) if (tid < 32) vdline[tid] += vdline[tid + 32];\r
 509                 if (winsz >= 32) if (tid < 16) vdline[tid] += vdline[tid + 16];\r
 510                 if (winsz >= 16) if (tid <  8) vdline[tid] += vdline[tid + 8];\r
 511                 if (winsz >=  8) if (tid <  4) vdline[tid] += vdline[tid + 4];\r
 512                 if (winsz >=  4) if (tid <  2) vdline[tid] += vdline[tid + 2];\r
 513                 if (winsz >=  2) if (tid <  1) vdline[tid] += vdline[tid + 1];\r
 514 \r
 515                 if (tid == 0)\r
 516                     data_cost[cdisp_step1 * d] = saturate_cast<T>(dline[0]);\r
 517             }\r
 518         }\r
 519 \r
 520         template <typename T>\r
 521         void compute_data_cost_caller_(const T* disp_selected_pyr, T* data_cost, int /*rows*/, int /*cols*/,\r
 522                                       int h, int w, int level, int nr_plane, int channels, cudaStream_t stream)\r
 523         {\r
 524             dim3 threads(32, 8, 1);\r
 525             dim3 grid(1, 1, 1);\r
 526 \r
 527             grid.x = divUp(w, threads.x);\r
 528             grid.y = divUp(h, threads.y);\r
 529 \r
 530             switch(channels)\r
 531             {\r
 532             case 1: compute_data_cost<T, 1><<<grid, threads, 0, stream>>>(disp_selected_pyr, data_cost, h, w, level, nr_plane); break;\r
 533             case 3: compute_data_cost<T, 3><<<grid, threads, 0, stream>>>(disp_selected_pyr, data_cost, h, w, level, nr_plane); break;\r
 534             case 4: compute_data_cost<T, 4><<<grid, threads, 0, stream>>>(disp_selected_pyr, data_cost, h, w, level, nr_plane); break;\r
 535             default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__, "compute_data_cost_caller_");\r
 536             }\r
 537         }\r
 538 \r
 539         template <typename T, int winsz>\r
 540         void compute_data_cost_reduce_caller_(const T* disp_selected_pyr, T* data_cost, int rows, int cols,\r
 541                                       int h, int w, int level, int nr_plane, int channels, cudaStream_t stream)\r
 542         {\r
 543             const int threadsNum = 256;\r
 544             const size_t smem_size = threadsNum * sizeof(float);\r
 545 \r
 546             dim3 threads(winsz, 1, threadsNum / winsz);\r
 547             dim3 grid(w, h, 1);\r
 548             grid.y *= divUp(nr_plane, threads.z);\r
 549 \r
 550             switch (channels)\r
 551             {\r
 552             case 1: compute_data_cost_reduce<T, winsz, 1><<<grid, threads, smem_size, stream>>>(disp_selected_pyr, data_cost, level, rows, cols, h, nr_plane); break;\r
 553             case 3: compute_data_cost_reduce<T, winsz, 3><<<grid, threads, smem_size, stream>>>(disp_selected_pyr, data_cost, level, rows, cols, h, nr_plane); break;\r
 554             case 4: compute_data_cost_reduce<T, winsz, 4><<<grid, threads, smem_size, stream>>>(disp_selected_pyr, data_cost, level, rows, cols, h, nr_plane); break;\r
 555             default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__, "compute_data_cost_reduce_caller_");\r
 556             }\r
 557         }\r
 558 \r
 559         template<class T>\r
 560         void compute_data_cost(const T* disp_selected_pyr, T* data_cost, size_t msg_step,\r
 561                                int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, cudaStream_t stream)\r
 562         {\r
 563             typedef void (*ComputeDataCostCaller)(const T* disp_selected_pyr, T* data_cost, int rows, int cols,\r
 564                 int h, int w, int level, int nr_plane, int channels, cudaStream_t stream);\r
 565 \r
 566             static const ComputeDataCostCaller callers[] =\r
 567             {\r
 568                 compute_data_cost_caller_<T>, compute_data_cost_caller_<T>, compute_data_cost_reduce_caller_<T, 4>,\r
 569                 compute_data_cost_reduce_caller_<T, 8>, compute_data_cost_reduce_caller_<T, 16>, compute_data_cost_reduce_caller_<T, 32>,\r
 570                 compute_data_cost_reduce_caller_<T, 64>, compute_data_cost_reduce_caller_<T, 128>, compute_data_cost_reduce_caller_<T, 256>\r
 571             };\r
 572 \r
 573             size_t disp_step1 = msg_step * h;\r
 574             size_t disp_step2 = msg_step * h2;\r
 575             cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step1, sizeof(size_t)) );\r
 576             cudaSafeCall( cudaMemcpyToSymbol(cdisp_step2, &disp_step2, sizeof(size_t)) );\r
 577             cudaSafeCall( cudaMemcpyToSymbol(cmsg_step,  &msg_step,  sizeof(size_t)) );\r
 578             \r
 579             callers[level](disp_selected_pyr, data_cost, rows, cols, h, w, level, nr_plane, channels, stream);\r
 580             cudaSafeCall( cudaGetLastError() );\r
 581 \r
 582             if (stream == 0)\r
 583                 cudaSafeCall( cudaDeviceSynchronize() );\r
 584         }\r
 585 \r
 586         template void compute_data_cost(const short* disp_selected_pyr, short* data_cost, size_t msg_step,\r
 587                                int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, cudaStream_t stream);\r
 588 \r
 589         template void compute_data_cost(const float* disp_selected_pyr, float* data_cost, size_t msg_step,\r
 590                                int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, cudaStream_t stream);\r
 591              \r
 592 \r
 593         ///////////////////////////////////////////////////////////////\r
 594         //////////////////////// init message /////////////////////////\r
 595         ///////////////////////////////////////////////////////////////\r
 596 \r
 597          \r
 598          template <typename T>\r
 599         __device__ void get_first_k_element_increase(T* u_new, T* d_new, T* l_new, T* r_new,\r
 600                                                      const T* u_cur, const T* d_cur, const T* l_cur, const T* r_cur,\r
 601                                                      T* data_cost_selected, T* disparity_selected_new, T* data_cost_new,\r
 602                                                      const T* data_cost_cur, const T* disparity_selected_cur,\r
 603                                                      int nr_plane, int nr_plane2)\r
 604         {\r
 605             for(int i = 0; i < nr_plane; i++)\r
 606             {\r
 607                 T minimum = numeric_limits<T>::max();\r
 608                 int id = 0;\r
 609                 for(int j = 0; j < nr_plane2; j++)\r
 610                 {\r
 611                     T cur = data_cost_new[j * cdisp_step1];\r
 612                     if(cur < minimum)\r
 613                     {\r
 614                         minimum = cur;\r
 615                         id = j;\r
 616                     }\r
 617                 }\r
 618 \r
 619                 data_cost_selected[i * cdisp_step1] = data_cost_cur[id * cdisp_step1];\r
 620                 disparity_selected_new[i * cdisp_step1] = disparity_selected_cur[id * cdisp_step2];\r
 621 \r
 622                 u_new[i * cdisp_step1] = u_cur[id * cdisp_step2];\r
 623                 d_new[i * cdisp_step1] = d_cur[id * cdisp_step2];\r
 624                 l_new[i * cdisp_step1] = l_cur[id * cdisp_step2];\r
 625                 r_new[i * cdisp_step1] = r_cur[id * cdisp_step2];\r
 626 \r
 627                 data_cost_new[id * cdisp_step1] = numeric_limits<T>::max();\r
 628             }\r
 629         }\r
 630 \r
 631         template <typename T>\r
 632         __global__ void init_message(T* u_new_, T* d_new_, T* l_new_, T* r_new_,\r
 633                                      const T* u_cur_, const T* d_cur_, const T* l_cur_, const T* r_cur_,\r
 634                                      T* selected_disp_pyr_new, const T* selected_disp_pyr_cur,\r
 635                                      T* data_cost_selected_, const T* data_cost_,\r
 636                                      int h, int w, int nr_plane, int h2, int w2, int nr_plane2)\r
 637         {\r
 638             int x = blockIdx.x * blockDim.x + threadIdx.x;\r
 639             int y = blockIdx.y * blockDim.y + threadIdx.y;\r
 640 \r
 641             if (y < h && x < w)\r
 642             {\r
 643                 const T* u_cur = u_cur_ + ::min(h2-1, y/2 + 1) * cmsg_step + x/2;\r
 644                 const T* d_cur = d_cur_ + ::max(0, y/2 - 1)    * cmsg_step + x/2;\r
 645                 const T* l_cur = l_cur_ + (y/2)                * cmsg_step + ::min(w2-1, x/2 + 1);\r
 646                 const T* r_cur = r_cur_ + (y/2)                * cmsg_step + ::max(0, x/2 - 1);\r
 647 \r
 648                 T* data_cost_new = (T*)ctemp + y * cmsg_step + x;\r
 649 \r
 650                 const T* disparity_selected_cur = selected_disp_pyr_cur + y/2 * cmsg_step + x/2;\r
 651                 const T* data_cost = data_cost_ + y * cmsg_step + x;\r
 652 \r
 653                 for(int d = 0; d < nr_plane2; d++)\r
 654                 {\r
 655                     int idx2 = d * cdisp_step2;\r
 656 \r
 657                     T val  = data_cost[d * cdisp_step1] + u_cur[idx2] + d_cur[idx2] + l_cur[idx2] + r_cur[idx2];\r
 658                     data_cost_new[d * cdisp_step1] = val;\r
 659                 }\r
 660 \r
 661                 T* data_cost_selected = data_cost_selected_ + y * cmsg_step + x;\r
 662                 T* disparity_selected_new = selected_disp_pyr_new + y * cmsg_step + x;\r
 663 \r
 664                 T* u_new = u_new_ + y * cmsg_step + x;\r
 665                 T* d_new = d_new_ + y * cmsg_step + x;\r
 666                 T* l_new = l_new_ + y * cmsg_step + x;\r
 667                 T* r_new = r_new_ + y * cmsg_step + x;\r
 668 \r
 669                 u_cur = u_cur_ + y/2 * cmsg_step + x/2;\r
 670                 d_cur = d_cur_ + y/2 * cmsg_step + x/2;\r
 671                 l_cur = l_cur_ + y/2 * cmsg_step + x/2;\r
 672                 r_cur = r_cur_ + y/2 * cmsg_step + x/2;\r
 673 \r
 674                 get_first_k_element_increase(u_new, d_new, l_new, r_new, u_cur, d_cur, l_cur, r_cur,\r
 675                                              data_cost_selected, disparity_selected_new, data_cost_new,\r
 676                                              data_cost, disparity_selected_cur, nr_plane, nr_plane2);\r
 677             }\r
 678         }\r
 679 \r
 680 \r
 681         template<class T>\r
 682         void init_message(T* u_new, T* d_new, T* l_new, T* r_new,\r
 683                           const T* u_cur, const T* d_cur, const T* l_cur, const T* r_cur,\r
 684                           T* selected_disp_pyr_new, const T* selected_disp_pyr_cur,\r
 685                           T* data_cost_selected, const T* data_cost, size_t msg_step,\r
 686                           int h, int w, int nr_plane, int h2, int w2, int nr_plane2, cudaStream_t stream)\r
 687         {\r
 688 \r
 689             size_t disp_step1 = msg_step * h;\r
 690             size_t disp_step2 = msg_step * h2;\r
 691             cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step1, sizeof(size_t)) );\r
 692             cudaSafeCall( cudaMemcpyToSymbol(cdisp_step2, &disp_step2, sizeof(size_t)) );\r
 693             cudaSafeCall( cudaMemcpyToSymbol(cmsg_step,   &msg_step, sizeof(size_t)) );\r
 694             \r
 695             dim3 threads(32, 8, 1);\r
 696             dim3 grid(1, 1, 1);\r
 697 \r
 698             grid.x = divUp(w, threads.x);\r
 699             grid.y = divUp(h, threads.y);\r
 700 \r
 701             init_message<<<grid, threads, 0, stream>>>(u_new, d_new, l_new, r_new,\r
 702                                                        u_cur, d_cur, l_cur, r_cur,\r
 703                                                        selected_disp_pyr_new, selected_disp_pyr_cur,\r
 704                                                        data_cost_selected, data_cost,\r
 705                                                        h, w, nr_plane, h2, w2, nr_plane2);\r
 706             cudaSafeCall( cudaGetLastError() );\r
 707 \r
 708             if (stream == 0)\r
 709                 cudaSafeCall( cudaDeviceSynchronize() );\r
 710         }\r
 711 \r
 712 \r
 713         template void init_message(short* u_new, short* d_new, short* l_new, short* r_new,\r
 714                           const short* u_cur, const short* d_cur, const short* l_cur, const short* r_cur,\r
 715                           short* selected_disp_pyr_new, const short* selected_disp_pyr_cur,\r
 716                           short* data_cost_selected, const short* data_cost, size_t msg_step,\r
 717                           int h, int w, int nr_plane, int h2, int w2, int nr_plane2, cudaStream_t stream);\r
 718 \r
 719         template void init_message(float* u_new, float* d_new, float* l_new, float* r_new,\r
 720                           const float* u_cur, const float* d_cur, const float* l_cur, const float* r_cur,\r
 721                           float* selected_disp_pyr_new, const float* selected_disp_pyr_cur,\r
 722                           float* data_cost_selected, const float* data_cost, size_t msg_step,\r
 723                           int h, int w, int nr_plane, int h2, int w2, int nr_plane2, cudaStream_t stream);        \r
 724 \r
 725         ///////////////////////////////////////////////////////////////\r
 726         ////////////////////  calc all iterations /////////////////////\r
 727         ///////////////////////////////////////////////////////////////\r
 728 \r
 729         template <typename T>\r
 730         __device__ void message_per_pixel(const T* data, T* msg_dst, const T* msg1, const T* msg2, const T* msg3,\r
 731                                           const T* dst_disp, const T* src_disp, int nr_plane, volatile T* temp)\r
 732         {\r
 733             T minimum = numeric_limits<T>::max();\r
 734 \r
 735             for(int d = 0; d < nr_plane; d++)\r
 736             {\r
 737                 int idx = d * cdisp_step1;\r
 738                 T val  = data[idx] + msg1[idx] + msg2[idx] + msg3[idx];\r
 739 \r
 740                 if(val < minimum)\r
 741                     minimum = val;\r
 742 \r
 743                 msg_dst[idx] = val;\r
 744             }\r
 745 \r
 746             float sum = 0;\r
 747             for(int d = 0; d < nr_plane; d++)\r
 748             {\r
 749                 float cost_min = minimum + cmax_disc_term;\r
 750                 T src_disp_reg = src_disp[d * cdisp_step1];\r
 751 \r
 752                 for(int d2 = 0; d2 < nr_plane; d2++)\r
 753                     cost_min = fmin(cost_min, msg_dst[d2 * cdisp_step1] + cdisc_single_jump * ::abs(dst_disp[d2 * cdisp_step1] - src_disp_reg));\r
 754 \r
 755                 temp[d * cdisp_step1] = saturate_cast<T>(cost_min);\r
 756                 sum += cost_min;\r
 757             }\r
 758             sum /= nr_plane;\r
 759 \r
 760             for(int d = 0; d < nr_plane; d++)\r
 761                 msg_dst[d * cdisp_step1] = saturate_cast<T>(temp[d * cdisp_step1] - sum);\r
 762         }\r
 763 \r
 764         template <typename T>\r
 765         __global__ void compute_message(T* u_, T* d_, T* l_, T* r_, const T* data_cost_selected, const T* selected_disp_pyr_cur, int h, int w, int nr_plane, int i)\r
 766         {\r
 767             int y = blockIdx.y * blockDim.y + threadIdx.y;\r
 768             int x = ((blockIdx.x * blockDim.x + threadIdx.x) << 1) + ((y + i) & 1);\r
 769 \r
 770             if (y > 0 && y < h - 1 && x > 0 && x < w - 1)\r
 771             {\r
 772                 const T* data = data_cost_selected + y * cmsg_step + x;\r
 773 \r
 774                 T* u = u_ + y * cmsg_step + x;\r
 775                 T* d = d_ + y * cmsg_step + x;\r
 776                 T* l = l_ + y * cmsg_step + x;\r
 777                 T* r = r_ + y * cmsg_step + x;\r
 778 \r
 779                 const T* disp = selected_disp_pyr_cur + y * cmsg_step + x;\r
 780 \r
 781                 T* temp = (T*)ctemp + y * cmsg_step + x;\r
 782 \r
 783                 message_per_pixel(data, u, r - 1, u + cmsg_step, l + 1, disp, disp - cmsg_step, nr_plane, temp);\r
 784                 message_per_pixel(data, d, d - cmsg_step, r - 1, l + 1, disp, disp + cmsg_step, nr_plane, temp);\r
 785                 message_per_pixel(data, l, u + cmsg_step, d - cmsg_step, l + 1, disp, disp - 1, nr_plane, temp);\r
 786                 message_per_pixel(data, r, u + cmsg_step, d - cmsg_step, r - 1, disp, disp + 1, nr_plane, temp);\r
 787             }\r
 788         }\r
 789 \r
 790 \r
 791         template<class T>\r
 792         void calc_all_iterations(T* u, T* d, T* l, T* r, const T* data_cost_selected,\r
 793             const T* selected_disp_pyr_cur, size_t msg_step, int h, int w, int nr_plane, int iters, cudaStream_t stream)\r
 794         {\r
 795             size_t disp_step = msg_step * h;\r
 796             cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step, sizeof(size_t)) );\r
 797             cudaSafeCall( cudaMemcpyToSymbol(cmsg_step,  &msg_step,  sizeof(size_t)) );\r
 798 \r
 799             dim3 threads(32, 8, 1);\r
 800             dim3 grid(1, 1, 1);\r
 801 \r
 802             grid.x = divUp(w, threads.x << 1);\r
 803             grid.y = divUp(h, threads.y);\r
 804 \r
 805             for(int t = 0; t < iters; ++t)\r
 806             {\r
 807                 compute_message<<<grid, threads, 0, stream>>>(u, d, l, r, data_cost_selected, selected_disp_pyr_cur, h, w, nr_plane, t & 1);\r
 808                 cudaSafeCall( cudaGetLastError() );                \r
 809             }\r
 810                         if (stream == 0)\r
 811                     cudaSafeCall( cudaDeviceSynchronize() );\r
 812         };\r
 813 \r
 814         template void calc_all_iterations(short* u, short* d, short* l, short* r, const short* data_cost_selected, const short* selected_disp_pyr_cur, size_t msg_step,\r
 815             int h, int w, int nr_plane, int iters, cudaStream_t stream);\r
 816 \r
 817         template void calc_all_iterations(float* u, float* d, float* l, float* r, const float* data_cost_selected, const float* selected_disp_pyr_cur, size_t msg_step, \r
 818             int h, int w, int nr_plane, int iters, cudaStream_t stream);\r
 819 \r
 820 \r
 821         ///////////////////////////////////////////////////////////////\r
 822         /////////////////////////// output ////////////////////////////\r
 823         ///////////////////////////////////////////////////////////////\r
 824 \r
 825 \r
 826         template <typename T>\r
 827         __global__ void compute_disp(const T* u_, const T* d_, const T* l_, const T* r_,\r
 828                                      const T* data_cost_selected, const T* disp_selected_pyr,\r
 829                                      PtrStepSz<short> disp, int nr_plane)\r
 830         {\r
 831             int x = blockIdx.x * blockDim.x + threadIdx.x;\r
 832             int y = blockIdx.y * blockDim.y + threadIdx.y;\r
 833 \r
 834             if (y > 0 && y < disp.rows - 1 && x > 0 && x < disp.cols - 1)\r
 835             {\r
 836                 const T* data = data_cost_selected + y * cmsg_step + x;\r
 837                 const T* disp_selected = disp_selected_pyr + y * cmsg_step + x;\r
 838 \r
 839                 const T* u = u_ + (y+1) * cmsg_step + (x+0);\r
 840                 const T* d = d_ + (y-1) * cmsg_step + (x+0);\r
 841                 const T* l = l_ + (y+0) * cmsg_step + (x+1);\r
 842                 const T* r = r_ + (y+0) * cmsg_step + (x-1);\r
 843 \r
 844                 int best = 0;\r
 845                 T best_val = numeric_limits<T>::max();\r
 846                 for (int i = 0; i < nr_plane; ++i)\r
 847                 {\r
 848                     int idx = i * cdisp_step1;\r
 849                     T val = data[idx]+ u[idx] + d[idx] + l[idx] + r[idx];\r
 850 \r
 851                     if (val < best_val)\r
 852                     {\r
 853                         best_val = val;\r
 854                         best = saturate_cast<short>(disp_selected[idx]);\r
 855                     }\r
 856                 }\r
 857                                 disp(y, x) = best;\r
 858             }\r
 859         }\r
 860 \r
 861         template<class T>\r
 862         void compute_disp(const T* u, const T* d, const T* l, const T* r, const T* data_cost_selected, const T* disp_selected, size_t msg_step,\r
 863             const DevMem2D_<short>& disp, int nr_plane, cudaStream_t stream)\r
 864         {\r
 865             size_t disp_step = disp.rows * msg_step;\r
 866             cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step, sizeof(size_t)) );\r
 867             cudaSafeCall( cudaMemcpyToSymbol(cmsg_step,  &msg_step,  sizeof(size_t)) );\r
 868 \r
 869             dim3 threads(32, 8, 1);\r
 870             dim3 grid(1, 1, 1);\r
 871 \r
 872             grid.x = divUp(disp.cols, threads.x);\r
 873             grid.y = divUp(disp.rows, threads.y);\r
 874 \r
 875             compute_disp<<<grid, threads, 0, stream>>>(u, d, l, r, data_cost_selected, disp_selected, disp, nr_plane);\r
 876             cudaSafeCall( cudaGetLastError() );\r
 877 \r
 878             if (stream == 0)\r
 879                 cudaSafeCall( cudaDeviceSynchronize() );\r
 880         }\r
 881 \r
 882         template void compute_disp(const short* u, const short* d, const short* l, const short* r, const short* data_cost_selected, const short* disp_selected, size_t msg_step, \r
 883             const DevMem2D_<short>& disp, int nr_plane, cudaStream_t stream);\r
 884 \r
 885         template void compute_disp(const float* u, const float* d, const float* l, const float* r, const float* data_cost_selected, const float* disp_selected, size_t msg_step,\r
 886             const DevMem2D_<short>& disp, int nr_plane, cudaStream_t stream);\r
 887     } // namespace stereocsbp\r
 888 }}} // namespace cv { namespace gpu { namespace device {\r