FilterDecimateVoxelsQuadratic.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 <mp2p_icp/metricmap.h>
#include <mp2p_icp_filters/FilterBase.h>
#include <mp2p_icp_filters/PointCloudToVoxelGrid.h>
#include <mrpt/maps/CPointsMap.h>
 
namespace mp2p_icp_filters
{
class FilterDecimateVoxelsQuadratic : public mp2p_icp_filters::FilterBase
{
    DEFINE_MRPT_OBJECT(FilterDecimateVoxelsQuadratic, mp2p_icp_filters)
   public:
    FilterDecimateVoxelsQuadratic();
 
    // See docs in base class.
    void initialize(const mrpt::containers::yaml& c) override;
 
    // See docs in FilterBase
    void filter(mp2p_icp::metric_map_t& inOut) const override;
 
    struct Parameters
    {
        void load_from_yaml(const mrpt::containers::yaml& c);
 
        std::string input_pointcloud_layer =
            mp2p_icp::metric_map_t::PT_LAYER_RAW;
 
        bool error_on_missing_input_layer = true;
 
        std::string output_pointcloud_layer;
 
        double voxel_filter_resolution = .20;  // [m]
 
        double quadratic_reference_radius = 20.0;  // [m]
 
        bool use_voxel_average = false;
 
        bool use_closest_to_voxel_average = false;
 
        bool use_random_point_within_voxel = false;
    };
 
    Parameters params_;
 
    inline float real2grid(float x) const
    {
        if (std::abs(x) > params_.quadratic_reference_radius)
            return x;
        else
            return mrpt::sign(x) * mrpt::square(x) *
                   quadratic_reference_radius_inv_;
    }
    inline float grid2real(float y) const
    {
        if (std::abs(y) > params_.quadratic_reference_radius)
            return y;
        else
            return std::sqrt(y * params_.quadratic_reference_radius) *
                   mrpt::sign(y);
    }
 
   private:
    mutable PointCloudToVoxelGrid filter_grid_;
 
    float quadratic_reference_radius_inv_ = 1.0f;
};
 
}  // namespace mp2p_icp_filters