VoxelGrid.cpp

Go to the documentation of this file.

#include "VoxelGrid.h"
 
#include "../utils.h"
 
#include <VisionX/libraries/VoxelGridCore/io/JsonIO.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <algorithm>
 
 
namespace visionx::voxelgrid::LabelDensity
{
 
    bool Voxel::isFree() const
    {
        return empty() || getTotal() == 0;
    }
 
    std::size_t Voxel::getTotal() const
    {
        std::size_t sum = 0;
        for (const auto& [_, num] : *this)
        {
            sum += num;
        }
        return sum;
    }
 
    std::map<Label, float> Voxel::getDensity() const
    {
        const float total = static_cast<float>(getTotal());
 
        std::map<Label, float> density;
        for (const auto& [label, num] : *this)
        {
            density[label] = total > 0.f ? num / total : 0.f;;
        }
        return density;
    }
 
    Label Voxel::getMaxLabel() const
    {
        return getMax().first;
    }
 
    std::pair<Label, std::size_t> Voxel::getMax() const
    {
        return *std::max_element(begin(), end());
    }
 
    bool Voxel::operator==(const Voxel& rhs) const
    {
        // Collect labels to compare.
        std::set<Label> labels;
        for (const Voxel* voxel :
             {
                 this, &rhs
             })
        {
            for (const auto& [label, num] : *voxel)
            {
                if (num > 0)  // No need to compare entries with count 0.
                {
                    labels.insert(label);
                }
            }
        }
        for (Label label : labels)
        {
            if ((*this).at(label) != rhs.at(label))
            {
                return false;
            }
        }
        return true;
    }
 
    bool Voxel::operator!=(const Voxel& rhs) const
    {
        return !(*this == rhs);
    }
 
    std::ostream& operator<<(std::ostream& os, const Voxel& voxel)
    {
        os << "{ ";
        std::vector<std::string> items;
        for (const auto& [label, num] : voxel)
        {
            items.push_back(std::to_string(label) + ": " + std::to_string(num));
        }
        os << simox::alg::join(items, ", ") << " }";
        return os;
    }
 
 
    template <class PointT>
    static void addPointCloud(
        VoxelGrid& grid,
        const pcl::PointCloud<PointT>& _pointCloud,
        const Eigen::Matrix4f& pointCloudPose)
    {
        // Transform the point cloud in one go to avoid transformation of each
        // point individually.
        const pcl::PointCloud<PointT> pointCloud = utils::transformPointCloudToGrid(
                    _pointCloud, pointCloudPose, grid.getPose());
 
        // After transforming the point cloud, it is local.
        const bool local = true;
 
        for (const PointT& point : pointCloud)
        {
            const Eigen::Vector3f vpoint(point.data);
 
            const Eigen::Vector3i indices = grid.getVoxelGridIndex(vpoint, local);
            if (!grid.isInside(indices))
            {
                continue;
            }
 
            auto& voxel = grid.getVoxel(indices);
            voxel[static_cast<Label>(point.label)]++;
        }
    }
 
 
 
    void VoxelGrid::addPointCloud(
        const pcl::PointCloud<pcl::PointXYZL>& pointCloud,
        const Eigen::Matrix4f& pointCloudPose)
    {
        LabelDensity::addPointCloud(*this, pointCloud, pointCloudPose);
    }
 
    void VoxelGrid::addPointCloud(
        const pcl::PointCloud<pcl::PointXYZRGBL>& pointCloud,
        const Eigen::Matrix4f& pointCloudPose)
    {
        LabelDensity::addPointCloud(*this, pointCloud, pointCloudPose);
    }
 
 
    std::set<Label> VoxelGrid::getUniqueLabels() const
    {
        std::set<Label> ids;
        for (const Voxel& voxel : *this)
        {
            for (const auto& item : voxel)
            {
                ids.insert(item.first);
            }
        }
        return ids;
    }
 
    void VoxelGrid::reduceNoise(std::size_t minNumPoints)
    {
        for (auto& voxel : *this)
        {
            std::set<Label> remove;
            for (const auto& [label, num] : voxel)
            {
                if (num < minNumPoints)
                {
                    remove.insert(label);
                }
            }
 
            for (Label label : remove)
            {
                voxel.erase(voxel.find(label));
            }
        }
    }
 
 
    static io::VoxelAttributeGetterMap<Voxel> makeJsonGetters(const VoxelGrid& grid)
    {
        voxelgrid::io::VoxelAttributeGetterMap<Voxel> getters;
 
        const std::set<Label> labels = grid.getUniqueLabels();
 
        for (Label label : labels)
        {
            getters[std::to_string(label)] = [label](const Voxel & voxel) -> nlohmann::json
            {
                const auto find = voxel.find(label);
                return find != voxel.end() ? find->second : 0;
            };
        }
 
        return getters;
    }
 
    static io::VoxelAttributeSetterMap<Voxel> makeJsonSetters(const std::set<Label>& labels)
    {
        voxelgrid::io::VoxelAttributeSetterMap<Voxel> setters;
        for (Label label : labels)
        {
            setters[std::to_string(label)] = [label](const nlohmann::json & j, Voxel & voxel)
            {
                voxel[label] = j;
            };
        }
        return setters;
    }
 
    static std::set<Label> getLabelsFromKeys(const nlohmann::json& json)
    {
        // All numeric top-level keys are labels.
 
        std::set<Label> labels;
        for (const auto& item : json.items())
        {
            if (item.key() == "structure")
            {
                continue;
            }
            try
            {
                const int key = std::stoi(item.key());
                if (key >= 0)
                {
                    labels.insert(static_cast<Label>(key));
                }
            }
            catch (std::invalid_argument&)
            {
                // Ignore this key.
            }
        }
 
        return labels;
    }
 
 
    void VoxelGrid::writeJson(std::ostream& os) const
    {
        io::JsonIO::write<Voxel>(os, *this, makeJsonGetters(*this));
    }
 
    void VoxelGrid::writeJson(const std::filesystem::path& file) const
    {
        io::JsonIO::write<Voxel>(file, *this, makeJsonGetters(*this));
    }
 
    void VoxelGrid::readJson(std::istream& is)
    {
        // Since we do not know which labels are present, we have to take a peak at
        // the JSON content.
        const nlohmann::json json = io::JsonIO::readJson(is);
        from_json(json, *this);
    }
 
    void VoxelGrid::readJson(const std::filesystem::path& file)
    {
        std::ifstream ifs(file);
        readJson(ifs);
    }
 
 
    void to_json(nlohmann::json& json, const VoxelGrid& grid)
    {
        io::JsonIO::toJson(json, grid, makeJsonGetters(grid));
    }
 
    void from_json(const nlohmann::json& json, VoxelGrid& grid)
    {
        io::JsonIO::fromJson(json, grid, makeJsonSetters(getLabelsFromKeys(json)));
    }
 
 
    void VoxelGrid::toCsv(std::ostream& os, bool includeTotal) const
    {
        static const std::string sep = ",";
 
        const std::set<Label> labels = getUniqueLabels();
 
        // Write header.
 
        std::vector<std::string> strings;
        // Index
        strings.push_back("index");
        // Counts
        std::transform(labels.begin(), labels.end(), std::back_inserter(strings),
                       [](Label l)
        {
            return std::to_string(l);
        });
 
        if (includeTotal)
        {
            // Total
            strings.push_back("total");
        }
 
        os << simox::alg::join(strings, sep) << std::endl;
 
        // Write data.
        for (std::size_t index = 0; index < numVoxels(); ++index)
        {
            const Voxel& voxel = getVoxel(index);
            if (!voxel.empty())
            {
                strings.clear();
 
                // Index
                strings.push_back(std::to_string(index));
                // Counts
                std::transform(labels.begin(), labels.end(), std::back_inserter(strings),
                               [&voxel](const Label id)
                {
                    auto it = voxel.find(id);
                    return std::to_string(it != voxel.end() ? it->second : 0);
                });
 
                // Total
                if (includeTotal)
                {
                    strings.push_back(std::to_string(voxel.getTotal()));
                }
 
                // Write line.
                os << simox::alg::join(strings, sep) << std::endl;
            }
        }
    }
 
    void VoxelGrid::toCsv(const std::filesystem::path& file, bool includeTotal) const
    {
        std::ofstream os(file);
        toCsv(os, includeTotal);
    }
 
}