PointCloudToVoxelGridSingle.h

Go to the documentation of this file.

/* -------------------------------------------------------------------------
 * A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#pragma once
 
#include <mrpt/core/pimpl.h>
#include <mrpt/maps/CPointsMap.h>
 
#include <optional>
 
namespace mp2p_icp_filters
{
class PointCloudToVoxelGridSingle
{
   public:
    PointCloudToVoxelGridSingle();
    ~PointCloudToVoxelGridSingle() {}
 
    void setResolution(const float voxel_size);
 
    void processPointCloud(const mrpt::maps::CPointsMap& p);
 
    void clear();
 
    struct voxel_t
    {
        std::optional<mrpt::math::TPoint3Df> point;
        std::optional<size_t>                pointIdx;  // Index in the source
        std::optional<const mrpt::maps::CPointsMap*> source;
 
        uint32_t pointCount = 0;
    };
 
    struct indices_t
    {
        indices_t(int32_t cx, int32_t cy, int32_t cz)
            : cx_(cx), cy_(cy), cz_(cz)
        {
        }
 
        int32_t cx_ = 0, cy_ = 0, cz_ = 0;
 
        bool operator==(const indices_t& o) const
        {
            return cx_ == o.cx_ && cy_ == o.cy_ && cz_ == o.cz_;
        }
    };
 
    struct IndicesHash
    {
        std::size_t operator()(const indices_t& k) const noexcept
        {
            // These are the implicit assumptions of the reinterpret cast below:
            static_assert(sizeof(indices_t::cx_) == sizeof(uint32_t));
            static_assert(offsetof(indices_t, cx_) == 0 * sizeof(uint32_t));
            static_assert(offsetof(indices_t, cy_) == 1 * sizeof(uint32_t));
            static_assert(offsetof(indices_t, cz_) == 2 * sizeof(uint32_t));
 
            const uint32_t* vec = reinterpret_cast<const uint32_t*>(&k);
            return ((1 << 20) - 1) &
                   (vec[0] * 73856093 ^ vec[1] * 19349663 ^ vec[2] * 83492791);
        }
 
        // k1 < k2?
        bool operator()(const indices_t& k1, const indices_t& k2) const noexcept
        {
            if (k1.cx_ != k2.cx_) return k1.cx_ < k2.cx_;
            if (k1.cy_ != k2.cy_) return k1.cy_ < k2.cy_;
            return k1.cz_ < k2.cz_;
        }
    };
 
    inline int32_t coord2idx(float xyz) const
    {
        return static_cast<int32_t>(xyz / resolution_);
    }
 
    void visit_voxels(
        const std::function<void(const indices_t idx, const voxel_t& vxl)>&
            userCode) const;
 
    size_t size() const;
 
   private:
    float resolution_ = 0.20f;
 
    struct Impl;
    mrpt::pimpl<Impl> impl_;
};
 
}  // namespace mp2p_icp_filters