VoxelGrid.h

Go to the documentation of this file.

#pragma once
 
#include <filesystem>
#include <map>
#include <set>
 
#include <Eigen/Core>
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
#include <SimoxUtility/json/json.hpp>
 
#include <VisionX/libraries/VoxelGridCore/VoxelGridCore.h>
 
#include "../types.h"
 
namespace visionx::voxelgrid::LabelDensity
{
 
    // VOXEL TYPE
 
    /**
     * @brief Map of labels (object IDs) to number of points.
     */
    class Voxel : public std::map<Label, std::size_t>
    {
        using Base = std::map<Label, std::size_t>;
 
    public:
        using Base::map;
 
 
        /// Indicate whether this voxel is free, i.e. it contains no points.
        bool isFree() const;
 
        /// Get the total number of points in this voxel.
        std::size_t getTotal() const;
 
        /// Get the relative frequency of labels, so that the sum is 1.0.
        /// If `getTotal()` is 0, the total of the returned density is also 0.0.
        std::map<Label, float> getDensity() const;
 
 
        /// Get the label with most points.
        Label getMaxLabel() const;
 
        /// Get the label with most points and the number of points.
        std::pair<Label, std::size_t> getMax() const;
 
 
        /// Equality operator.
        /// Considers a non-existent label and a label with count 0 equal.
        bool operator==(const Voxel& rhs) const;
        bool operator!=(const Voxel& rhs) const;
 
        /// Outstream operator.
        friend std::ostream& operator<<(std::ostream& os, const Voxel& voxel);
    };
 
    // VOXEL GRID
 
    /**
     * @brief Voxel grid storing the number of points per label in each voxel.
     */
    class VoxelGrid : public visionx::VoxelGrid<Voxel>
    {
        using Base = voxelgrid::VoxelGrid<Voxel>;
 
    public:
        using Base::VoxelGrid;
 
 
        /**
         * @brief Add the point cloud.
         * @param pointCloud The point cloud.
         * @param pointCloudPose The point cloud pose (in the same frame as the grid's pose).
         */
        void addPointCloud(const pcl::PointCloud<pcl::PointXYZL>& pointCloud,
                           const Eigen::Matrix4f& pointCloudPose = Eigen::Matrix4f::Identity());
        void addPointCloud(const pcl::PointCloud<pcl::PointXYZRGBL>& pointCloud,
                           const Eigen::Matrix4f& pointCloudPose = Eigen::Matrix4f::Identity());
 
 
        /// Get the set of unique labels.
        std::set<Label> getUniqueLabels() const;
 
 
        /// Remove entries with number of points below `minNumPoints`.
        void reduceNoise(std::size_t minNumPoints);
 
 
        // Serialization
 
        /// Write this voxel grid to JSON.
        void writeJson(std::ostream& os) const;
        void writeJson(const std::filesystem::path& file) const;
 
        /// Read this voxel grid from JSON.
        void readJson(std::istream& is);
        void readJson(const std::filesystem::path& file);
 
 
        /**
         * @brief Write the voxel data to a CSV file. (Structure is not stored.)
         *
         * The first column, "index", specified the flat voxel index.
         * The other columns represent the labels, and each row entry
         * specifies the number of points with that label in the respective
         * voxel.
         *
         * If `includeTotal` is true, a final column "total" is added, storing
         * the voxel-wise total number of points.
         */
        void toCsv(std::ostream& os, bool includeTotal = false) const;
        void toCsv(const std::filesystem::path& file, bool includeTotal = false) const;
    };
 
    /// Write the voxel grid to JSON.
    void to_json(nlohmann::json& json, const VoxelGrid& grid);
    /// Read the voxel grid from JSON.
    void from_json(const nlohmann::json& json, VoxelGrid& grid);
 
 
} // namespace visionx::voxelgrid::LabelDensity