fzi-forschungszentrum-informatik / Lanelet2

Map handling framework for automated driving
BSD 3-Clause "New" or "Revised" License
800 stars 327 forks source link

Failed to triangulate concave polygon #106

Closed mitsudome-r closed 1 year ago

mitsudome-r commented 4 years ago

https://github.com/fzi-forschungszentrum-informatik/Lanelet2/commit/1c521a116fde6fffd67453042ed905d69736ea9b

I have tried triangulation function added in above commit, but it seems to fail with concave polygon. White lines represent boundary of lanes, and light blue polygon is triangulated result. image

poggenhans commented 4 years ago

Hm, that looks strange. Can you provide us a file with your lanelet so that we can look into this?

mitsudome-r commented 4 years ago

This occurs with your sample file as well. Here is the result of triangulation of lanelet 45030. Original shape of the lanelet. Screenshot from 2020-04-15 23-27-50

Triangulation Result: Screenshot from 2020-04-15 23-35-35

mitsudome-r commented 4 years ago

Here's the code I used to generate the result.

#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_core/geometry/Polygon.h>
#include <lanelet2_io/Io.h>
#include <lanelet2_io/io_handlers/Factory.h>
#include <lanelet2_io/io_handlers/Writer.h>
#include <lanelet2_projection/UTM.h>
#include <cstdio>

namespace {
std::string exampleMapPath = std::string(PKG_DIR) + "/../lanelet2_maps/res/mapping_example.osm";
}  // namespace

void triangulation();

int main() {
  // this tutorial shows you how to load and write lanelet maps. It is divided into three parts:
  triangulation();
  return 0;
}

void triangulation() {
  using namespace lanelet;
  projection::UtmProjector projector(Origin({49, 8.4}));  // we will go into details later
  LaneletMapPtr map = load(exampleMapPath, projector);
  LaneletMap triangle_map;

  const auto lanelet = map->laneletLayer.get(45030);
  const auto polygon2d = lanelet.polygon2d();

  const auto triangles = geometry::triangulate(polygon2d);
  for(const auto& triangle : triangles)
  {
    Points3d points;
    for (const auto& index : triangle)
    {
      const auto pt = polygon2d[index];
      Point3d new_pt(utils::getId(), pt.x(), pt.y(), 0.0);
      points.push_back(new_pt);
    }
    // add first point to close loop
    points.push_back(points.front());

    Polygon3d triangle_poly(utils::getId(), points);
    triangle_map.add(triangle_poly);
  } 

  write("triangle_map.osm", triangle_map, projector);
}
mitsudome-r commented 4 years ago

Normal Delaunay triangulation creates convex hull of a polygon. You need constrained/conforming Delaunay Triangulation in order to triangulate concave polygon. https://stackoverflow.com/questions/1858307/how-do-i-cut-triangles-out-of-a-concave-delaunay-triangulation

joeda commented 4 years ago

You are correct, the implementation is flawed.

Unfortunately, boost does not offer constrained Delaunay triangulation. The convexPartition function overcomes this by merging triangles inside the polygon and stopping at the polygon border. A similar approach could be considered for the triangulate function.

@poggenhans I don't see any other reasonably simple solution for this.

mitsudome-r commented 4 years ago

It might be worth taking different approach.

One idea I can think of is reusing some functions in calculateCenterline(). Current implementation in Lanelet2 seems to "progress" index either in leftBound or rightBound in order to calculate centerline, which is can be used for doing triangulation. It doesn't have same property as Delaunay triangulation, but you can still achieve the goal of triangulating concave polygons.

I drew an image with blue line. image

poggenhans commented 4 years ago

Sadly, the centerline algorithm is not suitable for this approach. It is similar to Delaunay triangulation, but it takes a few shortcuts.

There are some cases (with very crude polygons), where the triangulation cannot be done by simply connecting points from the left and the right side.

A simple "hack" would be to filter the triangles with a point-in-polygon test after triangulation. But I suppose the overhead compared to the solution @joeda suggested is quite high.

github-actions[bot] commented 1 year ago

This issue is stale because it has been open for 90 days with no activity. Is this issue still work in progress? If yes, mark it as "WIP" or comment, otherwise it will be closed in 30 days.

github-actions[bot] commented 1 year ago

This issue was closed because it has been stalled for 30 days with no activity.