/******************************************************************** ** Image Component Library (ICL) ** ** ** ** Copyright (C) 2006-2012 CITEC, University of Bielefeld ** ** Neuroinformatics Group ** ** Website: www.iclcv.org and ** ** http://opensource.cit-ec.de/projects/icl ** ** ** ** File : ICLGeom/src/PointNormalEstimation.cpp ** ** Module : ICLGeom ** ** Authors: Andre Ueckermann ** ** ** ** ** ** Commercial License ** ** ICL can be used commercially, please refer to our website ** ** www.iclcv.org for more details. ** ** ** ** GNU General Public License Usage ** ** Alternatively, this file may be used under the terms of the ** ** GNU General Public License version 3.0 as published by the ** ** Free Software Foundation and appearing in the file LICENSE.GPL ** ** included in the packaging of this file. Please review the ** ** following information to ensure the GNU General Public License ** ** version 3.0 requirements will be met: ** ** http://www.gnu.org/copyleft/gpl.html. ** ** ** ** The development of this software was supported by the ** ** Excellence Cluster EXC 277 Cognitive Interaction Technology. ** ** The Excellence Cluster EXC 277 is a grant of the Deutsche ** ** Forschungsgemeinschaft (DFG) in the context of the German ** ** Excellence Initiative. ** ** ** *********************************************************************/ #define __CL_ENABLE_EXCEPTIONS //enables openCL error catching #ifdef HAVE_OPENCL #include #endif #include #include #include namespace icl{ namespace geom{ //OpenCL kernel code static char normalEstimationKernel[] = "__kernel void \n" "medianFilter(__global float const * in, __global float * out, int const w, int const h, int const medianFilterSize) \n" "{ \n" " size_t id = get_global_id(0); \n" " int y = (int)floor((float)id/(float)w); \n" " int x = id-y*w; \n" " if(y<(medianFilterSize-1)/2 || y>=h-(medianFilterSize-1)/2 || x<(medianFilterSize-1)/2 || x>=w-(medianFilterSize-1)/2) \n" " { \n" " out[id]=in[id]; \n" " } \n" " else \n" " { \n" " float mask[81]; \n" " for(int sy=-(medianFilterSize-1)/2; sy<=(medianFilterSize-1)/2; sy++){ \n" " for(int sx=-(medianFilterSize-1)/2; sx<=(medianFilterSize-1)/2; sx++){ \n" " mask[(sx+(medianFilterSize-1)/2)+(sy+(medianFilterSize-1)/2)*medianFilterSize]=in[(x+sx)+w*(y+sy)]; \n" " } \n" " } \n" " //bubble-sort: slow O(n^2), but easy to implement without recursion or complex data structurs \n" " bool sw=true; \n" " while(sw==true){ \n" " sw=false; \n" " for(int i=0; i=h-normalRange || x=w-normalRange) \n" " { \n" " out[id]=(0,0,0,0); \n" " } \n" " else \n" " { \n" " float4 fa; \n" " fa.x=(x+normalRange)-(x-normalRange); \n" " fa.y=(y-normalRange)-(y-normalRange); \n" " fa.z=in[(x+normalRange)+w*(y-normalRange)]-in[(x-normalRange)+w*(y-normalRange)]; \n" " fa.w=1; \n" " float4 fb; \n" " fb.x=(x)-(x-normalRange); \n" " fb.y=(y+normalRange)-(y-normalRange); \n" " fb.z=in[(x)+w*(y+normalRange)]-in[(x-normalRange)+w*(y-normalRange)]; \n" " fb.w=1; \n" " float4 n1=cross(fa,fb); \n" " out[id]=fast_normalize(n1); \n" " } \n" "} \n" "__kernel void \n" "normalAveraging(__global float4 const * in, __global float4 * out, int const w, int const h, int const normalAveragingRange)\n" "{ \n" " size_t id = get_global_id(0); \n" " int y = (int)floor((float)id/(float)w); \n" " int x = id-y*w; \n" " if(y=h-normalAveragingRange || x=w-normalAveragingRange) \n" " { \n" " out[id]=in[id]; \n" " } \n" " else \n" " { \n" " float4 avg; \n" " avg.x=0, avg.y=0, avg.z=0, avg.w=0; \n" " for(int sx=-normalAveragingRange; sx<=normalAveragingRange; sx++){ \n" " for(int sy=-normalAveragingRange; sy<=normalAveragingRange; sy++){ \n" " avg.x+=in[(x+sx)+w*(y+sy)].x; \n" " avg.y+=in[(x+sx)+w*(y+sy)].y; \n" " avg.z+=in[(x+sx)+w*(y+sy)].z; \n" " } \n" " } \n" " avg.x/=((1+2*normalAveragingRange)*(1+2*normalAveragingRange)); \n" " avg.y/=((1+2*normalAveragingRange)*(1+2*normalAveragingRange)); \n" " avg.z/=((1+2*normalAveragingRange)*(1+2*normalAveragingRange)); \n" " avg.w=1; \n" " out[id]=avg; \n" " } \n" "} \n" "__kernel void \n" "angleImageCalculation(__global float4 const * normals, __global float * out, int const w, int const h, int const neighborhoodRange, int const neighborhoodMode)\n" "{ \n" " size_t id = get_global_id(0); \n" " int y = (int)floor((float)id/(float)w); \n" " int x = id-y*w; \n" " if(y=h-(neighborhoodRange) || x=w-(neighborhoodRange)) \n" " { \n" " out[id]=0; \n" " } \n" " else \n" " { \n" " float snr=0;//sum right \n" " float snl=0;//sum left \n" " float snt=0;//sum top \n" " float snb=0;//sum bottom \n" " float sntr=0;//sum top-right \n" " float sntl=0;//sum top-left \n" " float snbr=0;//sum bottom-right \n" " float snbl=0;//sum bottom-left \n" " for(int z=1; z<=neighborhoodRange; z++){ \n" " float nr=(normals[id].x*normals[id+z].x+normals[id].y*normals[id+z].y+normals[id].z*normals[id+z].z); \n" " float nl=(normals[id].x*normals[id-z].x+normals[id].y*normals[id-z].y+normals[id].z*normals[id-z].z); \n" " float nt=(normals[id].x*normals[id+w*z].x+normals[id].y*normals[id+w*z].y+normals[id].z*normals[id+w*z].z); \n" " float nb=(normals[id].x*normals[id-w*z].x+normals[id].y*normals[id-w*z].y+normals[id].z*normals[id-w*z].z); \n" " float ntr=(normals[id].x*normals[id+w*z+z].x+normals[id].y*normals[id+w*z+z].y+normals[id].z*normals[id+w*z+z].z); \n" " float ntl=(normals[id].x*normals[id+w*z-z].x+normals[id].y*normals[id+w*z-z].y+normals[id].z*normals[id+w*z-z].z); \n" " float nbr=(normals[id].x*normals[id-w*z+z].x+normals[id].y*normals[id-w*z+z].y+normals[id].z*normals[id-w*z+z].z); \n" " float nbl=(normals[id].x*normals[id-w*z-z].x+normals[id].y*normals[id-w*z-z].y+normals[id].z*normals[id-w*z-z].z); \n" " if(nrsnl){ \n" " out[id]=snl; \n" " } \n" " if(out[id]>snt){ \n" " out[id]=snt; \n" " } \n" " if(out[id]>snb){ \n" " out[id]=snb; \n" " } \n" " if(out[id]>snbl){ \n" " out[id]=snbl; \n" " } \n" " if(out[id]>snbr){ \n" " out[id]=snbr; \n" " } \n" " if(out[id]>sntl){ \n" " out[id]=sntl; \n" " } \n" " if(out[id]>sntr){ \n" " out[id]=sntr; \n" " } \n" " } \n" " else if(neighborhoodMode==1){ \n" " out[id]=(snr+snl+snt+snb+sntr+sntl+snbr+snbl)/8; \n" " } \n" " else{ \n" " out[id]=0; \n" " } \n" " } \n" "} \n" "__kernel void \n" "imageBinarization(__global float const * in, __global float * out, int const w, int const h, float const binarizationThreshold)\n" "{ \n" " size_t id = get_global_id(0); \n" " if(in[id]>binarizationThreshold) \n" " { \n" " out[id]=255; \n" " } \n" " else \n" " { \n" " out[id]=0; \n" " } \n" "} \n" ; PointNormalEstimation::PointNormalEstimation(Size size){ //set default values medianFilterSize=3; normalRange=2; normalAveragingRange=1; neighborhoodMode=0; neighborhoodRange=3; binarizationThreshold=0.89; clReady=false; useCL=true; useNormalAveraging=true; //create arrays and images in given size if(size==Size::QVGA){ std::cout<<"Resolution: 320x240"< platformList;//get number of available openCL platforms int selectedDevice=0;//initially select platform 0 try{ if(cl::Platform::get(&platformList)==CL_SUCCESS){ std::cout<<"openCL platform found"< deviceList; if(platformList.at(i).getDevices(CL_DEVICE_TYPE_GPU, &deviceList)==CL_SUCCESS){ std::cout<<"GPU-DEVICE(S) FOUND"<(); std::cout<<"selected devices: "< list; for(int y=0; y=h-(medianFilterSize-1)/2 || x<(medianFilterSize-1)/2 || x>=w-(medianFilterSize-1)/2){ filteredImage(x,y,0)=rawImage(x,y,0); //pixel out of range }else{ for(int sx=-(medianFilterSize-1)/2; sx<=(medianFilterSize-1)/2; sx++){ for(int sy=-(medianFilterSize-1)/2; sy<=(medianFilterSize-1)/2; sy++){ list.push_back(rawImage(x+sx,y+sy,0)); } } std::sort(list.begin(),list.end()); filteredImage(x,y,0)=list[medianFilterSize*medianFilterSize/2]; list.clear(); } } } } } Img32f PointNormalEstimation::getFilteredImage(){ #ifdef HAVE_OPENCL if(useCL==true && clReady==true){ try{ queue.enqueueReadBuffer(//read output from kernel filteredImageBuffer, CL_TRUE, // block 0, w*h * sizeof(float), (float*) outputFilteredImage); filteredImage = Img32f(Size(w,h),1,std::vector(1,outputFilteredImage),false); } catch (cl::Error err) {//catch openCL error std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<< std::endl; } } #endif return filteredImage; } void PointNormalEstimation::setFilteredImage(Img32f &filteredImg){ filteredImage=filteredImg; #ifdef HAVE_OPENCL if(useCL==true && clReady==true){ filteredImageArray=filteredImage.begin(0);//image to float array try{ filteredImageBuffer = cl::Buffer(//write new image to buffer context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, w*h * sizeof(float), (void *) &filteredImageArray[0]); }catch (cl::Error err) {//catch openCL errors std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<=h-normalRange || x=w-normalRange){ normals[i].x=0;//points out of range normals[i].y=0; normals[i].z=0; }else{ //cross product normal determination fa1[0]=(x+normalRange)-(x-normalRange); fa1[1]=(y-normalRange)-(y-normalRange); fa1[2]=filteredImage(x+normalRange,y-normalRange,0)-filteredImage(x-normalRange,y-normalRange,0); fb1[0]=(x)-(x-normalRange); fb1[1]=(y+normalRange)-(y-normalRange); fb1[2]=filteredImage(x,y+normalRange,0)-filteredImage(x-normalRange,y-normalRange,0); n1[0]=fa1[1]*fb1[2]-fa1[2]*fb1[1]; n1[1]=fa1[2]*fb1[0]-fa1[0]*fb1[2]; n1[2]=fa1[0]*fb1[1]-fa1[1]*fb1[0]; n01[0]=n1[0]/norm3(n1); n01[1]=n1[1]/norm3(n1); n01[2]=n1[2]/norm3(n1); normals[i].x=n01[0]; normals[i].y=n01[1]; normals[i].z=n01[2]; normals[i].w=1; } } } } if(useNormalAveraging==true){ normalAveraging(); } } void PointNormalEstimation::normalAveraging(){ if(useCL==true && clReady==true){ #ifdef HAVE_OPENCL try{ kernelNormalAveraging.setArg(0, normalsBuffer);//set parameter for kernel kernelNormalAveraging.setArg(1, avgNormalsBuffer); kernelNormalAveraging.setArg(2, w); kernelNormalAveraging.setArg(3, h); kernelNormalAveraging.setArg(4, normalAveragingRange); queue.enqueueNDRangeKernel(//run kernel kernelNormalAveraging, cl::NullRange, cl::NDRange(w*h), //input size for get global id cl::NullRange); }catch (cl::Error err) {//catch openCL errors std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<=h-normalAveragingRange || x=w-normalAveragingRange){ avgNormals[i]=normals[i]; }else{ Vec4 avg; avg.x=0, avg.y=0, avg.z=0, avg.w=0; for(int sx=-normalAveragingRange; sx<=normalAveragingRange; sx++){ for(int sy=-normalAveragingRange; sy<=normalAveragingRange; sy++){ avg.x+=normals[(x+sx)+w*(y+sy)].x; avg.y+=normals[(x+sx)+w*(y+sy)].y; avg.z+=normals[(x+sx)+w*(y+sy)].z; } } avg.x/=((1+2*normalAveragingRange)*(1+2*normalAveragingRange)); avg.y/=((1+2*normalAveragingRange)*(1+2*normalAveragingRange)); avg.z/=((1+2*normalAveragingRange)*(1+2*normalAveragingRange)); avg.w=1; avgNormals[i]=avg; } } } } } PointNormalEstimation::Vec4* PointNormalEstimation::getNormals(){ if(useCL==true && clReady==true){ #ifdef HAVE_OPENCL try{ if(useNormalAveraging==true){ queue.enqueueReadBuffer(//read output from kernel avgNormalsBuffer, CL_TRUE, // block 0, w*h * sizeof(cl_float4), (cl_float4*) outputNormals); avgNormals=outputNormals; return avgNormals; }else{ queue.enqueueReadBuffer(//read output from kernel normalsBuffer, CL_TRUE, // block 0, w*h * sizeof(cl_float4), (cl_float4*) outputNormals); normals=outputNormals; return normals; } } catch (cl::Error err) {//catch openCL error std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<< std::endl; } #endif return normals; } else{ if(useNormalAveraging==true){ return avgNormals; } else{ return normals; } } } void PointNormalEstimation::setNormals(Vec4* pNormals){ if(useNormalAveraging==true){ avgNormals=pNormals; } else{ normals=pNormals; } #ifdef HAVE_OPENCL if(useCL==true && clReady==true){ try{ if(useNormalAveraging==true){ avgNormalsBuffer = cl::Buffer( context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, w*h * sizeof(cl_float4), (void *) &avgNormals[0]); }else{ normalsBuffer = cl::Buffer( context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, w*h * sizeof(cl_float4), (void *) &normals[0]); } }catch (cl::Error err) {//catch openCL errors std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<=h-(neighborhoodRange) || x=w-(neighborhoodRange)){ angleImage(x,y,0)=0; } else{ float snr=0;//sum right float snl=0;//sum left float snt=0;//sum top float snb=0;//sum bottom float sntr=0;//sum top-right float sntl=0;//sum top-left float snbr=0;//sum bottom-right float snbl=0;//sum bottom-left for(int z=1; z<=neighborhoodRange; z++){ //angle between normals float nr=(norm[i].x*norm[i+z].x+norm[i].y*norm[i+z].y+norm[i].z*norm[i+z].z); float nl=(norm[i].x*norm[i-z].x+norm[i].y*norm[i-z].y+norm[i].z*norm[i-z].z); float nt=(norm[i].x*norm[i+w*z].x+norm[i].y*norm[i+w*z].y+norm[i].z*norm[i+w*z].z); float nb=(norm[i].x*norm[i-w*z].x+norm[i].y*norm[i-w*z].y+norm[i].z*norm[i-w*z].z); float ntr=(norm[i].x*norm[i+w*z+z].x+norm[i].y*norm[i+w*z+z].y+norm[i].z*norm[i+w*z+z].z); float ntl=(norm[i].x*norm[i+w*z-z].x+norm[i].y*norm[i+w*z-z].y+norm[i].z*norm[i+w*z-z].z); float nbr=(norm[i].x*norm[i-w*z+z].x+norm[i].y*norm[i-w*z+z].y+norm[i].z*norm[i-w*z+z].z); float nbl=(norm[i].x*norm[i-w*z-z].x+norm[i].y*norm[i-w*z-z].y+norm[i].z*norm[i-w*z-z].z); //flip if angle is bigger than 90° if(nrsnl){ angleImage(x,y,0)=snl; } if(angleImage(x,y,0)>snt){ angleImage(x,y,0)=snt; } if(angleImage(x,y,0)>snb){ angleImage(x,y,0)=snb; } if(angleImage(x,y,0)>snbl){ angleImage(x,y,0)=snbl; } if(angleImage(x,y,0)>snbr){ angleImage(x,y,0)=snbr; } if(angleImage(x,y,0)>sntl){ angleImage(x,y,0)=sntl; } if(angleImage(x,y,0)>sntr){ angleImage(x,y,0)=sntr; } } else if(neighborhoodMode==1){ angleImage(x,y,0)=(snr+snl+snt+snb+sntr+sntl+snbr+snbl)/8; } else{ std::cout<<"Unknown neighborhood mode"<(1,outputAngleImage),false); } catch (cl::Error err) {//catch openCL error std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<< std::endl; } } #endif return angleImage; } void PointNormalEstimation::setAngleImage(Img32f &angleImg){ angleImage=angleImg; #ifdef HAVE_OPENCL if(useCL==true && clReady==true){ angleImageArray=angleImage.begin(0);//image to float array try{ angleImageBuffer = cl::Buffer(//write new image to buffer context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, w*h * sizeof(float), (void *) &angleImageArray[0]); }catch (cl::Error err) {//catch openCL errors std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<binarizationThreshold){ binarizedImage(x,y,0)=255; }else{ binarizedImage(x,y,0)=0; } } } } } Img32f PointNormalEstimation::getBinarizedImage(){ #ifdef HAVE_OPENCL if(useCL==true && clReady==true){ try{ queue.enqueueReadBuffer(//read output from kernel binarizedImageBuffer, CL_TRUE, // block 0, w*h * sizeof(float), (float*) outputBinarizedImage); binarizedImage = Img32f(Size(w,h),1,std::vector(1,outputBinarizedImage),false); } catch (cl::Error err) {//catch openCL error std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<< std::endl; } } #endif return binarizedImage; } void PointNormalEstimation::setMedianFilterSize(int size){ medianFilterSize=size; } void PointNormalEstimation::setNormalCalculationRange(int range){ normalRange=range; } void PointNormalEstimation::setNormalAveragingRange(int range){ normalAveragingRange=range; } void PointNormalEstimation::setAngleNeighborhoodMode(int mode){ neighborhoodMode=mode; } void PointNormalEstimation::setAngleNeighborhoodRange(int range){ neighborhoodRange=range; } void PointNormalEstimation::setBinarizationThreshold(float threshold){ binarizationThreshold=threshold; } void PointNormalEstimation::setUseCL(bool use){ useCL=use; } void PointNormalEstimation::setUseNormalAveraging(bool use){ useNormalAveraging=use; } bool PointNormalEstimation::isCLReady(){ return clReady; } bool PointNormalEstimation::isCLActive(){ return useCL; } Img32f PointNormalEstimation::calculate(const Img32f &depthImage, bool filter, bool average){ if(filter==false){ setFilteredImage((Img32f&)depthImage); } else{ setDepthImage(depthImage); medianFilter(); } useNormalAveraging=average; normalCalculation(); angleImageCalculation(); imageBinarization(); return getBinarizedImage(); } } // namespace geom }