// This file is auto-generated. Do not edit!

#include "opencv2/core.hpp"
#include "cvconfig.h"
#include "opencl_kernels_rgbd.hpp"

#ifdef HAVE_OPENCL

namespace cv
{
namespace ocl
{
namespace rgbd
{

static const char* const moduleName = "rgbd";

struct cv::ocl::internal::ProgramEntry hash_tsdf_oclsrc={moduleName, "hash_tsdf",
"#define USE_INTERPOLATION_IN_GETNORMAL 1\n"
"#define HASH_DIVISOR 32768\n"
"typedef char int8_t;\n"
"typedef uint int32_t;\n"
"typedef int8_t TsdfType;\n"
"typedef uchar WeightType;\n"
"struct TsdfVoxel\n"
"{\n"
"TsdfType tsdf;\n"
"WeightType weight;\n"
"};\n"
"static inline TsdfType floatToTsdf(float num)\n"
"{\n"
"int8_t res = (int8_t) ( (num * (-128)) );\n"
"res = res ? res : (num < 0 ? 1 : -1);\n"
"return res;\n"
"}\n"
"static inline float tsdfToFloat(TsdfType num)\n"
"{\n"
"return ( (float) num ) / (-128);\n"
"}\n"
"static uint calc_hash(int3 x)\n"
"{\n"
"unsigned int seed = 0;\n"
"unsigned int GOLDEN_RATIO = 0x9e3779b9;\n"
"seed ^= x.s0 + GOLDEN_RATIO + (seed << 6) + (seed >> 2);\n"
"seed ^= x.s1 + GOLDEN_RATIO + (seed << 6) + (seed >> 2);\n"
"seed ^= x.s2 + GOLDEN_RATIO + (seed << 6) + (seed >> 2);\n"
"return seed;\n"
"}\n"
"static int custom_find(int3 idx, const int hashDivisor, __global const int* hashes,\n"
"__global const int4* data)\n"
"{\n"
"int hash = calc_hash(idx) % hashDivisor;\n"
"int place = hashes[hash];\n"
"while (place >= 0)\n"
"{\n"
"if (all(data[place].s012 == idx))\n"
"break;\n"
"else\n"
"place = data[place].s3;\n"
"}\n"
"return place;\n"
"}\n"
"static void integrateVolumeUnit(\n"
"int x, int y,\n"
"__global const char * depthptr,\n"
"int depth_step, int depth_offset,\n"
"int depth_rows, int depth_cols,\n"
"__global struct TsdfVoxel * volumeptr,\n"
"const __global char * pixNormsPtr,\n"
"int pixNormsStep, int pixNormsOffset,\n"
"int pixNormsRows, int pixNormsCols,\n"
"const float16 vol2camMatrix,\n"
"const float voxelSize,\n"
"const int4 volResolution4,\n"
"const int4 volStrides4,\n"
"const float2 fxy,\n"
"const float2 cxy,\n"
"const float dfac,\n"
"const float truncDist,\n"
"const int maxWeight\n"
")\n"
"{\n"
"const int3 volResolution = volResolution4.xyz;\n"
"if(x >= volResolution.x || y >= volResolution.y)\n"
"return;\n"
"const int3 volStrides = volStrides4.xyz;\n"
"const float2 limits = (float2)(depth_cols-1, depth_rows-1);\n"
"const float4 vol2cam0 = vol2camMatrix.s0123;\n"
"const float4 vol2cam1 = vol2camMatrix.s4567;\n"
"const float4 vol2cam2 = vol2camMatrix.s89ab;\n"
"const float truncDistInv = 1.f/truncDist;\n"
"float4 inPt = (float4)(x*voxelSize, y*voxelSize, 0, 1);\n"
"float3 basePt = (float3)(dot(vol2cam0, inPt),\n"
"dot(vol2cam1, inPt),\n"
"dot(vol2cam2, inPt));\n"
"float3 camSpacePt = basePt;\n"
"float3 zStep = ((float3)(vol2cam0.z, vol2cam1.z, vol2cam2.z))*voxelSize;\n"
"int volYidx = x*volStrides.x + y*volStrides.y;\n"
"int startZ, endZ;\n"
"if(fabs(zStep.z) > 1e-5f)\n"
"{\n"
"int baseZ = convert_int(-basePt.z / zStep.z);\n"
"if(zStep.z > 0)\n"
"{\n"
"startZ = baseZ;\n"
"endZ = volResolution.z;\n"
"}\n"
"else\n"
"{\n"
"startZ = 0;\n"
"endZ = baseZ;\n"
"}\n"
"}\n"
"else\n"
"{\n"
"if(basePt.z > 0)\n"
"{\n"
"startZ = 0; endZ = volResolution.z;\n"
"}\n"
"else\n"
"{\n"
"startZ = endZ = 0;\n"
"}\n"
"}\n"
"startZ = max(0, startZ);\n"
"endZ = min(volResolution.z, endZ);\n"
"for(int z = startZ; z < endZ; z++)\n"
"{\n"
"camSpacePt += zStep;\n"
"if(camSpacePt.z <= 0)\n"
"continue;\n"
"float3 camPixVec = camSpacePt / camSpacePt.z;\n"
"float2 projected = mad(camPixVec.xy, fxy, cxy);\n"
"float v;\n"
"if(all(projected >= 0) && all(projected < limits))\n"
"{\n"
"float2 ip = floor(projected);\n"
"int xi = ip.x, yi = ip.y;\n"
"__global const float* row0 = (__global const float*)(depthptr + depth_offset +\n"
"(yi+0)*depth_step);\n"
"__global const float* row1 = (__global const float*)(depthptr + depth_offset +\n"
"(yi+1)*depth_step);\n"
"float v00 = row0[xi+0];\n"
"float v01 = row0[xi+1];\n"
"float v10 = row1[xi+0];\n"
"float v11 = row1[xi+1];\n"
"float4 vv = (float4)(v00, v01, v10, v11);\n"
"if(all(vv > 0))\n"
"{\n"
"float2 t = projected - ip;\n"
"float2 vf = mix(vv.xz, vv.yw, t.x);\n"
"v = mix(vf.s0, vf.s1, t.y);\n"
"}\n"
"else\n"
"continue;\n"
"}\n"
"else\n"
"continue;\n"
"if(v == 0)\n"
"continue;\n"
"int2 projInt = convert_int2(projected);\n"
"float pixNorm = *(__global const float*)(pixNormsPtr + pixNormsOffset + projInt.y*pixNormsStep + projInt.x*sizeof(float));\n"
"float sdf = pixNorm*(v*dfac - camSpacePt.z);\n"
"if(sdf >= -truncDist)\n"
"{\n"
"float tsdf = fmin(1.0f, sdf * truncDistInv);\n"
"int volIdx = volYidx + z*volStrides.z;\n"
"struct TsdfVoxel voxel = volumeptr[volIdx];\n"
"float value  = tsdfToFloat(voxel.tsdf);\n"
"int weight = voxel.weight;\n"
"value = (value*weight + tsdf) / (weight + 1);\n"
"weight = min(weight + 1, maxWeight);\n"
"voxel.tsdf = floatToTsdf(value);\n"
"voxel.weight = weight;\n"
"volumeptr[volIdx] = voxel;\n"
"}\n"
"}\n"
"}\n"
"__kernel void integrateAllVolumeUnits(\n"
"__global const char * depthptr,\n"
"int depth_step, int depth_offset,\n"
"int depth_rows, int depth_cols,\n"
"__global const int* hashes,\n"
"__global const int4* data,\n"
"__global struct TsdfVoxel * allVolumePtr,\n"
"int table_step, int table_offset,\n"
"int table_rows, int table_cols,\n"
"const __global char * pixNormsPtr,\n"
"int pixNormsStep, int pixNormsOffset,\n"
"int pixNormsRows, int pixNormsCols,\n"
"__global const uchar* isActiveFlagsPtr,\n"
"int isActiveFlagsStep, int isActiveFlagsOffset,\n"
"int isActiveFlagsRows, int isActiveFlagsCols,\n"
"const float16 vol2cam,\n"
"const float16 camInv,\n"
"const float voxelSize,\n"
"const int volUnitResolution,\n"
"const int4 volStrides4,\n"
"const float2 fxy,\n"
"const float2 cxy,\n"
"const float dfac,\n"
"const float truncDist,\n"
"const int maxWeight\n"
")\n"
"{\n"
"const int hash_divisor = HASH_DIVISOR;\n"
"int i = get_global_id(0);\n"
"int j = get_global_id(1);\n"
"int row = get_global_id(2);\n"
"int3 idx = data[row].xyz;\n"
"const int4 volResolution4 = (int4)(volUnitResolution,\n"
"volUnitResolution,\n"
"volUnitResolution,\n"
"volUnitResolution);\n"
"int isActive = *(__global const uchar*)(isActiveFlagsPtr + isActiveFlagsOffset + row);\n"
"if (isActive)\n"
"{\n"
"int volCubed = volUnitResolution * volUnitResolution * volUnitResolution;\n"
"__global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*)\n"
"(allVolumePtr + table_offset + row * volCubed);\n"
"float3 mulIdx = convert_float3(idx * volUnitResolution) * voxelSize;\n"
"float16 volUnit2cam = vol2cam;\n"
"volUnit2cam.s37b += (float3)(dot(mulIdx, camInv.s012),\n"
"dot(mulIdx, camInv.s456),\n"
"dot(mulIdx, camInv.s89a));\n"
"integrateVolumeUnit(\n"
"i, j,\n"
"depthptr,\n"
"depth_step, depth_offset,\n"
"depth_rows, depth_cols,\n"
"volumeptr,\n"
"pixNormsPtr,\n"
"pixNormsStep, pixNormsOffset,\n"
"pixNormsRows, pixNormsCols,\n"
"volUnit2cam,\n"
"voxelSize,\n"
"volResolution4,\n"
"volStrides4,\n"
"fxy,\n"
"cxy,\n"
"dfac,\n"
"truncDist,\n"
"maxWeight\n"
");\n"
"}\n"
"}\n"
"static struct TsdfVoxel at(int3 volumeIdx, int row, int volumeUnitDegree,\n"
"int3 volStrides, __global const struct TsdfVoxel * allVolumePtr, int table_offset)\n"
"{\n"
"if (any(volumeIdx >= (1 << volumeUnitDegree)) ||\n"
"any(volumeIdx < 0))\n"
"{\n"
"struct TsdfVoxel dummy;\n"
"dummy.tsdf = floatToTsdf(1.0f);\n"
"dummy.weight = 0;\n"
"return dummy;\n"
"}\n"
"int volCubed = 1 << (volumeUnitDegree*3);\n"
"__global struct TsdfVoxel * volData = (__global struct TsdfVoxel*)\n"
"(allVolumePtr + table_offset + row * volCubed);\n"
"int3 ismul = volumeIdx * volStrides;\n"
"int coordBase = ismul.x + ismul.y + ismul.z;\n"
"return volData[coordBase];\n"
"}\n"
"static struct TsdfVoxel atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int row,\n"
"int volumeUnitDegree, int3 volStrides,\n"
"__global const struct TsdfVoxel * allVolumePtr, int table_offset)\n"
"{\n"
"if (row < 0)\n"
"{\n"
"struct TsdfVoxel dummy;\n"
"dummy.tsdf = floatToTsdf(1.0f);\n"
"dummy.weight = 0;\n"
"return dummy;\n"
"}\n"
"int3 volUnitLocalIdx = volumeIdx - (volumeUnitIdx << volumeUnitDegree);\n"
"int volCubed = 1 << (volumeUnitDegree*3);\n"
"__global struct TsdfVoxel * volData = (__global struct TsdfVoxel*)\n"
"(allVolumePtr + table_offset + row * volCubed);\n"
"int3 ismul = volUnitLocalIdx * volStrides;\n"
"int coordBase = ismul.x + ismul.y + ismul.z;\n"
"return volData[coordBase];\n"
"}\n"
"inline float interpolate(float3 t, float8 vz)\n"
"{\n"
"float4 vy = mix(vz.s0246, vz.s1357, t.z);\n"
"float2 vx = mix(vy.s02, vy.s13, t.y);\n"
"return mix(vx.s0, vx.s1, t.x);\n"
"}\n"
"inline float3 getNormalVoxel(float3 ptVox, __global const struct TsdfVoxel* allVolumePtr,\n"
"int volumeUnitDegree,\n"
"const int hash_divisor,\n"
"__global const int* hashes,\n"
"__global const int4* data,\n"
"int3 volStrides, int table_offset)\n"
"{\n"
"float3 normal = (float3) (0.0f, 0.0f, 0.0f);\n"
"float3 fip = floor(ptVox);\n"
"int3 iptVox = convert_int3(fip);\n"
"int iterMap[8];\n"
"for (int i = 0; i < 8; i++)\n"
"{\n"
"iterMap[i] = -2;\n"
"}\n"
"#if !USE_INTERPOLATION_IN_GETNORMAL\n"
"int4 offsets[] = { (int4)( 1,  0,  0, 0), (int4)(-1,  0,  0, 0), (int4)( 0,  1,  0, 0),\n"
"(int4)( 0, -1,  0, 0), (int4)( 0,  0,  1, 0), (int4)( 0,  0, -1, 0)\n"
"};\n"
"const int nVals = 6;\n"
"float vals[6];\n"
"#else\n"
"int4 offsets[]={(int4)( 0,  0,  0, 0), (int4)( 0,  0,  1, 0), (int4)( 0,  1,  0, 0), (int4)( 0,  1,  1, 0),\n"
"(int4)( 1,  0,  0, 0), (int4)( 1,  0,  1, 0), (int4)( 1,  1,  0, 0), (int4)( 1,  1,  1, 0),\n"
"(int4)(-1,  0,  0, 0), (int4)(-1,  0,  1, 0), (int4)(-1,  1,  0, 0), (int4)(-1,  1,  1, 0),\n"
"(int4)( 2,  0,  0, 0), (int4)( 2,  0,  1, 0), (int4)( 2,  1,  0, 0), (int4)( 2,  1,  1, 0),\n"
"(int4)( 0, -1,  0, 0), (int4)( 0, -1,  1, 0), (int4)( 1, -1,  0, 0), (int4)( 1, -1,  1, 0),\n"
"(int4)( 0,  2,  0, 0), (int4)( 0,  2,  1, 0), (int4)( 1,  2,  0, 0), (int4)( 1,  2,  1, 0),\n"
"(int4)( 0,  0, -1, 0), (int4)( 0,  1, -1, 0), (int4)( 1,  0, -1, 0), (int4)( 1,  1, -1, 0),\n"
"(int4)( 0,  0,  2, 0), (int4)( 0,  1,  2, 0), (int4)( 1,  0,  2, 0), (int4)( 1,  1,  2, 0),\n"
"};\n"
"const int nVals = 32;\n"
"float vals[32];\n"
"#endif\n"
"for (int i = 0; i < nVals; i++)\n"
"{\n"
"int3 pt = iptVox + offsets[i].s012;\n"
"int3 volumeUnitIdx = pt >> volumeUnitDegree;\n"
"int3 vand = (volumeUnitIdx & 1);\n"
"int dictIdx = vand.s0 + vand.s1 * 2 + vand.s2 * 4;\n"
"int it = iterMap[dictIdx];\n"
"if (it < -1)\n"
"{\n"
"it = custom_find(volumeUnitIdx, hash_divisor, hashes, data);\n"
"iterMap[dictIdx] = it;\n"
"}\n"
"struct TsdfVoxel tmp = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnitDegree, volStrides, allVolumePtr, table_offset);\n"
"vals[i] = tsdfToFloat( tmp.tsdf );\n"
"}\n"
"#if !USE_INTERPOLATION_IN_GETNORMAL\n"
"float3 pv, nv;\n"
"pv = (float3)(vals[0*2  ], vals[1*2  ], vals[2*2  ]);\n"
"nv = (float3)(vals[0*2+1], vals[1*2+1], vals[2*2+1]);\n"
"normal = pv - nv;\n"
"#else\n"
"float cxv[8], cyv[8], czv[8];\n"
"const int idxxn[8] = {  8,  9, 10, 11,  0,  1,  2,  3 };\n"
"const int idxxp[8] = {  4,  5,  6,  7, 12, 13, 14, 15 };\n"
"const int idxyn[8] = { 16, 17,  0,  1, 18, 19,  4,  5 };\n"
"const int idxyp[8] = {  2,  3, 20, 21,  6,  7, 22, 23 };\n"
"const int idxzn[8] = { 24,  0, 25,  2, 26,  4, 27,  6 };\n"
"const int idxzp[8] = {  1, 28,  3, 29,  5, 30,  7, 31 };\n"
"float vcxp[8], vcxn[8];\n"
"float vcyp[8], vcyn[8];\n"
"float vczp[8], vczn[8];\n"
"for (int i = 0; i < 8; i++)\n"
"{\n"
"vcxp[i] = vals[idxxp[i]]; vcxn[i] = vals[idxxn[i]];\n"
"vcyp[i] = vals[idxyp[i]]; vcyn[i] = vals[idxyn[i]];\n"
"vczp[i] = vals[idxzp[i]]; vczn[i] = vals[idxzn[i]];\n"
"}\n"
"float8 cxp = vload8(0, vcxp), cxn = vload8(0, vcxn);\n"
"float8 cyp = vload8(0, vcyp), cyn = vload8(0, vcyn);\n"
"float8 czp = vload8(0, vczp), czn = vload8(0, vczn);\n"
"float8 cx = cxp - cxn;\n"
"float8 cy = cyp - cyn;\n"
"float8 cz = czp - czn;\n"
"float3 tv = ptVox - fip;\n"
"normal.x = interpolate(tv, cx);\n"
"normal.y = interpolate(tv, cy);\n"
"normal.z = interpolate(tv, cz);\n"
"#endif\n"
"float norm = sqrt(dot(normal, normal));\n"
"return norm < 0.0001f ? nan((uint)0) : normal / norm;\n"
"}\n"
"typedef float4 ptype;\n"
"__kernel void raycast(\n"
"__global const int* hashes,\n"
"__global const int4* data,\n"
"__global char * pointsptr,\n"
"int points_step, int points_offset,\n"
"__global char * normalsptr,\n"
"int normals_step, int normals_offset,\n"
"const int2 frameSize,\n"
"__global const struct TsdfVoxel * allVolumePtr,\n"
"int table_step, int table_offset,\n"
"int table_rows, int table_cols,\n"
"float16 cam2volRotGPU,\n"
"float16 vol2camRotGPU,\n"
"float truncateThreshold,\n"
"const float2 fixy, const float2 cxy,\n"
"const float4 boxDown4, const float4 boxUp4,\n"
"const float tstep,\n"
"const float voxelSize,\n"
"const float voxelSizeInv,\n"
"float volumeUnitSize,\n"
"float truncDist,\n"
"int volumeUnitDegree,\n"
"int4 volStrides4\n"
")\n"
"{\n"
"const int hash_divisor = HASH_DIVISOR;\n"
"int x = get_global_id(0);\n"
"int y = get_global_id(1);\n"
"if(x >= frameSize.x || y >= frameSize.y)\n"
"return;\n"
"float3 point  = nan((uint)0);\n"
"float3 normal = nan((uint)0);\n"
"const float3 camRot0  = cam2volRotGPU.s012;\n"
"const float3 camRot1  = cam2volRotGPU.s456;\n"
"const float3 camRot2  = cam2volRotGPU.s89a;\n"
"const float3 camTrans = cam2volRotGPU.s37b;\n"
"const float3 volRot0  = vol2camRotGPU.s012;\n"
"const float3 volRot1  = vol2camRotGPU.s456;\n"
"const float3 volRot2  = vol2camRotGPU.s89a;\n"
"const float3 volTrans = vol2camRotGPU.s37b;\n"
"float3 planed = (float3)(((float2)(x, y) - cxy)*fixy, 1.f);\n"
"planed = (float3)(dot(planed, camRot0),\n"
"dot(planed, camRot1),\n"
"dot(planed, camRot2));\n"
"float3 orig = (float3) (camTrans.s0, camTrans.s1, camTrans.s2);\n"
"float3 dir = fast_normalize(planed);\n"
"float3 origScaled = orig * voxelSizeInv;\n"
"float3 dirScaled = dir * voxelSizeInv;\n"
"float tmin = 0;\n"
"float tmax = truncateThreshold;\n"
"float tcurr = tmin;\n"
"float tprev = tcurr;\n"
"float prevTsdf = truncDist;\n"
"int3 volStrides = volStrides4.xyz;\n"
"while (tcurr < tmax)\n"
"{\n"
"float3 currRayPosVox = origScaled + tcurr * dirScaled;\n"
"int3 currVoxel = convert_int3(floor(currRayPosVox));\n"
"int3 currVolumeUnitIdx = currVoxel >> volumeUnitDegree;\n"
"int row = custom_find(currVolumeUnitIdx, hash_divisor, hashes, data);\n"
"float currTsdf = prevTsdf;\n"
"int currWeight = 0;\n"
"float stepSize = 0.5 * volumeUnitSize;\n"
"int3 volUnitLocalIdx;\n"
"if (row >= 0)\n"
"{\n"
"volUnitLocalIdx = currVoxel - (currVolumeUnitIdx << volumeUnitDegree);\n"
"struct TsdfVoxel currVoxel = at(volUnitLocalIdx, row, volumeUnitDegree, volStrides, allVolumePtr, table_offset);\n"
"currTsdf = tsdfToFloat(currVoxel.tsdf);\n"
"currWeight = currVoxel.weight;\n"
"stepSize = tstep;\n"
"}\n"
"if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0)\n"
"{\n"
"float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf);\n"
"if ( !isnan(tInterp) && !isinf(tInterp) )\n"
"{\n"
"float3 pvox = origScaled + tInterp * dirScaled;\n"
"float3 nv = getNormalVoxel( pvox, allVolumePtr, volumeUnitDegree,\n"
"hash_divisor, hashes, data,\n"
"volStrides, table_offset);\n"
"if(!any(isnan(nv)))\n"
"{\n"
"normal = (float3)(dot(nv, volRot0),\n"
"dot(nv, volRot1),\n"
"dot(nv, volRot2));\n"
"float3 pv = pvox * voxelSize;\n"
"point = (float3)(dot(pv, volRot0),\n"
"dot(pv, volRot1),\n"
"dot(pv, volRot2)) + volTrans;\n"
"}\n"
"}\n"
"break;\n"
"}\n"
"prevTsdf = currTsdf;\n"
"tprev = tcurr;\n"
"tcurr += stepSize;\n"
"}\n"
"__global float* pts = (__global float*)(pointsptr  +  points_offset + y*points_step   + x*sizeof(ptype));\n"
"__global float* nrm = (__global float*)(normalsptr + normals_offset + y*normals_step  + x*sizeof(ptype));\n"
"vstore4((float4)(point,  0), 0, pts);\n"
"vstore4((float4)(normal, 0), 0, nrm);\n"
"}\n"
"__kernel void markActive (\n"
"__global const int4* hashSetData,\n"
"__global char* isActiveFlagsPtr,\n"
"int isActiveFlagsStep, int isActiveFlagsOffset,\n"
"int isActiveFlagsRows, int isActiveFlagsCols,\n"
"__global char* lastVisibleIndicesPtr,\n"
"int lastVisibleIndicesStep, int lastVisibleIndicesOffset,\n"
"int lastVisibleIndicesRows, int lastVisibleIndicesCols,\n"
"const float16 vol2cam,\n"
"const float2 fxy,\n"
"const float2 cxy,\n"
"const int2 frameSz,\n"
"const float volumeUnitSize,\n"
"const int lastVolIndex,\n"
"const float truncateThreshold,\n"
"const int frameId\n"
")\n"
"{\n"
"const int hash_divisor = HASH_DIVISOR;\n"
"int row = get_global_id(0);\n"
"if (row < lastVolIndex)\n"
"{\n"
"int3 idx = hashSetData[row].xyz;\n"
"float3 volumeUnitPos = convert_float3(idx) * volumeUnitSize;\n"
"float3 volUnitInCamSpace = (float3) (dot(volumeUnitPos, vol2cam.s012),\n"
"dot(volumeUnitPos, vol2cam.s456),\n"
"dot(volumeUnitPos, vol2cam.s89a)) + vol2cam.s37b;\n"
"if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold)\n"
"{\n"
"*(isActiveFlagsPtr + isActiveFlagsOffset + row * isActiveFlagsStep) = 0;\n"
"return;\n"
"}\n"
"float2 cameraPoint;\n"
"float invz = 1.f / volUnitInCamSpace.z;\n"
"cameraPoint = fxy * volUnitInCamSpace.xy * invz + cxy;\n"
"if (all(cameraPoint >= 0) && all(cameraPoint < convert_float2(frameSz)))\n"
"{\n"
"*(__global int*)(lastVisibleIndicesPtr + lastVisibleIndicesOffset + row * lastVisibleIndicesStep) = frameId;\n"
"*(isActiveFlagsPtr + isActiveFlagsOffset + row * isActiveFlagsStep) = 1;\n"
"}\n"
"}\n"
"}\n"
, "ad8977a40f1ba1f499b05120287578a4", NULL};
struct cv::ocl::internal::ProgramEntry icp_oclsrc={moduleName, "icp",
"#define UTSIZE 27\n"
"typedef float4 ptype;\n"
"inline void calcAb7(__global const char * oldPointsptr,\n"
"int oldPoints_step, int oldPoints_offset,\n"
"__global const char * oldNormalsptr,\n"
"int oldNormals_step, int oldNormals_offset,\n"
"const int2 oldSize,\n"
"__global const char * newPointsptr,\n"
"int newPoints_step, int newPoints_offset,\n"
"__global const char * newNormalsptr,\n"
"int newNormals_step, int newNormals_offset,\n"
"const int2 newSize,\n"
"const float16 poseMatrix,\n"
"const float2 fxy,\n"
"const float2 cxy,\n"
"const float sqDistanceThresh,\n"
"const float minCos,\n"
"float* ab7\n"
")\n"
"{\n"
"const int x = get_global_id(0);\n"
"const int y = get_global_id(1);\n"
"if(x >= newSize.x || y >= newSize.y)\n"
"return;\n"
"const float3 poseRot0 = poseMatrix.s012;\n"
"const float3 poseRot1 = poseMatrix.s456;\n"
"const float3 poseRot2 = poseMatrix.s89a;\n"
"const float3 poseTrans = poseMatrix.s37b;\n"
"const float2 oldEdge = (float2)(oldSize.x - 1, oldSize.y - 1);\n"
"__global const ptype* newPtsRow = (__global const ptype*)(newPointsptr +\n"
"newPoints_offset +\n"
"y*newPoints_step);\n"
"__global const ptype* newNrmRow = (__global const ptype*)(newNormalsptr +\n"
"newNormals_offset +\n"
"y*newNormals_step);\n"
"float3 newP = newPtsRow[x].xyz;\n"
"float3 newN = newNrmRow[x].xyz;\n"
"if( any(isnan(newP)) || any(isnan(newN)) ||\n"
"any(isinf(newP)) || any(isinf(newN)) )\n"
"return;\n"
"newP = (float3)(dot(newP, poseRot0),\n"
"dot(newP, poseRot1),\n"
"dot(newP, poseRot2)) + poseTrans;\n"
"newN = (float3)(dot(newN, poseRot0),\n"
"dot(newN, poseRot1),\n"
"dot(newN, poseRot2));\n"
"float2 oldCoords = (newP.xy/newP.z)*fxy+cxy;\n"
"if(!(all(oldCoords >= 0.f) && all(oldCoords < oldEdge)))\n"
"return;\n"
"float3 oldP, oldN;\n"
"float2 ip = floor(oldCoords);\n"
"float2 t = oldCoords - ip;\n"
"int xi = ip.x, yi = ip.y;\n"
"__global const ptype* prow0 = (__global const ptype*)(oldPointsptr +\n"
"oldPoints_offset +\n"
"(yi+0)*oldPoints_step);\n"
"__global const ptype* prow1 = (__global const ptype*)(oldPointsptr +\n"
"oldPoints_offset +\n"
"(yi+1)*oldPoints_step);\n"
"float3 p00 = prow0[xi+0].xyz;\n"
"float3 p01 = prow0[xi+1].xyz;\n"
"float3 p10 = prow1[xi+0].xyz;\n"
"float3 p11 = prow1[xi+1].xyz;\n"
"__global const ptype* nrow0 = (__global const ptype*)(oldNormalsptr +\n"
"oldNormals_offset +\n"
"(yi+0)*oldNormals_step);\n"
"__global const ptype* nrow1 = (__global const ptype*)(oldNormalsptr +\n"
"oldNormals_offset +\n"
"(yi+1)*oldNormals_step);\n"
"float3 n00 = nrow0[xi+0].xyz;\n"
"float3 n01 = nrow0[xi+1].xyz;\n"
"float3 n10 = nrow1[xi+0].xyz;\n"
"float3 n11 = nrow1[xi+1].xyz;\n"
"float3 p0 = mix(p00, p01, t.x);\n"
"float3 p1 = mix(p10, p11, t.x);\n"
"oldP = mix(p0, p1, t.y);\n"
"float3 n0 = mix(n00, n01, t.x);\n"
"float3 n1 = mix(n10, n11, t.x);\n"
"oldN = mix(n0, n1, t.y);\n"
"if( any(isnan(oldP)) || any(isnan(oldN)) ||\n"
"any(isinf(oldP)) || any(isinf(oldN)) )\n"
"return;\n"
"float3 diff = newP - oldP;\n"
"if(dot(diff, diff) > sqDistanceThresh)\n"
"return;\n"
"if(fabs(dot(newN, oldN)) < minCos)\n"
"return;\n"
"float3 VxN = cross(newP, oldN);\n"
"float ab[7] = {VxN.x, VxN.y, VxN.z, oldN.x, oldN.y, oldN.z, -dot(oldN, diff)};\n"
"for(int i = 0; i < 7; i++)\n"
"ab7[i] = ab[i];\n"
"}\n"
"__kernel void getAb(__global const char * oldPointsptr,\n"
"int oldPoints_step, int oldPoints_offset,\n"
"__global const char * oldNormalsptr,\n"
"int oldNormals_step, int oldNormals_offset,\n"
"const int2 oldSize,\n"
"__global const char * newPointsptr,\n"
"int newPoints_step, int newPoints_offset,\n"
"__global const char * newNormalsptr,\n"
"int newNormals_step, int newNormals_offset,\n"
"const int2 newSize,\n"
"const float16 poseMatrix,\n"
"const float2 fxy,\n"
"const float2 cxy,\n"
"const float sqDistanceThresh,\n"
"const float minCos,\n"
"__local float * reducebuf,\n"
"__global char* groupedSumptr,\n"
"int groupedSum_step, int groupedSum_offset\n"
")\n"
"{\n"
"const int x = get_global_id(0);\n"
"const int y = get_global_id(1);\n"
"if(x >= newSize.x || y >= newSize.y)\n"
"return;\n"
"const int gx = get_group_id(0);\n"
"const int gy = get_group_id(1);\n"
"const int gw = get_num_groups(0);\n"
"const int gh = get_num_groups(1);\n"
"const int lx = get_local_id(0);\n"
"const int ly = get_local_id(1);\n"
"const int lw = get_local_size(0);\n"
"const int lh = get_local_size(1);\n"
"const int lsz = lw*lh;\n"
"const int lid = lx + ly*lw;\n"
"float ab[7];\n"
"for(int i = 0; i < 7; i++)\n"
"ab[i] = 0;\n"
"calcAb7(oldPointsptr,\n"
"oldPoints_step, oldPoints_offset,\n"
"oldNormalsptr,\n"
"oldNormals_step, oldNormals_offset,\n"
"oldSize,\n"
"newPointsptr,\n"
"newPoints_step, newPoints_offset,\n"
"newNormalsptr,\n"
"newNormals_step, newNormals_offset,\n"
"newSize,\n"
"poseMatrix,\n"
"fxy, cxy,\n"
"sqDistanceThresh,\n"
"minCos,\n"
"ab);\n"
"__local float* upperTriangle = reducebuf + lid*UTSIZE;\n"
"int pos = 0;\n"
"for(int i = 0; i < 6; i++)\n"
"{\n"
"for(int j = i; j < 7; j++)\n"
"{\n"
"upperTriangle[pos++] = ab[i]*ab[j];\n"
"}\n"
"}\n"
"const int c = clz(lsz & -lsz);\n"
"const int maxStep = c ? 31 - c : c;\n"
"for(int nstep = 1; nstep <= maxStep; nstep++)\n"
"{\n"
"if(lid % (1 << nstep) == 0)\n"
"{\n"
"__local float* rto   = reducebuf + UTSIZE*lid;\n"
"__local float* rfrom = reducebuf + UTSIZE*(lid+(1 << (nstep-1)));\n"
"for(int i = 0; i < UTSIZE; i++)\n"
"rto[i] += rfrom[i];\n"
"}\n"
"barrier(CLK_LOCAL_MEM_FENCE);\n"
"}\n"
"if(lid == 0)\n"
"{\n"
"__global float* groupedRow = (__global float*)(groupedSumptr +\n"
"groupedSum_offset +\n"
"gy*groupedSum_step);\n"
"for(int i = 0; i < UTSIZE; i++)\n"
"groupedRow[gx*UTSIZE + i] = reducebuf[i];\n"
"}\n"
"}\n"
, "db7fd764b651e87bd2c879fbef42e648", NULL};
struct cv::ocl::internal::ProgramEntry kinfu_frame_oclsrc={moduleName, "kinfu_frame",
"inline float3 reproject(float3 p, float2 fxyinv, float2 cxy)\n"
"{\n"
"float2 pp = p.z*(p.xy - cxy)*fxyinv;\n"
"return (float3)(pp, p.z);\n"
"}\n"
"typedef float4 ptype;\n"
"__kernel void computePointsNormals(__global char * pointsptr,\n"
"int points_step, int points_offset,\n"
"__global char * normalsptr,\n"
"int normals_step, int normals_offset,\n"
"__global const char * depthptr,\n"
"int depth_step, int depth_offset,\n"
"int depth_rows, int depth_cols,\n"
"const float2 fxyinv,\n"
"const float2 cxy,\n"
"const float dfac\n"
")\n"
"{\n"
"int x = get_global_id(0);\n"
"int y = get_global_id(1);\n"
"if(x >= depth_cols || y >= depth_rows)\n"
"return;\n"
"__global const float* row0 = (__global const float*)(depthptr + depth_offset +\n"
"(y+0)*depth_step);\n"
"__global const float* row1 = (__global const float*)(depthptr + depth_offset +\n"
"(y+1)*depth_step);\n"
"float d00 = row0[x];\n"
"float z00 = d00*dfac;\n"
"float3 p00 = (float3)(convert_float2((int2)(x, y)), z00);\n"
"float3 v00 = reproject(p00, fxyinv, cxy);\n"
"float3 p = nan((uint)0), n = nan((uint)0);\n"
"if(x < depth_cols - 1 && y < depth_rows - 1)\n"
"{\n"
"float d01 = row0[x+1];\n"
"float d10 = row1[x];\n"
"float z01 = d01*dfac;\n"
"float z10 = d10*dfac;\n"
"if(z00 != 0 && z01 != 0 && z10 != 0)\n"
"{\n"
"float3 p01 = (float3)(convert_float2((int2)(x+1, y+0)), z01);\n"
"float3 p10 = (float3)(convert_float2((int2)(x+0, y+1)), z10);\n"
"float3 v01 = reproject(p01, fxyinv, cxy);\n"
"float3 v10 = reproject(p10, fxyinv, cxy);\n"
"float3 vec = cross(v01 - v00, v10 - v00);\n"
"n = - normalize(vec);\n"
"p = v00;\n"
"}\n"
"}\n"
"__global float* pts = (__global float*)(pointsptr  +  points_offset + y*points_step  + x*sizeof(ptype));\n"
"__global float* nrm = (__global float*)(normalsptr + normals_offset + y*normals_step + x*sizeof(ptype));\n"
"vstore4((ptype)(p, 0), 0, pts);\n"
"vstore4((ptype)(n, 0), 0, nrm);\n"
"}\n"
"__kernel void pyrDownBilateral(__global const char * depthptr,\n"
"int depth_step, int depth_offset,\n"
"int depth_rows, int depth_cols,\n"
"__global char * depthDownptr,\n"
"int depthDown_step, int depthDown_offset,\n"
"int depthDown_rows, int depthDown_cols,\n"
"const float sigma\n"
")\n"
"{\n"
"int x = get_global_id(0);\n"
"int y = get_global_id(1);\n"
"if(x >= depthDown_cols || y >= depthDown_rows)\n"
"return;\n"
"const float sigma3 = sigma*3;\n"
"const int D = 5;\n"
"__global const float* srcCenterRow = (__global const float*)(depthptr + depth_offset +\n"
"(2*y)*depth_step);\n"
"float center = srcCenterRow[2*x];\n"
"int sx = max(0, 2*x - D/2), ex = min(2*x - D/2 + D, depth_cols-1);\n"
"int sy = max(0, 2*y - D/2), ey = min(2*y - D/2 + D, depth_rows-1);\n"
"float sum = 0;\n"
"int count = 0;\n"
"for(int iy = sy; iy < ey; iy++)\n"
"{\n"
"__global const float* srcRow = (__global const float*)(depthptr + depth_offset +\n"
"(iy)*depth_step);\n"
"for(int ix = sx; ix < ex; ix++)\n"
"{\n"
"float val = srcRow[ix];\n"
"if(fabs(val - center) < sigma3)\n"
"{\n"
"sum += val; count++;\n"
"}\n"
"}\n"
"}\n"
"__global float* downRow = (__global float*)(depthDownptr + depthDown_offset +\n"
"y*depthDown_step + x*sizeof(float));\n"
"*downRow = (count == 0) ? 0 : sum/convert_float(count);\n"
"}\n"
"__kernel void customBilateral(__global const char * srcptr,\n"
"int src_step, int src_offset,\n"
"__global char * dstptr,\n"
"int dst_step, int dst_offset,\n"
"const int2 frameSize,\n"
"const int kernelSize,\n"
"const float sigma_spatial2_inv_half,\n"
"const float sigma_depth2_inv_half\n"
")\n"
"{\n"
"int x = get_global_id(0);\n"
"int y = get_global_id(1);\n"
"if(x >= frameSize.x || y >= frameSize.y)\n"
"return;\n"
"__global const float* srcCenterRow = (__global const float*)(srcptr + src_offset +\n"
"y*src_step);\n"
"float value = srcCenterRow[x];\n"
"int tx = min (x - kernelSize / 2 + kernelSize, frameSize.x - 1);\n"
"int ty = min (y - kernelSize / 2 + kernelSize, frameSize.y - 1);\n"
"float sum1 = 0;\n"
"float sum2 = 0;\n"
"for (int cy = max (y - kernelSize / 2, 0); cy < ty; ++cy)\n"
"{\n"
"__global const float* srcRow = (__global const float*)(srcptr + src_offset +\n"
"cy*src_step);\n"
"for (int cx = max (x - kernelSize / 2, 0); cx < tx; ++cx)\n"
"{\n"
"float depth = srcRow[cx];\n"
"float space2 = convert_float((x - cx) * (x - cx) + (y - cy) * (y - cy));\n"
"float color2 = (value - depth) * (value - depth);\n"
"float weight = native_exp (-(space2 * sigma_spatial2_inv_half +\n"
"color2 * sigma_depth2_inv_half));\n"
"sum1 += depth * weight;\n"
"sum2 += weight;\n"
"}\n"
"}\n"
"__global float* dst = (__global float*)(dstptr + dst_offset +\n"
"y*dst_step + x*sizeof(float));\n"
"*dst = sum1/sum2;\n"
"}\n"
"__kernel void pyrDownPointsNormals(__global const char * pptr,\n"
"int p_step, int p_offset,\n"
"__global const char * nptr,\n"
"int n_step, int n_offset,\n"
"__global char * pdownptr,\n"
"int pdown_step, int pdown_offset,\n"
"__global char * ndownptr,\n"
"int ndown_step, int ndown_offset,\n"
"const int2 downSize\n"
")\n"
"{\n"
"int x = get_global_id(0);\n"
"int y = get_global_id(1);\n"
"if(x >= downSize.x || y >= downSize.y)\n"
"return;\n"
"float3 point = nan((uint)0), normal = nan((uint)0);\n"
"__global const ptype* pUpRow0 = (__global const ptype*)(pptr + p_offset + (2*y  )*p_step);\n"
"__global const ptype* pUpRow1 = (__global const ptype*)(pptr + p_offset + (2*y+1)*p_step);\n"
"float3 d00 = pUpRow0[2*x  ].xyz;\n"
"float3 d01 = pUpRow0[2*x+1].xyz;\n"
"float3 d10 = pUpRow1[2*x  ].xyz;\n"
"float3 d11 = pUpRow1[2*x+1].xyz;\n"
"if(!(any(isnan(d00)) || any(isnan(d01)) ||\n"
"any(isnan(d10)) || any(isnan(d11))))\n"
"{\n"
"point = (d00 + d01 + d10 + d11)*0.25f;\n"
"__global const ptype* nUpRow0 = (__global const ptype*)(nptr + n_offset + (2*y  )*n_step);\n"
"__global const ptype* nUpRow1 = (__global const ptype*)(nptr + n_offset + (2*y+1)*n_step);\n"
"float3 n00 = nUpRow0[2*x  ].xyz;\n"
"float3 n01 = nUpRow0[2*x+1].xyz;\n"
"float3 n10 = nUpRow1[2*x  ].xyz;\n"
"float3 n11 = nUpRow1[2*x+1].xyz;\n"
"normal = (n00 + n01 + n10 + n11)*0.25f;\n"
"}\n"
"__global ptype* pts = (__global ptype*)(pdownptr + pdown_offset + y*pdown_step);\n"
"__global ptype* nrm = (__global ptype*)(ndownptr + ndown_offset + y*ndown_step);\n"
"pts[x] = (ptype)(point,  0);\n"
"nrm[x] = (ptype)(normal, 0);\n"
"}\n"
"typedef char4 pixelType;\n"
"float specPow20(float x)\n"
"{\n"
"float x2 = x*x;\n"
"float x5 = x2*x2*x;\n"
"float x10 = x5*x5;\n"
"float x20 = x10*x10;\n"
"return x20;\n"
"}\n"
"__kernel void render(__global const char * pointsptr,\n"
"int points_step, int points_offset,\n"
"__global const char * normalsptr,\n"
"int normals_step, int normals_offset,\n"
"__global char * imgptr,\n"
"int img_step, int img_offset,\n"
"const int2 frameSize,\n"
"const float4 lightPt\n"
")\n"
"{\n"
"int x = get_global_id(0);\n"
"int y = get_global_id(1);\n"
"if(x >= frameSize.x || y >= frameSize.y)\n"
"return;\n"
"__global const ptype* ptsRow = (__global const ptype*)(pointsptr  + points_offset  + y*points_step  + x*sizeof(ptype));\n"
"__global const ptype* nrmRow = (__global const ptype*)(normalsptr + normals_offset + y*normals_step + x*sizeof(ptype));\n"
"float3 p = (*ptsRow).xyz;\n"
"float3 n = (*nrmRow).xyz;\n"
"pixelType color;\n"
"if(any(isnan(p)))\n"
"{\n"
"color = (pixelType)(0, 32, 0, 0);\n"
"}\n"
"else\n"
"{\n"
"const float Ka = 0.3f;\n"
"const float Kd = 0.5f;\n"
"const float Ks = 0.2f;\n"
"const float Ax = 1.f;\n"
"const float Dx = 1.f;\n"
"const float Sx = 1.f;\n"
"const float Lx = 1.f;\n"
"float3 l = normalize(lightPt.xyz - p);\n"
"float3 v = normalize(-p);\n"
"float3 r = normalize(2.f*n*dot(n, l) - l);\n"
"float val = (Ax*Ka*Dx + Lx*Kd*Dx*max(0.f, dot(n, l)) +\n"
"Lx*Ks*Sx*specPow20(max(0.f, dot(r, v))));\n"
"uchar ix = convert_uchar(val*255.f);\n"
"color = (pixelType)(ix, ix, ix, 0);\n"
"}\n"
"__global char* imgRow = (__global char*)(imgptr + img_offset + y*img_step + x*sizeof(pixelType));\n"
"vstore4(color, 0, imgRow);\n"
"}\n"
, "27f66c4eaad4d1a82ef9ef08a6803a08", NULL};
struct cv::ocl::internal::ProgramEntry tsdf_oclsrc={moduleName, "tsdf",
"typedef char int8_t;\n"
"typedef int8_t TsdfType;\n"
"typedef uchar WeightType;\n"
"struct TsdfVoxel\n"
"{\n"
"TsdfType tsdf;\n"
"WeightType weight;\n"
"};\n"
"static inline TsdfType floatToTsdf(float num)\n"
"{\n"
"int8_t res = (int8_t) ( (num * (-128)) );\n"
"res = res ? res : (num < 0 ? 1 : -1);\n"
"return res;\n"
"}\n"
"static inline float tsdfToFloat(TsdfType num)\n"
"{\n"
"return ( (float) num ) / (-128);\n"
"}\n"
"__kernel void integrate(__global const char * depthptr,\n"
"int depth_step, int depth_offset,\n"
"int depth_rows, int depth_cols,\n"
"__global struct TsdfVoxel * volumeptr,\n"
"const float16 vol2camMatrix,\n"
"const float voxelSize,\n"
"const int4 volResolution4,\n"
"const int4 volDims4,\n"
"const float2 fxy,\n"
"const float2 cxy,\n"
"const float dfac,\n"
"const float truncDist,\n"
"const int maxWeight,\n"
"const __global float * pixNorms)\n"
"{\n"
"int x = get_global_id(0);\n"
"int y = get_global_id(1);\n"
"const int3 volResolution = volResolution4.xyz;\n"
"if(x >= volResolution.x || y >= volResolution.y)\n"
"return;\n"
"const int3 volDims = volDims4.xyz;\n"
"const float2 limits = (float2)(depth_cols-1, depth_rows-1);\n"
"const float4 vol2cam0 = vol2camMatrix.s0123;\n"
"const float4 vol2cam1 = vol2camMatrix.s4567;\n"
"const float4 vol2cam2 = vol2camMatrix.s89ab;\n"
"const float truncDistInv = 1.f/truncDist;\n"
"float4 inPt = (float4)(x*voxelSize, y*voxelSize, 0, 1);\n"
"float3 basePt = (float3)(dot(vol2cam0, inPt),\n"
"dot(vol2cam1, inPt),\n"
"dot(vol2cam2, inPt));\n"
"float3 camSpacePt = basePt;\n"
"float3 zStep = ((float3)(vol2cam0.z, vol2cam1.z, vol2cam2.z))*voxelSize;\n"
"int volYidx = x*volDims.x + y*volDims.y;\n"
"int startZ, endZ;\n"
"if(fabs(zStep.z) > 1e-5f)\n"
"{\n"
"int baseZ = convert_int(-basePt.z / zStep.z);\n"
"if(zStep.z > 0)\n"
"{\n"
"startZ = baseZ;\n"
"endZ = volResolution.z;\n"
"}\n"
"else\n"
"{\n"
"startZ = 0;\n"
"endZ = baseZ;\n"
"}\n"
"}\n"
"else\n"
"{\n"
"if(basePt.z > 0)\n"
"{\n"
"startZ = 0; endZ = volResolution.z;\n"
"}\n"
"else\n"
"{\n"
"return;\n"
"}\n"
"}\n"
"startZ = max(0, startZ);\n"
"endZ = min(volResolution.z, endZ);\n"
"for(int z = startZ; z < endZ; z++)\n"
"{\n"
"camSpacePt += zStep;\n"
"if(camSpacePt.z <= 0)\n"
"continue;\n"
"float3 camPixVec = camSpacePt / camSpacePt.z;\n"
"float2 projected = mad(camPixVec.xy, fxy, cxy);\n"
"float v;\n"
"if(all(projected >= 0) && all(projected < limits))\n"
"{\n"
"float2 ip = floor(projected);\n"
"int xi = ip.x, yi = ip.y;\n"
"__global const float* row0 = (__global const float*)(depthptr + depth_offset +\n"
"(yi+0)*depth_step);\n"
"__global const float* row1 = (__global const float*)(depthptr + depth_offset +\n"
"(yi+1)*depth_step);\n"
"float v00 = row0[xi+0];\n"
"float v01 = row0[xi+1];\n"
"float v10 = row1[xi+0];\n"
"float v11 = row1[xi+1];\n"
"float4 vv = (float4)(v00, v01, v10, v11);\n"
"if(all(vv > 0))\n"
"{\n"
"float2 t = projected - ip;\n"
"float2 vf = mix(vv.xz, vv.yw, t.x);\n"
"v = mix(vf.s0, vf.s1, t.y);\n"
"}\n"
"else\n"
"continue;\n"
"}\n"
"else\n"
"continue;\n"
"if(v == 0)\n"
"continue;\n"
"int idx = projected.y * depth_cols + projected.x;\n"
"float pixNorm = pixNorms[idx];\n"
"float sdf = pixNorm*(v*dfac - camSpacePt.z);\n"
"if(sdf >= -truncDist)\n"
"{\n"
"float tsdf = fmin(1.0f, sdf * truncDistInv);\n"
"int volIdx = volYidx + z*volDims.z;\n"
"struct TsdfVoxel voxel = volumeptr[volIdx];\n"
"float value  = tsdfToFloat(voxel.tsdf);\n"
"int weight = voxel.weight;\n"
"value = (value*weight + tsdf) / (weight + 1);\n"
"weight = min(weight + 1, maxWeight);\n"
"voxel.tsdf = floatToTsdf(value);\n"
"voxel.weight = weight;\n"
"volumeptr[volIdx] = voxel;\n"
"}\n"
"}\n"
"}\n"
"inline float interpolateVoxel(float3 p, __global const struct TsdfVoxel* volumePtr,\n"
"int3 volDims, int8 neighbourCoords)\n"
"{\n"
"float3 fip = floor(p);\n"
"int3 ip = convert_int3(fip);\n"
"float3 t = p - fip;\n"
"int3 cmul = volDims*ip;\n"
"int coordBase = cmul.x + cmul.y + cmul.z;\n"
"int nco[8];\n"
"vstore8(neighbourCoords + coordBase, 0, nco);\n"
"float vaz[8];\n"
"for(int i = 0; i < 8; i++)\n"
"vaz[i] = tsdfToFloat(volumePtr[nco[i]].tsdf);\n"
"float8 vz = vload8(0, vaz);\n"
"float4 vy = mix(vz.s0246, vz.s1357, t.z);\n"
"float2 vx = mix(vy.s02, vy.s13, t.y);\n"
"return mix(vx.s0, vx.s1, t.x);\n"
"}\n"
"inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* volumePtr,\n"
"int3 volResolution, int3 volDims, int8 neighbourCoords)\n"
"{\n"
"if(any(p < 1) || any(p >= convert_float3(volResolution - 2)))\n"
"return nan((uint)0);\n"
"float3 fip = floor(p);\n"
"int3 ip = convert_int3(fip);\n"
"float3 t = p - fip;\n"
"int3 cmul = volDims*ip;\n"
"int coordBase = cmul.x + cmul.y + cmul.z;\n"
"int nco[8];\n"
"vstore8(neighbourCoords + coordBase, 0, nco);\n"
"int arDims[3];\n"
"vstore3(volDims, 0, arDims);\n"
"float an[3];\n"
"for(int c = 0; c < 3; c++)\n"
"{\n"
"int dim = arDims[c];\n"
"float vaz[8];\n"
"for(int i = 0; i < 8; i++)\n"
"vaz[i] = tsdfToFloat(volumePtr[nco[i] + dim].tsdf) -\n"
"tsdfToFloat(volumePtr[nco[i] - dim].tsdf);\n"
"float8 vz = vload8(0, vaz);\n"
"float4 vy = mix(vz.s0246, vz.s1357, t.z);\n"
"float2 vx = mix(vy.s02, vy.s13, t.y);\n"
"an[c] = mix(vx.s0, vx.s1, t.x);\n"
"}\n"
"float3 n = vload3(0, an);\n"
"float Norm = sqrt(n.x*n.x + n.y*n.y + n.z*n.z);\n"
"return Norm < 0.0001f ? nan((uint)0) : n / Norm;\n"
"}\n"
"typedef float4 ptype;\n"
"__kernel void raycast(__global char * pointsptr,\n"
"int points_step, int points_offset,\n"
"__global char * normalsptr,\n"
"int normals_step, int normals_offset,\n"
"const int2 frameSize,\n"
"__global const struct TsdfVoxel * volumeptr,\n"
"__global const float * vol2camptr,\n"
"__global const float * cam2volptr,\n"
"const float2 fixy,\n"
"const float2 cxy,\n"
"const float4 boxDown4,\n"
"const float4 boxUp4,\n"
"const float tstep,\n"
"const float voxelSize,\n"
"const int4 volResolution4,\n"
"const int4 volDims4,\n"
"const int8 neighbourCoords\n"
")\n"
"{\n"
"int x = get_global_id(0);\n"
"int y = get_global_id(1);\n"
"if(x >= frameSize.x || y >= frameSize.y)\n"
"return;\n"
"__global const float* cm = cam2volptr;\n"
"const float3 camRot0  = vload4(0, cm).xyz;\n"
"const float3 camRot1  = vload4(1, cm).xyz;\n"
"const float3 camRot2  = vload4(2, cm).xyz;\n"
"const float3 camTrans = (float3)(cm[3], cm[7], cm[11]);\n"
"__global const float* vm = vol2camptr;\n"
"const float3 volRot0  = vload4(0, vm).xyz;\n"
"const float3 volRot1  = vload4(1, vm).xyz;\n"
"const float3 volRot2  = vload4(2, vm).xyz;\n"
"const float3 volTrans = (float3)(vm[3], vm[7], vm[11]);\n"
"const float3 boxDown = boxDown4.xyz;\n"
"const float3 boxUp   = boxUp4.xyz;\n"
"const int3   volDims = volDims4.xyz;\n"
"const int3 volResolution = volResolution4.xyz;\n"
"const float invVoxelSize = native_recip(voxelSize);\n"
"float3 point  = nan((uint)0);\n"
"float3 normal = nan((uint)0);\n"
"float3 orig = camTrans;\n"
"float3 planed = (float3)(((float2)(x, y) - cxy)*fixy, 1.f);\n"
"planed = (float3)(dot(planed, camRot0),\n"
"dot(planed, camRot1),\n"
"dot(planed, camRot2));\n"
"float3 dir = fast_normalize(planed);\n"
"float3 rayinv = native_recip(dir);\n"
"float3 tbottom = rayinv*(boxDown - orig);\n"
"float3 ttop    = rayinv*(boxUp   - orig);\n"
"float3 minAx = min(ttop, tbottom);\n"
"float3 maxAx = max(ttop, tbottom);\n"
"const float clip = 0.f;\n"
"float tmin = max(max(max(minAx.x, minAx.y), max(minAx.x, minAx.z)), clip);\n"
"float tmax =     min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z));\n"
"tmin = tmin + tstep;\n"
"tmax = tmax - tstep;\n"
"if(tmin < tmax)\n"
"{\n"
"orig *= invVoxelSize;\n"
"dir  *= invVoxelSize;\n"
"float3 rayStep = dir*tstep;\n"
"float3 next = (orig + dir*tmin);\n"
"float f = interpolateVoxel(next, volumeptr, volDims, neighbourCoords);\n"
"float fnext = f;\n"
"int steps = 0;\n"
"int nSteps = floor(native_divide(tmax - tmin, tstep));\n"
"bool stop = false;\n"
"for(int i = 0; i < nSteps; i++)\n"
"{\n"
"if(!stop)\n"
"{\n"
"next += rayStep;\n"
"int3 ip = convert_int3(round(next));\n"
"int3 cmul = ip*volDims;\n"
"int idx = cmul.x + cmul.y + cmul.z;\n"
"fnext = tsdfToFloat(volumeptr[idx].tsdf);\n"
"if(fnext != f)\n"
"{\n"
"fnext = interpolateVoxel(next, volumeptr, volDims, neighbourCoords);\n"
"if(signbit(f) != signbit(fnext))\n"
"{\n"
"stop = true; continue;\n"
"}\n"
"f = fnext;\n"
"}\n"
"steps++;\n"
"}\n"
"}\n"
"if(f > 0 && fnext < 0)\n"
"{\n"
"float3 tp = next - rayStep;\n"
"float ft   = interpolateVoxel(tp,   volumeptr, volDims, neighbourCoords);\n"
"float ftdt = interpolateVoxel(next, volumeptr, volDims, neighbourCoords);\n"
"float ts = tmin + tstep*(steps - native_divide(ft, ftdt - ft));\n"
"if(!isnan(ts) && !isinf(ts))\n"
"{\n"
"float3 pv = orig + dir*ts;\n"
"float3 nv = getNormalVoxel(pv, volumeptr, volResolution, volDims, neighbourCoords);\n"
"if(!any(isnan(nv)))\n"
"{\n"
"normal = (float3)(dot(nv, volRot0),\n"
"dot(nv, volRot1),\n"
"dot(nv, volRot2));\n"
"pv *= voxelSize;\n"
"point = (float3)(dot(pv, volRot0),\n"
"dot(pv, volRot1),\n"
"dot(pv, volRot2)) + volTrans;\n"
"}\n"
"}\n"
"}\n"
"}\n"
"__global float* pts = (__global float*)(pointsptr  +  points_offset + y*points_step  + x*sizeof(ptype));\n"
"__global float* nrm = (__global float*)(normalsptr + normals_offset + y*normals_step + x*sizeof(ptype));\n"
"vstore4((float4)(point,  0), 0, pts);\n"
"vstore4((float4)(normal, 0), 0, nrm);\n"
"}\n"
"__kernel void getNormals(__global const char * pointsptr,\n"
"int points_step, int points_offset,\n"
"__global char * normalsptr,\n"
"int normals_step, int normals_offset,\n"
"const int2 frameSize,\n"
"__global const struct TsdfVoxel* volumeptr,\n"
"__global const float * volPoseptr,\n"
"__global const float * invPoseptr,\n"
"const float voxelSizeInv,\n"
"const int4 volResolution4,\n"
"const int4 volDims4,\n"
"const int8 neighbourCoords\n"
")\n"
"{\n"
"int x = get_global_id(0);\n"
"int y = get_global_id(1);\n"
"if(x >= frameSize.x || y >= frameSize.y)\n"
"return;\n"
"__global const float* vp = volPoseptr;\n"
"const float3 volRot0  = vload4(0, vp).xyz;\n"
"const float3 volRot1  = vload4(1, vp).xyz;\n"
"const float3 volRot2  = vload4(2, vp).xyz;\n"
"const float3 volTrans = (float3)(vp[3], vp[7], vp[11]);\n"
"__global const float* iv = invPoseptr;\n"
"const float3 invRot0 = vload4(0, iv).xyz;\n"
"const float3 invRot1 = vload4(1, iv).xyz;\n"
"const float3 invRot2 = vload4(2, iv).xyz;\n"
"const float3 invTrans = (float3)(iv[3], iv[7], iv[11]);\n"
"const int3 volResolution = volResolution4.xyz;\n"
"const int3 volDims = volDims4.xyz;\n"
"__global const ptype* ptsRow = (__global const ptype*)(pointsptr +\n"
"points_offset +\n"
"y*points_step);\n"
"float3 p = ptsRow[x].xyz;\n"
"float3 n = nan((uint)0);\n"
"if(!any(isnan(p)))\n"
"{\n"
"float3 voxPt = (float3)(dot(p, invRot0),\n"
"dot(p, invRot1),\n"
"dot(p, invRot2)) + invTrans;\n"
"voxPt = voxPt * voxelSizeInv;\n"
"n = getNormalVoxel(voxPt, volumeptr, volResolution, volDims, neighbourCoords);\n"
"n = (float3)(dot(n, volRot0),\n"
"dot(n, volRot1),\n"
"dot(n, volRot2));\n"
"}\n"
"__global float* nrm = (__global float*)(normalsptr +\n"
"normals_offset +\n"
"y*normals_step +\n"
"x*sizeof(ptype));\n"
"vstore4((float4)(n, 0), 0, nrm);\n"
"}\n"
"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics:enable\n"
"struct CoordReturn\n"
"{\n"
"bool result;\n"
"float3 point;\n"
"float3 normal;\n"
"};\n"
"inline struct CoordReturn coord(int x, int y, int z, float3 V, float v0, int axis,\n"
"__global const struct TsdfVoxel* volumeptr,\n"
"int3 volResolution, int3 volDims,\n"
"int8 neighbourCoords,\n"
"float voxelSize, float voxelSizeInv,\n"
"const float3 volRot0,\n"
"const float3 volRot1,\n"
"const float3 volRot2,\n"
"const float3 volTrans,\n"
"bool needNormals,\n"
"bool scan\n"
")\n"
"{\n"
"struct CoordReturn cr;\n"
"bool limits = false;\n"
"int3 shift;\n"
"float Vc = 0.f;\n"
"if(axis == 0)\n"
"{\n"
"shift = (int3)(1, 0, 0);\n"
"limits = (x + 1 < volResolution.x);\n"
"Vc = V.x;\n"
"}\n"
"if(axis == 1)\n"
"{\n"
"shift = (int3)(0, 1, 0);\n"
"limits = (y + 1 < volResolution.y);\n"
"Vc = V.y;\n"
"}\n"
"if(axis == 2)\n"
"{\n"
"shift = (int3)(0, 0, 1);\n"
"limits = (z + 1 < volResolution.z);\n"
"Vc = V.z;\n"
"}\n"
"if(limits)\n"
"{\n"
"int3 ip = ((int3)(x, y, z)) + shift;\n"
"int3 cmul = ip*volDims;\n"
"int idx = cmul.x + cmul.y + cmul.z;\n"
"struct TsdfVoxel voxel = volumeptr[idx];\n"
"float vd  = tsdfToFloat(voxel.tsdf);\n"
"int weight = voxel.weight;\n"
"if(weight != 0 && vd != 1.f)\n"
"{\n"
"if((v0 > 0 && vd < 0) || (v0 < 0 && vd > 0))\n"
"{\n"
"if(!scan)\n"
"{\n"
"float Vn = Vc + voxelSize;\n"
"float dinv = 1.f/(fabs(v0)+fabs(vd));\n"
"float inter = (Vc*fabs(vd) + Vn*fabs(v0))*dinv;\n"
"float3 p = (float3)(shift.x ? inter : V.x,\n"
"shift.y ? inter : V.y,\n"
"shift.z ? inter : V.z);\n"
"cr.point = (float3)(dot(p, volRot0),\n"
"dot(p, volRot1),\n"
"dot(p, volRot2)) + volTrans;\n"
"if(needNormals)\n"
"{\n"
"float3 nv = getNormalVoxel(p * voxelSizeInv,\n"
"volumeptr, volResolution, volDims, neighbourCoords);\n"
"cr.normal = (float3)(dot(nv, volRot0),\n"
"dot(nv, volRot1),\n"
"dot(nv, volRot2));\n"
"}\n"
"}\n"
"cr.result = true;\n"
"return cr;\n"
"}\n"
"}\n"
"}\n"
"cr.result = false;\n"
"return cr;\n"
"}\n"
"__kernel void scanSize(__global const struct TsdfVoxel* volumeptr,\n"
"const int4 volResolution4,\n"
"const int4 volDims4,\n"
"const int8 neighbourCoords,\n"
"__global const float * volPoseptr,\n"
"const float voxelSize,\n"
"const float voxelSizeInv,\n"
"__local int* reducebuf,\n"
"__global char* groupedSumptr,\n"
"int groupedSum_slicestep,\n"
"int groupedSum_step, int groupedSum_offset\n"
")\n"
"{\n"
"const int3 volDims = volDims4.xyz;\n"
"const int3 volResolution = volResolution4.xyz;\n"
"int x = get_global_id(0);\n"
"int y = get_global_id(1);\n"
"int z = get_global_id(2);\n"
"bool validVoxel = true;\n"
"if(x >= volResolution.x || y >= volResolution.y || z >= volResolution.z)\n"
"validVoxel = false;\n"
"const int gx = get_group_id(0);\n"
"const int gy = get_group_id(1);\n"
"const int gz = get_group_id(2);\n"
"const int lx = get_local_id(0);\n"
"const int ly = get_local_id(1);\n"
"const int lz = get_local_id(2);\n"
"const int lw = get_local_size(0);\n"
"const int lh = get_local_size(1);\n"
"const int ld = get_local_size(2);\n"
"const int lsz = lw*lh*ld;\n"
"const int lid = lx + ly*lw + lz*lw*lh;\n"
"__global const float* vp = volPoseptr;\n"
"const float3 volRot0  = vload4(0, vp).xyz;\n"
"const float3 volRot1  = vload4(1, vp).xyz;\n"
"const float3 volRot2  = vload4(2, vp).xyz;\n"
"const float3 volTrans = (float3)(vp[3], vp[7], vp[11]);\n"
"int npts = 0;\n"
"if(validVoxel)\n"
"{\n"
"int3 ip = (int3)(x, y, z);\n"
"int3 cmul = ip*volDims;\n"
"int idx = cmul.x + cmul.y + cmul.z;\n"
"struct TsdfVoxel voxel = volumeptr[idx];\n"
"float value  = tsdfToFloat(voxel.tsdf);\n"
"int weight = voxel.weight;\n"
"if(weight != 0 && value != 1.f)\n"
"{\n"
"float3 V = (((float3)(x, y, z)) + 0.5f)*voxelSize;\n"
"#pragma unroll\n"
"for(int i = 0; i < 3; i++)\n"
"{\n"
"struct CoordReturn cr;\n"
"cr = coord(x, y, z, V, value, i,\n"
"volumeptr, volResolution, volDims,\n"
"neighbourCoords,\n"
"voxelSize, voxelSizeInv,\n"
"volRot0, volRot1, volRot2, volTrans,\n"
"false, true);\n"
"if(cr.result)\n"
"{\n"
"npts++;\n"
"}\n"
"}\n"
"}\n"
"}\n"
"reducebuf[lid] = npts;\n"
"const int c = clz(lsz & -lsz);\n"
"const int maxStep = c ? 31 - c : c;\n"
"for(int nstep = 1; nstep <= maxStep; nstep++)\n"
"{\n"
"if(lid % (1 << nstep) == 0)\n"
"{\n"
"int rto   = lid;\n"
"int rfrom = lid + (1 << (nstep-1));\n"
"reducebuf[rto] += reducebuf[rfrom];\n"
"}\n"
"barrier(CLK_LOCAL_MEM_FENCE);\n"
"}\n"
"if(lid == 0)\n"
"{\n"
"__global int* groupedRow = (__global int*)(groupedSumptr +\n"
"groupedSum_offset +\n"
"gy*groupedSum_step +\n"
"gz*groupedSum_slicestep);\n"
"groupedRow[gx] = reducebuf[0];\n"
"}\n"
"}\n"
"__kernel void fillPtsNrm(__global const struct TsdfVoxel* volumeptr,\n"
"const int4 volResolution4,\n"
"const int4 volDims4,\n"
"const int8 neighbourCoords,\n"
"__global const float * volPoseptr,\n"
"const float voxelSize,\n"
"const float voxelSizeInv,\n"
"const int needNormals,\n"
"__local float* localbuf,\n"
"volatile __global int* atomicCtr,\n"
"__global const char* groupedSumptr,\n"
"int groupedSum_slicestep,\n"
"int groupedSum_step, int groupedSum_offset,\n"
"__global char * pointsptr,\n"
"int points_step, int points_offset,\n"
"__global char * normalsptr,\n"
"int normals_step, int normals_offset\n"
")\n"
"{\n"
"const int3 volDims = volDims4.xyz;\n"
"const int3 volResolution = volResolution4.xyz;\n"
"int x = get_global_id(0);\n"
"int y = get_global_id(1);\n"
"int z = get_global_id(2);\n"
"bool validVoxel = true;\n"
"if(x >= volResolution.x || y >= volResolution.y || z >= volResolution.z)\n"
"validVoxel = false;\n"
"const int gx = get_group_id(0);\n"
"const int gy = get_group_id(1);\n"
"const int gz = get_group_id(2);\n"
"__global int* groupedRow = (__global int*)(groupedSumptr +\n"
"groupedSum_offset +\n"
"gy*groupedSum_step +\n"
"gz*groupedSum_slicestep);\n"
"int nptsGroup = groupedRow[gx];\n"
"if(nptsGroup == 0)\n"
"return;\n"
"const int lx = get_local_id(0);\n"
"const int ly = get_local_id(1);\n"
"const int lz = get_local_id(2);\n"
"const int lw = get_local_size(0);\n"
"const int lh = get_local_size(1);\n"
"const int ld = get_local_size(2);\n"
"const int lsz = lw*lh*ld;\n"
"const int lid = lx + ly*lw + lz*lw*lh;\n"
"__global const float* vp = volPoseptr;\n"
"const float3 volRot0  = vload4(0, vp).xyz;\n"
"const float3 volRot1  = vload4(1, vp).xyz;\n"
"const float3 volRot2  = vload4(2, vp).xyz;\n"
"const float3 volTrans = (float3)(vp[3], vp[7], vp[11]);\n"
"int npts = 0;\n"
"float3 parr[3], narr[3];\n"
"if(validVoxel)\n"
"{\n"
"int3 ip = (int3)(x, y, z);\n"
"int3 cmul = ip*volDims;\n"
"int idx = cmul.x + cmul.y + cmul.z;\n"
"struct TsdfVoxel voxel = volumeptr[idx];\n"
"float value  = tsdfToFloat(voxel.tsdf);\n"
"int weight = voxel.weight;\n"
"if(weight != 0 && value != 1.f)\n"
"{\n"
"float3 V = (((float3)(x, y, z)) + 0.5f)*voxelSize;\n"
"#pragma unroll\n"
"for(int i = 0; i < 3; i++)\n"
"{\n"
"struct CoordReturn cr;\n"
"cr = coord(x, y, z, V, value, i,\n"
"volumeptr, volResolution, volDims,\n"
"neighbourCoords,\n"
"voxelSize, voxelSizeInv,\n"
"volRot0, volRot1, volRot2, volTrans,\n"
"needNormals, false);\n"
"if(cr.result)\n"
"{\n"
"parr[npts] = cr.point;\n"
"narr[npts] = cr.normal;\n"
"npts++;\n"
"}\n"
"}\n"
"}\n"
"}\n"
"const int elemStep = 4;\n"
"__local float* normAddr;\n"
"__local int localCtr;\n"
"if(lid == 0)\n"
"localCtr = 0;\n"
"int privateCtr = 0;\n"
"barrier(CLK_LOCAL_MEM_FENCE);\n"
"privateCtr = atomic_add(&localCtr, npts);\n"
"barrier(CLK_LOCAL_MEM_FENCE);\n"
"for(int i = 0; i < npts; i++)\n"
"{\n"
"__local float* addr = localbuf + (privateCtr+i)*elemStep;\n"
"vstore4((float4)(parr[i], 0), 0, addr);\n"
"}\n"
"if(needNormals)\n"
"{\n"
"normAddr = localbuf + localCtr*elemStep;\n"
"for(int i = 0; i < npts; i++)\n"
"{\n"
"__local float* addr = normAddr + (privateCtr+i)*elemStep;\n"
"vstore4((float4)(narr[i], 0), 0, addr);\n"
"}\n"
"}\n"
"if(lid == 0)\n"
"{\n"
"if(localCtr != nptsGroup)\n"
"{\n"
"printf(\"!!! fetchPointsNormals result may be incorrect, npts != localCtr at %3d %3d %3d: %3d vs %3d\\n\",\n"
"gx, gy, gz, localCtr, nptsGroup);\n"
"}\n"
"}\n"
"__local int whereToWrite;\n"
"if(lid == 0)\n"
"whereToWrite = atomic_add(atomicCtr, localCtr);\n"
"barrier(CLK_GLOBAL_MEM_FENCE);\n"
"event_t ev[2];\n"
"int evn = 0;\n"
"__global float* pts = (__global float*)(pointsptr +\n"
"points_offset +\n"
"whereToWrite*points_step);\n"
"ev[evn++] = async_work_group_copy(pts, localbuf, localCtr*elemStep, 0);\n"
"if(needNormals)\n"
"{\n"
"__global float* nrm = (__global float*)(normalsptr +\n"
"normals_offset +\n"
"whereToWrite*normals_step);\n"
"ev[evn++] = async_work_group_copy(nrm, normAddr, localCtr*elemStep, 0);\n"
"}\n"
"wait_group_events(evn, ev);\n"
"}\n"
, "5e8f0602bc335b694f2060f3c756c2b7", NULL};
struct cv::ocl::internal::ProgramEntry tsdf_functions_oclsrc={moduleName, "tsdf_functions",
"__kernel void preCalculationPixNorm (__global char * pixNormsPtr,\n"
"int pixNormsStep, int pixNormsOffset,\n"
"int pixNormsRows, int pixNormsCols,\n"
"const __global float * xx,\n"
"const __global float * yy)\n"
"{\n"
"int i = get_global_id(0);\n"
"int j = get_global_id(1);\n"
"if (i < pixNormsRows && j < pixNormsCols)\n"
"{\n"
"*(__global float*)(pixNormsPtr + pixNormsOffset + i*pixNormsStep + j*sizeof(float)) = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f);\n"
"}\n"
"}\n"
, "2c16a37c55c6c1c5edaa4e3bbecc80d8", NULL};

}}}
#endif
