OccupancyGrid::OccupancyGrid(
const VoxelGrid& voxel_grid,
const size_t density_channel_index,
const float occupancy_threshold)
: m_grid(
voxel_grid.get_xres(),
voxel_grid.get_yres(),
voxel_grid.get_zres(),
1)
{
initialize(
voxel_grid,
density_channel_index,
occupancy_threshold);
}
void write_voxel_grid(
const char* filename,
const VoxelGrid& grid)
{
FILE* file = fopen(filename, "wt");
if (file == 0)
return;
const size_t xres = grid.get_xres();
const size_t yres = grid.get_yres();
const size_t zres = grid.get_zres();
const size_t channel_count = grid.get_channel_count();
for (size_t z = 0; z < zres; ++z)
{
fprintf(file, "z " FMT_SIZE_T "\n\n", z);
for (size_t y = 0; y < yres; ++y)
{
for (size_t x = 0; x < xres; ++x)
{
if (x > 0)
fprintf(file, " ");
const float* voxel = grid.voxel(x, y, z);
for (size_t i = 0; i < channel_count; ++i)
{
if (i > 0)
fprintf(file, ",");
fprintf(file, "%f", voxel[i]);
}
}
fprintf(file, "\n");
}
fprintf(file, "\n");
}
fclose(file);
}
float OccupancyGrid::get_density_sum(
const VoxelGrid& voxel_grid,
const size_t density_channel_index,
const size_t x,
const size_t y,
const size_t z) const
{
float density_sum = 0.0f;
for (int dx = -1; dx <= +1; ++dx)
{
for (int dy = -1; dy <= +1; ++dy)
{
for (int dz = -1; dz <= +1; ++dz)
{
const int ix = static_cast<int>(x) + dx;
const int iy = static_cast<int>(y) + dy;
const int iz = static_cast<int>(z) + dz;
if (ix < 0 ||
iy < 0 ||
iz < 0 ||
ix >= static_cast<int>(voxel_grid.get_xres()) ||
iy >= static_cast<int>(voxel_grid.get_yres()) ||
iz >= static_cast<int>(voxel_grid.get_zres()))
continue;
const float* voxel = voxel_grid.voxel(ix, iy, iz);
assert(voxel[density_channel_index] >= 0.0f);
density_sum += voxel[density_channel_index];
}
}
}
return density_sum;
}
