+++ /dev/null
-/*M///////////////////////////////////////////////////////////////////////////////////////\r
-//\r
-// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
-//\r
-// By downloading, copying, installing or using the software you agree to this license.\r
-// If you do not agree to this license, do not download, install,\r
-// copy or use the software.\r
-//\r
-//\r
-// License Agreement\r
-// For Open Source Computer Vision Library\r
-//\r
-// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.\r
-// Copyright (C) 2009, Willow Garage Inc., all rights reserved.\r
-// Third party copyrights are property of their respective owners.\r
-//\r
-// Redistribution and use in source and binary forms, with or without modification,\r
-// are permitted provided that the following conditions are met:\r
-//\r
-// * Redistribution's of source code must retain the above copyright notice,\r
-// this list of conditions and the following disclaimer.\r
-//\r
-// * Redistribution's in binary form must reproduce the above copyright notice,\r
-// this list of conditions and the following disclaimer in the documentation\r
-// and/or other GpuMaterials provided with the distribution.\r
-//\r
-// * The name of the copyright holders may not be used to endorse or promote products\r
-// derived from this software without specific prior written permission.\r
-//\r
-// This software is provided by the copyright holders and contributors "as is" and\r
-// any express or implied warranties, including, but not limited to, the implied\r
-// warranties of merchantability and fitness for a particular purpose are disclaimed.\r
-// In no event shall the Intel Corporation or contributors be liable for any direct,\r
-// indirect, incidental, special, exemplary, or consequential damages\r
-// (including, but not limited to, procurement of substitute goods or services;\r
-// loss of use, data, or profits; or business interruption) however caused\r
-// and on any theory of liability, whether in contract, strict liability,\r
-// or tort (including negligence or otherwise) arising in any way out of\r
-// the use of this software, even if advised of the possibility of such damage.\r
-//\r
-//M*/\r
-\r
-#include "precomp.hpp"\r
-\r
-using namespace cv;\r
-using namespace cv::gpu;\r
-using namespace std;\r
-\r
-#if !defined (HAVE_CUDA)\r
-\r
-void cv::gpu::StereoConstantSpaceBP::estimateRecommendedParams(int, int, int&, int&, int&, int&) { throw_nogpu(); }\r
-\r
-cv::gpu::StereoConstantSpaceBP::StereoConstantSpaceBP(int, int, int, int, int) { throw_nogpu(); }\r
-cv::gpu::StereoConstantSpaceBP::StereoConstantSpaceBP(int, int, int, int, float, float, float, float, int, int) { throw_nogpu(); }\r
-\r
-void cv::gpu::StereoConstantSpaceBP::operator()(const GpuMat&, const GpuMat&, GpuMat&) { throw_nogpu(); }\r
-void cv::gpu::StereoConstantSpaceBP::operator()(const GpuMat&, const GpuMat&, GpuMat&, Stream&) { throw_nogpu(); }\r
-\r
-#else /* !defined (HAVE_CUDA) */\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, int min_disp_th,\r
- const DevMem2D& left, const DevMem2D& right, const DevMem2D& temp);\r
-\r
- template<class T>\r
- void init_data_cost(int rows, int cols, T* disp_selected_pyr, T* data_cost_selected, size_t msg_step,\r
- int h, int w, int level, int nr_plane, int ndisp, int channels, bool use_local_init_data_cost, cudaStream_t stream);\r
-\r
- template<class T>\r
- void compute_data_cost(const T* disp_selected_pyr, T* data_cost, size_t msg_step1, size_t msg_step2,\r
- int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, cudaStream_t stream);\r
-\r
- template<class T>\r
- void init_message(T* u_new, T* d_new, T* l_new, T* r_new,\r
- const T* u_cur, const T* d_cur, const T* l_cur, const T* r_cur,\r
- T* selected_disp_pyr_new, const T* selected_disp_pyr_cur,\r
- T* data_cost_selected, const T* data_cost, size_t msg_step1, size_t msg_step2,\r
- int h, int w, int nr_plane, int h2, int w2, int nr_plane2, cudaStream_t stream);\r
-\r
- template<class T>\r
- void calc_all_iterations(T* u, T* d, T* l, T* r, const T* data_cost_selected,\r
- const T* selected_disp_pyr_cur, size_t msg_step, int h, int w, int nr_plane, int iters, cudaStream_t stream);\r
-\r
- template<class T> \r
- 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
- const DevMem2D_<short>& disp, int nr_plane, cudaStream_t stream);\r
-\r
-}}}\r
-\r
-namespace\r
-{\r
- const float DEFAULT_MAX_DATA_TERM = 30.0f;\r
- const float DEFAULT_DATA_WEIGHT = 1.0f;\r
- const float DEFAULT_MAX_DISC_TERM = 160.0f;\r
- const float DEFAULT_DISC_SINGLE_JUMP = 10.0f;\r
-}\r
-\r
-void cv::gpu::StereoConstantSpaceBP::estimateRecommendedParams(int width, int height, int& ndisp, int& iters, int& levels, int& nr_plane)\r
-{\r
- ndisp = (int) ((float) width / 3.14f);\r
- if ((ndisp & 1) != 0) \r
- ndisp++;\r
-\r
- int mm = ::max(width, height);\r
- iters = mm / 100 + ((mm > 1200)? - 4 : 4);\r
-\r
- levels = (int)::log(static_cast<double>(mm)) * 2 / 3;\r
- if (levels == 0) levels++;\r
-\r
- nr_plane = (int) ((float) ndisp / pow(2.0, levels + 1));\r
-}\r
-\r
-cv::gpu::StereoConstantSpaceBP::StereoConstantSpaceBP(int ndisp_, int iters_, int levels_, int nr_plane_,\r
- int msg_type_)\r
-\r
- : ndisp(ndisp_), iters(iters_), levels(levels_), nr_plane(nr_plane_),\r
- max_data_term(DEFAULT_MAX_DATA_TERM), data_weight(DEFAULT_DATA_WEIGHT),\r
- max_disc_term(DEFAULT_MAX_DISC_TERM), disc_single_jump(DEFAULT_DISC_SINGLE_JUMP), min_disp_th(0),\r
- msg_type(msg_type_), use_local_init_data_cost(true)\r
-{\r
- CV_Assert(msg_type_ == CV_32F || msg_type_ == CV_16S);\r
-}\r
-\r
-cv::gpu::StereoConstantSpaceBP::StereoConstantSpaceBP(int ndisp_, int iters_, int levels_, int nr_plane_,\r
- float max_data_term_, float data_weight_, float max_disc_term_, float disc_single_jump_,\r
- int min_disp_th_, int msg_type_)\r
- : ndisp(ndisp_), iters(iters_), levels(levels_), nr_plane(nr_plane_),\r
- max_data_term(max_data_term_), data_weight(data_weight_),\r
- max_disc_term(max_disc_term_), disc_single_jump(disc_single_jump_), min_disp_th(min_disp_th_),\r
- msg_type(msg_type_), use_local_init_data_cost(true)\r
-{\r
- CV_Assert(msg_type_ == CV_32F || msg_type_ == CV_16S);\r
-}\r
-\r
-template<class T>\r
-static void csbp_operator(StereoConstantSpaceBP& rthis, 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& temp, GpuMat& out, const GpuMat& left, const GpuMat& right, GpuMat& disp, cudaStream_t stream)\r
-{\r
- CV_DbgAssert(0 < rthis.ndisp && 0 < rthis.iters && 0 < rthis.levels && 0 < rthis.nr_plane\r
- && left.rows == right.rows && left.cols == right.cols && left.type() == right.type());\r
-\r
- CV_Assert(rthis.levels <= 8 && (left.type() == CV_8UC1 || left.type() == CV_8UC3 || left.type() == CV_8UC4));\r
-\r
- const Scalar zero = Scalar::all(0);\r
-\r
- ////////////////////////////////////////////////////////////////////////////////////////////\r
- // Init\r
-\r
- int rows = left.rows;\r
- int cols = left.cols;\r
-\r
- rthis.levels = min(rthis.levels, int(log((double)rthis.ndisp) / log(2.0)));\r
- int levels = rthis.levels;\r
-\r
- AutoBuffer<int> buf(levels * 4);\r
-\r
- int* cols_pyr = buf;\r
- int* rows_pyr = cols_pyr + levels;\r
- int* nr_plane_pyr = rows_pyr + levels;\r
- int* step_pyr = nr_plane_pyr + levels;\r
-\r
- cols_pyr[0] = cols;\r
- rows_pyr[0] = rows;\r
- nr_plane_pyr[0] = rthis.nr_plane;\r
-\r
- const int n = 64;\r
- step_pyr[0] = alignSize(cols * sizeof(T), n) / sizeof(T);\r
- for (int i = 1; i < levels; i++)\r
- {\r
- cols_pyr[i] = (cols_pyr[i-1] + 1) / 2;\r
- rows_pyr[i] = (rows_pyr[i-1] + 1) / 2;\r
-\r
- nr_plane_pyr[i] = nr_plane_pyr[i-1] * 2;\r
-\r
- step_pyr[i] = alignSize(cols_pyr[i] * sizeof(T), n) / sizeof(T);\r
- }\r
-\r
- Size msg_size(step_pyr[0], rows * nr_plane_pyr[0]);\r
- Size data_cost_size(step_pyr[0], rows * nr_plane_pyr[0] * 2);\r
-\r
- u[0].create(msg_size, DataType<T>::type);\r
- d[0].create(msg_size, DataType<T>::type);\r
- l[0].create(msg_size, DataType<T>::type);\r
- r[0].create(msg_size, DataType<T>::type);\r
-\r
- u[1].create(msg_size, DataType<T>::type);\r
- d[1].create(msg_size, DataType<T>::type);\r
- l[1].create(msg_size, DataType<T>::type);\r
- r[1].create(msg_size, DataType<T>::type);\r
-\r
- disp_selected_pyr[0].create(msg_size, DataType<T>::type);\r
- disp_selected_pyr[1].create(msg_size, DataType<T>::type);\r
-\r
- data_cost.create(data_cost_size, DataType<T>::type);\r
- data_cost_selected.create(msg_size, DataType<T>::type);\r
-\r
- step_pyr[0] = data_cost.step / sizeof(T);\r
-\r
- Size temp_size = data_cost_size;\r
- if (data_cost_size.width * data_cost_size.height < step_pyr[levels - 1] * rows_pyr[levels - 1] * rthis.ndisp)\r
- temp_size = Size(step_pyr[levels - 1], rows_pyr[levels - 1] * rthis.ndisp);\r
-\r
- temp.create(temp_size, DataType<T>::type);\r
-\r
- ////////////////////////////////////////////////////////////////////////////\r
- // Compute\r
-\r
- csbp::load_constants(rthis.ndisp, rthis.max_data_term, rthis.data_weight,\r
- rthis.max_disc_term, rthis.disc_single_jump, rthis.min_disp_th, left, right, temp);\r
-\r
- l[0] = zero;\r
- d[0] = zero;\r
- r[0] = zero;\r
- u[0] = zero;\r
-\r
- l[1] = zero;\r
- d[1] = zero;\r
- r[1] = zero;\r
- u[1] = zero;\r
-\r
- data_cost = zero;\r
- data_cost_selected = zero;\r
-\r
- int cur_idx = 0;\r
-\r
- for (int i = levels - 1; i >= 0; i--)\r
- {\r
- if (i == levels - 1)\r
- {\r
- csbp::init_data_cost(left.rows, left.cols, disp_selected_pyr[cur_idx].ptr<T>(), data_cost_selected.ptr<T>(),\r
- step_pyr[i], rows_pyr[i], cols_pyr[i], i, nr_plane_pyr[i], rthis.ndisp, left.channels(), rthis.use_local_init_data_cost, stream);\r
- }\r
- else\r
- {\r
- csbp::compute_data_cost(disp_selected_pyr[cur_idx].ptr<T>(), data_cost.ptr<T>(), step_pyr[i], step_pyr[i+1],\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
- csbp::init_message(u[new_idx].ptr<T>(), d[new_idx].ptr<T>(), l[new_idx].ptr<T>(), r[new_idx].ptr<T>(),\r
- u[cur_idx].ptr<T>(), d[cur_idx].ptr<T>(), l[cur_idx].ptr<T>(), r[cur_idx].ptr<T>(),\r
- disp_selected_pyr[new_idx].ptr<T>(), disp_selected_pyr[cur_idx].ptr<T>(),\r
- data_cost_selected.ptr<T>(), data_cost.ptr<T>(), step_pyr[i], step_pyr[i+1], rows_pyr[i],\r
- cols_pyr[i], nr_plane_pyr[i], rows_pyr[i+1], cols_pyr[i+1], nr_plane_pyr[i+1], stream);\r
-\r
- cur_idx = new_idx;\r
- }\r
-\r
- csbp::calc_all_iterations(u[cur_idx].ptr<T>(), d[cur_idx].ptr<T>(), l[cur_idx].ptr<T>(), r[cur_idx].ptr<T>(),\r
- data_cost_selected.ptr<T>(), disp_selected_pyr[cur_idx].ptr<T>(), step_pyr[i],\r
- rows_pyr[i], cols_pyr[i], nr_plane_pyr[i], rthis.iters, stream);\r
- }\r
-\r
- if (disp.empty())\r
- disp.create(rows, cols, CV_16S);\r
-\r
- out = ((disp.type() == CV_16S) ? disp : (out.create(rows, cols, CV_16S), out));\r
- out = zero;\r
-\r
- csbp::compute_disp(u[cur_idx].ptr<T>(), d[cur_idx].ptr<T>(), l[cur_idx].ptr<T>(), r[cur_idx].ptr<T>(),\r
- data_cost_selected.ptr<T>(), disp_selected_pyr[cur_idx].ptr<T>(), step_pyr[0], out, nr_plane_pyr[0], stream);\r
-\r
- if (disp.type() != CV_16S)\r
- out.convertTo(disp, disp.type());\r
-}\r
-\r
-\r
-typedef void (*csbp_operator_t)(StereoConstantSpaceBP& rthis, 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& temp, GpuMat& out, const GpuMat& left, const GpuMat& right, GpuMat& disp, cudaStream_t stream);\r
-\r
-const static csbp_operator_t operators[] = {0, 0, 0, csbp_operator<short>, 0, csbp_operator<float>, 0, 0};\r
-\r
-void cv::gpu::StereoConstantSpaceBP::operator()(const GpuMat& left, const GpuMat& right, GpuMat& disp)\r
-{\r
- CV_Assert(msg_type == CV_32F || msg_type == CV_16S);\r
- operators[msg_type](*this, u, d, l, r, disp_selected_pyr, data_cost, data_cost_selected, temp, out, left, right, disp, 0);\r
-}\r
-\r
-void cv::gpu::StereoConstantSpaceBP::operator()(const GpuMat& left, const GpuMat& right, GpuMat& disp, Stream& stream)\r
-{\r
- CV_Assert(msg_type == CV_32F || msg_type == CV_16S);\r
- operators[msg_type](*this, u, d, l, r, disp_selected_pyr, data_cost, data_cost_selected, temp, out, left, right, disp, StreamAccessor::getStream(stream));\r
-}\r
-\r
-#endif /* !defined (HAVE_CUDA) */\r
+++ /dev/null
-/*M///////////////////////////////////////////////////////////////////////////////////////\r
-//\r
-// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
-//\r
-// By downloading, copying, installing or using the software you agree to this license.\r
-// If you do not agree to this license, do not download, install,\r
-// copy or use the software.\r
-//\r
-//\r
-// License Agreement\r
-// For Open Source Computer Vision Library\r
-//\r
-// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.\r
-// Copyright (C) 2009, Willow Garage Inc., all rights reserved.\r
-// Third party copyrights are property of their respective owners.\r
-//\r
-// Redistribution and use in source and binary forms, with or without modification,\r
-// are permitted provided that the following conditions are met:\r
-//\r
-// * Redistribution's of source code must retain the above copyright notice,\r
-// this list of conditions and the following disclaimer.\r
-//\r
-// * Redistribution's in binary form must reproduce the above copyright notice,\r
-// this list of conditions and the following disclaimer in the documentation\r
-// and/or other materials provided with the distribution.\r
-//\r
-// * The name of the copyright holders may not be used to endorse or promote products\r
-// derived from this software without specific prior written permission.\r
-//\r
-// This software is provided by the copyright holders and contributors "as is" and\r
-// any express or implied warranties, including, but not limited to, the implied\r
-// warranties of merchantability and fitness for a particular purpose are disclaimed.\r
-// In no event shall the Intel Corporation or contributors be liable for any direct,\r
-// indirect, incidental, special, exemplary, or consequential damages\r
-// (including, but not limited to, procurement of substitute goods or services;\r
-// loss of use, data, or profits; or business interruption) however caused\r
-// and on any theory of liability, whether in contract, strict liability,\r
-// or tort (including negligence or otherwise) arising in any way out of\r
-// the use of this software, even if advised of the possibility of such damage.\r
-//\r
-//M*/\r
-\r
-#include "opencv2/gpu/devmem2d.hpp"\r
-#include "opencv2/gpu/device/saturate_cast.hpp"\r
-#include "opencv2/gpu/device/limits_gpu.hpp"\r
-#include "safe_call.hpp"\r
-\r
-using namespace cv::gpu;\r
-using namespace cv::gpu::device;\r
-\r
-\r
-namespace cv { namespace gpu { namespace csbp\r
-{ \r
-\r
-///////////////////////////////////////////////////////////////\r
-/////////////////////// load constants ////////////////////////\r
-///////////////////////////////////////////////////////////////\r
-\r
- __constant__ int cndisp;\r
-\r
- __constant__ float cmax_data_term;\r
- __constant__ float cdata_weight;\r
- __constant__ float cmax_disc_term;\r
- __constant__ float cdisc_single_jump;\r
-\r
- __constant__ int cth;\r
-\r
- __constant__ size_t cimg_step;\r
- __constant__ size_t cmsg_step1;\r
- __constant__ size_t cmsg_step2;\r
- __constant__ size_t cdisp_step1;\r
- __constant__ size_t cdisp_step2;\r
-\r
- __constant__ uchar* cleft;\r
- __constant__ uchar* cright;\r
- __constant__ uchar* ctemp;\r
-\r
-\r
- 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
- const DevMem2D& left, const DevMem2D& right, const DevMem2D& temp)\r
- {\r
- cudaSafeCall( cudaMemcpyToSymbol(cndisp, &ndisp, sizeof(int)) );\r
-\r
- cudaSafeCall( cudaMemcpyToSymbol(cmax_data_term, &max_data_term, sizeof(float)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cdata_weight, &data_weight, sizeof(float)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cmax_disc_term, &max_disc_term, sizeof(float)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cdisc_single_jump, &disc_single_jump, sizeof(float)) );\r
-\r
- cudaSafeCall( cudaMemcpyToSymbol(cth, &min_disp_th, sizeof(int)) );\r
-\r
- cudaSafeCall( cudaMemcpyToSymbol(cimg_step, &left.step, sizeof(size_t)) );\r
-\r
- cudaSafeCall( cudaMemcpyToSymbol(cleft, &left.data, sizeof(left.data)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cright, &right.data, sizeof(right.data)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(ctemp, &temp.data, sizeof(temp.data)) );\r
- }\r
-\r
-///////////////////////////////////////////////////////////////\r
-/////////////////////// init data cost ////////////////////////\r
-///////////////////////////////////////////////////////////////\r
-\r
- template <int channels> struct DataCostPerPixel;\r
- template <> struct DataCostPerPixel<1>\r
- {\r
- static __device__ float compute(const uchar* left, const uchar* right)\r
- {\r
- return fmin(cdata_weight * abs((int)*left - *right), cdata_weight * cmax_data_term);\r
- }\r
- };\r
- template <> struct DataCostPerPixel<3>\r
- {\r
- static __device__ float compute(const uchar* left, const uchar* right)\r
- {\r
- float tb = 0.114f * abs((int)left[0] - right[0]);\r
- float tg = 0.587f * abs((int)left[1] - right[1]);\r
- float tr = 0.299f * abs((int)left[2] - right[2]);\r
-\r
- return fmin(cdata_weight * (tr + tg + tb), cdata_weight * cmax_data_term);\r
- }\r
- };\r
- template <> struct DataCostPerPixel<4>\r
- {\r
- static __device__ float compute(const uchar* left, const uchar* right)\r
- {\r
- uchar4 l = *((const uchar4*)left);\r
- uchar4 r = *((const uchar4*)right);\r
-\r
- float tb = 0.114f * abs((int)l.x - r.x);\r
- float tg = 0.587f * abs((int)l.y - r.y);\r
- float tr = 0.299f * abs((int)l.z - r.z);\r
-\r
- return fmin(cdata_weight * (tr + tg + tb), cdata_weight * cmax_data_term);\r
- }\r
- };\r
-\r
- template <typename T>\r
- __global__ void get_first_k_initial_global(T* data_cost_selected_, T *selected_disp_pyr, int h, int w, int nr_plane)\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
- 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*)ctemp + y * cmsg_step1 + x;\r
-\r
- for(int i = 0; i < nr_plane; i++)\r
- {\r
- T minimum = numeric_limits_gpu<T>::max();\r
- int id = 0;\r
- for(int d = 0; d < cndisp; d++)\r
- {\r
- T cur = data_cost[d * cdisp_step1];\r
- if(cur < minimum)\r
- {\r
- minimum = cur;\r
- id = d;\r
- }\r
- }\r
-\r
- data_cost_selected[i * cdisp_step1] = minimum;\r
- selected_disparity[i * cdisp_step1] = id;\r
- data_cost [id * cdisp_step1] = numeric_limits_gpu<T>::max();\r
- }\r
- }\r
- }\r
-\r
-\r
- template <typename T>\r
- __global__ void get_first_k_initial_local(T* data_cost_selected_, T* selected_disp_pyr, int h, int w, int nr_plane)\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
- 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*)ctemp + y * cmsg_step1 + x;\r
-\r
- int nr_local_minimum = 0;\r
-\r
- T prev = data_cost[0 * cdisp_step1];\r
- T cur = data_cost[1 * cdisp_step1];\r
- T next = data_cost[2 * cdisp_step1];\r
-\r
- for (int d = 1; d < cndisp - 1 && nr_local_minimum < nr_plane; d++)\r
- {\r
- if (cur < prev && cur < next)\r
- {\r
- data_cost_selected[nr_local_minimum * cdisp_step1] = cur;\r
- selected_disparity[nr_local_minimum * cdisp_step1] = d;\r
-\r
- data_cost[d * cdisp_step1] = numeric_limits_gpu<T>::max();\r
-\r
- nr_local_minimum++;\r
- }\r
- prev = cur;\r
- cur = next;\r
- next = data_cost[(d + 1) * cdisp_step1];\r
- }\r
-\r
- for (int i = nr_local_minimum; i < nr_plane; i++)\r
- {\r
- T minimum = numeric_limits_gpu<T>::max();\r
- int id = 0;\r
-\r
- for (int d = 0; d < cndisp; d++)\r
- {\r
- cur = data_cost[d * cdisp_step1];\r
- if (cur < minimum)\r
- {\r
- minimum = cur;\r
- id = d;\r
- }\r
- }\r
- data_cost_selected[i * cdisp_step1] = minimum;\r
- selected_disparity[i * cdisp_step1] = id;\r
-\r
- data_cost[id * cdisp_step1] = numeric_limits_gpu<T>::max();\r
- }\r
- }\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 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
- int d = (blockIdx.y / h) * blockDim.z + threadIdx.z;\r
-\r
- int tid = threadIdx.x;\r
-\r
- if (d < cndisp)\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 - d < 0 || d < 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 - d);\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
- volatile float* vdline = smem + winsz * threadIdx.z;\r
-\r
- if (winsz >= 64) if (tid < 32) vdline[tid] += vdline[tid + 32];\r
- if (winsz >= 32) if (tid < 16) vdline[tid] += vdline[tid + 16];\r
- if (winsz >= 16) if (tid < 8) vdline[tid] += vdline[tid + 8];\r
- if (winsz >= 8) if (tid < 4) vdline[tid] += vdline[tid + 4];\r
- if (winsz >= 4) if (tid < 2) vdline[tid] += vdline[tid + 2];\r
- if (winsz >= 2) if (tid < 1) vdline[tid] += vdline[tid + 1];\r
-\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
- }\r
- }\r
-\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, 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: init_data_cost<T, 1><<<grid, threads, 0, stream>>>(h, w, level); break;\r
- case 3: init_data_cost<T, 3><<<grid, threads, 0, stream>>>(h, w, level); break;\r
- case 4: init_data_cost<T, 4><<<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 init_data_cost_reduce_caller_(int rows, int cols, int h, int w, int level, int ndisp, int channels, 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(ndisp, threads.z);\r
-\r
- switch (channels)\r
- {\r
- case 1: init_data_cost_reduce<T, winsz, 1><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
- case 3: init_data_cost_reduce<T, winsz, 3><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
- case 4: init_data_cost_reduce<T, winsz, 4><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
- default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__);\r
- }\r
- }\r
-\r
- template<class T>\r
- void init_data_cost(int rows, int cols, T* disp_selected_pyr, T* data_cost_selected, size_t msg_step,\r
- int h, int w, int level, int nr_plane, int ndisp, int channels, bool use_local_init_data_cost, cudaStream_t stream)\r
- {\r
-\r
- typedef void (*InitDataCostCaller)(int cols, int rows, int w, int h, int level, int ndisp, int channels, cudaStream_t stream);\r
-\r
- static const InitDataCostCaller init_data_cost_callers[] =\r
- {\r
- init_data_cost_caller_<T>, init_data_cost_caller_<T>, init_data_cost_reduce_caller_<T, 4>,\r
- init_data_cost_reduce_caller_<T, 8>, init_data_cost_reduce_caller_<T, 16>, init_data_cost_reduce_caller_<T, 32>,\r
- init_data_cost_reduce_caller_<T, 64>, init_data_cost_reduce_caller_<T, 128>, init_data_cost_reduce_caller_<T, 256>\r
- };\r
-\r
- size_t disp_step = msg_step * h;\r
- cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step, sizeof(size_t)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cmsg_step1, &msg_step, sizeof(size_t)) );\r
-\r
- init_data_cost_callers[level](rows, cols, h, w, level, ndisp, channels, stream);\r
- if (stream == 0)\r
- cudaSafeCall( cudaThreadSynchronize() );\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
- if (use_local_init_data_cost == true)\r
- get_first_k_initial_local<<<grid, threads, 0, stream>>> (data_cost_selected, disp_selected_pyr, h, w, nr_plane);\r
- else\r
- get_first_k_initial_global<<<grid, threads, 0, stream>>>(data_cost_selected, disp_selected_pyr, h, w, nr_plane);\r
- if (stream == 0)\r
- cudaSafeCall( cudaThreadSynchronize() );\r
- }\r
-\r
- template void init_data_cost(int rows, int cols, short* disp_selected_pyr, short* data_cost_selected, size_t msg_step,\r
- int h, int w, int level, int nr_plane, int ndisp, int channels, bool use_local_init_data_cost, cudaStream_t stream);\r
-\r
- template void init_data_cost(int rows, int cols, float* disp_selected_pyr, float* data_cost_selected, size_t msg_step,\r
- int h, int w, int level, int nr_plane, int ndisp, int channels, bool use_local_init_data_cost, cudaStream_t stream);\r
-\r
-///////////////////////////////////////////////////////////////\r
-////////////////////// compute data cost //////////////////////\r
-///////////////////////////////////////////////////////////////\r
-\r
- template <typename T, int channels>\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
-\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
- 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
- {\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 sel_disp = selected_disparity[d * cdisp_step2];\r
- int xr = xi - sel_disp;\r
-\r
- if (xr < 0 || sel_disp < cth)\r
- val += cdata_weight * cmax_data_term;\r
- else\r
- {\r
- const uchar* left_x = cleft + yi * cimg_step + xi * channels;\r
- const uchar* right_x = cright + yi * cimg_step + xr * channels;\r
-\r
- val += DataCostPerPixel<channels>::compute(left_x, right_x);\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 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
- volatile float* vdline = smem + winsz * threadIdx.z;\r
-\r
- if (winsz >= 64) if (tid < 32) vdline[tid] += vdline[tid + 32];\r
- if (winsz >= 32) if (tid < 16) vdline[tid] += vdline[tid + 16];\r
- if (winsz >= 16) if (tid < 8) vdline[tid] += vdline[tid + 8];\r
- if (winsz >= 8) if (tid < 4) vdline[tid] += vdline[tid + 4];\r
- if (winsz >= 4) if (tid < 2) vdline[tid] += vdline[tid + 2];\r
- if (winsz >= 2) if (tid < 1) vdline[tid] += vdline[tid + 1];\r
-\r
- if (tid == 0)\r
- data_cost[cdisp_step1 * d] = saturate_cast<T>(dline[0]);\r
- }\r
- }\r
-\r
- template <typename T>\r
- void compute_data_cost_caller_(const T* disp_selected_pyr, T* data_cost, int /*rows*/, int /*cols*/,\r
- int h, int w, int level, int nr_plane, int channels, 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: compute_data_cost<T, 1><<<grid, threads, 0, stream>>>(disp_selected_pyr, data_cost, h, w, level, nr_plane); break;\r
- case 3: compute_data_cost<T, 3><<<grid, threads, 0, stream>>>(disp_selected_pyr, data_cost, h, w, level, nr_plane); break;\r
- case 4: compute_data_cost<T, 4><<<grid, threads, 0, stream>>>(disp_selected_pyr, data_cost, 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 T* disp_selected_pyr, T* data_cost, int rows, int cols,\r
- int h, int w, int level, int nr_plane, int channels, 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: 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
- 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
- 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
- default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__);\r
- }\r
- }\r
-\r
- template<class T>\r
- void compute_data_cost(const T* disp_selected_pyr, T* data_cost, size_t msg_step1, size_t msg_step2,\r
- int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, cudaStream_t stream)\r
- {\r
- typedef void (*ComputeDataCostCaller)(const T* disp_selected_pyr, T* data_cost, int rows, int cols,\r
- int h, int w, int level, int nr_plane, int channels, cudaStream_t stream);\r
-\r
- static const ComputeDataCostCaller callers[] =\r
- {\r
- compute_data_cost_caller_<T>, compute_data_cost_caller_<T>, compute_data_cost_reduce_caller_<T, 4>,\r
- compute_data_cost_reduce_caller_<T, 8>, compute_data_cost_reduce_caller_<T, 16>, compute_data_cost_reduce_caller_<T, 32>,\r
- compute_data_cost_reduce_caller_<T, 64>, compute_data_cost_reduce_caller_<T, 128>, compute_data_cost_reduce_caller_<T, 256>\r
- };\r
-\r
- size_t disp_step1 = msg_step1 * h;\r
- size_t disp_step2 = msg_step2 * h2;\r
- cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step1, sizeof(size_t)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cdisp_step2, &disp_step2, sizeof(size_t)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cmsg_step1, &msg_step1, sizeof(size_t)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cmsg_step2, &msg_step2, sizeof(size_t)) );\r
-\r
- callers[level](disp_selected_pyr, data_cost, rows, cols, h, w, level, nr_plane, channels, stream);\r
-\r
- if (stream == 0)\r
- cudaSafeCall( cudaThreadSynchronize() );\r
- }\r
-\r
- template void compute_data_cost(const short* disp_selected_pyr, short* data_cost, size_t msg_step1, size_t msg_step2,\r
- int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, cudaStream_t stream);\r
-\r
- template void compute_data_cost(const float* disp_selected_pyr, float* data_cost, size_t msg_step1, size_t msg_step2,\r
- int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, cudaStream_t stream);\r
- \r
-\r
-///////////////////////////////////////////////////////////////\r
-//////////////////////// init message /////////////////////////\r
-///////////////////////////////////////////////////////////////\r
-\r
- \r
- template <typename T>\r
- __device__ void get_first_k_element_increase(T* u_new, T* d_new, T* l_new, T* r_new,\r
- const T* u_cur, const T* d_cur, const T* l_cur, const T* r_cur,\r
- T* data_cost_selected, T* disparity_selected_new, T* data_cost_new,\r
- const T* data_cost_cur, const T* disparity_selected_cur,\r
- int nr_plane, int nr_plane2)\r
- {\r
- for(int i = 0; i < nr_plane; i++)\r
- {\r
- T minimum = numeric_limits_gpu<T>::max();\r
- int id = 0;\r
- for(int j = 0; j < nr_plane2; j++)\r
- {\r
- T cur = data_cost_new[j * cdisp_step1];\r
- if(cur < minimum)\r
- {\r
- minimum = cur;\r
- id = j;\r
- }\r
- }\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_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
- l_new[i * cdisp_step1] = l_cur[id * cdisp_step2];\r
- r_new[i * cdisp_step1] = r_cur[id * cdisp_step2];\r
-\r
- data_cost_new[id * cdisp_step1] = numeric_limits_gpu<T>::max();\r
- }\r
- }\r
-\r
- template <typename T>\r
- __global__ void init_message(T* u_new_, T* d_new_, T* l_new_, T* r_new_,\r
- const T* u_cur_, const T* d_cur_, const T* l_cur_, const T* r_cur_,\r
- T* selected_disp_pyr_new, const T* selected_disp_pyr_cur,\r
- T* data_cost_selected_, const T* data_cost_,\r
- int h, int w, int nr_plane, int h2, int w2, int nr_plane2)\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
- const T* u_cur = u_cur_ + min(h2-1, y/2 + 1) * cmsg_step2 + x/2;\r
- const T* d_cur = d_cur_ + max(0, y/2 - 1) * cmsg_step2 + x/2;\r
- 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* 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
- const T* data_cost = data_cost_ + y * cmsg_step1 + x;\r
-\r
- for(int d = 0; d < nr_plane2; d++)\r
- {\r
- int idx2 = d * cdisp_step2;\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
-\r
- T* data_cost_selected = data_cost_selected_ + y * cmsg_step1 + x;\r
- T* disparity_selected_new = selected_disp_pyr_new + y * cmsg_step1 + x;\r
-\r
- T* u_new = u_new_ + y * cmsg_step1 + x;\r
- T* d_new = d_new_ + y * cmsg_step1 + x;\r
- T* l_new = l_new_ + y * cmsg_step1 + x;\r
- T* r_new = r_new_ + y * cmsg_step1 + x;\r
-\r
- u_cur = u_cur_ + y/2 * cmsg_step2 + x/2;\r
- d_cur = d_cur_ + y/2 * cmsg_step2 + x/2;\r
- l_cur = l_cur_ + y/2 * cmsg_step2 + x/2;\r
- r_cur = r_cur_ + y/2 * cmsg_step2 + x/2;\r
-\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, nr_plane, nr_plane2);\r
- }\r
- }\r
-\r
-\r
- template<class T>\r
- void init_message(T* u_new, T* d_new, T* l_new, T* r_new,\r
- const T* u_cur, const T* d_cur, const T* l_cur, const T* r_cur,\r
- T* selected_disp_pyr_new, const T* selected_disp_pyr_cur,\r
- T* data_cost_selected, const T* data_cost, size_t msg_step1, size_t msg_step2,\r
- int h, int w, int nr_plane, int h2, int w2, int nr_plane2, cudaStream_t stream)\r
- {\r
-\r
- size_t disp_step1 = msg_step1 * h;\r
- size_t disp_step2 = msg_step2 * h2;\r
- cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step1, sizeof(size_t)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cdisp_step2, &disp_step2, sizeof(size_t)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cmsg_step1, &msg_step1, sizeof(size_t)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cmsg_step2, &msg_step2, sizeof(size_t)) );\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
- init_message<<<grid, threads, 0, stream>>>(u_new, d_new, l_new, r_new,\r
- u_cur, d_cur, l_cur, r_cur,\r
- selected_disp_pyr_new, selected_disp_pyr_cur,\r
- data_cost_selected, data_cost,\r
- h, w, nr_plane, h2, w2, nr_plane2);\r
-\r
- if (stream == 0)\r
- cudaSafeCall( cudaThreadSynchronize() );\r
- }\r
-\r
-\r
- template void init_message(short* u_new, short* d_new, short* l_new, short* r_new,\r
- const short* u_cur, const short* d_cur, const short* l_cur, const short* r_cur,\r
- short* selected_disp_pyr_new, const short* selected_disp_pyr_cur,\r
- short* data_cost_selected, const short* data_cost, size_t msg_step1, size_t msg_step2,\r
- int h, int w, int nr_plane, int h2, int w2, int nr_plane2, cudaStream_t stream);\r
-\r
- template void init_message(float* u_new, float* d_new, float* l_new, float* r_new,\r
- const float* u_cur, const float* d_cur, const float* l_cur, const float* r_cur,\r
- float* selected_disp_pyr_new, const float* selected_disp_pyr_cur,\r
- float* data_cost_selected, const float* data_cost, size_t msg_step1, size_t msg_step2,\r
- int h, int w, int nr_plane, int h2, int w2, int nr_plane2, cudaStream_t stream); \r
-\r
-///////////////////////////////////////////////////////////////\r
-//////////////////// calc all iterations /////////////////////\r
-///////////////////////////////////////////////////////////////\r
-\r
- template <typename T>\r
- __device__ void message_per_pixel(const T* data, T* msg_dst, const T* msg1, const T* msg2, const T* msg3,\r
- const T* dst_disp, const T* src_disp, int nr_plane, T* temp)\r
- {\r
- T minimum = numeric_limits_gpu<T>::max();\r
-\r
- for(int d = 0; d < nr_plane; d++)\r
- {\r
- int idx = d * cdisp_step1;\r
- T val = data[idx] + msg1[idx] + msg2[idx] + msg3[idx];\r
-\r
- if(val < minimum)\r
- minimum = val;\r
-\r
- msg_dst[idx] = val;\r
- }\r
-\r
- float sum = 0;\r
- for(int d = 0; d < nr_plane; d++)\r
- {\r
- float cost_min = minimum + cmax_disc_term;\r
- T src_disp_reg = src_disp[d * cdisp_step1];\r
-\r
- for(int d2 = 0; d2 < nr_plane; d2++)\r
- cost_min = fmin(cost_min, msg_dst[d2 * cdisp_step1] + cdisc_single_jump * abs(dst_disp[d2 * cdisp_step1] - src_disp_reg));\r
-\r
- temp[d * cdisp_step1] = saturate_cast<T>(cost_min);\r
- sum += cost_min;\r
- }\r
- sum /= nr_plane;\r
-\r
- for(int d = 0; d < nr_plane; d++)\r
- msg_dst[d * cdisp_step1] = saturate_cast<T>(temp[d * cdisp_step1] - sum);\r
- }\r
-\r
- template <typename T>\r
- __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
- {\r
- int y = blockIdx.y * blockDim.y + threadIdx.y;\r
- int x = ((blockIdx.x * blockDim.x + threadIdx.x) << 1) + ((y + i) & 1);\r
-\r
- if (y > 0 && y < h - 1 && x > 0 && x < w - 1)\r
- {\r
- const T* data = data_cost_selected + y * cmsg_step1 + x;\r
-\r
- T* u = u_ + y * cmsg_step1 + x;\r
- T* d = d_ + y * cmsg_step1 + x;\r
- T* l = l_ + y * cmsg_step1 + x;\r
- T* r = r_ + y * cmsg_step1 + x;\r
-\r
- const T* disp = selected_disp_pyr_cur + y * cmsg_step1 + x;\r
-\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
- message_per_pixel(data, l, u + cmsg_step1, d - cmsg_step1, l + 1, disp, disp - 1, nr_plane, temp);\r
- message_per_pixel(data, r, u + cmsg_step1, d - cmsg_step1, r - 1, disp, disp + 1, nr_plane, temp);\r
- }\r
- }\r
-\r
-\r
- template<class T>\r
- void calc_all_iterations(T* u, T* d, T* l, T* r, const T* data_cost_selected,\r
- const T* selected_disp_pyr_cur, size_t msg_step, int h, int w, int nr_plane, int iters, cudaStream_t stream)\r
- {\r
- size_t disp_step = msg_step * h;\r
- cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step, sizeof(size_t)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cmsg_step1, &msg_step, sizeof(size_t)) );\r
-\r
- dim3 threads(32, 8, 1);\r
- dim3 grid(1, 1, 1);\r
-\r
- grid.x = divUp(w, threads.x << 1);\r
- grid.y = divUp(h, threads.y);\r
-\r
- for(int t = 0; t < iters; ++t)\r
- {\r
- compute_message<<<grid, threads, 0, stream>>>(u, d, l, r, data_cost_selected, selected_disp_pyr_cur, h, w, nr_plane, t & 1);\r
-\r
- if (stream == 0)\r
- cudaSafeCall( cudaThreadSynchronize() );\r
- }\r
- };\r
- \r
- 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
- int h, int w, int nr_plane, int iters, cudaStream_t stream);\r
-\r
- 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
- int h, int w, int nr_plane, int iters, cudaStream_t stream);\r
-\r
-\r
-///////////////////////////////////////////////////////////////\r
-/////////////////////////// output ////////////////////////////\r
-///////////////////////////////////////////////////////////////\r
-\r
-\r
- template <typename T>\r
- __global__ void compute_disp(const T* u_, const T* d_, const T* l_, const T* r_,\r
- const T* data_cost_selected, const T* disp_selected_pyr,\r
- short* disp, size_t res_step, int cols, int rows, int nr_plane)\r
- {\r
- int x = blockIdx.x * blockDim.x + threadIdx.x;\r
- int y = blockIdx.y * blockDim.y + threadIdx.y;\r
-\r
- if (y > 0 && y < rows - 1 && x > 0 && x < cols - 1)\r
- {\r
- const T* data = data_cost_selected + y * cmsg_step1 + x;\r
- const T* disp_selected = disp_selected_pyr + y * cmsg_step1 + x;\r
-\r
- const T* u = u_ + (y+1) * cmsg_step1 + (x+0);\r
- const T* d = d_ + (y-1) * cmsg_step1 + (x+0);\r
- const T* l = l_ + (y+0) * cmsg_step1 + (x+1);\r
- const T* r = r_ + (y+0) * cmsg_step1 + (x-1);\r
-\r
- int best = 0;\r
- T best_val = numeric_limits_gpu<T>::max();\r
- for (int i = 0; i < nr_plane; ++i)\r
- {\r
- int idx = i * cdisp_step1;\r
- T val = data[idx]+ u[idx] + d[idx] + l[idx] + r[idx];\r
-\r
- if (val < best_val)\r
- {\r
- best_val = val;\r
- best = saturate_cast<short>(disp_selected[idx]);\r
- }\r
- }\r
- disp[res_step * y + x] = best;\r
- }\r
- }\r
-\r
- template<class T>\r
- 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
- const DevMem2D_<short>& disp, int nr_plane, cudaStream_t stream)\r
- {\r
- size_t disp_step = disp.rows * msg_step;\r
- cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step, sizeof(size_t)) );\r
- cudaSafeCall( cudaMemcpyToSymbol(cmsg_step1, &msg_step, sizeof(size_t)) );\r
-\r
- dim3 threads(32, 8, 1);\r
- dim3 grid(1, 1, 1);\r
-\r
- grid.x = divUp(disp.cols, threads.x);\r
- grid.y = divUp(disp.rows, threads.y);\r
-\r
- compute_disp<<<grid, threads, 0, stream>>>(u, d, l, r, data_cost_selected, disp_selected,\r
- disp.data, disp.step / disp.elemSize(), disp.cols, disp.rows, nr_plane);\r
- if (stream == 0)\r
- cudaSafeCall( cudaThreadSynchronize() );\r
- }\r
-\r
- 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
- const DevMem2D_<short>& disp, int nr_plane, cudaStream_t stream);\r
-\r
- 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
- const DevMem2D_<short>& disp, int nr_plane, cudaStream_t stream);\r
-}}}\r
--- /dev/null
+/*M///////////////////////////////////////////////////////////////////////////////////////\r
+//\r
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
+//\r
+// By downloading, copying, installing or using the software you agree to this license.\r
+// If you do not agree to this license, do not download, install,\r
+// copy or use the software.\r
+//\r
+//\r
+// License Agreement\r
+// For Open Source Computer Vision Library\r
+//\r
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.\r
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.\r
+// Third party copyrights are property of their respective owners.\r
+//\r
+// Redistribution and use in source and binary forms, with or without modification,\r
+// are permitted provided that the following conditions are met:\r
+//\r
+// * Redistribution's of source code must retain the above copyright notice,\r
+// this list of conditions and the following disclaimer.\r
+//\r
+// * Redistribution's in binary form must reproduce the above copyright notice,\r
+// this list of conditions and the following disclaimer in the documentation\r
+// and/or other materials provided with the distribution.\r
+//\r
+// * The name of the copyright holders may not be used to endorse or promote products\r
+// derived from this software without specific prior written permission.\r
+//\r
+// This software is provided by the copyright holders and contributors "as is" and\r
+// any express or implied warranties, including, but not limited to, the implied\r
+// warranties of merchantability and fitness for a particular purpose are disclaimed.\r
+// In no event shall the Intel Corporation or contributors be liable for any direct,\r
+// indirect, incidental, special, exemplary, or consequential damages\r
+// (including, but not limited to, procurement of substitute goods or services;\r
+// loss of use, data, or profits; or business interruption) however caused\r
+// and on any theory of liability, whether in contract, strict liability,\r
+// or tort (including negligence or otherwise) arising in any way out of\r
+// the use of this software, even if advised of the possibility of such damage.\r
+//\r
+//M*/\r
+\r
+#include "opencv2/gpu/devmem2d.hpp"\r
+#include "opencv2/gpu/device/saturate_cast.hpp"\r
+#include "opencv2/gpu/device/limits_gpu.hpp"\r
+#include "safe_call.hpp"\r
+\r
+using namespace cv::gpu;\r
+using namespace cv::gpu::device;\r
+\r
+\r
+namespace cv { namespace gpu { namespace csbp\r
+{ \r
+\r
+///////////////////////////////////////////////////////////////\r
+/////////////////////// load constants ////////////////////////\r
+///////////////////////////////////////////////////////////////\r
+\r
+ __constant__ int cndisp;\r
+\r
+ __constant__ float cmax_data_term;\r
+ __constant__ float cdata_weight;\r
+ __constant__ float cmax_disc_term;\r
+ __constant__ float cdisc_single_jump;\r
+\r
+ __constant__ int cth;\r
+\r
+ __constant__ size_t cimg_step;\r
+ __constant__ size_t cmsg_step1;\r
+ __constant__ size_t cmsg_step2;\r
+ __constant__ size_t cdisp_step1;\r
+ __constant__ size_t cdisp_step2;\r
+\r
+ __constant__ uchar* cleft;\r
+ __constant__ uchar* cright;\r
+ __constant__ uchar* ctemp;\r
+\r
+\r
+ 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
+ const DevMem2D& left, const DevMem2D& right, const DevMem2D& temp)\r
+ {\r
+ cudaSafeCall( cudaMemcpyToSymbol(cndisp, &ndisp, sizeof(int)) );\r
+\r
+ cudaSafeCall( cudaMemcpyToSymbol(cmax_data_term, &max_data_term, sizeof(float)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cdata_weight, &data_weight, sizeof(float)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cmax_disc_term, &max_disc_term, sizeof(float)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cdisc_single_jump, &disc_single_jump, sizeof(float)) );\r
+\r
+ cudaSafeCall( cudaMemcpyToSymbol(cth, &min_disp_th, sizeof(int)) );\r
+\r
+ cudaSafeCall( cudaMemcpyToSymbol(cimg_step, &left.step, sizeof(size_t)) );\r
+\r
+ cudaSafeCall( cudaMemcpyToSymbol(cleft, &left.data, sizeof(left.data)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cright, &right.data, sizeof(right.data)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(ctemp, &temp.data, sizeof(temp.data)) );\r
+ }\r
+\r
+///////////////////////////////////////////////////////////////\r
+/////////////////////// init data cost ////////////////////////\r
+///////////////////////////////////////////////////////////////\r
+\r
+ template <int channels> struct DataCostPerPixel;\r
+ template <> struct DataCostPerPixel<1>\r
+ {\r
+ static __device__ float compute(const uchar* left, const uchar* right)\r
+ {\r
+ return fmin(cdata_weight * abs((int)*left - *right), cdata_weight * cmax_data_term);\r
+ }\r
+ };\r
+ template <> struct DataCostPerPixel<3>\r
+ {\r
+ static __device__ float compute(const uchar* left, const uchar* right)\r
+ {\r
+ float tb = 0.114f * abs((int)left[0] - right[0]);\r
+ float tg = 0.587f * abs((int)left[1] - right[1]);\r
+ float tr = 0.299f * abs((int)left[2] - right[2]);\r
+\r
+ return fmin(cdata_weight * (tr + tg + tb), cdata_weight * cmax_data_term);\r
+ }\r
+ };\r
+ template <> struct DataCostPerPixel<4>\r
+ {\r
+ static __device__ float compute(const uchar* left, const uchar* right)\r
+ {\r
+ uchar4 l = *((const uchar4*)left);\r
+ uchar4 r = *((const uchar4*)right);\r
+\r
+ float tb = 0.114f * abs((int)l.x - r.x);\r
+ float tg = 0.587f * abs((int)l.y - r.y);\r
+ float tr = 0.299f * abs((int)l.z - r.z);\r
+\r
+ return fmin(cdata_weight * (tr + tg + tb), cdata_weight * cmax_data_term);\r
+ }\r
+ };\r
+\r
+ template <typename T>\r
+ __global__ void get_first_k_initial_global(T* data_cost_selected_, T *selected_disp_pyr, int h, int w, int nr_plane)\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
+ 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*)ctemp + y * cmsg_step1 + x;\r
+\r
+ for(int i = 0; i < nr_plane; i++)\r
+ {\r
+ T minimum = numeric_limits_gpu<T>::max();\r
+ int id = 0;\r
+ for(int d = 0; d < cndisp; d++)\r
+ {\r
+ T cur = data_cost[d * cdisp_step1];\r
+ if(cur < minimum)\r
+ {\r
+ minimum = cur;\r
+ id = d;\r
+ }\r
+ }\r
+\r
+ data_cost_selected[i * cdisp_step1] = minimum;\r
+ selected_disparity[i * cdisp_step1] = id;\r
+ data_cost [id * cdisp_step1] = numeric_limits_gpu<T>::max();\r
+ }\r
+ }\r
+ }\r
+\r
+\r
+ template <typename T>\r
+ __global__ void get_first_k_initial_local(T* data_cost_selected_, T* selected_disp_pyr, int h, int w, int nr_plane)\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
+ 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*)ctemp + y * cmsg_step1 + x;\r
+\r
+ int nr_local_minimum = 0;\r
+\r
+ T prev = data_cost[0 * cdisp_step1];\r
+ T cur = data_cost[1 * cdisp_step1];\r
+ T next = data_cost[2 * cdisp_step1];\r
+\r
+ for (int d = 1; d < cndisp - 1 && nr_local_minimum < nr_plane; d++)\r
+ {\r
+ if (cur < prev && cur < next)\r
+ {\r
+ data_cost_selected[nr_local_minimum * cdisp_step1] = cur;\r
+ selected_disparity[nr_local_minimum * cdisp_step1] = d;\r
+\r
+ data_cost[d * cdisp_step1] = numeric_limits_gpu<T>::max();\r
+\r
+ nr_local_minimum++;\r
+ }\r
+ prev = cur;\r
+ cur = next;\r
+ next = data_cost[(d + 1) * cdisp_step1];\r
+ }\r
+\r
+ for (int i = nr_local_minimum; i < nr_plane; i++)\r
+ {\r
+ T minimum = numeric_limits_gpu<T>::max();\r
+ int id = 0;\r
+\r
+ for (int d = 0; d < cndisp; d++)\r
+ {\r
+ cur = data_cost[d * cdisp_step1];\r
+ if (cur < minimum)\r
+ {\r
+ minimum = cur;\r
+ id = d;\r
+ }\r
+ }\r
+ data_cost_selected[i * cdisp_step1] = minimum;\r
+ selected_disparity[i * cdisp_step1] = id;\r
+\r
+ data_cost[id * cdisp_step1] = numeric_limits_gpu<T>::max();\r
+ }\r
+ }\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 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
+ int d = (blockIdx.y / h) * blockDim.z + threadIdx.z;\r
+\r
+ int tid = threadIdx.x;\r
+\r
+ if (d < cndisp)\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 - d < 0 || d < 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 - d);\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
+ volatile float* vdline = smem + winsz * threadIdx.z;\r
+\r
+ if (winsz >= 64) if (tid < 32) vdline[tid] += vdline[tid + 32];\r
+ if (winsz >= 32) if (tid < 16) vdline[tid] += vdline[tid + 16];\r
+ if (winsz >= 16) if (tid < 8) vdline[tid] += vdline[tid + 8];\r
+ if (winsz >= 8) if (tid < 4) vdline[tid] += vdline[tid + 4];\r
+ if (winsz >= 4) if (tid < 2) vdline[tid] += vdline[tid + 2];\r
+ if (winsz >= 2) if (tid < 1) vdline[tid] += vdline[tid + 1];\r
+\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
+ }\r
+ }\r
+\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, 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: init_data_cost<T, 1><<<grid, threads, 0, stream>>>(h, w, level); break;\r
+ case 3: init_data_cost<T, 3><<<grid, threads, 0, stream>>>(h, w, level); break;\r
+ case 4: init_data_cost<T, 4><<<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 init_data_cost_reduce_caller_(int rows, int cols, int h, int w, int level, int ndisp, int channels, 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(ndisp, threads.z);\r
+\r
+ switch (channels)\r
+ {\r
+ case 1: init_data_cost_reduce<T, winsz, 1><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
+ case 3: init_data_cost_reduce<T, winsz, 3><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
+ case 4: init_data_cost_reduce<T, winsz, 4><<<grid, threads, smem_size, stream>>>(level, rows, cols, h); break;\r
+ default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__);\r
+ }\r
+ }\r
+\r
+ template<class T>\r
+ void init_data_cost(int rows, int cols, T* disp_selected_pyr, T* data_cost_selected, size_t msg_step,\r
+ int h, int w, int level, int nr_plane, int ndisp, int channels, bool use_local_init_data_cost, cudaStream_t stream)\r
+ {\r
+\r
+ typedef void (*InitDataCostCaller)(int cols, int rows, int w, int h, int level, int ndisp, int channels, cudaStream_t stream);\r
+\r
+ static const InitDataCostCaller init_data_cost_callers[] =\r
+ {\r
+ init_data_cost_caller_<T>, init_data_cost_caller_<T>, init_data_cost_reduce_caller_<T, 4>,\r
+ init_data_cost_reduce_caller_<T, 8>, init_data_cost_reduce_caller_<T, 16>, init_data_cost_reduce_caller_<T, 32>,\r
+ init_data_cost_reduce_caller_<T, 64>, init_data_cost_reduce_caller_<T, 128>, init_data_cost_reduce_caller_<T, 256>\r
+ };\r
+\r
+ size_t disp_step = msg_step * h;\r
+ cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step, sizeof(size_t)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cmsg_step1, &msg_step, sizeof(size_t)) );\r
+\r
+ init_data_cost_callers[level](rows, cols, h, w, level, ndisp, channels, stream);\r
+ if (stream == 0)\r
+ cudaSafeCall( cudaThreadSynchronize() );\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
+ if (use_local_init_data_cost == true)\r
+ get_first_k_initial_local<<<grid, threads, 0, stream>>> (data_cost_selected, disp_selected_pyr, h, w, nr_plane);\r
+ else\r
+ get_first_k_initial_global<<<grid, threads, 0, stream>>>(data_cost_selected, disp_selected_pyr, h, w, nr_plane);\r
+ if (stream == 0)\r
+ cudaSafeCall( cudaThreadSynchronize() );\r
+ }\r
+\r
+ template void init_data_cost(int rows, int cols, short* disp_selected_pyr, short* data_cost_selected, size_t msg_step,\r
+ int h, int w, int level, int nr_plane, int ndisp, int channels, bool use_local_init_data_cost, cudaStream_t stream);\r
+\r
+ template void init_data_cost(int rows, int cols, float* disp_selected_pyr, float* data_cost_selected, size_t msg_step,\r
+ int h, int w, int level, int nr_plane, int ndisp, int channels, bool use_local_init_data_cost, cudaStream_t stream);\r
+\r
+///////////////////////////////////////////////////////////////\r
+////////////////////// compute data cost //////////////////////\r
+///////////////////////////////////////////////////////////////\r
+\r
+ template <typename T, int channels>\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
+\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
+ 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
+ {\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 sel_disp = selected_disparity[d * cdisp_step2];\r
+ int xr = xi - sel_disp;\r
+\r
+ if (xr < 0 || sel_disp < cth)\r
+ val += cdata_weight * cmax_data_term;\r
+ else\r
+ {\r
+ const uchar* left_x = cleft + yi * cimg_step + xi * channels;\r
+ const uchar* right_x = cright + yi * cimg_step + xr * channels;\r
+\r
+ val += DataCostPerPixel<channels>::compute(left_x, right_x);\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 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
+ volatile float* vdline = smem + winsz * threadIdx.z;\r
+\r
+ if (winsz >= 64) if (tid < 32) vdline[tid] += vdline[tid + 32];\r
+ if (winsz >= 32) if (tid < 16) vdline[tid] += vdline[tid + 16];\r
+ if (winsz >= 16) if (tid < 8) vdline[tid] += vdline[tid + 8];\r
+ if (winsz >= 8) if (tid < 4) vdline[tid] += vdline[tid + 4];\r
+ if (winsz >= 4) if (tid < 2) vdline[tid] += vdline[tid + 2];\r
+ if (winsz >= 2) if (tid < 1) vdline[tid] += vdline[tid + 1];\r
+\r
+ if (tid == 0)\r
+ data_cost[cdisp_step1 * d] = saturate_cast<T>(dline[0]);\r
+ }\r
+ }\r
+\r
+ template <typename T>\r
+ void compute_data_cost_caller_(const T* disp_selected_pyr, T* data_cost, int /*rows*/, int /*cols*/,\r
+ int h, int w, int level, int nr_plane, int channels, 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: compute_data_cost<T, 1><<<grid, threads, 0, stream>>>(disp_selected_pyr, data_cost, h, w, level, nr_plane); break;\r
+ case 3: compute_data_cost<T, 3><<<grid, threads, 0, stream>>>(disp_selected_pyr, data_cost, h, w, level, nr_plane); break;\r
+ case 4: compute_data_cost<T, 4><<<grid, threads, 0, stream>>>(disp_selected_pyr, data_cost, 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 T* disp_selected_pyr, T* data_cost, int rows, int cols,\r
+ int h, int w, int level, int nr_plane, int channels, 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: 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
+ 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
+ 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
+ default: cv::gpu::error("Unsupported channels count", __FILE__, __LINE__);\r
+ }\r
+ }\r
+\r
+ template<class T>\r
+ void compute_data_cost(const T* disp_selected_pyr, T* data_cost, size_t msg_step1, size_t msg_step2,\r
+ int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, cudaStream_t stream)\r
+ {\r
+ typedef void (*ComputeDataCostCaller)(const T* disp_selected_pyr, T* data_cost, int rows, int cols,\r
+ int h, int w, int level, int nr_plane, int channels, cudaStream_t stream);\r
+\r
+ static const ComputeDataCostCaller callers[] =\r
+ {\r
+ compute_data_cost_caller_<T>, compute_data_cost_caller_<T>, compute_data_cost_reduce_caller_<T, 4>,\r
+ compute_data_cost_reduce_caller_<T, 8>, compute_data_cost_reduce_caller_<T, 16>, compute_data_cost_reduce_caller_<T, 32>,\r
+ compute_data_cost_reduce_caller_<T, 64>, compute_data_cost_reduce_caller_<T, 128>, compute_data_cost_reduce_caller_<T, 256>\r
+ };\r
+\r
+ size_t disp_step1 = msg_step1 * h;\r
+ size_t disp_step2 = msg_step2 * h2;\r
+ cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step1, sizeof(size_t)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cdisp_step2, &disp_step2, sizeof(size_t)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cmsg_step1, &msg_step1, sizeof(size_t)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cmsg_step2, &msg_step2, sizeof(size_t)) );\r
+\r
+ callers[level](disp_selected_pyr, data_cost, rows, cols, h, w, level, nr_plane, channels, stream);\r
+\r
+ if (stream == 0)\r
+ cudaSafeCall( cudaThreadSynchronize() );\r
+ }\r
+\r
+ template void compute_data_cost(const short* disp_selected_pyr, short* data_cost, size_t msg_step1, size_t msg_step2,\r
+ int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, cudaStream_t stream);\r
+\r
+ template void compute_data_cost(const float* disp_selected_pyr, float* data_cost, size_t msg_step1, size_t msg_step2,\r
+ int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, cudaStream_t stream);\r
+ \r
+\r
+///////////////////////////////////////////////////////////////\r
+//////////////////////// init message /////////////////////////\r
+///////////////////////////////////////////////////////////////\r
+\r
+ \r
+ template <typename T>\r
+ __device__ void get_first_k_element_increase(T* u_new, T* d_new, T* l_new, T* r_new,\r
+ const T* u_cur, const T* d_cur, const T* l_cur, const T* r_cur,\r
+ T* data_cost_selected, T* disparity_selected_new, T* data_cost_new,\r
+ const T* data_cost_cur, const T* disparity_selected_cur,\r
+ int nr_plane, int nr_plane2)\r
+ {\r
+ for(int i = 0; i < nr_plane; i++)\r
+ {\r
+ T minimum = numeric_limits_gpu<T>::max();\r
+ int id = 0;\r
+ for(int j = 0; j < nr_plane2; j++)\r
+ {\r
+ T cur = data_cost_new[j * cdisp_step1];\r
+ if(cur < minimum)\r
+ {\r
+ minimum = cur;\r
+ id = j;\r
+ }\r
+ }\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_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
+ l_new[i * cdisp_step1] = l_cur[id * cdisp_step2];\r
+ r_new[i * cdisp_step1] = r_cur[id * cdisp_step2];\r
+\r
+ data_cost_new[id * cdisp_step1] = numeric_limits_gpu<T>::max();\r
+ }\r
+ }\r
+\r
+ template <typename T>\r
+ __global__ void init_message(T* u_new_, T* d_new_, T* l_new_, T* r_new_,\r
+ const T* u_cur_, const T* d_cur_, const T* l_cur_, const T* r_cur_,\r
+ T* selected_disp_pyr_new, const T* selected_disp_pyr_cur,\r
+ T* data_cost_selected_, const T* data_cost_,\r
+ int h, int w, int nr_plane, int h2, int w2, int nr_plane2)\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
+ const T* u_cur = u_cur_ + min(h2-1, y/2 + 1) * cmsg_step2 + x/2;\r
+ const T* d_cur = d_cur_ + max(0, y/2 - 1) * cmsg_step2 + x/2;\r
+ 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* 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
+ const T* data_cost = data_cost_ + y * cmsg_step1 + x;\r
+\r
+ for(int d = 0; d < nr_plane2; d++)\r
+ {\r
+ int idx2 = d * cdisp_step2;\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
+\r
+ T* data_cost_selected = data_cost_selected_ + y * cmsg_step1 + x;\r
+ T* disparity_selected_new = selected_disp_pyr_new + y * cmsg_step1 + x;\r
+\r
+ T* u_new = u_new_ + y * cmsg_step1 + x;\r
+ T* d_new = d_new_ + y * cmsg_step1 + x;\r
+ T* l_new = l_new_ + y * cmsg_step1 + x;\r
+ T* r_new = r_new_ + y * cmsg_step1 + x;\r
+\r
+ u_cur = u_cur_ + y/2 * cmsg_step2 + x/2;\r
+ d_cur = d_cur_ + y/2 * cmsg_step2 + x/2;\r
+ l_cur = l_cur_ + y/2 * cmsg_step2 + x/2;\r
+ r_cur = r_cur_ + y/2 * cmsg_step2 + x/2;\r
+\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, nr_plane, nr_plane2);\r
+ }\r
+ }\r
+\r
+\r
+ template<class T>\r
+ void init_message(T* u_new, T* d_new, T* l_new, T* r_new,\r
+ const T* u_cur, const T* d_cur, const T* l_cur, const T* r_cur,\r
+ T* selected_disp_pyr_new, const T* selected_disp_pyr_cur,\r
+ T* data_cost_selected, const T* data_cost, size_t msg_step1, size_t msg_step2,\r
+ int h, int w, int nr_plane, int h2, int w2, int nr_plane2, cudaStream_t stream)\r
+ {\r
+\r
+ size_t disp_step1 = msg_step1 * h;\r
+ size_t disp_step2 = msg_step2 * h2;\r
+ cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step1, sizeof(size_t)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cdisp_step2, &disp_step2, sizeof(size_t)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cmsg_step1, &msg_step1, sizeof(size_t)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cmsg_step2, &msg_step2, sizeof(size_t)) );\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
+ init_message<<<grid, threads, 0, stream>>>(u_new, d_new, l_new, r_new,\r
+ u_cur, d_cur, l_cur, r_cur,\r
+ selected_disp_pyr_new, selected_disp_pyr_cur,\r
+ data_cost_selected, data_cost,\r
+ h, w, nr_plane, h2, w2, nr_plane2);\r
+\r
+ if (stream == 0)\r
+ cudaSafeCall( cudaThreadSynchronize() );\r
+ }\r
+\r
+\r
+ template void init_message(short* u_new, short* d_new, short* l_new, short* r_new,\r
+ const short* u_cur, const short* d_cur, const short* l_cur, const short* r_cur,\r
+ short* selected_disp_pyr_new, const short* selected_disp_pyr_cur,\r
+ short* data_cost_selected, const short* data_cost, size_t msg_step1, size_t msg_step2,\r
+ int h, int w, int nr_plane, int h2, int w2, int nr_plane2, cudaStream_t stream);\r
+\r
+ template void init_message(float* u_new, float* d_new, float* l_new, float* r_new,\r
+ const float* u_cur, const float* d_cur, const float* l_cur, const float* r_cur,\r
+ float* selected_disp_pyr_new, const float* selected_disp_pyr_cur,\r
+ float* data_cost_selected, const float* data_cost, size_t msg_step1, size_t msg_step2,\r
+ int h, int w, int nr_plane, int h2, int w2, int nr_plane2, cudaStream_t stream); \r
+\r
+///////////////////////////////////////////////////////////////\r
+//////////////////// calc all iterations /////////////////////\r
+///////////////////////////////////////////////////////////////\r
+\r
+ template <typename T>\r
+ __device__ void message_per_pixel(const T* data, T* msg_dst, const T* msg1, const T* msg2, const T* msg3,\r
+ const T* dst_disp, const T* src_disp, int nr_plane, T* temp)\r
+ {\r
+ T minimum = numeric_limits_gpu<T>::max();\r
+\r
+ for(int d = 0; d < nr_plane; d++)\r
+ {\r
+ int idx = d * cdisp_step1;\r
+ T val = data[idx] + msg1[idx] + msg2[idx] + msg3[idx];\r
+\r
+ if(val < minimum)\r
+ minimum = val;\r
+\r
+ msg_dst[idx] = val;\r
+ }\r
+\r
+ float sum = 0;\r
+ for(int d = 0; d < nr_plane; d++)\r
+ {\r
+ float cost_min = minimum + cmax_disc_term;\r
+ T src_disp_reg = src_disp[d * cdisp_step1];\r
+\r
+ for(int d2 = 0; d2 < nr_plane; d2++)\r
+ cost_min = fmin(cost_min, msg_dst[d2 * cdisp_step1] + cdisc_single_jump * abs(dst_disp[d2 * cdisp_step1] - src_disp_reg));\r
+\r
+ temp[d * cdisp_step1] = saturate_cast<T>(cost_min);\r
+ sum += cost_min;\r
+ }\r
+ sum /= nr_plane;\r
+\r
+ for(int d = 0; d < nr_plane; d++)\r
+ msg_dst[d * cdisp_step1] = saturate_cast<T>(temp[d * cdisp_step1] - sum);\r
+ }\r
+\r
+ template <typename T>\r
+ __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
+ {\r
+ int y = blockIdx.y * blockDim.y + threadIdx.y;\r
+ int x = ((blockIdx.x * blockDim.x + threadIdx.x) << 1) + ((y + i) & 1);\r
+\r
+ if (y > 0 && y < h - 1 && x > 0 && x < w - 1)\r
+ {\r
+ const T* data = data_cost_selected + y * cmsg_step1 + x;\r
+\r
+ T* u = u_ + y * cmsg_step1 + x;\r
+ T* d = d_ + y * cmsg_step1 + x;\r
+ T* l = l_ + y * cmsg_step1 + x;\r
+ T* r = r_ + y * cmsg_step1 + x;\r
+\r
+ const T* disp = selected_disp_pyr_cur + y * cmsg_step1 + x;\r
+\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
+ message_per_pixel(data, l, u + cmsg_step1, d - cmsg_step1, l + 1, disp, disp - 1, nr_plane, temp);\r
+ message_per_pixel(data, r, u + cmsg_step1, d - cmsg_step1, r - 1, disp, disp + 1, nr_plane, temp);\r
+ }\r
+ }\r
+\r
+\r
+ template<class T>\r
+ void calc_all_iterations(T* u, T* d, T* l, T* r, const T* data_cost_selected,\r
+ const T* selected_disp_pyr_cur, size_t msg_step, int h, int w, int nr_plane, int iters, cudaStream_t stream)\r
+ {\r
+ size_t disp_step = msg_step * h;\r
+ cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step, sizeof(size_t)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cmsg_step1, &msg_step, sizeof(size_t)) );\r
+\r
+ dim3 threads(32, 8, 1);\r
+ dim3 grid(1, 1, 1);\r
+\r
+ grid.x = divUp(w, threads.x << 1);\r
+ grid.y = divUp(h, threads.y);\r
+\r
+ for(int t = 0; t < iters; ++t)\r
+ {\r
+ compute_message<<<grid, threads, 0, stream>>>(u, d, l, r, data_cost_selected, selected_disp_pyr_cur, h, w, nr_plane, t & 1);\r
+\r
+ if (stream == 0)\r
+ cudaSafeCall( cudaThreadSynchronize() );\r
+ }\r
+ };\r
+ \r
+ 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
+ int h, int w, int nr_plane, int iters, cudaStream_t stream);\r
+\r
+ 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
+ int h, int w, int nr_plane, int iters, cudaStream_t stream);\r
+\r
+\r
+///////////////////////////////////////////////////////////////\r
+/////////////////////////// output ////////////////////////////\r
+///////////////////////////////////////////////////////////////\r
+\r
+\r
+ template <typename T>\r
+ __global__ void compute_disp(const T* u_, const T* d_, const T* l_, const T* r_,\r
+ const T* data_cost_selected, const T* disp_selected_pyr,\r
+ short* disp, size_t res_step, int cols, int rows, int nr_plane)\r
+ {\r
+ int x = blockIdx.x * blockDim.x + threadIdx.x;\r
+ int y = blockIdx.y * blockDim.y + threadIdx.y;\r
+\r
+ if (y > 0 && y < rows - 1 && x > 0 && x < cols - 1)\r
+ {\r
+ const T* data = data_cost_selected + y * cmsg_step1 + x;\r
+ const T* disp_selected = disp_selected_pyr + y * cmsg_step1 + x;\r
+\r
+ const T* u = u_ + (y+1) * cmsg_step1 + (x+0);\r
+ const T* d = d_ + (y-1) * cmsg_step1 + (x+0);\r
+ const T* l = l_ + (y+0) * cmsg_step1 + (x+1);\r
+ const T* r = r_ + (y+0) * cmsg_step1 + (x-1);\r
+\r
+ int best = 0;\r
+ T best_val = numeric_limits_gpu<T>::max();\r
+ for (int i = 0; i < nr_plane; ++i)\r
+ {\r
+ int idx = i * cdisp_step1;\r
+ T val = data[idx]+ u[idx] + d[idx] + l[idx] + r[idx];\r
+\r
+ if (val < best_val)\r
+ {\r
+ best_val = val;\r
+ best = saturate_cast<short>(disp_selected[idx]);\r
+ }\r
+ }\r
+ disp[res_step * y + x] = best;\r
+ }\r
+ }\r
+\r
+ template<class T>\r
+ 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
+ const DevMem2D_<short>& disp, int nr_plane, cudaStream_t stream)\r
+ {\r
+ size_t disp_step = disp.rows * msg_step;\r
+ cudaSafeCall( cudaMemcpyToSymbol(cdisp_step1, &disp_step, sizeof(size_t)) );\r
+ cudaSafeCall( cudaMemcpyToSymbol(cmsg_step1, &msg_step, sizeof(size_t)) );\r
+\r
+ dim3 threads(32, 8, 1);\r
+ dim3 grid(1, 1, 1);\r
+\r
+ grid.x = divUp(disp.cols, threads.x);\r
+ grid.y = divUp(disp.rows, threads.y);\r
+\r
+ compute_disp<<<grid, threads, 0, stream>>>(u, d, l, r, data_cost_selected, disp_selected,\r
+ disp.data, disp.step / disp.elemSize(), disp.cols, disp.rows, nr_plane);\r
+ if (stream == 0)\r
+ cudaSafeCall( cudaThreadSynchronize() );\r
+ }\r
+\r
+ 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
+ const DevMem2D_<short>& disp, int nr_plane, cudaStream_t stream);\r
+\r
+ 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
+ const DevMem2D_<short>& disp, int nr_plane, cudaStream_t stream);\r
+}}}\r
--- /dev/null
+\r
+\r
+\r
+\r
+\r
+\r
+//////////////////////////////////////////////////////////////////////////////////////////////////\r
+////////////////////////// Unary per-element operations /////////////////////////////////////////\r
+// operation(GpuMat src, GpuMat dst)\r
+\r
+\r
+\r
+\r
+//////////////////////////////////////////////////////////////////////////////////////////////////\r
+////////////////////////// Binary per-element operations ////////////////////////////////////////\r
+// operation(GpuMat src1, GpuMat src2, GpuMat dst)\r
--- /dev/null
+/*M///////////////////////////////////////////////////////////////////////////////////////\r
+//\r
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
+//\r
+// By downloading, copying, installing or using the software you agree to this license.\r
+// If you do not agree to this license, do not download, install,\r
+// copy or use the software.\r
+//\r
+//\r
+// License Agreement\r
+// For Open Source Computer Vision Library\r
+//\r
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.\r
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.\r
+// Third party copyrights are property of their respective owners.\r
+//\r
+// Redistribution and use in source and binary forms, with or without modification,\r
+// are permitted provided that the following conditions are met:\r
+//\r
+// * Redistribution's of source code must retain the above copyright notice,\r
+// this list of conditions and the following disclaimer.\r
+//\r
+// * Redistribution's in binary form must reproduce the above copyright notice,\r
+// this list of conditions and the following disclaimer in the documentation\r
+// and/or other GpuMaterials provided with the distribution.\r
+//\r
+// * The name of the copyright holders may not be used to endorse or promote products\r
+// derived from this software without specific prior written permission.\r
+//\r
+// This software is provided by the copyright holders and contributors "as is" and\r
+// any express or implied warranties, including, but not limited to, the implied\r
+// warranties of merchantability and fitness for a particular purpose are disclaimed.\r
+// In no event shall the Intel Corporation or contributors be liable for any direct,\r
+// indirect, incidental, special, exemplary, or consequential damages\r
+// (including, but not limited to, procurement of substitute goods or services;\r
+// loss of use, data, or profits; or business interruption) however caused\r
+// and on any theory of liability, whether in contract, strict liability,\r
+// or tort (including negligence or otherwise) arising in any way out of\r
+// the use of this software, even if advised of the possibility of such damage.\r
+//\r
+//M*/\r
+\r
+#include "precomp.hpp"\r
+\r
+using namespace cv;\r
+using namespace cv::gpu;\r
+using namespace std;\r
+\r
+#if !defined (HAVE_CUDA)\r
+\r
+void cv::gpu::StereoConstantSpaceBP::estimateRecommendedParams(int, int, int&, int&, int&, int&) { throw_nogpu(); }\r
+\r
+cv::gpu::StereoConstantSpaceBP::StereoConstantSpaceBP(int, int, int, int, int) { throw_nogpu(); }\r
+cv::gpu::StereoConstantSpaceBP::StereoConstantSpaceBP(int, int, int, int, float, float, float, float, int, int) { throw_nogpu(); }\r
+\r
+void cv::gpu::StereoConstantSpaceBP::operator()(const GpuMat&, const GpuMat&, GpuMat&) { throw_nogpu(); }\r
+void cv::gpu::StereoConstantSpaceBP::operator()(const GpuMat&, const GpuMat&, GpuMat&, Stream&) { throw_nogpu(); }\r
+\r
+#else /* !defined (HAVE_CUDA) */\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, int min_disp_th,\r
+ const DevMem2D& left, const DevMem2D& right, const DevMem2D& temp);\r
+\r
+ template<class T>\r
+ void init_data_cost(int rows, int cols, T* disp_selected_pyr, T* data_cost_selected, size_t msg_step,\r
+ int h, int w, int level, int nr_plane, int ndisp, int channels, bool use_local_init_data_cost, cudaStream_t stream);\r
+\r
+ template<class T>\r
+ void compute_data_cost(const T* disp_selected_pyr, T* data_cost, size_t msg_step1, size_t msg_step2,\r
+ int rows, int cols, int h, int w, int h2, int level, int nr_plane, int channels, cudaStream_t stream);\r
+\r
+ template<class T>\r
+ void init_message(T* u_new, T* d_new, T* l_new, T* r_new,\r
+ const T* u_cur, const T* d_cur, const T* l_cur, const T* r_cur,\r
+ T* selected_disp_pyr_new, const T* selected_disp_pyr_cur,\r
+ T* data_cost_selected, const T* data_cost, size_t msg_step1, size_t msg_step2,\r
+ int h, int w, int nr_plane, int h2, int w2, int nr_plane2, cudaStream_t stream);\r
+\r
+ template<class T>\r
+ void calc_all_iterations(T* u, T* d, T* l, T* r, const T* data_cost_selected,\r
+ const T* selected_disp_pyr_cur, size_t msg_step, int h, int w, int nr_plane, int iters, cudaStream_t stream);\r
+\r
+ template<class T> \r
+ 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
+ const DevMem2D_<short>& disp, int nr_plane, cudaStream_t stream);\r
+\r
+}}}\r
+\r
+namespace\r
+{\r
+ const float DEFAULT_MAX_DATA_TERM = 30.0f;\r
+ const float DEFAULT_DATA_WEIGHT = 1.0f;\r
+ const float DEFAULT_MAX_DISC_TERM = 160.0f;\r
+ const float DEFAULT_DISC_SINGLE_JUMP = 10.0f;\r
+}\r
+\r
+void cv::gpu::StereoConstantSpaceBP::estimateRecommendedParams(int width, int height, int& ndisp, int& iters, int& levels, int& nr_plane)\r
+{\r
+ ndisp = (int) ((float) width / 3.14f);\r
+ if ((ndisp & 1) != 0) \r
+ ndisp++;\r
+\r
+ int mm = ::max(width, height);\r
+ iters = mm / 100 + ((mm > 1200)? - 4 : 4);\r
+\r
+ levels = (int)::log(static_cast<double>(mm)) * 2 / 3;\r
+ if (levels == 0) levels++;\r
+\r
+ nr_plane = (int) ((float) ndisp / pow(2.0, levels + 1));\r
+}\r
+\r
+cv::gpu::StereoConstantSpaceBP::StereoConstantSpaceBP(int ndisp_, int iters_, int levels_, int nr_plane_,\r
+ int msg_type_)\r
+\r
+ : ndisp(ndisp_), iters(iters_), levels(levels_), nr_plane(nr_plane_),\r
+ max_data_term(DEFAULT_MAX_DATA_TERM), data_weight(DEFAULT_DATA_WEIGHT),\r
+ max_disc_term(DEFAULT_MAX_DISC_TERM), disc_single_jump(DEFAULT_DISC_SINGLE_JUMP), min_disp_th(0),\r
+ msg_type(msg_type_), use_local_init_data_cost(true)\r
+{\r
+ CV_Assert(msg_type_ == CV_32F || msg_type_ == CV_16S);\r
+}\r
+\r
+cv::gpu::StereoConstantSpaceBP::StereoConstantSpaceBP(int ndisp_, int iters_, int levels_, int nr_plane_,\r
+ float max_data_term_, float data_weight_, float max_disc_term_, float disc_single_jump_,\r
+ int min_disp_th_, int msg_type_)\r
+ : ndisp(ndisp_), iters(iters_), levels(levels_), nr_plane(nr_plane_),\r
+ max_data_term(max_data_term_), data_weight(data_weight_),\r
+ max_disc_term(max_disc_term_), disc_single_jump(disc_single_jump_), min_disp_th(min_disp_th_),\r
+ msg_type(msg_type_), use_local_init_data_cost(true)\r
+{\r
+ CV_Assert(msg_type_ == CV_32F || msg_type_ == CV_16S);\r
+}\r
+\r
+template<class T>\r
+static void csbp_operator(StereoConstantSpaceBP& rthis, 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& temp, GpuMat& out, const GpuMat& left, const GpuMat& right, GpuMat& disp, cudaStream_t stream)\r
+{\r
+ CV_DbgAssert(0 < rthis.ndisp && 0 < rthis.iters && 0 < rthis.levels && 0 < rthis.nr_plane\r
+ && left.rows == right.rows && left.cols == right.cols && left.type() == right.type());\r
+\r
+ CV_Assert(rthis.levels <= 8 && (left.type() == CV_8UC1 || left.type() == CV_8UC3 || left.type() == CV_8UC4));\r
+\r
+ const Scalar zero = Scalar::all(0);\r
+\r
+ ////////////////////////////////////////////////////////////////////////////////////////////\r
+ // Init\r
+\r
+ int rows = left.rows;\r
+ int cols = left.cols;\r
+\r
+ rthis.levels = min(rthis.levels, int(log((double)rthis.ndisp) / log(2.0)));\r
+ int levels = rthis.levels;\r
+\r
+ AutoBuffer<int> buf(levels * 4);\r
+\r
+ int* cols_pyr = buf;\r
+ int* rows_pyr = cols_pyr + levels;\r
+ int* nr_plane_pyr = rows_pyr + levels;\r
+ int* step_pyr = nr_plane_pyr + levels;\r
+\r
+ cols_pyr[0] = cols;\r
+ rows_pyr[0] = rows;\r
+ nr_plane_pyr[0] = rthis.nr_plane;\r
+\r
+ const int n = 64;\r
+ step_pyr[0] = alignSize(cols * sizeof(T), n) / sizeof(T);\r
+ for (int i = 1; i < levels; i++)\r
+ {\r
+ cols_pyr[i] = (cols_pyr[i-1] + 1) / 2;\r
+ rows_pyr[i] = (rows_pyr[i-1] + 1) / 2;\r
+\r
+ nr_plane_pyr[i] = nr_plane_pyr[i-1] * 2;\r
+\r
+ step_pyr[i] = alignSize(cols_pyr[i] * sizeof(T), n) / sizeof(T);\r
+ }\r
+\r
+ Size msg_size(step_pyr[0], rows * nr_plane_pyr[0]);\r
+ Size data_cost_size(step_pyr[0], rows * nr_plane_pyr[0] * 2);\r
+\r
+ u[0].create(msg_size, DataType<T>::type);\r
+ d[0].create(msg_size, DataType<T>::type);\r
+ l[0].create(msg_size, DataType<T>::type);\r
+ r[0].create(msg_size, DataType<T>::type);\r
+\r
+ u[1].create(msg_size, DataType<T>::type);\r
+ d[1].create(msg_size, DataType<T>::type);\r
+ l[1].create(msg_size, DataType<T>::type);\r
+ r[1].create(msg_size, DataType<T>::type);\r
+\r
+ disp_selected_pyr[0].create(msg_size, DataType<T>::type);\r
+ disp_selected_pyr[1].create(msg_size, DataType<T>::type);\r
+\r
+ data_cost.create(data_cost_size, DataType<T>::type);\r
+ data_cost_selected.create(msg_size, DataType<T>::type);\r
+\r
+ step_pyr[0] = data_cost.step / sizeof(T);\r
+\r
+ Size temp_size = data_cost_size;\r
+ if (data_cost_size.width * data_cost_size.height < step_pyr[levels - 1] * rows_pyr[levels - 1] * rthis.ndisp)\r
+ temp_size = Size(step_pyr[levels - 1], rows_pyr[levels - 1] * rthis.ndisp);\r
+\r
+ temp.create(temp_size, DataType<T>::type);\r
+\r
+ ////////////////////////////////////////////////////////////////////////////\r
+ // Compute\r
+\r
+ csbp::load_constants(rthis.ndisp, rthis.max_data_term, rthis.data_weight,\r
+ rthis.max_disc_term, rthis.disc_single_jump, rthis.min_disp_th, left, right, temp);\r
+\r
+ l[0] = zero;\r
+ d[0] = zero;\r
+ r[0] = zero;\r
+ u[0] = zero;\r
+\r
+ l[1] = zero;\r
+ d[1] = zero;\r
+ r[1] = zero;\r
+ u[1] = zero;\r
+\r
+ data_cost = zero;\r
+ data_cost_selected = zero;\r
+\r
+ int cur_idx = 0;\r
+\r
+ for (int i = levels - 1; i >= 0; i--)\r
+ {\r
+ if (i == levels - 1)\r
+ {\r
+ csbp::init_data_cost(left.rows, left.cols, disp_selected_pyr[cur_idx].ptr<T>(), data_cost_selected.ptr<T>(),\r
+ step_pyr[i], rows_pyr[i], cols_pyr[i], i, nr_plane_pyr[i], rthis.ndisp, left.channels(), rthis.use_local_init_data_cost, stream);\r
+ }\r
+ else\r
+ {\r
+ csbp::compute_data_cost(disp_selected_pyr[cur_idx].ptr<T>(), data_cost.ptr<T>(), step_pyr[i], step_pyr[i+1],\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
+ csbp::init_message(u[new_idx].ptr<T>(), d[new_idx].ptr<T>(), l[new_idx].ptr<T>(), r[new_idx].ptr<T>(),\r
+ u[cur_idx].ptr<T>(), d[cur_idx].ptr<T>(), l[cur_idx].ptr<T>(), r[cur_idx].ptr<T>(),\r
+ disp_selected_pyr[new_idx].ptr<T>(), disp_selected_pyr[cur_idx].ptr<T>(),\r
+ data_cost_selected.ptr<T>(), data_cost.ptr<T>(), step_pyr[i], step_pyr[i+1], rows_pyr[i],\r
+ cols_pyr[i], nr_plane_pyr[i], rows_pyr[i+1], cols_pyr[i+1], nr_plane_pyr[i+1], stream);\r
+\r
+ cur_idx = new_idx;\r
+ }\r
+\r
+ csbp::calc_all_iterations(u[cur_idx].ptr<T>(), d[cur_idx].ptr<T>(), l[cur_idx].ptr<T>(), r[cur_idx].ptr<T>(),\r
+ data_cost_selected.ptr<T>(), disp_selected_pyr[cur_idx].ptr<T>(), step_pyr[i],\r
+ rows_pyr[i], cols_pyr[i], nr_plane_pyr[i], rthis.iters, stream);\r
+ }\r
+\r
+ if (disp.empty())\r
+ disp.create(rows, cols, CV_16S);\r
+\r
+ out = ((disp.type() == CV_16S) ? disp : (out.create(rows, cols, CV_16S), out));\r
+ out = zero;\r
+\r
+ csbp::compute_disp(u[cur_idx].ptr<T>(), d[cur_idx].ptr<T>(), l[cur_idx].ptr<T>(), r[cur_idx].ptr<T>(),\r
+ data_cost_selected.ptr<T>(), disp_selected_pyr[cur_idx].ptr<T>(), step_pyr[0], out, nr_plane_pyr[0], stream);\r
+\r
+ if (disp.type() != CV_16S)\r
+ out.convertTo(disp, disp.type());\r
+}\r
+\r
+\r
+typedef void (*csbp_operator_t)(StereoConstantSpaceBP& rthis, 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& temp, GpuMat& out, const GpuMat& left, const GpuMat& right, GpuMat& disp, cudaStream_t stream);\r
+\r
+const static csbp_operator_t operators[] = {0, 0, 0, csbp_operator<short>, 0, csbp_operator<float>, 0, 0};\r
+\r
+void cv::gpu::StereoConstantSpaceBP::operator()(const GpuMat& left, const GpuMat& right, GpuMat& disp)\r
+{\r
+ CV_Assert(msg_type == CV_32F || msg_type == CV_16S);\r
+ operators[msg_type](*this, u, d, l, r, disp_selected_pyr, data_cost, data_cost_selected, temp, out, left, right, disp, 0);\r
+}\r
+\r
+void cv::gpu::StereoConstantSpaceBP::operator()(const GpuMat& left, const GpuMat& right, GpuMat& disp, Stream& stream)\r
+{\r
+ CV_Assert(msg_type == CV_32F || msg_type == CV_16S);\r
+ operators[msg_type](*this, u, d, l, r, disp_selected_pyr, data_cost, data_cost_selected, temp, out, left, right, disp, StreamAccessor::getStream(stream));\r
+}\r
+\r
+#endif /* !defined (HAVE_CUDA) */\r
projects / platform / upstream / opencv.git / commitdiff