CollisionTest.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * 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_COLLISIONTEST_H_
#define _MIRA_COLLISIONTEST_H_

#include <boost/optional.hpp>

#include <serialization/DefaultInitializer.h>
#include <image/Img.h>
#include <math/Angle.h>
#include <utils/Path.h>
#include <geometry/Point.h>

#include <model/Footprint.h>
#include <model/RigidModelExports.h>

namespace mira { namespace model {


class MIRA_RIGID_MODEL_EXPORT CollisionTest
{
public:

    CollisionTest(float cellSize = 0.02f, uint32 segments=360)
    {
        MIRA_INITIALIZE_THIS;
        mCellSize = cellSize;
        mSegments = segments;
    }

    CollisionTest(const CollisionTest& other) : mCellSize(other.mCellSize),
                                                mSegments(other.mSegments),
                                                mShapeLUTBB(other.mShapeLUTBB),
                                                mShapeDimension(other.mShapeDimension),
                                                mObstacleThreshold(other.mObstacleThreshold),
                                                mSegmentStep(other.mSegmentStep)
    {
        mShapeLUT.reserve(other.mShapeLUT.size());
        std::transform(other.mShapeLUT.begin(), other.mShapeLUT.end(),
                       std::back_inserter(mShapeLUT),
                       std::mem_fn(&Img8U1::clone));
    }

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        r.member("CellSize", mCellSize,
                 "Resolution of the collision map [m]", 0.02f);
        r.member("Segments", mSegments,
                 "Size of the collision LUT (360 deg is divided in #segments)",
                 360);
        r.property("ObstacleThreshold", mObstacleThreshold,
                   "Occupancy grid values above this threshold are counted as obstacles",
                   140, PropertyHints::limits(0, 255) | PropertyHints::step(1));
    }

    CollisionTest& operator=(const CollisionTest& other)
    {
        mCellSize = other.mCellSize;
        mSegments = other.mSegments;
        mShapeLUTBB = other.mShapeLUTBB;

        mShapeLUT.clear();
        mShapeLUT.reserve(other.mShapeLUT.size());
        std::transform(other.mShapeLUT.begin(), other.mShapeLUT.end(),
                       std::back_inserter(mShapeLUT),
                       std::mem_fn(&Img8U1::clone));

        mShapeDimension = other.mShapeDimension;
        mObstacleThreshold = other.mObstacleThreshold;
        mSegmentStep = other.mSegmentStep;

        return *this;
    }

    bool testCollision(const Img8U1& map, const Point2i& p, float phi) const;

    float distanceToObstacle(const Img32F1& distanceMap, const Point2i& p, float phi,
                             Point2i* oClosestModelPoint = NULL) const;

    static boost::optional<Point2i> obstaclePosition(
                                      const Img32F1& distanceMap, const Img<int32>& labelMap,
                                      const Point2i& closestModelPoint,
                                      std::map<int, Point2i>* ioCache = NULL);

    void distanceTransform(const Img8U1& map, Img32F1& oDistanceMap) const;

    void distanceTransform(const Img8U1& map, Img32F1& oDistanceMap,
                           Img<int32>& oLabelMap) const;

    void distanceTransformAndClearShape(const Img8U1& map, Img32F1& oDistanceMap, 
                                        const Point2i& p, float phi) const;

    void distanceTransformAndClearShape(const Img8U1& map, Img32F1& oDistanceMap,
                                        Img<int32>& oLabelMap,
                                        const Point2i& p, float phi) const;

    const Img8U1& getShape(float phi) const;

    const Rect2i& getShapeBB(float phi) const;

    void initialize(const Footprint& footprint);

    void initialize(const Footprint& footprint, float cellSize);

    void initialize(const Footprint& footprint, float cellSize, uint32 segments);

    void clearShape(Img8U1& map, const Point2i& p, float phi,
                    uint8 freeValue = 0) const;

    void decayShape(Img8U1& map, const Point2i& p, float phi,
                    float decayRate, float neutral = 127.f,
                    bool decayObstacles = true, bool decayFreespace = false,
                    bool invertShape = false) const;


protected:

    int calculateSegment(float phi) const;
    std::pair<Point2i, Rect2i> calculateOffsetAndOverlap(const Size2i& map,
                                                         const Point2i& p,
                                                         float phi) const;

protected:

    float mCellSize;  
    uint32 mSegments; 

    std::vector<Img8U1> mShapeLUT;    
    std::vector<Rect2i> mShapeLUTBB;  

    int mShapeDimension;

    int mObstacleThreshold;

    float mSegmentStep;
};


}}

#endif