PointCloudToVoxelGrid.h

Go to the documentation of this file.

/* -------------------------------------------------------------------------
 * A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2021 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#pragma once

#include <mrpt/containers/CDynamicGrid3D.h>
#include <mrpt/maps/CPointsMap.h>
#include <mrpt/math/TPoint3D.h>

#include <vector>

namespace mp2p_icp_filters
{
class PointCloudToVoxelGrid
{
   public:
    PointCloudToVoxelGrid() = default;
    ~PointCloudToVoxelGrid() {}

    void resize(
        const mrpt::math::TPoint3D& min_corner,
        const mrpt::math::TPoint3D& max_corner, const float voxel_size);
    void processPointCloud(const mrpt::maps::CPointsMap& p);
    void clear();

    struct Parameters
    {
        float min_consecutive_distance{.0f};
    };

    Parameters params_;

    struct voxel_t
    {
        std::vector<std::size_t> indices;
        bool                     is_empty{true};
    };
    using grid_t = mrpt::containers::CDynamicGrid3D<voxel_t, float>;

    grid_t pts_voxels;

    std::vector<uint32_t> used_voxel_indices;
};

}  // namespace mp2p_icp_filters