PointCloudToVoxelGrid.cpp

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.
 * ------------------------------------------------------------------------- */
#include <mp2p_icp_filters/PointCloudToVoxelGrid.h>

using namespace mp2p_icp_filters;

void PointCloudToVoxelGrid::resize(
    const mrpt::math::TPoint3D& min_corner,
    const mrpt::math::TPoint3D& max_corner, const float voxel_size)
{
    MRPT_START

    pts_voxels.clear();
    pts_voxels.setSize(
        min_corner.x, max_corner.x, min_corner.y, max_corner.y, min_corner.z,
        max_corner.z, voxel_size, voxel_size);

    used_voxel_indices.clear();
    used_voxel_indices.reserve(pts_voxels.getVoxelCount() / 4);

    MRPT_END
}

void PointCloudToVoxelGrid::processPointCloud(const mrpt::maps::CPointsMap& p)
{
    using mrpt::max3;
    using std::abs;

    const auto& xs   = p.getPointsBufferRef_x();
    const auto& ys   = p.getPointsBufferRef_y();
    const auto& zs   = p.getPointsBufferRef_z();
    const auto  npts = xs.size();

    // Previous point:
    float x0, y0, z0;
    x0 = y0 = z0 = std::numeric_limits<float>::max();

    for (std::size_t i = 0; i < npts; i++)
    {
        // Skip this point?
        if (params_.min_consecutive_distance != .0f &&
            max3(abs(x0 - xs[i]), abs(y0 - ys[i]), abs(z0 - zs[i])) <
                params_.min_consecutive_distance)
            continue;

        // Save for the next point:
        x0 = xs[i];
        y0 = ys[i];
        z0 = zs[i];

        const auto cx      = pts_voxels.x2idx(x0);
        const auto cy      = pts_voxels.y2idx(y0);
        const auto cz      = pts_voxels.z2idx(z0);
        const auto vxl_idx = pts_voxels.cellAbsIndexFromCXCYCZ(cx, cy, cz);
        if (vxl_idx == grid_t::INVALID_VOXEL_IDX) continue;

        auto* c = pts_voxels.cellByIndex(vxl_idx);
        if (!c) continue;
        c->indices.push_back(i);  // only if not out of grid range

        if (c->is_empty)
        {
            c->is_empty = false;
            used_voxel_indices.push_back(vxl_idx);
        }
    }
}

void PointCloudToVoxelGrid::clear()
{
    for (auto idx : used_voxel_indices)
    {
        auto c = pts_voxels.cellByIndex(idx);
        c->indices.clear();
        c->is_empty = true;
    }

    used_voxel_indices.clear();
}