/********************************************************************
**                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/Segmentation3D.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 <CL/cl.hpp>
#endif

#include <ICLGeom/Segmentation3D.h>

#include <ICLQt/Quick.h>
#include <ICLGeom/GeomDefs.h>

namespace icl{
  namespace geom{

#ifdef HAVE_OPENCL
    //OpenCL kernel code
    static char segmentationKernel[] = 
      "  #pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable                                                           \n"
      "__kernel void                                                                                                                  \n"
      "calculatePointAssignment(__global float4 const * xyz, __global bool * elements, __global int const * assignment, int const radius, int const numFaces, __global bool * neighbours, __global int * assignmentOut, int const w, int const h, float const maxDist)                                                                   \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"
      "  float dist=100000;                                                                                                           \n"
      "  int ass=0;                                                                                                                   \n"
      "  bool assigned=false;                                                                                                         \n"
      "  if(elements[id]==true && assignment[id]==0){                                                                                 \n"
      "    bool adj[100];                                                                                                             \n"
      "    for(int a=0; a<numFaces; a++){                                                                                             \n"
      "      adj[a]=false;                                                                                                            \n"
      "    }                                                                                                                          \n"
      "    for(int xx=-radius; xx<=radius; xx++){                                                                                     \n"
      "      for(int yy=-radius; yy<=radius; yy++){                                                                                   \n"
      "        if(x+xx>=0 && x+xx<w && y+yy>=0 && y+yy<h && assignment[(x+xx)+w*(y+yy)]!=0){                                          \n"
      "          float4 pointa=xyz[id];                                                                                               \n"
      "          pointa.w=1.0;                                                                                                        \n"
      "          float4 pointb=xyz[(x+xx)+w*(y+yy)];                                                                                  \n"
      "          pointb.w=1.0;                                                                                                        \n"
      "          float dist3 = distance(pointa,pointb);                                                                               \n"
      "          if(dist3<maxDist){                                                                                                   \n"
      "            adj[assignment[(x+xx)+w*(y+yy)]-1]=true;                                                                           \n"
      "          }                                                                                                                    \n"
      "          if(dist3<dist && dist3<maxDist){                                                                                     \n"   
      "            dist=dist3;                                                                                                        \n"
      "            ass=assignment[(x+xx)+w*(y+yy)];                                                                                   \n"
      "            assigned=true;                                                                                                     \n"
      "          }                                                                                                                    \n"
      "        }                                                                                                                      \n"
      "      }                                                                                                                        \n"
      "    }                                                                                                                          \n"
      "    for(int a=0; a<numFaces-1; a++){                                                                                           \n"
      "      for (int b=a+1; b<numFaces; b++){                                                                                        \n"
      "        if(adj[a]==true && adj[b]==true){                                                                                      \n"
      "          neighbours[a*numFaces+b]=true;                                                                                       \n"
      "          neighbours[b*numFaces+a]=true;                                                                                       \n"
      "        }                                                                                                                      \n"
      "      }                                                                                                                        \n"
      "    }                                                                                                                          \n"
      "    if(assigned==true){                                                                                                        \n"
      "      assignmentOut[id]=ass;                                                                                                   \n"
      "      elements[id]=false;                                                                                                      \n"
      "    }                                                                                                                          \n"
      "  }                                                                                                                            \n"
      "}                                                                                                                              \n"
      "__kernel void                                                                                                                  \n"
      "checkRANSAC(__global float4 const * xyz, int const RANSACpasses, __global float4 const * n0, __global float const * dist, __global int * countAbove, __global int * countBelow, __global int * countOn, int const RANSACeucl, int const numPoints, __global const int * points, int const subset)                                  \n"
      "{                                                                                                                              \n"
      "  size_t id =  get_global_id(0);                                                                                               \n"
      "  int pass = id%RANSACpasses;                                                                                                  \n"
      "  int point = ((int)floor((float)id/(float)RANSACpasses))*subset;                                                              \n"
      "  int cPoint = points[point];                                                                                                  \n"
      "  float4 selPoint = xyz[cPoint];                                                                                               \n"
      "  float4 seln0 = n0[pass];                                                                                                     \n"
      "  float distance1 = dist[pass];                                                                                                \n"
      "  float s1 = (selPoint.x*seln0.x+selPoint.y*seln0.y+selPoint.z*seln0.z)-distance1;                                             \n"
      "  if((s1>=-RANSACeucl && s1<=RANSACeucl)){                                                                                     \n"
      "    atomic_inc(&countOn[pass]);                                                                                                \n"
      "  }else if(s1>RANSACeucl){                                                                                                     \n"
      "    atomic_inc(&countAbove[pass]);                                                                                             \n"
      "  }else if(s1<-RANSACeucl){                                                                                                    \n"
      "    atomic_inc(&countBelow[pass]);                                                                                             \n"
      "  }                                                                                                                            \n"
      "}                                                                                                                              \n"
      "__kernel void                                                                                                                  \n"
      "assignRANSAC(__global float4 const * xyz, __global bool * elements, __global int * assignment, float4 const n0, float const d, int const euclDist, __global int * oldAssignment, int maxID) \n"
      "{                                                                                                                              \n"
      "  size_t id =  get_global_id(0);                                                                                               \n"
      "  if(oldAssignment[id]==maxID)                                                                                                 \n"
      "  {                                                                                                                            \n"
      "    assignment[id]=1;                                                                                                          \n"
      "    elements[id]=false;                                                                                                        \n"
      "  }                                                                                                                            \n"
      "  else                                                                                                                         \n"
      "  {                                                                                                                            \n"
      "    float4 xyzD=xyz[id];                                                                                                       \n"
      "    float s1 = (xyzD.x*n0.x+xyzD.y*n0.y+xyzD.z*n0.z)-d;                                                                        \n"
      "    if((s1>=-euclDist && s1<=euclDist) && elements[id]==true)                                                                  \n"
      "    {                                                                                                                          \n"
      "      assignment[id]=1;                                                                                                        \n"
      "      elements[id]=false;                                                                                                      \n"
      "    }                                                                                                                          \n"
      "  }                                                                                                                            \n"
      "}                                                                                                                              \n"
      "__kernel void                                                                                                                  \n"
      "segmentColoring(__global int const * assignment, __global uchar * colorR, __global uchar * colorG, __global uchar * colorB)    \n"
      "{                                                                                                                              \n"
      "  size_t id =  get_global_id(0);                                                                                               \n"
      "  if(assignment[id]==0)                                                                                                        \n"
      "  {                                                                                                                            \n"
      "    colorR[id]=128;                                                                                                            \n"
      "    colorG[id]=128;                                                                                                            \n"
      "    colorB[id]=128;                                                                                                            \n"
      "  }                                                                                                                            \n"
      "  else                                                                                                                         \n"
      "  {                                                                                                                            \n"
      "    int H=(int)(assignment[id]*35.)%360;                                                                                       \n"
      "    float S=1.0-assignment[id]*0.01;                                                                                           \n"
      "    float hi=floor((float)H/60.);                                                                                              \n"
			"	   float f=((float)H/60.)-hi;                                                                                                 \n"
			"	   float pp=1.0-S;                                                                                                            \n"
			"	   float qq=1.0-S*f;                                                                                                          \n"
			"	   float tt=1.0-S*(1.-f);                                                                                                     \n"
			"	   float newR=0;                                                                                                              \n"
			"	   float newG=0;                                                                                                              \n"
			"	   float newB=0;                                                                                                              \n"
			"	   if((int)hi==0 || (int)hi==6){                                                                                              \n"
			"	  	 newR=1.0;                                                                                                                \n"
			"	     newG=tt;                                                                                                                 \n"
			"		   newB=pp;                                                                                                                 \n"
			"	   }else if((int)hi==1){                                                                                                      \n"
			"		   newR=qq;                                                                                                                 \n"
			"		   newG=1.0;                                                                                                                \n"
			"		   newB=pp;                                                                                                                 \n"
			"	   }else if((int)hi==2){                                                                                                      \n"
			"		   newR=pp;                                                                                                                 \n"
			"		   newG=1.0;                                                                                                                \n"
			"	     newB=tt;                                                                                                                 \n"
			"	   }else if((int)hi==3){                                                                                                      \n"
			"		   newR=pp;                                                                                                                 \n"
			"	     newG=qq;                                                                                                                 \n"
			"	     newB=1.0;                                                                                                                \n"
			"	   }else if((int)hi==4){                                                                                                      \n"
			"		   newR=tt;                                                                                                                 \n"
			"		   newG=pp;                                                                                                                 \n"
			"		   newB=1.0;                                                                                                                \n"
			"	   }else if((int)hi==5){                                                                                                      \n"
			"	     newR=1.0;                                                                                                                \n"
			"		   newG=pp;                                                                                                                 \n"
			"		   newB=qq;                                                                                                                 \n"
			"	   }                                                                                                                          \n"
		  "    colorR[id]=(unsigned char)(newR*255.);                                                                                     \n"
		  "    colorG[id]=(unsigned char)(newG*255.);                                                                                     \n"
		  "    colorB[id]=(unsigned char)(newB*255.);                                                                                     \n"	
		  "  }                                                                                                                            \n"
		  "}                                                                                                                              \n"
    ;
  #endif
                
    Segmentation3D::Segmentation3D(Size size){
      //set default values
      clReady=false;
      useCL=true;
      w=size.width;
      h=size.height;
      dim=w*h;
      s=size;  
      
      minClusterSize=25;
      useFastGrowing=false;
      assignmentRadius=5;
      assignmentMaxDistance=15;
      
      RANSACeuclDistance=15; 
      RANSACpasses=20; 
      RANSACtolerance=30; 
      RANSACsubset=2;
      BLOBSeuclDistance=15;
            
      useROI=false;
      xMinROI=0, xMaxROI=0, yMinROI=0; yMaxROI=0;
      elements=new bool[w*h];
      assignment=new int[w*h];
      assignmentRemaining=new int[w*h];
      elementsBlobs=new bool[w*h];
      assignmentBlobs=new int[w*h];
      
      normalEdgeImage.setSize(Size(w,h));
      normalEdgeImage.setChannels(1);
      depthImage.setSize(Size(w,h));
      depthImage.setChannels(1);
      
      segmentColorImage.setSize(Size(w,h));
      segmentColorImage.setChannels(3);
      
      region=new RegionDetector(25, 4000000, 254, 255, false);
         
    #ifdef HAVE_OPENCL
      //create openCL context
      segmentColorImageRArray = new cl_uchar[w*h];
      segmentColorImageGArray = new cl_uchar[w*h];
      segmentColorImageBArray = new cl_uchar[w*h];
      
      std::vector<cl::Platform> 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"<<std::endl;
        }else{
          std::cout<<"no openCL platform available"<<std::endl;
        }
        std::cout<<"number of openCL platforms: "<<platformList.size()<<std::endl;
          
        //check devices on platforms
        for(unsigned int i=0; i<platformList.size(); i++){//check all platforms
          std::cout<<"platform "<<i+1<<":"<<std::endl;
          std::vector<cl::Device> deviceList;
          if(platformList.at(i).getDevices(CL_DEVICE_TYPE_GPU, &deviceList)==CL_SUCCESS){
            std::cout<<"GPU-DEVICE(S) FOUND"<<std::endl;
            selectedDevice=i; //if GPU found on platform, select this platform
            clReady=true; //and mark CL context as available
          }else if(platformList.at(i).getDevices(CL_DEVICE_TYPE_CPU, &deviceList)==CL_SUCCESS){
            std::cout<<"CPU-DEVICE(S) FOUND"<<std::endl;
          }else{
            std::cout<<"UNKNOWN DEVICE(S) FOUND"<<std::endl;
          }
          std::cout<<"number of devices: "<<deviceList.size()<<std::endl;
        }
      }catch (cl::Error err) {//catch openCL errors
        std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<< std::endl;
        std::cout<<"OpenCL not ready"<<std::endl;
        clReady=false;//disable openCL on error
      }  
        
      if(clReady==true){//only if CL context is available
        try{
          cl_context_properties cprops[] = {CL_CONTEXT_PLATFORM, (cl_context_properties)(platformList[selectedDevice])(), 0};//get context properties of selected platform
          context = cl::Context(CL_DEVICE_TYPE_GPU, cprops);//select GPU device
          devices = context.getInfo<CL_CONTEXT_DEVICES>();
                
          std::cout<<"selected devices: "<<devices.size()<<std::endl;
                
          cl::Program::Sources sources(1, std::make_pair(segmentationKernel, 0)); //kernel source
          program=cl::Program(context, sources); //program (bind context and source)
          program.build(devices);//build program
  
          //create buffer for memory access and allocation
          segmentColorImageRBuffer = cl::Buffer(
  					  context, 
  					  CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, //
  					  w*h * sizeof(cl_uchar), 
  					  (void *) &segmentColorImageRArray[0]);
  			
  			  segmentColorImageGBuffer = cl::Buffer(
  					  context, 
  					  CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, //
  					  w*h * sizeof(cl_uchar), 
  					  (void *) &segmentColorImageGArray[0]);
  			
  			  segmentColorImageBBuffer = cl::Buffer(
  					  context, 
  					  CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, //
  					  w*h * sizeof(cl_uchar), 
  					  (void *) &segmentColorImageBArray[0]);
  					              
          //create kernels  
          kernelSegmentColoring=cl::Kernel(program, "segmentColoring");  
          kernelPointAssignment=cl::Kernel(program, "calculatePointAssignment");
          kernelCheckRANSAC=cl::Kernel(program, "checkRANSAC");
          kernelAssignRANSAC=cl::Kernel(program, "assignRANSAC");

          queue=cl::CommandQueue(context, devices[0], 0);//create command queue        
        }catch (cl::Error err) {//catch openCL errors
          std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<< std::endl;
          clReady=false;
        }
      }
        
    #else
      std::cout<<"no openCL parallelization available"<<std::endl;
      clReady=false;
    #endif   
    }
  
  
    Segmentation3D::~Segmentation3D(){
      delete[] elements;
      delete[] assignment;
      delete[] assignmentRemaining;
      delete[] elementsBlobs;
      delete[] assignmentBlobs;
    #ifdef HAVE_OPENCL
      delete[] segmentColorImageRArray;
      delete[] segmentColorImageGArray;
      delete[] segmentColorImageBArray;
    #endif
    }  
 
    
    Img8u Segmentation3D::segmentation(DataSegment<float,4> xyz, const Img8u &edgeImg, const Img32f &depthImg){
      xyzData=xyz;
      normalEdgeImage=edgeImg;
      depthImage=depthImg;
      clearData();  
      regionGrow();
	    calculatePointAssignmentAndAdjacency();
	    calculateCutfreeMatrix();
	    greedyComposition();
	    calculateRemainingPoints();
	    colorPointcloud();
		  return segmentColorImage;
		}
		
		
		Img8u Segmentation3D::segmentationBlobs(DataSegment<float,4> xyz, const Img8u &edgeImg, const Img32f &depthImg){
		  xyzData=xyz;
      normalEdgeImage=edgeImg;
      depthImage=depthImg;
      clearData();  
      regionGrow();
		  blobSegmentation();
	    colorPointcloud();
		  return segmentColorImage;
		}   
		
		
    void Segmentation3D::setUseCL(bool use){
      useCL=use;
    }
  

    void Segmentation3D::setUseROI(bool use){
      useROI=use;
    }
    
    
    void Segmentation3D::setROI(float xMin, float xMax, float yMin, float yMax){
      xMinROI=xMin;
      xMaxROI=xMax;
      yMinROI=yMin;
      yMaxROI=yMax;
    }

    
    void Segmentation3D::setMinClusterSize(unsigned int size){
      minClusterSize=size;
    }
    
    
    void Segmentation3D::setUseFastGrowing(bool use){
      useFastGrowing=use;
    }
   
    
    void Segmentation3D::setAssignmentRadius(int radius){
      assignmentRadius=radius;
    }
    
    		
    void Segmentation3D::setAssignmentMaxDistance(float maxDistance){
      assignmentMaxDistance=maxDistance;
    }
    
    
    void Segmentation3D::setRANSACeuclDistance(int distance){
      RANSACeuclDistance=distance;
    }
		
		   
    void Segmentation3D::setRANSACpasses(int passes){
      RANSACpasses=passes;
    }
    
      
    void Segmentation3D::setRANSACtolerance(int tolerance){
      RANSACtolerance=tolerance;
    }
    
      
    void Segmentation3D::setRANSACsubset(int subset){
      RANSACsubset=subset;
    }
    
    
    void Segmentation3D::setBLOBSeuclDistance(int distance){
      BLOBSeuclDistance=distance;
    }

   
    bool Segmentation3D::isCLReady(){
      return clReady;
    }
  
  
    bool Segmentation3D::isCLActive(){
      return useCL;
    }
    
    
    Img8u Segmentation3D::getSegmentColorImage(){
      return segmentColorImage;
    }
    
    
    std::vector<std::vector<int> > Segmentation3D::getCluster(){
      return cluster;
    }
    
    
		std::vector<std::vector<int> > Segmentation3D::getBlobs(){
		  return blobs;
		}
		
		
		DynMatrix<bool> Segmentation3D::getNeigboursMatrix(){
		  return neighbours;
		}
		
		
		DynMatrix<float> Segmentation3D::getProbabilityMatrix(){
		  return probabilities;
		} 
     
      
    void Segmentation3D::setXYZH(DataSegment<float,4> xyz){
      xyzData=xyz;
    }
    
        
    void Segmentation3D::setEdgeImage(const Img8u &edgeImage){
      normalEdgeImage=edgeImage;
    }
     
     
    void Segmentation3D::setDepthImage(const core::Img32f &depth){
      depthImage=depth;
    }
  
     
    void Segmentation3D::clearData(){
		  cluster.clear();
		  blobs.clear();
		  if(useROI){
		    for(int y=0;y<h;++y){
	     		for(int x=0;x<w;++x){
				    int i = x+w*y;
            if( xyzData[i][0]<xMinROI || xyzData[i][0]>xMaxROI || xyzData[i][1]<yMinROI || xyzData[i][1]>yMaxROI){
				      elements[i]=false;
        			assignment[i]=0;
        			assignmentRemaining[i]=0;
        			elementsBlobs[i]=false;
        			assignmentBlobs[i]=0;
        		}else{		
						  elements[i]=true;
		      		assignment[i]=0;
		      		assignmentRemaining[i]=0;
		      		elementsBlobs[i]=true;
		      		assignmentBlobs[i]=0;
		  			}
		      }
		    }
	    }else{
		    for(int y=0;y<h;++y){
	     		for(int x=0;x<w;++x){
				    int i = x+w*y;	
            elements[i]=true;
	        	assignment[i]=0;
	        	assignmentRemaining[i]=0;
	        	elementsBlobs[i]=true;
	        	assignmentBlobs[i]=0;
		  		}
		  	}
		  }
    }
  
  
    void Segmentation3D::regionGrow(){
      if(useFastGrowing){
        int numCluster=0;
        region->setConstraints (minClusterSize, 4000000, 254, 255);
        std::vector<std::vector<int> > remove;
        std::vector<ImageRegion> regions;
        regions = region->detect(&normalEdgeImage); 	
        for(unsigned int i=0; i<regions.size(); i++){
          numCluster++;
          std::vector<utils::Point> ps = regions.at(i).getPixels();
          std::vector<int> data;
          for(unsigned int j=0; j<ps.size(); j++){
            int v=ps.at(j)[0]+w*ps.at(j)[1];
            if(elements[v]==true){
              assignment[v]=numCluster;
              elements[v]=false;
              data.push_back(v);
            }
          }
          if(data.size()<minClusterSize){
			      remove.push_back(data);
			      numCluster--;
			    }
			    else{
			      cluster.push_back(data);
			    }
        }
        for(unsigned int i=0; i<remove.size(); i++){
		      for(unsigned int j=0; j<remove.at(i).size(); j++){
		        elements[remove.at(i).at(j)]=true;
		        assignment[remove.at(i).at(j)]=0;
		      }
		    } 
      }else{
        int numCluster=0;
        std::vector<std::vector<int> > remove;
		    for(int y=0;y<h;++y){
			    for(int x=0;x<w;++x){
				    int i = x+w*y;
				    if(elements[i]==true && normalEdgeImage(x,y,0)==255){
					    elements[i]=false;
					    numCluster++;
					    assignment[i]=numCluster;
					    std::vector<int> data;
					    data.push_back(i);
					    checkNeighbourGrayThreshold(x,y,assignment[i], 255, &data);
					    if(data.size()<minClusterSize){
					      remove.push_back(data);
					      numCluster--;
					    }
					    else{
					      cluster.push_back(data);
					    }
				    }
          }    			
		    }
		    for(unsigned int i=0; i<remove.size(); i++){
		      for(unsigned int j=0; j<remove.at(i).size(); j++){
		        elements[remove.at(i).at(j)]=true;
		        assignment[remove.at(i).at(j)]=0;
		      }
		    }
      }
    }
	  	
    
    void Segmentation3D::calculatePointAssignmentAndAdjacency(){
      if(useCL==true && clReady==true){
    #ifdef HAVE_OPENCL
        try{
          int numFaces=cluster.size();       
          DynMatrix<bool> newMatrix(numFaces,numFaces,false);
          neighbours=newMatrix;
          
          neighboursBuffer = cl::Buffer(
				           context, 
				           CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, 
				           numFaces*numFaces * sizeof(bool), 
				           (void *) &neighbours[0]);
				  xyzBuffer = cl::Buffer(
				           context, 
				           CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
				           w*h * sizeof(cl_float4), 
        				   (void *) &xyzData[0]);
				  elementsBuffer = cl::Buffer(
				           context, 
				           CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, 
				           w*h * sizeof(bool), 
				           (void *) &elements[0]);
				  assignmentBuffer = cl::Buffer(
				           context, 
				           CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
				           w*h * sizeof(int), 
				           (void *) &assignment[0]);
          assignmentOutBuffer = cl::Buffer(
				           context, 
				           CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, 
				           w*h * sizeof(int), 
				           (void *) &assignment[0]);
          
          kernelPointAssignment.setArg(0, xyzBuffer);//set parameter for kernel
          kernelPointAssignment.setArg(1, elementsBuffer);
          kernelPointAssignment.setArg(2, assignmentBuffer);
          kernelPointAssignment.setArg(3, assignmentRadius);
          kernelPointAssignment.setArg(4, numFaces);
          kernelPointAssignment.setArg(5, neighboursBuffer);
          kernelPointAssignment.setArg(6, assignmentOutBuffer);
          kernelPointAssignment.setArg(7, w);
          kernelPointAssignment.setArg(8, h);
          kernelPointAssignment.setArg(9, assignmentMaxDistance);

          queue.enqueueNDRangeKernel(//run kernel
				     kernelPointAssignment, 
				     cl::NullRange, 
				     cl::NDRange(w*h), //input size for get global id
				     cl::NullRange);
    	    
          queue.enqueueReadBuffer(//read output from kernel
			          neighboursBuffer,
			          CL_TRUE, // block 
			          0,
			          numFaces*numFaces * sizeof(bool),
			          (bool*) neighbours.data());
			          
		      queue.enqueueReadBuffer(//read output from kernel
              assignmentOutBuffer,
              CL_TRUE, // block 
              0,
              w*h * sizeof(int),
              (int*) assignment);
          
          queue.enqueueReadBuffer(//read output from kernel
              elementsBuffer,
              CL_TRUE, // block 
              0,
              w*h * sizeof(bool),
              (bool*) elements);
              
          for(int i=0; i<numFaces; i++){///DIAG
            neighbours(i,i)=true;
          }
          cluster.clear();
          for(int x=0; x<w*h; x++){
            if(assignment[x]!=0){
					    if(assignment[x]>(int)cluster.size()){
						    cluster.resize(assignment[x]);
						  }
						  cluster.at(assignment[x]-1).push_back(x);
	          }	 
	        }
        }catch (cl::Error err) {//catch openCL errors
          std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<<std::endl;
        }
    #endif
      }
      else{
        int numFaces=cluster.size();
        DynMatrix<bool> newMatrix(numFaces,numFaces,false);
        neighbours=newMatrix;
        int assignmentOut[w*h];
        for(int x=0; x<w; x++){
          for(int y=0; y<h; y++){
            int i=x+w*y;
            float dist=100000;
            int ass=0;
            bool assigned=false;
            if(elements[i]==true && assignment[i]==0){
              bool adj[numFaces];
              for(int a=0; a<numFaces; a++){
                adj[a]=false;
              }
              for(int xx=-assignmentRadius; xx<=assignmentRadius; xx++){
                for(int yy=-assignmentRadius; yy<=assignmentRadius; yy++){
                  if(x+xx>=0 && x+xx<w && y+yy>=0 && y+yy<h && assignment[(x+xx)+w*(y+yy)]!=0){
                    Vec p1=xyzData[i];
                    Vec p2=xyzData[(x+xx)+w*(y+yy)];
                    float distance=dist3(p1, p2);
                    if(distance<assignmentMaxDistance){
                      adj[assignment[(x+xx)+w*(y+yy)]-1]=true;
                    }
                    if(distance<dist && distance<assignmentMaxDistance){   
                      dist=distance;
                      ass=assignment[(x+xx)+w*(y+yy)];
                      assigned=true;
                    }
                  }
                }
              }
              for(int a=0; a<numFaces-1; a++){
                for (int b=a+1; b<numFaces; b++){
                  if(adj[a]==true && adj[b]==true){
                    neighbours(a,b)=true;
                    neighbours(b,a)=true;
                  }
                }
              }
              if(assigned==true){
                cluster.at(ass-1).push_back(i);
                elements[i]=false;
                assignmentOut[i]=ass;
              }
              else{
                assignmentOut[i]=assignment[i];
              }
            }
            else{
              assignmentOut[i]=assignment[i];
            }
          }
        }
        for(int i=0; i<numFaces; i++){
           neighbours(i,i)=true;
        }
        memcpy(assignment, assignmentOut, sizeof(assignmentOut));
      }
    }
    
    
    void Segmentation3D::calculateCutfreeMatrix(){
      DynMatrix<bool> newMatrix(neighbours.rows(),neighbours.cols(),false);
      cutfree=newMatrix;
    
      for(unsigned int a=0; a<neighbours.rows(); a++){
        #ifdef HAVE_OPENCL
        int numPoints=cluster.at(a).size();
        cl::Buffer RANSACpointsBuffer;
        cl_mem RANSACpointsMem = RANSACpointsBuffer();
        RANSACpointsBuffer = cl::Buffer(
				                   context, 
				                   CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
				                   numPoints * sizeof(int), 
				                   (void *) &cluster.at(a)[0]);
			  #endif
        for(unsigned int b=0; b<neighbours.cols(); b++){
          if(a==b){
            cutfree(a,b)=true;
          }else if(neighbours(a,b)==false){
            cutfree(a,b)=false;
          }else{
            int countAcc=0;
            int countNAcc=0;
            
            if(useCL==true && clReady==true){
            #ifdef HAVE_OPENCL
             
              Vec n0[RANSACpasses];
              float dist[RANSACpasses];  
              int cAbove[RANSACpasses];
              int cBelow[RANSACpasses];
              int cOn[RANSACpasses];
              
              
              int cAboveRead[RANSACpasses];
              int cBelowRead[RANSACpasses];
              int cOnRead[RANSACpasses];
              
              for(int i=0; i<RANSACpasses; i++){
                cAbove[i]=0;
                cBelow[i]=0;
                cOn[i]=0;
                
                cAboveRead[i]=0;
                cBelowRead[i]=0;
                cOnRead[i]=0;
                
                
                Vec rPoint1;
                Vec n01;		
                int p0i=cluster.at(b).at(rand()%cluster.at(b).size());
                int p1i=cluster.at(b).at(rand()%cluster.at(b).size());
                int p2i=cluster.at(b).at(rand()%cluster.at(b).size());
                Vec fa1;
                Vec fb1;
                Vec n1;
              	fa1 = xyzData[p1i]-xyzData[p0i];
              	fb1 = xyzData[p2i]-xyzData[p0i];
                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);
            	 	rPoint1 = xyzData[p0i];
                float distance1 = rPoint1[0]*n01[0]+rPoint1[1]*n01[1]+ rPoint1[2]*n01[2];
                dist[i]=distance1;
                n0[i]=n01;
              }
              
              try{         
                cl::Event waitEvent;
                cl::Buffer n0Buffer;
                cl::Buffer distBuffer;
                cl::Buffer countAboveBuffer;
                cl::Buffer countBelowBuffer;
                cl::Buffer countOnBuffer;
      
                cl_mem n0Mem = n0Buffer();
                cl_mem distMem = distBuffer();
                cl_mem countAboveMem = countAboveBuffer();
                cl_mem countBelowMem = countBelowBuffer();
                cl_mem countOnMem = countOnBuffer();
                
                n0Buffer = cl::Buffer(
		                     context, 
		                     CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
		                     RANSACpasses * sizeof(cl_float4), 
		                     (void *) &n0[0]);
		            distBuffer = cl::Buffer(
		                     context, 
		                     CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
		                     RANSACpasses * sizeof(float), 
		                     (void *) &dist[0]); 
		            countAboveBuffer = cl::Buffer(
		                     context, 
		                     CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, 
		                     RANSACpasses * sizeof(int), 
		                     (void *) &cAbove[0]);
		            countBelowBuffer = cl::Buffer(
		                     context, 
		                     CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, 
		                     RANSACpasses * sizeof(int), 
		                     (void *) &cBelow[0]);
                countOnBuffer = cl::Buffer(
		                     context, 
		                     CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, 
		                     RANSACpasses * sizeof(int), 
		                     (void *) &cOn[0]);

                kernelCheckRANSAC.setArg(0, xyzBuffer);//set parameter for kernel
                kernelCheckRANSAC.setArg(1, RANSACpasses);
                kernelCheckRANSAC.setArg(2, n0Buffer);
                kernelCheckRANSAC.setArg(3, distBuffer);
                kernelCheckRANSAC.setArg(4, countAboveBuffer);
                kernelCheckRANSAC.setArg(5, countBelowBuffer);
                kernelCheckRANSAC.setArg(6, countOnBuffer);
                kernelCheckRANSAC.setArg(7, RANSACeuclDistance);
                kernelCheckRANSAC.setArg(8, numPoints);
                kernelCheckRANSAC.setArg(9, RANSACpointsBuffer);
                kernelCheckRANSAC.setArg(10, RANSACsubset);
                              
                int idSize=RANSACpasses*numPoints/RANSACsubset;
                queue.enqueueNDRangeKernel(//run kernel
		               kernelCheckRANSAC, 
		               cl::NullRange, 
		               cl::NDRange(idSize), //input size for get global id
		               cl::NullRange,
		               NULL,
		               &waitEvent);
          	                	    
                queue.enqueueReadBuffer(//read output from kernel
                    countAboveBuffer,
                    CL_TRUE, // block 
                    0,
                    RANSACpasses * sizeof(int),
                    (int*) cAboveRead,
                    NULL,&waitEvent);
                               
                queue.enqueueReadBuffer(//read output from kernel
                    countBelowBuffer,
                    CL_TRUE, // block 
                    0,
                    RANSACpasses * sizeof(int),
                    (int*) cBelowRead,
                    NULL,&waitEvent);
                            
                queue.enqueueReadBuffer(//read output from kernel
                    countOnBuffer,
                    CL_TRUE, // block 
                    0,
                    RANSACpasses * sizeof(int),
                    (int*) cOnRead,
                    NULL,&waitEvent);
                 
                clFinish(queue()); 
                  
                clReleaseMemObject(n0Mem);
                clReleaseMemObject(distMem);
                clReleaseMemObject(countAboveMem);
                clReleaseMemObject(countBelowMem);
                clReleaseMemObject(countOnMem);
          
              }catch (cl::Error err) {//catch openCL errors
                std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<<std::endl;
              }
              for(int i=0; i<RANSACpasses; i++){
                if(cAboveRead[i]<RANSACtolerance || cBelowRead[i]<RANSACtolerance){
                countAcc++;
                }else{
                  countNAcc++;
                }
              }
              
              if(countAcc>countNAcc){
                cutfree(a,b)=true;
              }else{
                cutfree(a,b)=false;
              }
            #endif
            }
            else{             
              for(int p=0; p<RANSACpasses; p++){
                Vec rPoint1;
                Vec n01;		
                int p0i=cluster.at(a).at(rand()%cluster.at(a).size());
                int p1i=cluster.at(a).at(rand()%cluster.at(a).size());
                int p2i=cluster.at(a).at(rand()%cluster.at(a).size());
                Vec fa1;
                Vec fb1;
                Vec n1;
                //PlaneFitting
              	fa1 = xyzData[p1i]-xyzData[p0i];
              	fb1 = xyzData[p2i]-xyzData[p0i];
                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);
            	 	rPoint1 = xyzData[p0i];
                float distance1 = rPoint1[0]*n01[0]+rPoint1[1]*n01[1]+ rPoint1[2]*n01[2];
                    
                int countAbove=0;
                int countBelow=0;
                int countOn=0;
                for(unsigned int p=0; p<cluster.at(b).size(); p++){
                  float s1 = (xyzData[cluster.at(b).at(p)][0]*n01[0]+xyzData[cluster.at(b).at(p)][1]*n01[1]+xyzData[cluster.at(b).at(p)][2]*n01[2])-distance1;      
                  if((s1>=-RANSACeuclDistance && s1<=RANSACeuclDistance)){
                    countOn++;
                  }else if(s1>RANSACeuclDistance){
                    countAbove++;
                  }else if(s1<-RANSACeuclDistance){
                    countBelow++;
                  }
                }  
                if(countAbove<RANSACtolerance || countBelow<RANSACtolerance){
                  countAcc++;
                }else{
                  countNAcc++;
                }
              }
              if(countAcc>countNAcc){
                cutfree(a,b)=true;
              }else{
                cutfree(a,b)=false;
              }
            }
          }
        }
        #ifdef HAVE_OPENCL
        clReleaseMemObject(RANSACpointsMem);  
        #endif
      }    
    }
    
    
    void Segmentation3D::greedyComposition(){
      DynMatrix<bool> combinable=DynMatrix<bool>(cluster.size(),cluster.size(),false);
      probabilities=DynMatrix<float>(cluster.size(),cluster.size(),0.0);
      for(unsigned int a=0; a<cutfree.cols(); a++){
        for(unsigned int b=a; b<cutfree.cols(); b++){
          if(a==b){combinable(a,b)=0;}
          else{
            if(cutfree(a,b)==1 && cutfree(b,a)==1){
              combinable(a,b)=1;
              combinable(b,a)=1;
		        }
		        else{
		          combinable(a,b)=0;
		          combinable(b,a)=0;
		        }
		      }
        }
      }

      for(unsigned int a=0;a<combinable.cols(); a++){
        int count=0;
        for(unsigned int b=0; b<combinable.rows(); b++){
          if(combinable(b,a)==true) count++;
        }
        for(unsigned int b=0; b<combinable.rows(); b++){
          if(combinable(b,a)==true) probabilities(b,a)=1./(float)count;
        }
      }

      DynMatrix<float> W = probabilities;

      std::vector<std::vector<int> > facesCom;
      std::vector<float> probsCom;
      for(unsigned int a=0; a<W.cols(); a++){
        for(unsigned int b=0; b<W.cols(); b++){
          if(W(a,b)>0){
            std::vector<int> add;
            add.push_back(a);
            add.push_back(b);
            facesCom.push_back(add);
            probsCom.push_back(W(a,b));
          }
        }
      }
      for(unsigned int a=0; a<facesCom.size(); a++){
        for(unsigned int b=0; b<W.rows(); b++){
          bool breaking=false;
          for(unsigned int c=0; c<facesCom.at(a).size(); c++){
            if(facesCom.at(a).at(c)==(int)b){
              breaking=true;
              break;
            }
            else if(W(facesCom.at(a).at(c),b)==0){
              breaking=true;
              break;
            }
            else{
            }
          }
          if(breaking==false){
            float sum=0.0;
            std::vector<int> add;
            for(unsigned int c=0; c<facesCom.at(a).size(); c++){
              add.push_back(facesCom.at(a).at(c));
            }
            add.push_back(b);
            facesCom.push_back(add);
            for(unsigned int d=1; d<facesCom.at(facesCom.size()-1).size(); d++){
              sum+=W(facesCom.at(facesCom.size()-1).at(0),facesCom.at(facesCom.size()-1).at(d));
            }
            probsCom.push_back(sum);
          }
        }
      }

      while (probsCom.size()>0){
        float maxProb=0;
        int maxProbPos=0;
        for(unsigned int a=0; a<probsCom.size(); a++){
          if((probsCom.at(a)>maxProb) || (probsCom.at(a)==maxProb && facesCom.at(a).size()>facesCom.at(maxProbPos).size())){
            maxProb=probsCom.at(a);
            maxProbPos=a;
          }     
        }
        std::vector<int> faces;
        faces=facesCom.at(maxProbPos);
        blobs.push_back(faces);
        for(unsigned int a=0; a<facesCom.size(); a++){
          bool doubleF=false;
          for(unsigned int b=0; b<facesCom.at(a).size(); b++){  
            for(unsigned int c=0; c<faces.size(); c++){
              if(faces.at(c)==facesCom.at(a).at(b)){
                doubleF=true;
                break;
              }
            }
            if(doubleF==true) break;
          }
          if(doubleF==true){
            probsCom.erase(probsCom.begin()+a);
            facesCom.erase(facesCom.begin()+a);
            a--;
          }
        }
      }
      
      bool alrSet[W.cols()];
      for(unsigned int i=0; i<W.cols(); i++){
        alrSet[i]=false;
      }
      for(unsigned int i=0; i<blobs.size(); i++){
        for(unsigned int j=0; j<blobs.at(i).size(); j++){
          alrSet[blobs.at(i).at(j)]=true;
        }
      }
      for(unsigned int i=0; i<W.cols(); i++){
        if(alrSet[i]==false){
          std::vector<int> f;
          f.push_back(i);
          blobs.push_back(f);
        }
      }
      
      int comp[cluster.size()];
      
      for(unsigned int x=0; x<blobs.size(); x++){
        for(unsigned int y=0; y<blobs.at(x).size(); y++){
          comp[blobs.at(x).at(y)]=x+1;
        }
      }
      for(int i=0; i<w*h; i++){
        if(assignment[i]>0){
          assignment[i]=comp[assignment[i]-1];
        }
      }      
    }
    
    
    void Segmentation3D::calculateRemainingPoints(){          
      int numCluster=cluster.size();
      int currentClusterSize=cluster.size();
      for(int y=0;y<h;++y){
        for(int x=0;x<w;++x){
          int i = x+w*y;
          if(elements[i]==true){ 
	          elements[i]=false;
	          numCluster++;
	          assignmentRemaining[i]=numCluster;
	          std::vector<int> data;
	          data.push_back(i);
	          checkNeighbourDistanceRemaining(x,y,assignmentRemaining[i], &data);
            cluster.push_back(data);
          }
        }    			
      }
      for(unsigned int x=currentClusterSize; x<cluster.size(); x++){ //determine neighbours
        std::vector<int> nb;
        for(unsigned int y=0; y<cluster.at(x).size(); y++){
          if((int)cluster.at(x).at(y)-1>=0){
            if(checkNotExist(assignment[cluster.at(x).at(y)-1], nb) && dist3(xyzData[cluster.at(x).at(y)], xyzData[cluster.at(x).at(y)-1])<BLOBSeuclDistance){
              nb.push_back(assignment[cluster.at(x).at(y)-1]);
            }
          }
          if((int)cluster.at(x).at(y)+1<w*h){
            if(checkNotExist(assignment[cluster.at(x).at(y)+1], nb) && dist3(xyzData[cluster.at(x).at(y)], xyzData[cluster.at(x).at(y)+1])<BLOBSeuclDistance){
              nb.push_back(assignment[cluster.at(x).at(y)+1]);
            }
          }
          if((int)cluster.at(x).at(y)-w>=0){
            if(checkNotExist(assignment[cluster.at(x).at(y)-w], nb) && dist3(xyzData[cluster.at(x).at(y)], xyzData[cluster.at(x).at(y)-w])<BLOBSeuclDistance){
              nb.push_back(assignment[cluster.at(x).at(y)-w]);
            }
          }
          if((int)cluster.at(x).at(y)+w<w*h){
            if(checkNotExist(assignment[cluster.at(x).at(y)+w], nb) && dist3(xyzData[cluster.at(x).at(y)], xyzData[cluster.at(x).at(y)+w])<BLOBSeuclDistance){
              nb.push_back(assignment[cluster.at(x).at(y)+w]);
            }
          }
          if((int)cluster.at(x).at(y)+w-1<w*h){
            if(checkNotExist(assignment[cluster.at(x).at(y)+w-1], nb) && dist3(xyzData[cluster.at(x).at(y)], xyzData[cluster.at(x).at(y)+w-1])<BLOBSeuclDistance){
              nb.push_back(assignment[cluster.at(x).at(y)+w-1]);
            }
          }
          if((int)cluster.at(x).at(y)+w+1<w*h){
            if(checkNotExist(assignment[cluster.at(x).at(y)+w+1], nb) && dist3(xyzData[cluster.at(x).at(y)], xyzData[cluster.at(x).at(y)+w+1])<BLOBSeuclDistance){
              nb.push_back(assignment[cluster.at(x).at(y)+w+1]);
            }
          }
          if((int)cluster.at(x).at(y)-w-1>=0){
            if(checkNotExist(assignment[cluster.at(x).at(y)-w-1], nb) && dist3(xyzData[cluster.at(x).at(y)], xyzData[cluster.at(x).at(y)-w-1])<BLOBSeuclDistance){
              nb.push_back(assignment[cluster.at(x).at(y)-w-1]);
            }
          }
          if((int)cluster.at(x).at(y)-w+1>=0){
            if(checkNotExist(assignment[cluster.at(x).at(y)-w+1], nb) && dist3(xyzData[cluster.at(x).at(y)], xyzData[cluster.at(x).at(y)-w+1])<BLOBSeuclDistance){
              nb.push_back(assignment[cluster.at(x).at(y)-w+1]);
            }
          }
        }

        if(nb.size()==0){ //no neighbours -> new blob
          std::vector<int> blob;
          blob.push_back(x);
          blobs.push_back(blob);
          for(unsigned int i=0; i<cluster.at(x).size(); i++){
            assignment[cluster.at(x).at(i)]=blobs.size();
          }
        }
           
        else if(nb.size()==1 && cluster.at(x).size()<15){ //very small -> assign
          blobs.at(nb.at(0)-1).push_back(x);
          for(unsigned int p=0; p<cluster.at(x).size(); p++){
            assignment[cluster.at(x).at(p)]=nb.at(0);
          }
        }
                    
        else if(nb.size()==1){
          bool assignElement=false;
          int tol=0;
          for(unsigned int a=0; a<cluster.at(x).size(); a++){
            int yy = (int)floor((float)cluster.at(x).at(a)/(float)w);
            int xx = cluster.at(x).at(a)-yy*w;          
            if(cluster.at(x).at(a)<w+1 || cluster.at(x).at(a)>w*h-w-1){
            }
            else{ 
              if(assignment[cluster.at(x).at(a)-1]!=nb.at(0) && assignmentRemaining[cluster.at(x).at(a)-1]!=(int)x+1 && depthImage(xx-1,yy,0)!=2047){
                if(tol<9){
                  tol++;
                }else{
                  assignElement=true;
                  break;
                }
              }
              if(assignment[cluster.at(x).at(a)+1]!=nb.at(0) && assignmentRemaining[cluster.at(x).at(a)+1]!=(int)x+1 && depthImage(xx+1,yy,0)!=2047){
                if(tol<9){
                  tol++;
                }else{
                  assignElement=true;
                  break;
                }
              }
              if(assignment[cluster.at(x).at(a)-w]!=nb.at(0) && assignmentRemaining[cluster.at(x).at(a)-w]!=(int)x+1 && depthImage(xx,yy-1,0)!=2047){
                if(tol<9){
                  tol++;
                }else{
                  assignElement=true;
                  break;
                }
              }
              if(assignment[cluster.at(x).at(a)+w]!=nb.at(0) && assignmentRemaining[cluster.at(x).at(a)+w]!=(int)x+1 && depthImage(xx,yy+1,0)!=2047){
                if(tol<9){
                  tol++;
                }else{
                  assignElement=true;
                  break;
                }
              }
              if(assignment[cluster.at(x).at(a)-w-1]!=nb.at(0) && assignmentRemaining[cluster.at(x).at(a)-w-1]!=(int)x+1 && depthImage(xx-1,yy-1,0)!=2047){
                if(tol<9){
                  tol++;
                }else{
                  assignElement=true;
                  break;
                }
              }
              if(assignment[cluster.at(x).at(a)-w+1]!=nb.at(0) && assignmentRemaining[cluster.at(x).at(a)-w+1]!=(int)x+1 && depthImage(xx+1,yy-1,0)!=2047){
                if(tol<9){
                  tol++;
                }else{
                  assignElement=true;
                  break;
                }
              }
              if(assignment[cluster.at(x).at(a)+w-1]!=nb.at(0) && assignmentRemaining[cluster.at(x).at(a)+w-1]!=(int)x+1 && depthImage(xx-1,yy+1,0)!=2047){
                if(tol<9){
                  tol++;
                }else{
                  assignElement=true;
                  break;
                }
              }
              if(assignment[cluster.at(x).at(a)+w+1]!=nb.at(0) && assignmentRemaining[cluster.at(x).at(a)+w+1]!=(int)x+1 && depthImage(xx+1,yy+1,0)!=2047){
                if(tol<9){
                  tol++;
                }else{
                  assignElement=true;
                  break;
                }
              }
            
            }
          }
          if(assignElement==false){ //new blob
            std::vector<int> blob;
            blob.push_back(x);
            blobs.push_back(blob);
            for(unsigned int i=0; i<cluster.at(x).size(); i++){
              assignment[cluster.at(x).at(i)]=blobs.size();
            }                
          }
          else{ //assign
            blobs.at(nb.at(0)-1).push_back(x);
            for(unsigned int p=0; p<cluster.at(x).size(); p++){
              assignment[cluster.at(x).at(p)]=nb.at(0);
            }
          }
        }    
             
        else if(nb.size()>1){
          int zuws[nb.size()];
          for(unsigned int a=0; a<blobs.size(); a++){
            for(unsigned int b=0; b<blobs.at(a).size(); b++){
              for(unsigned int c=0; c<nb.size(); c++){
                if(blobs.at(a).at(b)==nb.at(c)-1){
                  zuws[c]=a;
                }
              }
            }
          }
          bool same=true;
          for(unsigned int a=1; a<nb.size(); a++){
            if(zuws[a]!=zuws[0]){
              same=false;
            }
          }
          if(same==true){ //same blob->assign
            blobs.at(nb.at(0)-1).push_back(x);
            for(unsigned int p=0; p<cluster.at(x).size(); p++){
              assignment[cluster.at(x).at(p)]=nb.at(0);
            }
          }
          else{ //different blob -> determine best match
            //ToDo: find better solution
            
            /*float zuwsSize[nb.size()];
            for(unsigned int a=0; a<nb.size(); a++){
              zuwsSize[a]=0.0;
              Vec rPoint1;
              Vec n01;		
              int p0i=cluster.at(nb.at(a)-1).at(rand()%cluster.at(nb.at(a)-1).size());
              int p1i=cluster.at(nb.at(a)-1).at(rand()%cluster.at(nb.at(a)-1).size());
              int p2i=cluster.at(nb.at(a)-1).at(rand()%cluster.at(nb.at(a)-1).size());
              Vec fa1;
              Vec fb1;
              Vec n1;
            	fa1 = xyzData[p1i]-xyzData[p0i];
            	fb1 = xyzData[p2i]-xyzData[p0i];
              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);
          	 	rPoint1 = xyzData[p0i];
              float distance1 = rPoint1[0]*n01[0]+rPoint1[1]*n01[1]+ rPoint1[2]*n01[2];
              for(unsigned int p=0; p<cluster.at(x).size(); p++){
                float s1 = (xyzData[cluster.at(x).at(p)][0]*n01[0]+xyzData[cluster.at(x).at(p)][1]*n01[1]+xyzData[cluster.at(x).at(p)][2]*n01[2])-distance1;      
                zuwsSize[a]+=fabs(s1);
              }
            }
            int selection=0;
            for (unsigned int a=1; a<nb.size(); a++){
              if(zuwsSize[a]<zuwsSize[selection]){
                selection=a;
              }
            }
            int selectedBlob=0;
            for(unsigned int a=0; a<blobs.size(); a++){
              for(unsigned int b=0; b<blobs.at(a).size(); b++){
                if(blobs.at(a).at(b)==nb.at(selection)-1){
                  selectedBlob=a;
                }
              }
            }              
            blobs.at(nb.at(selection)-1).push_back(x);
            for(unsigned int p=0; p<cluster.at(x).size(); p++){
              assignment[cluster.at(x).at(p)]=nb.at(selection);
            }
          */
          }
        }
      } 
    }

    
    void Segmentation3D::blobSegmentation(){
      int maxID=0;
      unsigned int maxSize=0;
      for(unsigned int i=0; i<cluster.size(); i++){
        if(cluster.at(i).size()>maxSize){
          maxID=i;
          maxSize=cluster.at(i).size();
        }
      }
     
#ifdef HAVE_OPENCL
      int numPoints=cluster.at(maxID).size();
      int cAbove[RANSACpasses];
      int cBelow[RANSACpasses];
      int cOn[RANSACpasses];
      int cAboveRead[RANSACpasses];
      int cBelowRead[RANSACpasses];
#endif
      Vec n0[RANSACpasses];
      float dist[RANSACpasses];  
      int cOnRead[RANSACpasses];
      
      for(int i=0; i<RANSACpasses; i++){
#ifdef HAVE_OPENCL
        cAbove[i]=0;
        cBelow[i]=0;
        cOn[i]=0;   
        cAboveRead[i]=0;
        cBelowRead[i]=0;
#endif
        cOnRead[i]=0;
        
        Vec rPoint1;
        Vec n01;		
        int p0i=cluster.at(maxID).at(rand()%cluster.at(maxID).size());
        int p1i=cluster.at(maxID).at(rand()%cluster.at(maxID).size());
        int p2i=cluster.at(maxID).at(rand()%cluster.at(maxID).size());
        Vec fa1;
        Vec fb1;
        Vec n1;
      	fa1 = xyzData[p1i]-xyzData[p0i];
      	fb1 = xyzData[p2i]-xyzData[p0i];
        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);
    	 	rPoint1 = xyzData[p0i];
        float distance1 = rPoint1[0]*n01[0]+rPoint1[1]*n01[1]+ rPoint1[2]*n01[2];
        dist[i]=distance1;
        n0[i]=n01;
      }
        
      if(useCL==true && clReady==true){
      #ifdef HAVE_OPENCL    
        try{         
          cl::Buffer RANSACpointsBuffer;
          cl_mem RANSACpointsMem = RANSACpointsBuffer();
          RANSACpointsBuffer = cl::Buffer(
                     context, 
                     CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
                     numPoints * sizeof(int), 
                     (void *) &cluster.at(maxID)[0]);
                     
          cl::Event waitEvent;
          cl::Buffer n0Buffer;
          cl::Buffer distBuffer;
          cl::Buffer countAboveBuffer;
          cl::Buffer countBelowBuffer;
          cl::Buffer countOnBuffer;

          cl_mem n0Mem = n0Buffer();
          cl_mem distMem = distBuffer();
          cl_mem countAboveMem = countAboveBuffer();
          cl_mem countBelowMem = countBelowBuffer();
          cl_mem countOnMem = countOnBuffer();
          
          xyzBuffer = cl::Buffer(
				           context, 
				           CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
				           w*h * sizeof(cl_float4), 
        				   (void *) &xyzData[0]);
          
          n0Buffer = cl::Buffer(
                   context, 
                   CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
                   RANSACpasses * sizeof(cl_float4), 
                   (void *) &n0[0]);
          distBuffer = cl::Buffer(
                   context, 
                   CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
                   RANSACpasses * sizeof(float), 
                   (void *) &dist[0]); 
          countAboveBuffer = cl::Buffer(
                   context, 
                   CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, 
                   RANSACpasses * sizeof(int), 
                   (void *) &cAbove[0]);
          countBelowBuffer = cl::Buffer(
                   context, 
                   CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, 
                   RANSACpasses * sizeof(int), 
                   (void *) &cBelow[0]);
          countOnBuffer = cl::Buffer(
                   context, 
                   CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, 
                   RANSACpasses * sizeof(int), 
                   (void *) &cOn[0]);

          kernelCheckRANSAC.setArg(0, xyzBuffer);//set parameter for kernel
          kernelCheckRANSAC.setArg(1, RANSACpasses);
          kernelCheckRANSAC.setArg(2, n0Buffer);
          kernelCheckRANSAC.setArg(3, distBuffer);
          kernelCheckRANSAC.setArg(4, countAboveBuffer);
          kernelCheckRANSAC.setArg(5, countBelowBuffer);
          kernelCheckRANSAC.setArg(6, countOnBuffer);
          kernelCheckRANSAC.setArg(7, RANSACeuclDistance/2);
          kernelCheckRANSAC.setArg(8, numPoints);
          kernelCheckRANSAC.setArg(9, RANSACpointsBuffer);
          kernelCheckRANSAC.setArg(10, RANSACsubset);
                        
          int idSize=RANSACpasses*numPoints/RANSACsubset;
          queue.enqueueNDRangeKernel(//run kernel
             kernelCheckRANSAC, 
             cl::NullRange, 
             cl::NDRange(idSize), //input size for get global id
             cl::NullRange,
             NULL,
             &waitEvent);
    	                	    
          queue.enqueueReadBuffer(//read output from kernel
              countAboveBuffer,
              CL_TRUE, // block 
              0,
              RANSACpasses * sizeof(int),
              (int*) cAboveRead,
              NULL,&waitEvent);
                         
          queue.enqueueReadBuffer(//read output from kernel
              countBelowBuffer,
              CL_TRUE, // block 
              0,
              RANSACpasses * sizeof(int),
              (int*) cBelowRead,
              NULL,&waitEvent);
                      
          queue.enqueueReadBuffer(//read output from kernel
              countOnBuffer,
              CL_TRUE, // block 
              0,
              RANSACpasses * sizeof(int),
              (int*) cOnRead,
              NULL,&waitEvent);
           
          clFinish(queue()); 
            
          clReleaseMemObject(n0Mem);
          clReleaseMemObject(distMem);
          clReleaseMemObject(countAboveMem);
          clReleaseMemObject(countBelowMem);
          clReleaseMemObject(countOnMem);
    
          clReleaseMemObject(RANSACpointsMem);  
        
        }catch (cl::Error err) {//catch openCL errors
          std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<<std::endl;
        }       
      #endif
      }
      else{             
        for(int p=0; p<RANSACpasses; p++){
          for(unsigned int q=0; q<cluster.at(maxID).size(); q++){
            Vec n01 = n0[p];
            float s1 = (xyzData[cluster.at(maxID).at(q)][0]*n01[0]+xyzData[cluster.at(maxID).at(q)][1]*n01[1]+xyzData[cluster.at(maxID).at(q)][2]*n01[2])-dist[p];      
            if((s1>=-RANSACeuclDistance/2 && s1<=RANSACeuclDistance/2)){
              cOnRead[p]++;
            }  
          }
        }
      }
      int maxMatch=0;
      int maxMatchID=0;
      for(int i=0;i<RANSACpasses; i++){
        if(cOnRead[i]>maxMatch){
          maxMatch=cOnRead[i];
          maxMatchID=i;
        }
      }
	    
	    if(useCL==true && clReady==true){
      #ifdef HAVE_OPENCL
        try{           
          elementsBlobsBuffer = cl::Buffer(
				           context, 
				           CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, 
				           w*h * sizeof(bool), 
				           (void *) &elementsBlobs[0]);
				  assignmentBlobsBuffer = cl::Buffer(
				           context, 
				           CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
				           w*h * sizeof(int), 
				           (void *) &assignmentBlobs[0]);
				           
				  assignmentBuffer = cl::Buffer(
				           context, 
				           CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
				           w*h * sizeof(int), 
				           (void *) &assignment[0]);
                  
          kernelAssignRANSAC.setArg(0, xyzBuffer);//set parameter for kernel
          kernelAssignRANSAC.setArg(1, elementsBlobsBuffer);
          kernelAssignRANSAC.setArg(2, assignmentBlobsBuffer);
          kernelAssignRANSAC.setArg(3, n0[maxMatchID]);
          kernelAssignRANSAC.setArg(4, dist[maxMatchID]);
          kernelAssignRANSAC.setArg(5, RANSACeuclDistance);
          kernelAssignRANSAC.setArg(6, assignmentBuffer);
          kernelAssignRANSAC.setArg(7, maxID+1);
          
         
          queue.enqueueNDRangeKernel(//run kernel
				     kernelAssignRANSAC, 
				     cl::NullRange, 
				     cl::NDRange(w*h), //input size for get global id
				     cl::NullRange);
    	    			          
		      queue.enqueueReadBuffer(//read output from kernel
              assignmentBlobsBuffer,
              CL_TRUE, // block 
              0,
              w*h * sizeof(int),
              (int*) assignmentBlobs);
          
          queue.enqueueReadBuffer(//read output from kernel
              elementsBlobsBuffer,
              CL_TRUE, // block 
              0,
              w*h * sizeof(bool),
              (bool*) elementsBlobs);
        
        }catch (cl::Error err) {//catch openCL errors
          std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<<std::endl;
        }       
      #endif
      }
      else{                  
        for(int i=0; i<w*h; i++){
          if(assignment[i]==maxID+1){
            assignmentBlobs[i]=1;
            elementsBlobs[i]=false;
          }else{
            Vec n01 = n0[maxMatchID];
            float s1 = (xyzData[i][0]*n01[0]+xyzData[i][1]*n01[1]+xyzData[i][2]*n01[2])-dist[maxMatchID];  
            if((s1>=-RANSACeuclDistance && s1<=RANSACeuclDistance) && elementsBlobs[i]==true){
              assignmentBlobs[i]=1;
              elementsBlobs[i]=false;
            }
          }
        }    
      }
      	    
      regionGrowBlobs();    
      assignment=assignmentBlobs;
    }
    
    
    void Segmentation3D::colorPointcloud(){
  	  if(useCL==true && clReady==true){
      #ifdef HAVE_OPENCL
        try{  
          assignmentBuffer = cl::Buffer(
				         context, 
				         CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 
				         w*h * sizeof(int), 
				         (void *) &assignment[0]);
				           
          kernelSegmentColoring.setArg(0, assignmentBuffer);//set parameter for kernel
          kernelSegmentColoring.setArg(1, segmentColorImageRBuffer);
          kernelSegmentColoring.setArg(2, segmentColorImageGBuffer);
          kernelSegmentColoring.setArg(3, segmentColorImageBBuffer);
          
          queue.enqueueNDRangeKernel(//run kernel
				     kernelSegmentColoring, 
				     cl::NullRange, 
				     cl::NDRange(w*h), //input size for get global id
				     cl::NullRange);
				     
          queue.enqueueReadBuffer(//read output from kernel
              segmentColorImageRBuffer,
              CL_TRUE, // block 
              0,
              w*h * sizeof(cl_uchar),
              (cl_uchar*) segmentColorImageRArray);
              
          queue.enqueueReadBuffer(//read output from kernel
              segmentColorImageGBuffer,
              CL_TRUE, // block 
              0,
              w*h * sizeof(cl_uchar),
              (cl_uchar*) segmentColorImageGArray);
              
          queue.enqueueReadBuffer(//read output from kernel
              segmentColorImageBBuffer,
              CL_TRUE, // block 
              0,
              w*h * sizeof(cl_uchar),
              (cl_uchar*) segmentColorImageBArray);
              
          std::vector<icl8u*> data(3);
          data[0] = segmentColorImageRArray; 
          data[1] = segmentColorImageGArray;
          data[2] = segmentColorImageBArray; 
          segmentColorImage = Img8u(Size(w,h),3,data,false);

        }catch (cl::Error err) {//catch openCL errors
          std::cout<< "ERROR: "<< err.what()<< "("<< err.err()<< ")"<<std::endl;
        }
      #endif
  	  }else{
        for(int y=0; y<h; y++){
          for(int x=0; x<w; x++){
            int i=x+w*y;
            if(assignment[i]==0){
              segmentColorImage(x,y,0)=128;
              segmentColorImage(x,y,1)=128;
              segmentColorImage(x,y,2)=128;
            }else{            
              int H=(int)(assignment[i]*35.)%360;
              float S=1.0-assignment[i]*0.01;
              float hi=floor((float)H/60.);
			        float f=((float)H/60.)-hi;
			        float pp=1.0-S;
			        float qq=1.0-S*f;
			        float tt=1.0-S*(1.-f);
			        float newR=0;
			        float newG=0;
			        float newB=0;
			        if((int)hi==0 || (int)hi==6){
			          newR=1.0;
			          newG=tt;
			          newB=pp;
			        }else if((int)hi==1){
			          newR=qq;
			          newG=1.0;
			          newB=pp;
			        }else if((int)hi==2){
			          newR=pp;
			          newG=1.0;
			          newB=tt;
			        }else if((int)hi==3){
			          newR=pp;
			          newG=qq;
			          newB=1.0;
			        }else if((int)hi==4){
			          newR=tt;
			          newG=pp;
			          newB=1.0;
			        }else if((int)hi==5){
			          newR=1.0;
			          newG=pp;
			          newB=qq;
			        }
	            segmentColorImage(x,y,0)=(unsigned char)(newR*255.);
	            segmentColorImage(x,y,1)=(unsigned char)(newG*255.);
	            segmentColorImage(x,y,2)=(unsigned char)(newB*255.);
	          }
	        } 
        }
      }
    }
    
    
    	    
	  void Segmentation3D::checkNeighbourGrayThreshold(int x, int y, int zuw, int threshold, std::vector<int> *data){
	    std::vector<int> toProcessX;
	    std::vector<int> toProcessY;
	    bool process=true;
	    unsigned int index=0;
		  toProcessX.push_back(x);
		  toProcessY.push_back(y);
		
	    while(process==true){
	      int i = toProcessX.at(index)+w*toProcessY.at(index);
	      x = toProcessX.at(index);
	      y = toProcessY.at(index);
		     
		    if(elements[i+1]==true && x+1<w && normalEdgeImage(x+1,y,0)==threshold){
			    elements[i+1]=false;
			    assignment[i+1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)+1);
			    toProcessY.push_back(toProcessY.at(index));
			    data->push_back(i+1);
		    }
		    if(elements[i-1]==true && x-1>=0 && normalEdgeImage(x-1,y,0)==threshold){
			    elements[i-1]=false;
			    assignment[i-1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)-1);
			    toProcessY.push_back(toProcessY.at(index));
			    data->push_back(i-1);
		    }
		    if(elements[i+w]==true && y+1<h && normalEdgeImage(x,y+1,0)==threshold){
			    elements[i+w]=false;
			    assignment[i+w]=zuw;
			    toProcessX.push_back(toProcessX.at(index));
			    toProcessY.push_back(toProcessY.at(index)+1);
			    data->push_back(i+w);
		    }
		    if(elements[i+w-1]==true && x-1>=0 && y+1<h && normalEdgeImage(x-1,y+1,0)==threshold){
			    elements[i+w-1]=false;
			    assignment[i+w-1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)-1);
			    toProcessY.push_back(toProcessY.at(index)+1);
			    data->push_back(i+w-1);
		    }
		    if(elements[i+w+1]==true && x+1<w && y+1<h && normalEdgeImage(x+1,y+1,0)==threshold){
			    elements[i+w+1]=false;
			    assignment[i+w+1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)+1);
			    toProcessY.push_back(toProcessY.at(index)+1);
			    data->push_back(i+w+1);
		    }
		    if(elements[i-w]==true && y-1>=0 && normalEdgeImage(x,y-1,0)==threshold){
			    elements[i-w]=false;
			    assignment[i-w]=zuw;
			    toProcessX.push_back(toProcessX.at(index));
			    toProcessY.push_back(toProcessY.at(index)-1);
			    data->push_back(i-w);
		    }
		    if(elements[i-w-1]==true && x-1>=0 && y-1>=0 && normalEdgeImage(x-1,y-1,0)==threshold){
			    elements[i-w-1]=false;
			    assignment[i-w-1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)-1);
			    toProcessY.push_back(toProcessY.at(index)-1);
			    data->push_back(i-w-1);
		    }
		    if(elements[i-w+1]==true && x+1<w && y-1>=0 && normalEdgeImage(x+1,y-1,0)==threshold){
			    elements[i-w+1]=false;
			    assignment[i-w+1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)+1);
			    toProcessY.push_back(toProcessY.at(index)-1);
			    data->push_back(i-w+1);
		    } 
		      
        index++;
        if(index>=toProcessX.size()){
          process=false;
        }
      }
	  }
	   
    
    void Segmentation3D::checkNeighbourDistanceRemaining(int x, int y, int zuw, std::vector<int> *data){
	    std::vector<int> toProcessX;
	    std::vector<int> toProcessY;
	    bool process=true;
	    unsigned int index=0;
		  toProcessX.push_back(x);
		  toProcessY.push_back(y);
		
	    while(process==true){
	      int i = toProcessX.at(index)+w*toProcessY.at(index);
	      x = toProcessX.at(index);
	      y = toProcessY.at(index);
	       
		    if(elements[i+1]==true && x+1<w && dist3(xyzData[i],xyzData[i+1])<BLOBSeuclDistance){
			    elements[i+1]=false;
			    assignmentRemaining[i+1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)+1);
			    toProcessY.push_back(toProcessY.at(index));
			    data->push_back(i+1);
		    }
		    if(elements[i-1]==true && x-1>=0 && dist3(xyzData[i],xyzData[i-1])<BLOBSeuclDistance){
			    elements[i-1]=false;
			    assignmentRemaining[i-1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)-1);
			    toProcessY.push_back(toProcessY.at(index));
			    data->push_back(i-1);
		    }
		    if(elements[i+w]==true && y+1<h && dist3(xyzData[i],xyzData[i+w])<BLOBSeuclDistance){
			    elements[i+w]=false;
			    assignmentRemaining[i+w]=zuw;
			    toProcessX.push_back(toProcessX.at(index));
			    toProcessY.push_back(toProcessY.at(index)+1);
			    data->push_back(i+w);
		    }
		    if(elements[i+w-1]==true && x-1>=0 && y+1<h && dist3(xyzData[i],xyzData[i+w-1])<BLOBSeuclDistance){
			    elements[i+w-1]=false;
			    assignmentRemaining[i+w-1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)-1);
			    toProcessY.push_back(toProcessY.at(index)+1);
			    data->push_back(i+w-1);
		    }
		    if(elements[i+w+1]==true && x+1<w && y+1<h && dist3(xyzData[i],xyzData[i+w+1])<BLOBSeuclDistance){
			    elements[i+w+1]=false;
			    assignmentRemaining[i+w+1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)+1);
			    toProcessY.push_back(toProcessY.at(index)+1);
			    data->push_back(i+w+1);
		    }
		    if(elements[i-w]==true && y-1>=0 && dist3(xyzData[i],xyzData[i-w])<BLOBSeuclDistance){
			    elements[i-w]=false;
			    assignmentRemaining[i-w]=zuw;
			    toProcessX.push_back(toProcessX.at(index));
			    toProcessY.push_back(toProcessY.at(index)-1);
			    data->push_back(i-w);
		    }
		    if(elements[i-w-1]==true && x-1>=0 && y-1>=0 && dist3(xyzData[i],xyzData[i-w-1])<BLOBSeuclDistance){
			    elements[i-w-1]=false;
			    assignmentRemaining[i-w-1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)-1);
			    toProcessY.push_back(toProcessY.at(index)-1);
			    data->push_back(i-w-1);
		    }
		    if(elements[i-w+1]==true && x+1<w && y-1>=0 && dist3(xyzData[i],xyzData[i-w+1])<BLOBSeuclDistance){
			    elements[i-w+1]=false;
			    assignmentRemaining[i-w+1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)+1);
			    toProcessY.push_back(toProcessY.at(index)-1);
			    data->push_back(i-w+1);
		    } 
		      
        index++;
        if(index>=toProcessX.size()){
          process=false;
        }
      }
	  }


    void Segmentation3D::regionGrowBlobs(){     
      int numCluster=1;
      std::vector<std::vector<int> > remove;
      for(int y=0;y<h;++y){
	      for(int x=0;x<w;++x){
		      int i = x+w*y;
		      if(elementsBlobs[i]==true){
			      elementsBlobs[i]=false;
			      numCluster++;
			      assignmentBlobs[i]=numCluster;
			      std::vector<int> data;
			      data.push_back(i);
			      checkNeighbourDistance(x,y,assignmentBlobs[i], &data);
			      if(data.size()<minClusterSize){
			        remove.push_back(data);
			        numCluster--;
			      }
		      }
        }    			
      }
      for(unsigned int i=0; i<remove.size(); i++){
        for(unsigned int j=0; j<remove.at(i).size(); j++){
          elementsBlobs[remove.at(i).at(j)]=true;
          assignmentBlobs[remove.at(i).at(j)]=0;
        }
      }
    }


    void Segmentation3D::checkNeighbourDistance(int x, int y, int zuw, std::vector<int> *data){
	    std::vector<int> toProcessX;
	    std::vector<int> toProcessY;
	    bool process=true;
	    unsigned int index=0;
		  toProcessX.push_back(x);
		  toProcessY.push_back(y);
		
	    while(process==true){
	      int i = toProcessX.at(index)+w*toProcessY.at(index);
	      x = toProcessX.at(index);
	      y = toProcessY.at(index);
	       
		    if(elementsBlobs[i+1]==true && x+1<w && fabs(depthImage(x,y,0)-depthImage(x+1,y,0))<BLOBSeuclDistance){
			    elementsBlobs[i+1]=false;
			    assignmentBlobs[i+1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)+1);
			    toProcessY.push_back(toProcessY.at(index));
			    data->push_back(i+1);
		    }
		    if(elementsBlobs[i-1]==true && x-1>=0 && fabs(depthImage(x,y,0)-depthImage(x-1,y,0))<BLOBSeuclDistance){
			    elementsBlobs[i-1]=false;
			    assignmentBlobs[i-1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)-1);
			    toProcessY.push_back(toProcessY.at(index));
			    data->push_back(i-1);
		    }
		    if(elementsBlobs[i+w]==true && y+1<h && fabs(depthImage(x,y,0)-depthImage(x,y+1,0))<BLOBSeuclDistance){
			    elementsBlobs[i+w]=false;
			    assignmentBlobs[i+w]=zuw;
			    toProcessX.push_back(toProcessX.at(index));
			    toProcessY.push_back(toProcessY.at(index)+1);
			    data->push_back(i+w);
		    }
		    if(elementsBlobs[i+w-1]==true && x-1>=0 && y+1<h && fabs(depthImage(x,y,0)-depthImage(x-1,y+1,0))<BLOBSeuclDistance){
			    elementsBlobs[i+w-1]=false;
			    assignmentBlobs[i+w-1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)-1);
			    toProcessY.push_back(toProcessY.at(index)+1);
			    data->push_back(i+w-1);
		    }
		    if(elementsBlobs[i+w+1]==true && x+1<w && y+1<h && fabs(depthImage(x,y,0)-depthImage(x+1,y+1,0))<BLOBSeuclDistance){
			    elementsBlobs[i+w+1]=false;
			    assignmentBlobs[i+w+1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)+1);
			    toProcessY.push_back(toProcessY.at(index)+1);
			    data->push_back(i+w+1);
		    }
		    if(elementsBlobs[i-w]==true && y-1>=0 && fabs(depthImage(x,y,0)-depthImage(x,y-1,0))<BLOBSeuclDistance){
			    elementsBlobs[i-w]=false;
			    assignmentBlobs[i-w]=zuw;
			    toProcessX.push_back(toProcessX.at(index));
			    toProcessY.push_back(toProcessY.at(index)-1);
			    data->push_back(i-w);
		    }
		    if(elementsBlobs[i-w-1]==true && x-1>=0 && y-1>=0 && fabs(depthImage(x,y,0)-depthImage(x-1,y-1,0))<BLOBSeuclDistance){
			    elementsBlobs[i-w-1]=false;
			    assignmentBlobs[i-w-1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)-1);
			    toProcessY.push_back(toProcessY.at(index)-1);
			    data->push_back(i-w-1);
		    }
		    if(elementsBlobs[i-w+1]==true && x+1<w && y-1>=0 && fabs(depthImage(x,y,0)-depthImage(x+1,y-1,0))<BLOBSeuclDistance){
			    elementsBlobs[i-w+1]=false;
			    assignmentBlobs[i-w+1]=zuw;
			    toProcessX.push_back(toProcessX.at(index)+1);
			    toProcessY.push_back(toProcessY.at(index)-1);
			    data->push_back(i-w+1);
		    } 
		      
        index++;
        if(index>=toProcessX.size()){
          process=false;
        }
      }
	  }
 

    bool Segmentation3D::checkNotExist(int zw, std::vector<int> &nb){   
      if(zw!=0){
        for(unsigned int z=0; z<nb.size(); z++){
          if(nb.at(z)==zw){
            return false;
          }
        }
        return true;
      }
      return false;
    }


    float Segmentation3D::dist3(const Vec &a, const Vec &b){
      return norm3(a-b);
	  }
 
  } // namespace geom
}