1 /*M/////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 5 // By downloading, copying, installing or using the software you agree to this license. 6 // If you do not agree to this license, do not download, install, 7 // copy or use the software. 8 // 9 // 10 // License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. 14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved. 15 // Third party copyrights are property of their respective owners. 16 // 17 // Redistribution and use in source and binary forms, with or without modification, 18 // are permitted provided that the following conditions are met: 19 // 20 // * Redistribution's of source code must retain the above copyright notice, 21 // this list of conditions and the following disclaimer. 22 // 23 // * Redistribution's in binary form must reproduce the above copyright notice, 24 // this list of conditions and the following disclaimer in the documentation 25 // and/or other materials provided with the distribution. 26 // 27 // * The name of the copyright holders may not be used to endorse or promote products 28 // derived from this software without specific prior written permission. 29 // 30 // This software is provided by the copyright holders and contributors "as is" and 31 // any express or implied warranties, including, but not limited to, the implied 32 // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 // In no event shall the Intel Corporation or contributors be liable for any direct, 34 // indirect, incidental, special, exemplary, or consequential damages 35 // (including, but not limited to, procurement of substitute goods or services; 36 // loss of use, data, or profits; or business interruption) however caused 37 // and on any theory of liability, whether in contract, strict liability, 38 // or tort (including negligence or otherwise) arising in any way out of 39 // the use of this software, even if advised of the possibility of such damage. 40 // 41 //M*/ 42 43 #if !defined CUDA_DISABLER 44 45 #include "opencv2/core/cuda/common.hpp" 46 #include "opencv2/core/cuda/transform.hpp" 47 #include "opencv2/core/cuda/functional.hpp" 48 #include "opencv2/core/cuda/reduce.hpp" 49 50 namespace cv { namespace cuda { namespace device 51 { 52 /////////////////////////////////// reprojectImageTo3D /////////////////////////////////////////////// 53 54 __constant__ float cq[16]; 55 56 template <typename T, typename D> 57 __global__ void reprojectImageTo3D(const PtrStepSz<T> disp, PtrStep<D> xyz) 58 { 59 const int x = blockIdx.x * blockDim.x + threadIdx.x; 60 const int y = blockIdx.y * blockDim.y + threadIdx.y; 61 62 if (y >= disp.rows || x >= disp.cols) 63 return; 64 65 const float qx = x * cq[ 0] + y * cq[ 1] + cq[ 3]; 66 const float qy = x * cq[ 4] + y * cq[ 5] + cq[ 7]; 67 const float qz = x * cq[ 8] + y * cq[ 9] + cq[11]; 68 const float qw = x * cq[12] + y * cq[13] + cq[15]; 69 70 const T d = disp(y, x); 71 72 const float iW = 1.f / (qw + cq[14] * d); 73 74 D v = VecTraits<D>::all(1.0f); 75 v.x = (qx + cq[2] * d) * iW; 76 v.y = (qy + cq[6] * d) * iW; 77 v.z = (qz + cq[10] * d) * iW; 78 79 xyz(y, x) = v; 80 } 81 82 template <typename T, typename D> 83 void reprojectImageTo3D_gpu(const PtrStepSzb disp, PtrStepSzb xyz, const float* q, cudaStream_t stream) 84 { 85 dim3 block(32, 8); 86 dim3 grid(divUp(disp.cols, block.x), divUp(disp.rows, block.y)); 87 88 cudaSafeCall( cudaMemcpyToSymbol(cq, q, 16 * sizeof(float)) ); 89 90 reprojectImageTo3D<T, D><<<grid, block, 0, stream>>>((PtrStepSz<T>)disp, (PtrStepSz<D>)xyz); 91 cudaSafeCall( cudaGetLastError() ); 92 93 if (stream == 0) 94 cudaSafeCall( cudaDeviceSynchronize() ); 95 } 96 97 template void reprojectImageTo3D_gpu<uchar, float3>(const PtrStepSzb disp, PtrStepSzb xyz, const float* q, cudaStream_t stream); 98 template void reprojectImageTo3D_gpu<uchar, float4>(const PtrStepSzb disp, PtrStepSzb xyz, const float* q, cudaStream_t stream); 99 template void reprojectImageTo3D_gpu<short, float3>(const PtrStepSzb disp, PtrStepSzb xyz, const float* q, cudaStream_t stream); 100 template void reprojectImageTo3D_gpu<short, float4>(const PtrStepSzb disp, PtrStepSzb xyz, const float* q, cudaStream_t stream); 101 102 /////////////////////////////////// drawColorDisp /////////////////////////////////////////////// 103 104 template <typename T> 105 __device__ unsigned int cvtPixel(T d, int ndisp, float S = 1, float V = 1) 106 { 107 unsigned int H = ((ndisp-d) * 240)/ndisp; 108 109 unsigned int hi = (H/60) % 6; 110 float f = H/60.f - H/60; 111 float p = V * (1 - S); 112 float q = V * (1 - f * S); 113 float t = V * (1 - (1 - f) * S); 114 115 float3 res; 116 117 if (hi == 0) //R = V, G = t, B = p 118 { 119 res.x = p; 120 res.y = t; 121 res.z = V; 122 } 123 124 if (hi == 1) // R = q, G = V, B = p 125 { 126 res.x = p; 127 res.y = V; 128 res.z = q; 129 } 130 131 if (hi == 2) // R = p, G = V, B = t 132 { 133 res.x = t; 134 res.y = V; 135 res.z = p; 136 } 137 138 if (hi == 3) // R = p, G = q, B = V 139 { 140 res.x = V; 141 res.y = q; 142 res.z = p; 143 } 144 145 if (hi == 4) // R = t, G = p, B = V 146 { 147 res.x = V; 148 res.y = p; 149 res.z = t; 150 } 151 152 if (hi == 5) // R = V, G = p, B = q 153 { 154 res.x = q; 155 res.y = p; 156 res.z = V; 157 } 158 const unsigned int b = (unsigned int)(::max(0.f, ::min(res.x, 1.f)) * 255.f); 159 const unsigned int g = (unsigned int)(::max(0.f, ::min(res.y, 1.f)) * 255.f); 160 const unsigned int r = (unsigned int)(::max(0.f, ::min(res.z, 1.f)) * 255.f); 161 const unsigned int a = 255U; 162 163 return (a << 24) + (r << 16) + (g << 8) + b; 164 } 165 166 __global__ void drawColorDisp(uchar* disp, size_t disp_step, uchar* out_image, size_t out_step, int width, int height, int ndisp) 167 { 168 const int x = (blockIdx.x * blockDim.x + threadIdx.x) << 2; 169 const int y = blockIdx.y * blockDim.y + threadIdx.y; 170 171 if(x < width && y < height) 172 { 173 uchar4 d4 = *(uchar4*)(disp + y * disp_step + x); 174 175 uint4 res; 176 res.x = cvtPixel(d4.x, ndisp); 177 res.y = cvtPixel(d4.y, ndisp); 178 res.z = cvtPixel(d4.z, ndisp); 179 res.w = cvtPixel(d4.w, ndisp); 180 181 uint4* line = (uint4*)(out_image + y * out_step); 182 line[x >> 2] = res; 183 } 184 } 185 186 __global__ void drawColorDisp(short* disp, size_t disp_step, uchar* out_image, size_t out_step, int width, int height, int ndisp) 187 { 188 const int x = (blockIdx.x * blockDim.x + threadIdx.x) << 1; 189 const int y = blockIdx.y * blockDim.y + threadIdx.y; 190 191 if(x < width && y < height) 192 { 193 short2 d2 = *(short2*)(disp + y * disp_step + x); 194 195 uint2 res; 196 res.x = cvtPixel(d2.x, ndisp); 197 res.y = cvtPixel(d2.y, ndisp); 198 199 uint2* line = (uint2*)(out_image + y * out_step); 200 line[x >> 1] = res; 201 } 202 } 203 204 205 void drawColorDisp_gpu(const PtrStepSzb& src, const PtrStepSzb& dst, int ndisp, const cudaStream_t& stream) 206 { 207 dim3 threads(16, 16, 1); 208 dim3 grid(1, 1, 1); 209 grid.x = divUp(src.cols, threads.x << 2); 210 grid.y = divUp(src.rows, threads.y); 211 212 drawColorDisp<<<grid, threads, 0, stream>>>(src.data, src.step, dst.data, dst.step, src.cols, src.rows, ndisp); 213 cudaSafeCall( cudaGetLastError() ); 214 215 if (stream == 0) 216 cudaSafeCall( cudaDeviceSynchronize() ); 217 } 218 219 void drawColorDisp_gpu(const PtrStepSz<short>& src, const PtrStepSzb& dst, int ndisp, const cudaStream_t& stream) 220 { 221 dim3 threads(32, 8, 1); 222 dim3 grid(1, 1, 1); 223 grid.x = divUp(src.cols, threads.x << 1); 224 grid.y = divUp(src.rows, threads.y); 225 226 drawColorDisp<<<grid, threads, 0, stream>>>(src.data, src.step / sizeof(short), dst.data, dst.step, src.cols, src.rows, ndisp); 227 cudaSafeCall( cudaGetLastError() ); 228 229 if (stream == 0) 230 cudaSafeCall( cudaDeviceSynchronize() ); 231 } 232 }}} // namespace cv { namespace cuda { namespace cudev 233 234 235 #endif /* CUDA_DISABLER */ 236
