filters.hpp

Go to the documentation of this file.

#pragma once

#include <filters/filter_base.h>
#include <grid_map_core/grid_map_core.hpp>

namespace mitre_fast_layered_map
{
    /*
    * This filter finds all obstacles that are in the current map
    * and adds cost buffers around them. Users need to specify the layer to
    * operate on and the inflation size for space around obstacles.
    */
    class Inflation : public filters::FilterBase<grid_map::GridMap>
    {
    public:
        Inflation();

        virtual ~Inflation();

        virtual bool configure();

        virtual bool update(const grid_map::GridMap &_mapIn, grid_map::GridMap &_mapOut);

    private:
        std::string layer_;         
        double inflationSideLen_m_; 
    };

    class OutlierRemoval : public filters::FilterBase<grid_map::GridMap>
    {
    public:
        OutlierRemoval();

        virtual ~OutlierRemoval();

        virtual bool configure();

        virtual bool update(const grid_map::GridMap &_mapIn, grid_map::GridMap &_mapOut);

        bool IsOutlierPoint(grid_map::GridMap &, const grid_map::Index &);

    private:
        std::string layer_; 
    };

    class RayTrace2d : public filters::FilterBase<grid_map::GridMap>
    {
    public:
        RayTrace2d();

        virtual ~RayTrace2d();

        virtual bool configure();

        virtual bool update(const grid_map::GridMap &_mapIn, grid_map::GridMap &_mapOut);

        bool trace(grid_map::GridMap &_map, const grid_map::Index &start, const grid_map::Index &end);

    private:
        std::string nongroundLayer_; 
        std::string groundLayer_; 
        std::string mode_; 
    };


    class BayesUpdate: public filters::FilterBase<grid_map::GridMap>
    {
        public:
            BayesUpdate();
            
            virtual ~BayesUpdate();
            
            virtual bool configure();

            virtual bool update(const grid_map::GridMap &_mapIn, grid_map::GridMap &_mapOut);

            double dynamicSenseOccGivenOcc(int numHits);
            double dynamicSenseOccGivenEmp(int numHits);

        private:
            std::string probabilityLayerStr_; 
            std::string historyLayerPrefix_; 
            int historyCount_; 

            double startingProb_{0.5};
            double probSenseEmpGivenOcc_{0.4};
            double probSenseEmpGivenEmp_{0.8};
            double probSenseOccGivenOccRate_{0.1};
            double probSenseOccGivenOccOffset_{0.3};
            double probSenseOccGivenEmpRate_{-0.1};
            double probSenseOccGivenEmpOffset_{0.3};
    };

    class ThresholdFilter: public filters::FilterBase<grid_map::GridMap>
    {
        public:
            ThresholdFilter();

            virtual ~ThresholdFilter();

            virtual bool configure();

            virtual bool update(const grid_map::GridMap &_mapIn, grid_map::GridMap &_mapOut);

        private:
            std::string valueLayerStr_;
            std::string outputLayerStr_;
            float threshold_;
            float outputValue_;
    };
} // namespace mitre_fast_layered_map