 
   const cv::KeyPoint *tmpg = &pts[first];
 
-  for (int i = threadIdx.x; i < 8 * BitonicSortThreads;
+  for (int i = threadIdx.x; i < 8192;
        i += BitonicSortThreads) {
     if (i < nkpts) {
       shm[i].idx = i;
       shm[i].y = __float2half(tmpg[i].pt.y);
       shm[i].x = __float2half(tmpg[i].pt.x);
     } else {
-      shm[i].idx = i;
+      shm[i].idx = -1;
       shm[i].y = __float2half(0.f);
       shm[i].x = __float2half(0.f);
     }
   }
   __syncthreads();
 
-  for (int i = 1; i < 2 * BitonicSortThreads; i <<= 1) {
-      int tx = threadIdx.x;
-      for (int ti=0; ti<4; ++ti) {
-	  int sortdir = (tx & i) > 0 ? 0 : 1;
-	  for (int j = i; j > 0; j >>= 1) {
-	      int mask = 0x0fffffff * j;
+
+  for (int i=1; i<8192; i <<= 1) {
+      for (int j=i; j>0; j >>= 1) {
+	  int tx = threadIdx.x;
+	  int mask = 0x0fffffff * j;
+	  for (int idx=0; idx<4096; idx+=BitonicSortThreads) {
+	      int sortdir = (tx & i) > 0 ? 0 : 1;
 	      int tidx = ((tx & mask) << 1) + (tx & ~mask);
-	      atomicSort(shm, tidx, j, j * sortdir);
+	      atomicSort(shm, tidx, j, j*sortdir);
+	      tx += BitonicSortThreads;
 	      __syncthreads();
 	  }
-	  tx += BitonicSortThreads;
-      }
-  }
-
-  /*for (int i = 1; i < 2 * BitonicSortThreads; i <<= 1) {
-    int sortdir = (threadIdx.x & i) > 0 ? 0 : 1;
-    for (int j = i; j > 0; j >>= 1) {
-      int mask = 0x0fffffff * j;
-      int tidx = ((threadIdx.x & mask) << 1) + (threadIdx.x & ~mask);
-      atomicSort(shm, tidx, j, j * sortdir);
-      __syncthreads();
-    }
-    }*/
-
+      }
+  }
+  
 
   cv::KeyPoint *tmpnewg = &newpts[first];
-  for (int i = 0; i < 8 * BitonicSortThreads * sizeof(T) / sizeof(int);
-       i += BitonicSortThreads) {
+  for (int i = 0; i < 8192; i += BitonicSortThreads) {
     if (i + threadIdx.x < nkpts) {
       tmpnewg[i + threadIdx.x].angle = tmpg[shm[i + threadIdx.x].idx].angle;
-      tmpnewg[i + threadIdx.x].class_id =
-          tmpg[shm[i + threadIdx.x].idx].class_id;
+      tmpnewg[i + threadIdx.x].class_id = tmpg[shm[i + threadIdx.x].idx].class_id;
       tmpnewg[i + threadIdx.x].octave = tmpg[shm[i + threadIdx.x].idx].octave;
       tmpnewg[i + threadIdx.x].pt.y = tmpg[shm[i + threadIdx.x].idx].pt.y;
       tmpnewg[i + threadIdx.x].pt.x = tmpg[shm[i + threadIdx.x].idx].pt.x;

  }
}

#define FindNeighborsThreads 512
__global__ void FindNeighbors(cv::KeyPoint *pts, short *kptindices, int width) {
  __shared__ int gidx[1];

  // which scale?
  int scale = pts[blockIdx.x].class_id;

  int cmpIdx = scale < 2 ? 0 : d_ExtremaIdx[scale - 2];

  float size = pts[blockIdx.x].size;


  dim3 blocks(16, 1, 1);
  dim3 threads(BitonicSortThreads, 1, 1);
  bitonicSort << <blocks, threads>>> (pts, newpts);

  /*unsigned int extremaidx_h[16];
  cudaMemcpyFromSymbol(extremaidx_h,d_ExtremaIdx,16*sizeof(unsigned int));
  for (int i=0; i<16; ++i) {
      std::cout << i << ": " << extremaidx_h[i] << std::endl;
  }
  */
  
/*  cv::KeyPoint* newpts_h = new cv::KeyPoint[nump];
  cudaMemcpy(newpts_h,newpts,nump*sizeof(cv::KeyPoint),cudaMemcpyDeviceToHost);

  int scale = 0;
  for (int i=1; i<nump; ++i) {
      cv::KeyPoint &k0 = newpts_h[i-1];
      cv::KeyPoint &k1 = newpts_h[i];

      std::cout << i << ": " << newpts_h[i].class_id << ": " << newpts_h[i].pt.y << " " << newpts_h[i].pt.x << ", " << newpts_h[i].size;

      if (!(k0.pt.y<k1.pt.y || (k0.pt.y==k1.pt.y && k0.pt.x<k1.pt.x))) std::cout << "  <<<<";
      if (k1.size < 0 ) std::cout << "  ##############";
       

      std::cout << "\n";

  }
*/

    // Find all neighbors
  cudaStreamSynchronize(copyStream);
  blocks.x = nump;
  threads.x = FindNeighborsThreads;
  FindNeighbors << <blocks, threads>>> (newpts, kptindices, width);
  //cudaDeviceSynchronize();
  //safeCall(cudaGetLastError());
  
  // Filter extrema
  blocks.x = 1;
