PolygonTools.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_TOOLBOXES_MAPPING_INCLUDE_MAPPING_POYLGONTOOLS_H_
#define _MIRA_TOOLBOXES_MAPPING_INCLUDE_MAPPING_POYLGONTOOLS_H_

#include <geometry/RasterPolygon.h>
#include <maps/GridMap.h>
#include <transform/Pose.h>

namespace mira { namespace maps { namespace polygon {


template<class Celltype>
class TransformToMapFunctor
{
public:
    TransformToMapFunctor(const float rotation, const Point2f& translation,
                          const maps::GridMap<Celltype>& map)
    : mCos_phi(std::cos(rotation)), mSin_phi(std::sin(rotation)), mTranslation(translation), mMap(map)
    {}

    TransformToMapFunctor(const Pose2& pose, const maps::GridMap<Celltype>& map)
    : TransformToMapFunctor(pose.phi(), pose.t, map)
    {}

    inline Point2f operator()(const Point2f& p) const
    {
        return mMap.world2mapf(shiftPoint(rotatePoint(p)));
    }

private:
    inline Point2f rotatePoint(const Point2f& p) const
    {
        return Point2f(p.x() * mCos_phi - p.y() * mSin_phi, p.x() * mSin_phi + p.y() * mCos_phi);
    }

    inline Point2f shiftPoint(Point2f&& p) const
    {
        return p + mTranslation;
    }

private:
    float mCos_phi;
    float mSin_phi;
    Point2f mTranslation;
    const maps::GridMap<Celltype>& mMap;
};

template<class Celltype>
Celltype getMaximum(const maps::GridMap<Celltype>& map, const Polygon2f& polygon, const Pose2& polygonPose,
                    uint8 precision = 1)
{
    auto maximum = std::numeric_limits<Celltype>::lowest();

    rasterPolygon(polygon, Rect2i(0, 0, map.size().width(), map.size().height()),
                  TransformToMapFunctor<Celltype>(polygonPose, map),
                  [&](int x, int y) {
                      maximum = std::max(map(x, y), maximum);
                      return false;
                  },
                  precision);
    return maximum;
}

template<class Celltype>
Celltype getMaximum(const maps::GridMap<Celltype>& map, const std::vector<Polygon2f>& polygons,
                    const Pose2& polygonPose, uint8 precision = 1)
{
    auto maximum = std::numeric_limits<Celltype>::lowest();
    for (const auto& polygon : polygons) {
        auto currentMax = getMaximum(map, polygon, polygonPose, precision);
        maximum = std::max(maximum, currentMax);
    }
    return maximum;
}

template<class Celltype>
void fill(maps::GridMap<Celltype>& map, const Polygon2f& polygon, const Pose2& polygonPose,
          const Celltype fillValue, uint8 precision = 1)
{
    rasterPolygon(polygon, Rect2i(0, 0, map.size().width(), map.size().height()),
                  TransformToMapFunctor<Celltype>(polygonPose, map),
                  [&](int x, int y) {
                      map(x, y) = fillValue;
                      return false;
                  },
                  precision);
}

template<class Celltype>
void fill(maps::GridMap<Celltype>& map, const std::vector<Polygon2f>& polygons, const Pose2& polygonPose,
          const Celltype fillValue, uint8 precision = 1)
{
    for (const auto& polygon : polygons) {
        fill(map, polygon, polygonPose, fillValue, precision);
    }
}

// Not tested functions
//
// template<class Celltype, class TerminationCondition>
// Celltype getMinimum(const maps::GridMap<Celltype>& map, const Polygon2f& polygon, const Pose2& polygonPose,
//                    TerminationCondition&& terminate, uint8 precision = 1)
//{
//
//  auto minimum = std::numeric_limits<Celltype>::max();
//
//  rasterPolygon(polygon, Rect2i(0, 0, map.size().width(), map.size().height()),
//                TransformToMapFunctor<Celltype>(polygonPose, map),
//                [&](int x, int y) {
//                    minimum = std::min(minimum, map(x, y));
//                    return terminate(minimum);
//                },
//                precision);
//  return minimum;
//}
//
//template<class Celltype>
//Celltype getMinimum(const maps::GridMap<Celltype>& map, const std::vector<Polygon2f>& polygons,
//                    const Pose2& polygonPose, Celltype lowerBound, uint8 precision = 1)
//{
//  auto minimum = std::numeric_limits<Celltype>::max();
//  for (const auto& polygon : polygons) {
//      auto polygonMin =
//          getMinimum(map, polygon, polygonPose,
//                     [=](const Celltype currentMin) { return currentMin <= lowerBound; }, precision);
//
//      if (polygonMin <= lowerBound)
//          return polygonMin;
//
//      minimum = std::min(polygonMin, minimum);
//  }
//  return minimum;
//}
//
// GridMap<float> distanceTransformAndClearPolygon(const OccupancyGrid& map, const Polygon2f& polygon,
//                                                const Pose2& polygonPose, Img32F1&& buffer,
//                                                uint8 obstacleThreshold = 127)
//{
//  auto res = GridMap<float>(std::move(buffer), map.getCellSize(), map.getOffset());
//  // buffer is to large or to small? no problem
//  res.clip(map.getMapRegion(), 0);
//
//  OccupancyGrid clearedMap = map.clone();
//  cv::threshold(map, clearedMap, obstacleThreshold, 255, cv::THRESH_BINARY_INV);
//  // Clear the map on the actual position with the shape.
//  fill(clearedMap, polygon, polygonPose, (uint8)255);
//
//#if CV_MAJOR_VERSION >= 3
//  cv::distanceTransform(clearedMap, res, cv::DIST_L2, 5);
//#else
//  cv::distanceTransform(clearedMap, res, CV_DIST_L2, 5);
//#endif
//
//  return res;
//}
//
// GridMap<float> distanceTransformAndClearPolygon(const OccupancyGrid& map,
//                                                const std::vector<Polygon2f>& polygons,
//                                                const Pose2& polygonPose, Img32F1&& buffer,
//                                                uint8 obstacleThreshold = 127)
//{
//  auto res = GridMap<float>(std::move(buffer), map.getCellSize(), map.getOffset());
//  // buffer is to large or to small? no problem
//  res.clip(map.getMapRegion(), 0);
//
//  OccupancyGrid clearedMap = map.clone();
//  cv::threshold(map, clearedMap, obstacleThreshold, 255, cv::THRESH_BINARY_INV);
//  // Clear the map on the actual position with the shape.
//  fill(clearedMap, polygons, polygonPose, (uint8)255);
//
//#if CV_MAJOR_VERSION >= 3
//  cv::distanceTransform(clearedMap, res, cv::DIST_L2, 5);
//#else
//  cv::distanceTransform(clearedMap, res, CV_DIST_L2, 5);
//#endif
//
//  return res;
//}
//
// GridMap<float> distanceTransformAndClearPolygon(const OccupancyGrid& map, const Polygon2f& polygon,
//                                                const Pose2 polygonPose, uint8 obstacleThreshold = 127)
//{
//  return distanceTransformAndClearPolygon(map, polygon, polygonPose, Img32F1{}, obstacleThreshold);
//}
//
// GridMap<float> distanceTransformAndClearPolygon(const OccupancyGrid& map,
//                                                const std::vector<Polygon2f>& polygons,
//                                                const Pose2 polygonPose, uint8 obstacleThreshold = 127)
//{
//  return distanceTransformAndClearPolygon(map, polygons, polygonPose, Img32F1{}, obstacleThreshold);
//}


}}} // namespace mira::maps::polygon

#endif