Exemplo n.º 1
0
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);
}
Exemplo n.º 2
0
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);
}
Exemplo n.º 3
0
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;
}