PolygonTriangulationGeometry.cpp

Go to the documentation of this file.

#include <boost/polygon/voronoi.hpp>

#include "lanelet2_core/geometry/Polygon.h"

using VoronoiPrecision = std::int64_t;

namespace {
VoronoiPrecision toVoronoi(double v) { return static_cast<VoronoiPrecision>(v * 100); }
}  // namespace

using AlignedBasicPoint2d = Eigen::Matrix<double, 2, 1>;

namespace boost {
namespace polygon {
template <>
struct geometry_concept<lanelet::ConstPoint2d> {
  using type = point_concept;  // NOLINT
};

// specialize getter
template <>
struct point_traits<lanelet::ConstPoint2d> {
  using coordinate_type = VoronoiPrecision;  // NOLINT
  static inline coordinate_type get(const lanelet::ConstPoint2d& point, const orientation_2d& orient) {
    return toVoronoi((orient == HORIZONTAL) ? point.x() : point.y());
  }
};

// specialize setter
template <>
struct point_mutable_traits<lanelet::ConstPoint2d> {
  using coordinate_type = VoronoiPrecision;  // NOLINT

  static inline void set(lanelet::ConstPoint2d& /*point*/, const orientation_2d& /*orient*/,
                         VoronoiPrecision /*value*/) {
    assert(false && "boost polygon should never call this!");
  }
};

template <>
struct geometry_concept<lanelet::BasicPoint2d> {
  using type = point_concept;  // NOLINT
};

// specialize getter
template <>
struct point_traits<lanelet::BasicPoint2d> {
  using coordinate_type = VoronoiPrecision;  // NOLINT
  static inline coordinate_type get(const lanelet::BasicPoint2d& point, const orientation_2d& orient) {
    return toVoronoi((orient == HORIZONTAL) ? point.x() : point.y());
  }
};

// specialize setter
template <>
struct point_mutable_traits<lanelet::BasicPoint2d> {
  using coordinate_type = VoronoiPrecision;  // NOLINT

  static inline void set(lanelet::BasicPoint2d& /*point*/, const orientation_2d& /*orient*/,
                         VoronoiPrecision /*value*/) {
    assert(false && "boost polygon should never call this!");
  }
};

template <>
struct geometry_concept<AlignedBasicPoint2d> {
  using type = point_concept;  // NOLINT
};

// specialize getter
template <>
struct point_traits<AlignedBasicPoint2d> {
  using coordinate_type = VoronoiPrecision;  // NOLINT
  static inline coordinate_type get(const AlignedBasicPoint2d& point, const orientation_2d& orient) {
    return toVoronoi((orient == HORIZONTAL) ? point.x() : point.y());
  }
};

// specialize setter
template <>
struct point_mutable_traits<AlignedBasicPoint2d> {
  using coordinate_type = VoronoiPrecision;  // NOLINT

  static inline void set(AlignedBasicPoint2d& /*point*/, const orientation_2d& /*orient*/, VoronoiPrecision /*value*/) {
    assert(false && "boost polygon should never call this!");
  }
};
}  // namespace polygon
}  // namespace boost

namespace {
using namespace lanelet;
using namespace lanelet::geometry;

using IndexedPolygon = std::vector<size_t>;
using IndexedPolygons = std::vector<IndexedPolygon>;

bool isConnectionConvex(const BasicPoint2d& seg1, const BasicPoint2d& seg2,
                        const double eps = 4 * std::numeric_limits<double>::epsilon()) {
  return lanelet::geometry::internal::crossProd(seg1.normalized(), seg2.normalized()).z() <= eps;
}
using IndexedSegment = std::pair<size_t, size_t>;
using IndexedSegments = std::vector<IndexedSegment>;
using IndexedSegmentPair = std::pair<IndexedSegment, IndexedSegment>;
IndexedSegment flip(const IndexedSegment& seg) { return std::make_pair(seg.second, seg.first); }
bool isBorder(const IndexedSegment& seg, const size_t polySize) {
  const auto distance = seg.first > seg.second ? seg.first - seg.second : seg.second - seg.first;
  if (distance == 1) {
    return true;
  }
  return (seg.first == 0 || seg.second == 0) && distance + 1 == polySize;
}

using PolygonIterator = IndexedPolygon::const_iterator;
using PolygonIteratorPair = std::pair<PolygonIterator, PolygonIterator>;
using VoronoiGraph = boost::polygon::voronoi_diagram<double>;

PolygonIteratorPair getAlignedIterators(const IndexedPolygon& poly, const IndexedSegment& segment) {
  if (poly.size() < 3) {
    throw InvalidInputError("Can't find segment from polygon with less than 3 vertices");
  }
  PolygonIteratorPair itPair = std::make_pair(std::find(poly.begin(), poly.end(), segment.first),
                                              std::find(poly.begin(), poly.end(), segment.second));
  if (itPair.first == poly.end() || itPair.second == poly.end()) {
    throw InvalidInputError("Index " + std::to_string(segment.first) + "-" + std::to_string(segment.second) +
                            " not found in indices");
  }
  const auto dist = std::distance(itPair.first, itPair.second);
  if (dist == -1 || dist == poly.size() - 1) {
    std::swap(itPair.first, itPair.second);
  }
  return itPair;
}

IndexedSegmentPair getAdjacent(const IndexedPolygon& poly, const IndexedSegment& seg) {
  auto itP = getAlignedIterators(poly, seg);
  return {{*itP.first, itP.first == poly.begin() ? poly.back() : *std::prev(itP.first)},
          {*itP.second, itP.second == std::prev(poly.end()) ? poly.front() : *std::next(itP.second)}};
}

IndexedSegments getSegmentsExcept(const IndexedPolygon& p, const IndexedSegment& excluded) {
  IndexedSegments res;
  res.reserve(p.size() - 1);
  for (size_t i = 0; i < p.size(); ++i) {
    IndexedSegment seg{p.at(i), p.at((i + 1) % p.size())};
    if (seg != excluded && seg != flip(excluded)) {
      res.emplace_back(seg);
    }
  }
  return res;
}

IndexedSegments getSegmentIndexesExcept(const IndexedPolygon& p, const IndexedSegment& excluded) {
  IndexedSegments res;
  res.reserve(p.size() - 1);
  for (size_t i = 0; i < p.size(); ++i) {
    IndexedSegment seg{p.at(i), p.at((i + 1) % p.size())};
    if (seg != excluded && seg != flip(excluded)) {
      res.emplace_back(IndexedSegment{i, (i + 1) % p.size()});
    }
  }
  return res;
}

IndexedPolygon removeSegment(const IndexedPolygon& poly, const IndexedSegment& excluded) {
  auto itP = getAlignedIterators(poly, excluded);
  IndexedPolygon res(std::next(itP.second), std::next(itP.first) == poly.end() ? itP.first : poly.end());
  if (itP.first != std::prev(poly.end())) {
    res.insert(res.end(), poly.begin(), itP.first);
  }
  return res;
}

struct MergeResult {
  IndexedPolygon merged;
  IndexedSegments addedSegments;
};

void insertPolygonExcept(IndexedPolygon& poly, const IndexedPolygon& toMerge, const IndexedSegment& border) {
  auto mergeIt = getAlignedIterators(toMerge, border);
  auto npIt = getAlignedIterators(poly, border);
  auto noWrap = std::next(mergeIt.first) == toMerge.end();
  poly.insert(npIt.second, std::next(mergeIt.second), noWrap ? mergeIt.first : toMerge.end());
  if (!noWrap) {
    poly.insert(std::next(npIt.second, std::distance(std::next(mergeIt.second), toMerge.end())), toMerge.begin(),
                mergeIt.first);
  }
}

MergeResult merge(const IndexedPolygon& poly, const IndexedPolygon& toMerge, const IndexedSegment& border) {
  MergeResult result{{}, getSegmentsExcept(toMerge, border)};
  result.merged.reserve(poly.size() + toMerge.size());  // das hier ist die einzige Allokation
  result.merged = poly;
  insertPolygonExcept(result.merged, toMerge, border);
  return result;
}

using MetricSegmentPair = std::pair<BasicPoint2d, BasicPoint2d>;

MetricSegmentPair getMetricAdjacentSegmentPair(const BasicPolygon2d& metricPolygon, const IndexedPolygon& poly,
                                               const IndexedSegment& segment) {
  const auto newSegments = getAdjacent(poly, segment);
  const BasicPoint2d newFirst = metricPolygon.at(newSegments.first.second) - metricPolygon.at(newSegments.first.first);
  const BasicPoint2d newSecond =
      metricPolygon.at(newSegments.second.second) - metricPolygon.at(newSegments.second.first);
  return {newFirst, newSecond};
}
double getOpposingAngle(const MetricSegmentPair& segmentPair, const size_t polySize) {
  return polySize != 3 ? -1. : segmentPair.first.normalized().dot(segmentPair.second.normalized());
}
bool connectionConvex(const MetricSegmentPair& local, const MetricSegmentPair& extension) {
  return isConnectionConvex(-local.first, extension.second) && isConnectionConvex(-extension.first, local.second);
}
using AdjacentPolygons = std::map<IndexedSegment, size_t>;
using Visited = std::set<size_t>;
struct Gate {
  IndexedSegment border;
  size_t source;
};
using Gates = std::vector<Gate>;
Gates makeGates(const IndexedSegments& borders, const size_t src) {
  return utils::transform(borders, [&](auto& b) { return Gate{b, src}; });
}
struct Head {
  IndexedPolygon poly;
  Gates gates;  // run ccw
};
using Heads = std::vector<Head>;
struct Connection {
  IndexedSegment border;
  double opposingAngle;
  MetricSegmentPair connectors;
  size_t newPoly;
};
using Connections = std::vector<Connection>;
struct TraversalResult {
  std::vector<IndexedPolygon> convex;
  Visited visited;
};

template <typename K, typename V>
class VectorMap {
 public:
  using KeyType = K;
  using ValueType = V;
  using ElementType = std::pair<K, V>;
  using ContainerType = std::vector<ElementType>;

  void append(const KeyType& key, const ValueType& value) {
    if (frozen_) {
      throw std::runtime_error("Can't add to frozen vector map");
    }
    storage_.emplace_back(ElementType{key, value});
  }
  void append(const ElementType& elem) {
    if (frozen_) {
      throw std::runtime_error("Can't add to frozen vector map");
    }
    storage_.emplace_back(elem);
  }
  void freeze() {
    if (!frozen_) {
      std::sort(storage_.begin(), storage_.end());
      frozen_ = true;
    }
  }

  const ValueType& at(const KeyType& key) const {
    if (!frozen_) {
      throw std::runtime_error("Can't search in unfrozen vector map");
    }
    auto it = std::lower_bound(storage_.begin(), storage_.end(), key,
                               [](auto& value, const KeyType& k) { return value.first < k; });
    if (it == storage_.end() || key != it->first) {
      throw std::runtime_error("Item not found in vector map");
    }
    return it->second;
  }

 private:
  ContainerType storage_;
  bool frozen_{false};
};
using Adjacencies = VectorMap<IndexedSegment, size_t>;

class SegmentationData {
 public:
  explicit SegmentationData(const size_t convexPartsSize) { convexParts_.resize(convexPartsSize); }
  void addConvex(const IndexedPolygon& convexPoly, const int idx) { convexParts_[idx] = convexPoly; }
  void addSegment(const IndexedSegment& segment, const size_t adjacentPolygonIdx) {
    adjacent_.append(flip(segment), adjacentPolygonIdx);
  }
  const Adjacencies& getAdjacencies() const { return adjacent_; }
  const IndexedPolygons& getConvexPolygons() const { return convexParts_; }
  size_t getDestination(const Gate& gate) const { return adjacent_.at(gate.border); }
  const IndexedPolygon& getPolygonByIdx(const size_t idx) const { return convexParts_.at(idx); }
  void freeze() { adjacent_.freeze(); }

 private:
  Adjacencies adjacent_;
  std::vector<IndexedPolygon> convexParts_;
};

Connections findGates(const BasicPolygon2d& metricPolygon, const Gates& candidates, const Visited& overallVisited,
                      const SegmentationData& segData) {
  Connections gates;
  for (const auto& gate : candidates) {
    if (isBorder(gate.border, metricPolygon.size())) {
      continue;
    }
    const auto opposingPolygonIdx = segData.getDestination(gate);
    if (overallVisited.find(opposingPolygonIdx) != overallVisited.end()) {
      continue;
    }
    const auto& opposingPoly = segData.getPolygonByIdx(opposingPolygonIdx);
    auto metricSegPair = getMetricAdjacentSegmentPair(metricPolygon, opposingPoly, gate.border);
    gates.emplace_back(Connection{gate.border, getOpposingAngle(metricSegPair, opposingPoly.size()), metricSegPair,
                                  opposingPolygonIdx});
  }

  std::sort(gates.begin(), gates.end(),
            [](const auto& lhs, const auto& rhs) { return lhs.opposingAngle < rhs.opposingAngle; });
  return gates;
}

void updateState(Head& head, TraversalResult& traversalResult, const IndexedPolygon& newPoly, const Connection& conn) {
  traversalResult.visited.insert(conn.newPoly);
  auto mergeResult = merge(head.poly, newPoly, conn.border);
  head.poly = std::move(mergeResult.merged);
  head.gates = makeGates(mergeResult.addedSegments, conn.newPoly);
}

void traverse(const BasicPolygon2d& metricPolygon, Head& head, TraversalResult& traversalResult,
              const SegmentationData& segData, const bool isOrigin) {
  auto gates = findGates(metricPolygon, head.gates, traversalResult.visited, segData);
  std::vector<Connection> closedGates;
  for (const auto& conn : gates) {
    if (traversalResult.visited.find(conn.newPoly) != traversalResult.visited.end()) {
      continue;
    }
    const auto headsideAdjacent = getMetricAdjacentSegmentPair(metricPolygon, head.poly, conn.border);
    if (connectionConvex(headsideAdjacent, conn.connectors)) {
      updateState(head, traversalResult, segData.getPolygonByIdx(conn.newPoly), conn);
      traverse(metricPolygon, head, traversalResult, segData, false);
    } else {
      closedGates.emplace_back(conn);
    }
  }
  if (isOrigin) {
    traversalResult.convex.emplace_back(head.poly);
  }
  for (const auto& conn : closedGates) {
    if (traversalResult.visited.find(conn.newPoly) != traversalResult.visited.end()) {
      continue;
    }
    const auto& newPoly = segData.getPolygonByIdx(conn.newPoly);
    Head newHead{newPoly, makeGates(getSegmentsExcept(newPoly, conn.border), conn.newPoly)};
    traverse(metricPolygon, newHead, traversalResult, segData, true);
  }
}

std::vector<IndexedPolygon> mergeTriangulation(const BasicPolygon2d& metricPolygon, const SegmentationData& segData) {
  IndexedSegment init{1, 0};
  auto firstPolyIdx = segData.getAdjacencies().at(init);
  const auto& firstPoly = segData.getPolygonByIdx(firstPolyIdx);
  TraversalResult t{{}, Visited{firstPolyIdx}};
  Head initHead{firstPoly, makeGates(getSegmentsExcept(firstPoly, init), firstPolyIdx)};
  traverse(metricPolygon, initHead, t, segData, true);
  return t.convex;
}

BasicPolygon2d extractPolygon(const BasicPolygon2d& poly, const std::vector<size_t>& idxs) {
  BasicPolygon2d res(idxs.size());
  for (size_t i{0}; i < idxs.size(); ++i) {
    res[i] = poly.at(idxs.at(i));
  }
  return res;
}

void subdivide(IndexedTriangles& target, const IndexedPolygon& poly) {
  target.reserve(target.size() + poly.size() - 2);
  for (auto first = std::next(poly.begin()), second = std::next(poly.begin(), 2); second != poly.end();
       ++second, ++first) {
    target.emplace_back(IndexedTriangle{poly.front(), *first, *second});
  }
}

bool isTriangular(const IndexedPolygon& poly) { return poly.size() == 3; }

SegmentationData makeVoronoi(const BasicPolygon2d& poly) {
  VoronoiGraph graph;
  boost::polygon::construct_voronoi(poly.begin(), poly.end(), &graph);
  SegmentationData t(graph.num_vertices());
  auto advance = [](const VoronoiGraph::edge_type*& edge, VoronoiGraph::cell_type::source_index_type& lastIndex,
                    VoronoiGraph::cell_type::source_index_type& curIndex) {
    lastIndex = curIndex;
    edge = edge->rot_prev();
    curIndex = edge->cell()->source_index();
  };
  for (size_t i{0}; i < graph.num_vertices(); ++i) {
    const auto* const firstEdge = graph.vertices().at(i).incident_edge();
    auto startIdx = firstEdge->cell()->source_index();
    const auto* curEdge = firstEdge->rot_prev();
    auto curIdx = curEdge->cell()->source_index();
    auto lastIdx = startIdx;
    IndexedPolygon curPoly{startIdx};
    while (curIdx != startIdx) {
      t.addSegment({lastIdx, curIdx}, i);
      curPoly.emplace_back(curIdx);
      advance(curEdge, lastIdx, curIdx);
    }
    t.addSegment({lastIdx, curIdx}, i);
    t.addConvex(curPoly, i);
  }
  return t;
}

BasicPolygons2d convexPartitionImpl(const BasicPolygon2d& poly) {
  if (poly.size() < 3) {
    throw GeometryError("Can't partition a polygon with less than 3 points");
  }
  BasicPolygons2d res;
  auto segData = makeVoronoi(poly);
  segData.freeze();
  auto indexed = mergeTriangulation(poly, segData);
  for (const auto& i : indexed) {
    res.emplace_back(extractPolygon(poly, i));
  }
  return res;
}

IndexedTriangles triangulateImpl(const BasicPolygon2d& poly) {
  if (poly.size() < 3) {
    throw GeometryError("Can't triangulate a polygon with less than 3 points");
  }
  IndexedTriangles res;
  res.reserve(poly.size() - 2);
  auto segData = makeVoronoi(poly);
  for (const auto& i : segData.getConvexPolygons()) {
    if (isTriangular(i)) {
      res.emplace_back(IndexedTriangle{i.at(0), i.at(1), i.at(2)});
    } else {
      subdivide(res, i);
    }
  }
  return res;
}
}  // namespace

namespace lanelet {
namespace geometry {
namespace internal {
BasicPolygons2d convexPartition(const BasicPolygon2d& poly) { return convexPartitionImpl(poly); }
IndexedTriangles triangulate(const BasicPolygon2d& poly) { return triangulateImpl(poly); }
}  // namespace internal
}  // namespace geometry
}  // namespace lanelet