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