     double* h_matrixRT = new double[16];
     vtkMatrixToDoubleTable(currentData->GetMatrixTR(), h_matrixRT);
 
-    printf("id : %d (size : %d)\n", i, currentData->GetDepthMap()->GetNumberOfCells());
-
     CudaErrorCheck(cudaMemcpy(d_depthMap, h_depthMap, nbPixelOnDepthMap * sizeof(double), cudaMemcpyHostToDevice));
     CudaErrorCheck(cudaMemcpy(d_matrixK, h_matrixK, matrix4Size * sizeof(double), cudaMemcpyHostToDevice));
     CudaErrorCheck(cudaMemcpy(d_matrixRT, h_matrixRT, matrix4Size * sizeof(double), cudaMemcpyHostToDevice));
 
     // run code into device
     depthMapKernel << <dimGrid, dimBlock >> >(d_depthMap, d_matrixK, d_matrixRT, d_outScalar);
-
-    cudaError_t err = cudaGetLastError();
-    if (cudaSuccess != err)
-      std::cout << "CUDA ERROR__: " << cudaGetErrorString(err) << std::endl;
 
     // Wait that all threads have finished
     CudaErrorCheck(cudaDeviceSynchronize());


  // Get the center of the voxel
  double voxelCenterTemp[3];
  voxelCenterTemp[0] = c_gridOrig.x + (voxelCoordinate[0] + 0.5) * c_gridSpacing.x;
  voxelCenterTemp[1] = c_gridOrig.y + (voxelCoordinate[1] + 0.5) * c_gridSpacing.y;
  voxelCenterTemp[2] = c_gridOrig.z + (voxelCoordinate[2] + 0.5) * c_gridSpacing.z;

  // Transform voxel from grid to real coord
  double* voxelCenter = new double[3];



// ----------------------------------------------------------------------------
__device__ void transformFrom4Matrix(double matrix[16], double point[3], double output[3])
{
  output[0] = matrix[0] * point[0] + matrix[1] * point[1] + matrix[2] * point[2] + matrix[3];
  output[1] = matrix[4] * point[0] + matrix[5] * point[1] + matrix[6] * point[2] + matrix[7];
  output[2] = matrix[8] * point[0] + matrix[9] * point[1] + matrix[10] * point[2] + matrix[11];
}


// ----------------------------------------------------------------------------
__device__ double norm(double vec[3])
