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;
}
}
