float TSDFVolumeOctree::interpolateTrilinearly (float x, float y, float z, bool* valid) const {
        int xi, yi, zi;
        bool exists = getVoxelIndex (x, y, z, xi, yi, zi);
        if (!exists || xi <= 0 || xi >= xres_-1 || yi <= 0 || yi >= yres_ -1 || zi <= 0 || zi >= zres_ -1)
        {
            if (valid != NULL) *valid = false;
            return (std::numeric_limits<float>::quiet_NaN());
        }
        pcl::PointXYZ v = getVoxelCenter (xi, yi, zi);
        if (x < v.x) xi -= 1;
        if (y < v.y) yi -= 1;
        if (z < v.z) zi -= 1;
        v = getVoxelCenter (xi, yi, zi);
        pcl::PointXYZ vx = getVoxelCenter (xi+1, yi, zi);
        pcl::PointXYZ vy = getVoxelCenter (xi, yi+1, zi);
        pcl::PointXYZ vz = getVoxelCenter (xi, yi, zi+1);
        pcl::PointXYZ vxy = getVoxelCenter (xi+1, yi+1, zi);
        pcl::PointXYZ vxz = getVoxelCenter (xi+1, yi, zi+1);
        pcl::PointXYZ vyz = getVoxelCenter (xi, yi+1, zi+1);
        pcl::PointXYZ vxyz = getVoxelCenter (xi+1, yi+1, zi+1);

        float a = (x - v.x) * xres_ / xsize_;
        float b = (y - v.y) * yres_ / ysize_;
        float c = (z - v.z) * zres_ / zsize_;

        const OctreeNode* vo = octree_->getContainingVoxel (v.x, v.y, v.z);
        const OctreeNode* vox = octree_->getContainingVoxel (vx.x, vx.y, vx.z);
        const OctreeNode* voy = octree_->getContainingVoxel (vy.x, vy.y, vy.z);
        const OctreeNode* voz = octree_->getContainingVoxel (vz.x, vz.y, vz.z);

        const OctreeNode* voxy = octree_->getContainingVoxel (vxy.x, vxy.y, vxy.z);
        const OctreeNode* voxz = octree_->getContainingVoxel (vxz.x, vxz.y, vxz.z);
        const OctreeNode* voyz = octree_->getContainingVoxel (vyz.z, vyz.y, vyz.z);

        const OctreeNode* voxyz = octree_->getContainingVoxel (vxyz.x, vxyz.y, vxyz.z);

        if (valid != NULL) {
            *valid &= (vo->w_ > 0);
            *valid &= (vox->w_ > 0);
            *valid &= (voy->w_ > 0);
            *valid &= (voz->w_ > 0);
            *valid &= (voxy->w_ > 0);
            *valid &= (voxz->w_ > 0);
            *valid &= (voyz->w_ > 0);
            *valid &= (voxyz->w_ > 0);
        }

        return (vo->d_    * (1 - a) * (1 - b) * (1 - c) +
                voz->d_   * (1 - a) * (1 - b) * (c) +
                voy->d_   * (1 - a) * (1 - b) * (1 - c) +
                voyz->d_  * (1- a)  * (b)     * (c) +
                vox->d_   * (a)     * (1 - b) * (1 - c) +
                voxz->d_  * (a)     * (1 - b) * (c) +
                voxy->d_  * (a)     * (b)     * (1 - c) +
                voxyz->d_ * (a)     * (b)     * (c));
    }
__global__
void kernelAddressingTest(const Voxel* voxelmap, const Vector3ui* dimensions, const float *voxel_side_length,
                          const Vector3f *testpoint, bool* success)
{
//  const uint32_t i = blockIdx.x * blockDim.x + threadIdx.x;

    Vector3ui test_ccords = mapToVoxels(*voxel_side_length, *testpoint);
    Voxel* testvoxel = getVoxelPtr(voxelmap, dimensions, test_ccords.x, test_ccords.y, test_ccords.z);
    Vector3ui int_coords = mapToVoxels(voxelmap, dimensions, testvoxel);
    Vector3f center = getVoxelCenter(voxelmap, dimensions, *voxel_side_length, &int_coords);

//  printf("TestCoord    (%f,%f,%f)\n",testpoint->x, testpoint->y, testpoint->z);
//  printf("TestIntCoord (%d,%d,%d)\n",int_coords.x, int_coords.y, int_coords.z);
//  printf("ReturnCoord  (%f,%f,%f)\n",center.x, center.y, center.z);

    if ((abs(center.x - testpoint->x) > *voxel_side_length / 2.0)
            || (abs(center.y - testpoint->y) > *voxel_side_length / 2.0)
            || (abs(center.z - testpoint->z) > *voxel_side_length / 2.0))
    {
        *success = false;
    }
    else
    {
        *success = true;
    }
}