Efficent way to generate an occupation pointcloud

[ceccocats]

Im using an OccupancyLayer to integrate mesurament from lidar and i want to output an occupancy pointcloud filtering the voxels by log_odds.
I tried using callFunctionOnAllVoxels but is extremely slow:

nvblox::OccupancyLayer& layer = mMapper->occupancy_layer(); // layer is on kDevice

mCloud.resize(0);
auto new_lambda = [&](const nvblox::Index3D& block_index,
                      const nvblox::Index3D& voxel_index,
                      const nvblox::OccupancyLayer::VoxelType* voxel) {
    if (voxel->log_odds > 1.0) {
        auto pt = nvblox::getCenterPositionFromBlockIndexAndVoxelIndex(
          layer.block_size(), block_index, voxel_index);
        mCloud.push_back(tk::core::cloud::PointXYZI(pt.x(), pt.y(), pt.z(), voxel->log_odds));
    }
};
// Call above lambda on every voxel in the layer.
nvblox::callFunctionOnAllVoxels<nvblox::OccupancyLayer::VoxelType>(layer, new_lambda);

I also tried with a cuda kernel but then I have a really big pointcloud that i have to filter to remove point on zero:

__global__ void build_cloud_kernel(float block_size,
                                   Index3DDeviceHashMapType<BlockType> block_hash,
                                   tk::cuda::Vector<Index3D> block_indices_d,
                                   tk::cuda::Cloud<tk::core::cloud::PointXYZI> cloud)
{

    int i = blockDim.x * blockIdx.x + threadIdx.x;
    if (i >= block_indices_d.size())
        return;

    const Index3D& block_index = block_indices_d[i];

    int idx = i * kVoxelsPerSide * kVoxelsPerSide * kVoxelsPerSide;
    for (int x = 0; x < kVoxelsPerSide; ++x) {
        for (int y = 0; y < kVoxelsPerSide; ++y) {
            for (int z = 0; z < kVoxelsPerSide; ++z) {
                const VoxelType& voxel = getBlockPtr(block_hash, block_index)->voxels[x][y][z];

                const Vector3f pos = getCenterPositionFromBlockIndexAndVoxelIndex(
                  block_size, block_index, { x, y, z });

                auto& pt = cloud[idx];
                if (voxel.log_odds > 1.0f) {
                    pt.x = pos.x();
                    pt.y = pos.y();
                    pt.z = pos.z();
                    pt.intensity = voxel.log_odds;
                } else {
                    pt.x = 0;
                    pt.y = 0;
                    pt.z = 0;
                    pt.intensity = 0;
                }
                idx++;
            }
        }
    }
}

[...]
void extractCloud() {
  const std::vector<Index3D>& block_indices = layer.getAllBlockIndices();
  if (block_indices.size() == 0)
      return;
  
  block_indices_d.upload(block_indices.data(), block_indices.size(), ctx);
  const int size = block_indices.size();
  
  cloud_d.resize(size * kVoxelsPerSide * kVoxelsPerSide * kVoxelsPerSide, ctx);
  
  // Launch the kernel
  int blocks = (size + 255) / 256;
  int threads = 256;
  std::cout << "kernel: " << blocks << " " << threads << " stream: " << ctx.getStream()
            << std::endl;
  build_cloud_kernel<<<blocks, threads, 0, ctx.getStream()>>>(
    block_size, gpu_layer.getHash().impl_, block_indices_d, cloud_d);
  cudaStreamSynchronize(ctx.getStream());
}

Do you suggest any other way to obtain such cloud?
thanks