OccupancyGridMapping.h

Go to the documentation of this file.

/*
 * Copyright (C) by
 *   MetraLabs GmbH (MLAB), GERMANY
 *  and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Redistribution and modification of this code is strictly prohibited.
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_OCCUPANCYGRIDMAPPING_H_
#define _MIRA_OCCUPANCYGRIDMAPPING_H_


#include <robot/RangeScan.h>
#include <transform/Pose.h>

#include <maps/OccupancyGrid.h>

#include <mapping/MappingExports.h>
#include <mapping/RasterRangeScan.h>

namespace mira { namespace maps {


class MIRA_MAPPING_MAPPING_EXPORT OccupancyGridMapping
{
public:

    OccupancyGridMapping();
    ~OccupancyGridMapping();

public:

    void insertScan(OccupancyGrid& map, const Pose3& pose,
                    const robot::RangeScan& scan, float maxRange = 5.0f)
    {
        insertScan(map, pose, pose, scan, maxRange);
    }
    void insertScan(OccupancyGrid& map, const Pose3& poseStart, const Pose3& poseEnd,
                    const robot::RangeScan& scan, float maxRange = 5.0f);


    static void clampToMapOccupancyLimits(OccupancyGrid& map, const Rect2f& region, uint8 min, uint8 max);

public:

    void setSensorReliabilitySigma(float sigma) { 
        mSensorReliabilitySigma = sigma; 
        invalidateReliabilityLUT(); 
    }

    float getSensorReliabilitySigma() const {
        return mSensorReliabilitySigma;
    }

    void setEstimationOccupancyLimits(uint8 min, uint8 max) {
        mMaxEstimationOccupancy = max;
        mMinEstimationOccupancy = min;
    }
    void setMinEstimationOccupancyLimit(uint8 min) {
        mMinEstimationOccupancy = min;
    }
    uint8 getMinEstimationOccupancyLimit() const {
        return mMinEstimationOccupancy;
    }

    void setMaxEstimationOccupancyLimit(uint8 max) {
        mMaxEstimationOccupancy = max;
    }
    uint8 getMaxEstimationOccupancyLimit() const {
        return mMaxEstimationOccupancy;
    }

    void setMapOccupancyLimits(uint8 min, uint8 max);
    void setMinOccupancyLimit(uint8 min) {
        setMapOccupancyLimits(min, mMaxMapOccupancy);
    }
    uint8 getMinOccupancyLimit() const {
        return mMinMapOccupancy;
    }

    void setMaxOccupancyLimit(uint8 max) {
        setMapOccupancyLimits(mMinMapOccupancy, max);
    }
    uint8 getMaxOccupancyLimit() const {
        return mMaxMapOccupancy;
    }

    void setEnsureOccupancyLimitsAfterUpdate(bool enable);

    bool getEnsureOccupancyLimitsAfterUpdate() const {
        return mEnsureOccupancyLimitsAfterUpdate;
    }

    void setLocalCropRadius(float localRadius) {
        mLocalCropRadius = localRadius;
    }
    float getLocalCropRadius() const {
        return mLocalCropRadius;
    }

    void setMinCertainty(float certainty) {
        mMinCertainty = certainty;
    }

    float getMinCertainty() const {
        return mMinCertainty;
    }

private:

    template <bool useReliability, bool freeOnly>
    class Visitor;

    template <bool useReliability, bool freeOnly>
    friend class Visitor;

private:

    template<bool useReliability>
    void update(OccupancyGrid& map,
                const Pose3& poseStart, const Pose3& poseEnd,
                const robot::RangeScan& scan, float maxRange);


private:

    // the parameters that can be set above
    uint8 mMaxEstimationOccupancy;
    uint8 mMinEstimationOccupancy;
    uint8 mMaxMapOccupancy;
    uint8 mMinMapOccupancy;
    bool mEnsureOccupancyLimitsAfterUpdate;

    float mSensorReliabilitySigma;

    float mLocalCropRadius;
    float mMinCertainty;

private:

    uint8 mBayesLUT[256][256];
    void buildBayesLUT();

    enum { RELIABILITY_LUT_RESOLUTION = 1024 };
    std::vector<uint8> mReliabilityLUT;
    uint8 getReliability(float d) const;

    void buildReliabilityLUT(float max_d);
    void invalidateReliabilityLUT();

};

}}

#endif