VoxelGrid.cpp

Go to the documentation of this file.

#include "VoxelGrid.h"
 
#include <algorithm>
 
#include <VisionX/libraries/VoxelGridCore/io/JsonIO.h>
 
#include "../utils.h"
 
namespace visionx::voxelgrid::LabelOccupancy
{
 
    Voxel::Voxel()
    {
        setFree();
    }
 
    Voxel::Voxel(Label label)
    {
        setOccupied(label);
    }
 
    bool
    Voxel::isFree() const
    {
        return !isOccupied();
    }
 
    bool
    Voxel::isOccupied() const
    {
        return _isOccupied;
    }
 
    Label
    Voxel::getLabel() const
    {
        ARMARX_CHECK(isOccupied());
        return _label;
    }
 
    void
    Voxel::setFree()
    {
        _isOccupied = false;
        _label = 0;
    }
 
    void
    Voxel::setOccupied(Label label)
    {
        _isOccupied = true;
        this->_label = label;
    }
 
    void
    to_json(nlohmann::json& json, const Voxel& voxel)
    {
        if (voxel.isOccupied())
        {
            json = voxel.getLabel();
        }
        else
        {
            json = -1;
        }
    }
 
    void
    from_json(const nlohmann::json& json, Voxel& voxel)
    {
        if (json.get<int>() < 0)
        {
            voxel.setFree();
        }
        else
        {
            voxel.setOccupied(json.get<Label>());
        }
    }
 
    const std::string VoxelGrid::JSON_VOXEL_ARRAY_NAME = "labels";
 
    VoxelGrid
    VoxelGrid::fromLabelDensity(const LabelDensity::VoxelGrid& density)
    {
        VoxelGrid grid(density.getStructure());
        grid.setByLabelDensity(density);
        return grid;
    }
 
    std::set<Label>
    VoxelGrid::getUniqueLabels() const
    {
        std::set<Label> ids;
        for (const Voxel& voxel : *this)
        {
            if (voxel.isOccupied())
            {
                ids.insert(voxel.getLabel());
            }
        }
        return ids;
    }
 
    void
    VoxelGrid::setByLabelDensity(const LabelDensity::VoxelGrid& density, bool adoptStructure)
    {
        if (density.getStructure() != getStructure())
        {
            if (adoptStructure)
            {
                resetStructure(density.getStructure());
            }
            else
            {
                throw voxelgrid::error::InvalidStructure(
                    density.getStructure(),
                    getStructure(),
                    "Provided density grid has different structure, but adoptStructure is false.");
            }
        }
 
        for (std::size_t i = 0; i < numVoxels(); ++i)
        {
            const LabelDensity::Voxel& voxel = density.getVoxel(i);
            if (voxel.isFree())
            {
                getVoxel(i).setFree();
            }
            else
            {
                getVoxel(i).setOccupied(voxel.getMaxLabel());
            }
        }
    }
 
    void
    VoxelGrid::writeJson(std::ostream& os) const
    {
        io::JsonIO::writeJson(os, io::JsonIO::toJson(*this, JSON_VOXEL_ARRAY_NAME));
    }
 
    void
    VoxelGrid::writeJson(const std::filesystem::path& file) const
    {
        io::JsonIO::writeJson(file, io::JsonIO::toJson(*this, JSON_VOXEL_ARRAY_NAME));
    }
 
    void
    VoxelGrid::readJson(std::istream& is)
    {
        io::JsonIO::fromJson(io::JsonIO::readJson(is), *this, JSON_VOXEL_ARRAY_NAME);
    }
 
    void
    VoxelGrid::readJson(const std::filesystem::path& file)
    {
        io::JsonIO::fromJson(io::JsonIO::readJson(file), *this, JSON_VOXEL_ARRAY_NAME);
    }
 
    void
    to_json(nlohmann::json& json, const VoxelGrid& grid)
    {
        io::JsonIO::toJson(json, grid, VoxelGrid::JSON_VOXEL_ARRAY_NAME);
    }
 
    void
    from_json(const nlohmann::json& json, VoxelGrid& grid)
    {
        io::JsonIO::fromJson(json, grid, VoxelGrid::JSON_VOXEL_ARRAY_NAME);
    }
 
} // namespace visionx::voxelgrid::LabelOccupancy