FilterDecimateVoxels.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/FilterDecimateVoxels.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/ops_containers.h>  // dotProduct
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/random/RandomGenerators.h>
 
IMPLEMENTS_MRPT_OBJECT(
    FilterDecimateVoxels, mp2p_icp_filters::FilterBase, mp2p_icp_filters)
 
using namespace mp2p_icp_filters;
 
void FilterDecimateVoxels::Parameters::load_from_yaml(
    const mrpt::containers::yaml& c)
{
    MCP_LOAD_OPT(c, input_pointcloud_layer);
    MCP_LOAD_OPT(c, error_on_missing_input_layer);
 
    MCP_LOAD_REQ(c, output_pointcloud_layer);
 
    MCP_LOAD_REQ(c, voxel_filter_resolution);
    MCP_LOAD_REQ(c, use_voxel_average);
 
    ASSERT_(
        c.has("bounding_box_min") && c["bounding_box_min"].isSequence() &&
        c["bounding_box_min"].asSequence().size() == 3);
    ASSERT_(
        c.has("bounding_box_max") && c["bounding_box_max"].isSequence() &&
        c["bounding_box_max"].asSequence().size() == 3);
 
    const auto bboxMin = c["bounding_box_min"].toStdVector<double>();
    const auto bboxMax = c["bounding_box_max"].toStdVector<double>();
 
    for (int i = 0; i < 3; i++)
    {
        bounding_box.min[i] = bboxMin.at(i);
        bounding_box.max[i] = bboxMax.at(i);
    }
}
 
FilterDecimateVoxels::FilterDecimateVoxels() = default;
 
void FilterDecimateVoxels::initialize(const mrpt::containers::yaml& c)
{
    MRPT_START
 
    MRPT_LOG_DEBUG_STREAM("Loading these params:\n" << c);
    params_.load_from_yaml(c);
 
    filter_grid_.resize(
        params_.bounding_box.min, params_.bounding_box.max,
        params_.voxel_filter_resolution);
 
    MRPT_END
}
 
void FilterDecimateVoxels::filter(mp2p_icp::metric_map_t& inOut) const
{
    MRPT_START
 
    // Out:
    ASSERT_(!params_.output_pointcloud_layer.empty());
 
    // Create if new: Append to existing layer, if already existed.
    mrpt::maps::CPointsMap::Ptr outPc;
    if (auto itLy = inOut.layers.find(params_.output_pointcloud_layer);
        itLy != inOut.layers.end())
    {
        outPc = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(itLy->second);
        if (!outPc)
            THROW_EXCEPTION_FMT(
                "Layer '%s' must be of point cloud type.",
                params_.output_pointcloud_layer.c_str());
    }
    else
    {
        outPc = mrpt::maps::CSimplePointsMap::Create();
        inOut.layers[params_.output_pointcloud_layer] = outPc;
    }
 
    // In:
    mrpt::maps::CPointsMap::Ptr pcPtr;
    if (auto itLy = inOut.layers.find(params_.input_pointcloud_layer);
        itLy != inOut.layers.end())
    {
        pcPtr = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(itLy->second);
        if (!pcPtr)
            THROW_EXCEPTION_FMT(
                "Layer '%s' must be of point cloud type.",
                params_.input_pointcloud_layer.c_str());
    }
    else
    {
        // Input layer doesn't exist:
        if (params_.error_on_missing_input_layer)
        {
            THROW_EXCEPTION_FMT(
                "Input layer '%s' not found on input map.",
                params_.input_pointcloud_layer.c_str());
        }
        else
        {
            // Silently return with an unmodified output layer "outPc"
            return;
        }
    }
 
    const auto& pc = *pcPtr;
 
    // Do filter:
    outPc->reserve(outPc->size() + pc.size() / 10);
 
    filter_grid_.clear();
    filter_grid_.processPointCloud(pc);
 
    const auto& xs = pc.getPointsBufferRef_x();
    const auto& ys = pc.getPointsBufferRef_y();
    const auto& zs = pc.getPointsBufferRef_z();
 
    auto rng = mrpt::random::CRandomGenerator();
    // TODO?: rng.randomize(seed);
 
    size_t nonEmptyVoxels = 0;
    for (const auto& vxl_pts : filter_grid_.pts_voxels)
    {
        if (vxl_pts.indices.empty()) continue;
 
        nonEmptyVoxels++;
 
        if (params_.use_voxel_average)
        {
            // Analyze the voxel contents:
            auto        mean  = mrpt::math::TPoint3Df(0, 0, 0);
            const float inv_n = (1.0f / vxl_pts.indices.size());
            for (size_t i = 0; i < vxl_pts.indices.size(); i++)
            {
                const auto pt_idx = vxl_pts.indices[i];
                mean.x += xs[pt_idx];
                mean.y += ys[pt_idx];
                mean.z += zs[pt_idx];
            }
            mean *= inv_n;
 
            // Insert the mean:
            outPc->insertPointFast(mean.x, mean.y, mean.z);
        }
        else
        {
            // Insert a randomly-picked point:
            const auto idxInVoxel =
                rng.drawUniform64bit() % vxl_pts.indices.size();
 
            const auto pt_idx = vxl_pts.indices.at(idxInVoxel);
            outPc->insertPointFast(xs[pt_idx], ys[pt_idx], zs[pt_idx]);
        }
    }
    MRPT_LOG_DEBUG_STREAM("Voxel counts: total=" << nonEmptyVoxels);
 
    MRPT_END
}