 
       int W = depth.cols();
       int H = depth.rows();
-      
+
       cudaChannelFormatDesc channeldesc = cudaCreateChannelDesc<unsigned short>();
 
       depthTex.addressMode[0] = cudaAddressModeClamp;
       cudaSafeCall( cudaBindTexture2D(0, depthTex, depth.ptr(), channeldesc, W, H, depth.step()) );
-        
 
       dim3 block(16, 16);
       dim3 grid( divUp(W, block.x), divUp(H, block.y) );
-      
+
       KernelCUDA_runTree<<< grid, block >>>( W, H, focal, treeHeight, numNodes, 
                                                        (const Node*)  nodes_device, 
                                                        (const Label*) leaves_device, 


namespace pcl
{
    namespace device
    {        

      texture<unsigned short, 2, cudaReadModeElementType> depthTex;
      texture<unsigned short, 2, cudaReadModeElementType> maskTex;
      texture<char4, 2, cudaReadModeElementType> multilabelTex;


      cudaSafeCall( cudaGetLastError() );
      cudaSafeCall( cudaThreadSynchronize() );

      cudaSafeCall( cudaUnbindTexture(depthTex) );
      cudaSafeCall( cudaUnbindTexture(maskTex) );
    }
  }
}

///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////

namespace pcl
{
  namespace device
  {
    __global__ void KernelCUDA_MultiTreePass( const int    treeId,
                                              const int    W,
                                              const int    H,
                                              const float  f,
                                              const int    treeHeight,
                                              const int    numNodes,
                                              const Node*  nodes,
                                              const Label* leaves,
                                              Label*       multiLabels)
    {
      uint u = blockIdx.x * blockDim.x + threadIdx.x;
      uint v = blockIdx.y * blockDim.y + threadIdx.y;

      if( u >=W ) return;
      if( v >=H ) return;

      // init
      int    depth = tex2D(depthTex, u,v );
      float  scale = f/float(depth);

      // go down the tree
      int nid = 0;
      for(int nodeDepth=0;nodeDepth<treeHeight;++nodeDepth)
      {
        const Node& node = nodes[nid];
        const AttribLocation& loc = node.loc;
        int d1 = tex2D(depthTex, u+float(loc.du1)*scale, v+float(loc.dv1)*scale);
        int d2 = tex2D(depthTex, u+float(loc.du2)*scale, v+float(loc.dv2)*scale);
        int delta = d1-d2;
        bool test = delta > int(node.thresh);
        if( test ) nid = nid*2+2;
        else       nid = nid*2+1;
      }

      // we try to synchronize the write
      __syncthreads();
      multiLabels[(v*W+u)*4+treeId] = leaves[nid-numNodes];
    }

    __global__ void KernelCUDA_MultiTreePassFG( const int    treeId,
                                              const int    FGThresh,
                                              const int    W,
                                              const int    H,
                                              const float  f,
                                              const int    treeHeight,
                                              const int    numNodes,
                                              const Node*  nodes,
                                              const Label* leaves,
                                              Label*       multiLabels)
    {
      uint u = blockIdx.x * blockDim.x + threadIdx.x;
      uint v = blockIdx.y * blockDim.y + threadIdx.y;

      if( u >=W ) return;
      if( v >=H ) return;

      // init
      int    depth = tex2D(depthTex, u,v );
      float  scale = f/float(depth);

      // go down the tree
      int nid = 0;
      for(int nodeDepth=0;nodeDepth<treeHeight;++nodeDepth)
      {
        const Node& node = nodes[nid];
        const AttribLocation& loc = node.loc;
        int d1 = tex2D(depthTex, u+float(loc.du1)*scale, v+float(loc.dv1)*scale);
        int d2 = tex2D(depthTex, u+float(loc.du2)*scale, v+float(loc.dv2)*scale);
        if( d1 - depth > FGThresh ) d1 = 32767; 
        if( d2 - depth > FGThresh ) d2 = 32767;
        int delta = d1-d2;
        bool test = delta > int(node.thresh);
        if( test ) nid = nid*2+2;
        else       nid = nid*2+1;
      }

      // we try to synchronize the write
      __syncthreads();
      multiLabels[(v*W+u)*4+treeId] = leaves[nid-numNodes];
    }

    __device__ int findMaxId( int numBins, char* bins ) 
    {
      // HACK .. not testing against numBins = 0
      int maxId   = 0;
      char maxVal = bins[0];
      for(int i=1;i<numBins;++i) 
      {
        char val = bins[i];
        if( val > maxVal ) { maxId = i; maxVal = val; }
      }
      return maxId;
    }

    //this will find the max Index but return -1 if there is a tie
    __device__ int findMaxId_testTie(int numBins, char* bins) 
    {
      int maxId = 0;
      int maxId_other = -1;
      char maxVal = bins[0];

      for(int i=1;i<numBins;++i) {
        char val = bins[i];
        if( val == maxVal ) { maxId_other = i; }
        if( val > maxVal ) { maxId = i; maxId_other = -1; maxVal = val; }
      }

      if( maxId_other != -1) return -1;
      else                   return maxId;
    }

    __global__ void KernelCUDA_MultiTreeMerge( const int    numTrees,
                                               const int    W,
                                               const int    H,
                                               PtrStep<Label> labels )
    {
      uint u = blockIdx.x * blockDim.x + threadIdx.x;
      uint v = blockIdx.y * blockDim.y + threadIdx.y;

      if( u >=W ) return;
      if( v >=H ) return;

      // reset the bins
      char bins[NUMLABELS];
      for(int li=0;li<NUMLABELS;++li) { bins[li] = 0; }

      // find a consensus with the current trees
      {
        char4 pixlabels = tex2D(multilabelTex, u ,v);
        char* bob = (char*)&pixlabels; //horrible but char4's have xyzw members
        for(int ti=0;ti<numTrees;++ti) {
          bins[ bob[ti] ]++;
        }
      }

      int res = findMaxId_testTie(NUMLABELS, bins);

      // if this fails... find a consensus in a 1 neighbourhood
      if( res < 0 ) {
        int depth = tex2D(depthTex, u,v);
        for(int i=-1;i<=1;++i) 
        {
          for(int j=-1;j<=1;++j) 
          {
            int   depth_neighbor  = tex2D(depthTex,u+i,v+j);
            char4 labels_neighbor = tex2D(multilabelTex, u+i,v+j); 
            char* bob = (char*)&labels_neighbor; //horrible but char4's have xyzw members
            int weight = abs(depth-depth_neighbor) < 50 ? 1:0; // 5cms
            for(int ti=0;ti<numTrees;++ti)
              bins[ bob[ti] ] += weight;
          }
        }
        res = findMaxId( NUMLABELS, bins );
      }
      __syncthreads();
      labels.ptr(v)[u] = res;
    }

    void CUDA_runMultiTreePassFG( int          treeId,
                                const int    FGThresh,
                                const float  focal,
                                const int    treeHeight,
                                const int    numNodes,
                                const void*  nodes_device,
                                const void*  leaves_device,
                                const Depth& depth,
                                void*        multilabel_device )
    {
      assert( treeId >= 0 );
      assert( treeId < 4 );

      int W = depth.cols();
      int H = depth.rows();

      depthTex.addressMode[0] = cudaAddressModeClamp;
      cudaChannelFormatDesc channeldesc = cudaCreateChannelDesc<unsigned short>();  
      cudaSafeCall( cudaBindTexture2D(0, depthTex, depth.ptr(), channeldesc, W, H, depth.step()) );

      dim3 block(16, 16);
      dim3 grid( divUp(W, block.x), divUp(H, block.y) );

      KernelCUDA_MultiTreePassFG<<< grid, block >>>( treeId, FGThresh, W, H, focal, treeHeight, numNodes,
                                                     (const Node*)  nodes_device, 
                                                     (const Label*) leaves_device, 
                                                     (Label*)       multilabel_device);

      cudaSafeCall( cudaGetLastError() );
      cudaSafeCall( cudaThreadSynchronize() );
      cudaSafeCall( cudaUnbindTexture(depthTex) );
    }

    void CUDA_runMultiTreePass( int          treeId,
                                const float  focal,
                                const int    treeHeight,
                                const int    numNodes,
                                const void*  nodes_device,
                                const void*  leaves_device,
                                const Depth& depth,
                                void*        multilabel_device )
    {
      assert( treeId >= 0 );
      assert( treeId < 4 );

      int W = depth.cols();
      int H = depth.rows();

      depthTex.addressMode[0] = cudaAddressModeClamp;
      cudaChannelFormatDesc channeldesc = cudaCreateChannelDesc<unsigned short>();  
      cudaSafeCall( cudaBindTexture2D(0, depthTex, depth.ptr(), channeldesc, W, H, depth.step()) );

      dim3 block(16, 16);
      dim3 grid( divUp(W, block.x), divUp(H, block.y) );

      KernelCUDA_MultiTreePass<<< grid, block >>>( treeId, W, H, focal, treeHeight, numNodes, 
                                                     (const Node*)  nodes_device, 
                                                     (const Label*) leaves_device, 
                                                     (Label*)       multilabel_device);

      cudaSafeCall( cudaGetLastError() );
      cudaSafeCall( cudaThreadSynchronize() );
      cudaSafeCall( cudaUnbindTexture(depthTex) );
    }

    void CUDA_runMultiTreeMerge( int          numTrees,
                                 const Depth& depth,
                                 void*        multilabel_device,
                                 Labels& labels)
    {
      labels.create(depth.rows(), depth.cols());

      assert( numTrees <= 4 );

      int W = depth.cols();
      int H = depth.rows();

      {
        depthTex.addressMode[0] = cudaAddressModeClamp;
        cudaChannelFormatDesc channeldesc = cudaCreateChannelDesc<unsigned short>();
        cudaSafeCall( cudaBindTexture2D(0, depthTex, depth.ptr(), channeldesc, W, H, depth.step()) );
      }
      {
        multilabelTex.addressMode[0] = cudaAddressModeClamp;
        cudaChannelFormatDesc channeldesc = cudaCreateChannelDesc<char4>();
        cudaSafeCall( cudaBindTexture2D(0, multilabelTex, multilabel_device, channeldesc, W, H, W*sizeof(char4)) );
      }

      dim3 block(16, 16);
      dim3 grid( divUp(W, block.x), divUp(H, block.y) );

      KernelCUDA_MultiTreeMerge<<< grid, block >>>( numTrees, W, H, labels );

      cudaSafeCall( cudaGetLastError() );
      cudaSafeCall( cudaThreadSynchronize() );
      cudaSafeCall( cudaUnbindTexture(depthTex) );
      cudaSafeCall( cudaUnbindTexture(multilabelTex) );
    }
  }
}

//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////

pcl::device::CUDATree::CUDATree(int treeHeight_arg, const vector<Node>& nodes, const vector<Label>& leaves)
{
  treeHeight = treeHeight_arg;
  numNodes = (1 << treeHeight) - 1;
  assert( nodes.size()  == (size_t)numNodes );
  assert( leaves.size() == (size_t)(1 << treeHeight) );

  nodes_device.upload(nodes);
  leaves_device.upload(leaves);
}

void pcl::device::MultiTreeLiveProc::process(const Depth& dmap, Labels& lmap)
{
  assert(!trees.empty());

  if (trees.size() == 1)
  {
    const CUDATree& t = trees[0];
    device::CUDA_runTree( focal, t.treeHeight, t.numNodes, t.nodes_device, t.leaves_device, dmap,  lmap );
    return;
  }

  process(dmap, lmap, std::numeric_limits<Attrib>::max());
}

void pcl::device::MultiTreeLiveProc::process(const Depth& dmap, Labels& lmap, int FGThresh)
{
  assert(!trees.empty());

  int numTrees = (int)trees.size();

  multilmap_device.create(dmap.cols() * dmap.rows() * numTrees);

  // 1 - run the multi passes  
  for( int ti = 0; ti < numTrees; ++ti ) 
  {
    const CUDATree& t = trees[ti];

    if( FGThresh == std::numeric_limits<Attrib>::max () ) 
    {
      device::CUDA_runMultiTreePass( ti, (float)focal, t.treeHeight, t.numNodes,
          t.nodes_device, t.leaves_device, dmap, multilmap_device );
    }
    else
    {
        device::CUDA_runMultiTreePassFG( ti, FGThresh, (float)focal, t.treeHeight, t.numNodes, t.nodes_device, t.leaves_device, dmap, multilmap_device );
    }
  }
  // 2 - run the merging 
  device::CUDA_runMultiTreeMerge(numTrees, dmap, multilmap_device, lmap);
}
