Improve resolution of elevation profiles with 3D Douglas Peucker and long edge sampling (graphhopper#1953)

* replace calcMean with bilinear interpolate

* fix

* refactor all distance calculations to be elevation-aware

* some edge case handling and remove DistanceCalc2D

* throw error on graph.elevation.calcmean

* tighten range check

* change config parameter to 'graph.elevation.interpolate: bilinear'

* rm extra change

* Revert "change config parameter to 'graph.elevation.interpolate: bilinear'"

This reverts commit 25c3d9b.

* handle graph.elevation.interpolate=bilinear

* edge sampling and 3d polyline simplification

* add integration test

* integration test

* todo

* simplify IT

* license

* switch elevation factor to elevation max distance

* fix comment and warn on long edge sampling without bilinear interpolation

* new line

* add import

* config-example comments

* tweak config-example

* fix integration test

* fix bad merge

* calc 3d distance

* fix

* fix start/end being the same during simplification

* fix normalized edge distance 3d euclidean

* fix calc edge distance

* fix

* use great circle interpolation on long segments

* dont recompute same thing

* default values

* fix test

* fix merge issues

* rm extra file

* remove local, duplicated variable elevationWayPointMaxDistance

* let elevation_way_point_max_distance be controlled per-request

Co-authored-by: Peter <[email protected]>

Author: msbarry

api/src/main/java/com/graphhopper/util/DistanceCalc.java

@@ -113,6 +113,12 @@ GHPoint calcCrossingPointToEdge(double r_lat_deg, double r_lon_deg,
     GHPoint projectCoordinate(double lat, double lon,
                               double distanceInMeter, double headingClockwiseFromNorth);
 
+    /**
+     * This methods creates a point (lat, lon in degrees) a fraction of the distance along the path from (lat1, lon1)
+     * to (lat2, lon2).
+     */
+    GHPoint intermediatePoint(double f, double lat1, double lon1, double lat2, double lon2);
+
     /*
      * Simple heuristic to detect if the specified two points are crossing the boundary +-180°. See
      * #667

api/src/main/java/com/graphhopper/util/DistanceCalcEarth.java

@@ -119,91 +119,71 @@ public BBox createBBox(double lat, double lon, double radiusInMeter) {
     public double calcNormalizedEdgeDistance(double r_lat_deg, double r_lon_deg,
                                              double a_lat_deg, double a_lon_deg,
                                              double b_lat_deg, double b_lon_deg) {
-        return calcNormalizedEdgeDistanceNew(r_lat_deg, r_lon_deg, a_lat_deg, a_lon_deg, b_lat_deg, b_lon_deg, false);
-    }
-
-    @Override
-    public double calcNormalizedEdgeDistance3D(double r_lat_deg, double r_lon_deg, double r_ele_m,
-                                               double a_lat_deg, double a_lon_deg, double a_ele_m,
-                                               double b_lat_deg, double b_lon_deg, double b_ele_m) {
-        if (Double.isNaN(r_ele_m) || Double.isNaN(a_ele_m) || Double.isNaN(b_ele_m))
-            return calcNormalizedEdgeDistance(r_lat_deg, r_lon_deg, a_lat_deg, a_lon_deg, b_lat_deg, b_lon_deg);
-
         double shrinkFactor = calcShrinkFactor(a_lat_deg, b_lat_deg);
 
         double a_lat = a_lat_deg;
         double a_lon = a_lon_deg * shrinkFactor;
-        double a_ele = a_ele_m / METERS_PER_DEGREE;
 
         double b_lat = b_lat_deg;
         double b_lon = b_lon_deg * shrinkFactor;
-        double b_ele = b_ele_m / METERS_PER_DEGREE;
 
         double r_lat = r_lat_deg;
         double r_lon = r_lon_deg * shrinkFactor;
-        double r_ele = r_ele_m / METERS_PER_DEGREE;
 
         double delta_lon = b_lon - a_lon;
         double delta_lat = b_lat - a_lat;
-        double delta_ele = b_ele - a_ele;
 
-        double norm = delta_lon * delta_lon + delta_lat * delta_lat + delta_ele * delta_ele;
-        double factor = ((r_lon - a_lon) * delta_lon + (r_lat - a_lat) * delta_lat + (r_ele - a_ele) * delta_ele) / norm;
+        if (delta_lat == 0)
+            // special case: horizontal edge
+            return calcNormalizedDist(a_lat_deg, r_lon_deg, r_lat_deg, r_lon_deg);
 
-        // x,y,z is projection of r onto segment a-b
+        if (delta_lon == 0)
+            // special case: vertical edge
+            return calcNormalizedDist(r_lat_deg, a_lon_deg, r_lat_deg, r_lon_deg);
+
+        double norm = delta_lon * delta_lon + delta_lat * delta_lat;
+        double factor = ((r_lon - a_lon) * delta_lon + (r_lat - a_lat) * delta_lat) / norm;
+
+        // x,y is projection of r onto segment a-b
         double c_lon = a_lon + factor * delta_lon;
         double c_lat = a_lat + factor * delta_lat;
-        double c_ele_m = (a_ele + factor * delta_ele) * METERS_PER_DEGREE;
-        return calcNormalizedDist(c_lat, c_lon / shrinkFactor, r_lat_deg, r_lon_deg) + calcNormalizedDist(r_ele_m - c_ele_m);
+        return calcNormalizedDist(c_lat, c_lon / shrinkFactor, r_lat_deg, r_lon_deg);
     }
 
-    /**
-     * New edge distance calculation where no validEdgeDistance check would be necessary
-     * <p>
-     *
-     * @return the normalized distance of the query point "r" to the project point "c" onto the line
-     * segment a-b
-     */
-    public double calcNormalizedEdgeDistanceNew(double r_lat_deg, double r_lon_deg,
-                                                double a_lat_deg, double a_lon_deg,
-                                                double b_lat_deg, double b_lon_deg,
-                                                boolean reduceToSegment) {
+    @Override
+    public double calcNormalizedEdgeDistance3D(double r_lat_deg, double r_lon_deg, double r_ele_m,
+                                               double a_lat_deg, double a_lon_deg, double a_ele_m,
+                                               double b_lat_deg, double b_lon_deg, double b_ele_m) {
+        if (Double.isNaN(r_ele_m) || Double.isNaN(a_ele_m) || Double.isNaN(b_ele_m))
+            return calcNormalizedEdgeDistance(r_lat_deg, r_lon_deg, a_lat_deg, a_lon_deg, b_lat_deg, b_lon_deg);
+
         double shrinkFactor = calcShrinkFactor(a_lat_deg, b_lat_deg);
 
         double a_lat = a_lat_deg;
         double a_lon = a_lon_deg * shrinkFactor;
+        double a_ele = a_ele_m / METERS_PER_DEGREE;
 
         double b_lat = b_lat_deg;
         double b_lon = b_lon_deg * shrinkFactor;
+        double b_ele = b_ele_m / METERS_PER_DEGREE;
 
         double r_lat = r_lat_deg;
         double r_lon = r_lon_deg * shrinkFactor;
+        double r_ele = r_ele_m / METERS_PER_DEGREE;
 
         double delta_lon = b_lon - a_lon;
         double delta_lat = b_lat - a_lat;
+        double delta_ele = b_ele - a_ele;
 
-        if (delta_lat == 0)
-            // special case: horizontal edge
-            return calcNormalizedDist(a_lat_deg, r_lon_deg, r_lat_deg, r_lon_deg);
-
-        if (delta_lon == 0)
-            // special case: vertical edge
-            return calcNormalizedDist(r_lat_deg, a_lon_deg, r_lat_deg, r_lon_deg);
-
-        double norm = delta_lon * delta_lon + delta_lat * delta_lat;
-        double factor = ((r_lon - a_lon) * delta_lon + (r_lat - a_lat) * delta_lat) / norm;
+        double norm = delta_lon * delta_lon + delta_lat * delta_lat + delta_ele * delta_ele;
+        double factor = ((r_lon - a_lon) * delta_lon + (r_lat - a_lat) * delta_lat + (r_ele - a_ele) * delta_ele) / norm;
+        if (Double.isNaN(factor)) factor = 0;
 
-        // make new calculation compatible to old
-        if (reduceToSegment) {
-            if (factor > 1)
-                factor = 1;
-            else if (factor < 0)
-                factor = 0;
-        }
-        // x,y is projection of r onto segment a-b
+        // x,y,z is projection of r onto segment a-b
         double c_lon = a_lon + factor * delta_lon;
         double c_lat = a_lat + factor * delta_lat;
-        return calcNormalizedDist(c_lat, c_lon / shrinkFactor, r_lat_deg, r_lon_deg);
+        double c_ele_m = (a_ele + factor * delta_ele) * METERS_PER_DEGREE;
+        return calcNormalizedDist(c_lat, c_lon / shrinkFactor, r_lat_deg, r_lon_deg) + calcNormalizedDist(r_ele_m - c_ele_m);
     }
 
     double calcShrinkFactor(double a_lat_deg, double b_lat_deg) {
@@ -300,6 +280,41 @@ public GHPoint projectCoordinate(double latInDeg, double lonInDeg, double distan
         return new GHPoint(projectedLat, projectedLon);
     }
 
+    @Override
+    public GHPoint intermediatePoint(double f, double lat1, double lon1, double lat2, double lon2) {
+        double lat1radians = Math.toRadians(lat1);
+        double lon1radians = Math.toRadians(lon1);
+        double lat2radians = Math.toRadians(lat2);
+        double lon2radians = Math.toRadians(lon2);
+
+        // This formula is taken from: (http://www.movable-type.co.uk/scripts/latlong.html -> https://github.com/chrisveness/geodesy MIT)
+
+        double deltaLat = lat2radians - lat1radians;
+        double deltaLon = lon2radians - lon1radians;
+        double cosLat1 = cos(lat1radians);
+        double cosLat2 = cos(lat2radians);
+        double sinHalfDeltaLat = sin(deltaLat / 2);
+        double sinHalfDeltaLon = sin(deltaLon / 2);
+
+        double a = sinHalfDeltaLat * sinHalfDeltaLat + cosLat1 * cosLat2 * sinHalfDeltaLon * sinHalfDeltaLon;
+        double angularDistance =  2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
+        double sinDistance = sin(angularDistance);
+
+        if (angularDistance == 0) return new GHPoint(lat1, lon1);
+
+        double A = Math.sin((1-f)*angularDistance) / sinDistance;
+        double B = Math.sin(f*angularDistance) / sinDistance;
+
+        double x = A * cosLat1 * cos(lon1radians) + B * cosLat2 * cos(lon2radians);
+        double y = A * cosLat1 * sin(lon1radians) + B * cosLat2 * sin(lon2radians);
+        double z = A * sin(lat1radians) + B * sin(lat2radians);
+
+        double midLat = Math.toDegrees(Math.atan2(z, Math.sqrt(x*x + y*y)));
+        double midLon = Math.toDegrees(Math.atan2(y, x));
+
+        return new GHPoint(midLat, midLon);
+    }
+
     @Override
     public boolean isCrossBoundary(double lon1, double lon2) {
         return abs(lon1 - lon2) > 300;

api/src/main/java/com/graphhopper/util/Parameters.java

@@ -110,6 +110,7 @@ public static final class Routing {
          */
         public static final String WAY_POINT_MAX_DISTANCE = "way_point_max_distance";
         public static final String INIT_WAY_POINT_MAX_DISTANCE = ROUTING_INIT_PREFIX + "way_point_max_distance";
+        public static final String ELEVATION_WAY_POINT_MAX_DISTANCE = "elevation_way_point_max_distance";
         /**
          * true or false. If routes at via points should avoid u-turns. (not for CH) See related
          * 'heading' parameter:

api/src/test/java/com/graphhopper/util/DistanceCalcEarthTest.java

@@ -142,6 +142,46 @@ public void testEdgeDistance3dPlane() {
         assertEquals(10, calc.calcDenormalizedDist(dist), 1e-4);
     }
 
+    @Test
+    public void testEdgeDistanceStartEndSame() {
+        DistanceCalc calc = new DistancePlaneProjection();
+        // just change elevation
+        double dist = calc.calcNormalizedEdgeDistance3D(0, 0, 10,
+                0, 0, 0,
+                0, 0, 0);
+        assertEquals(10, calc.calcDenormalizedDist(dist), 1e-4);
+        // just change lat
+        dist = calc.calcNormalizedEdgeDistance3D(1, 0, 0,
+                0, 0, 0,
+                0, 0, 0);
+        assertEquals(DistanceCalcEarth.METERS_PER_DEGREE, calc.calcDenormalizedDist(dist), 1e-4);
+        // just change lon
+        dist = calc.calcNormalizedEdgeDistance3D(0, 1, 0,
+                0, 0, 0,
+                0, 0, 0);
+        assertEquals(DistanceCalcEarth.METERS_PER_DEGREE, calc.calcDenormalizedDist(dist), 1e-4);
+    }
+
+    @Test
+    public void testEdgeDistanceStartEndDifferentElevation() {
+        DistanceCalc calc = new DistancePlaneProjection();
+        // just change elevation
+        double dist = calc.calcNormalizedEdgeDistance3D(0, 0, 10,
+                0, 0, 0,
+                0, 0, 1);
+        assertEquals(0, calc.calcDenormalizedDist(dist), 1e-4);
+        // just change lat
+        dist = calc.calcNormalizedEdgeDistance3D(1, 0, 0,
+                0, 0, 0,
+                0, 0, 1);
+        assertEquals(DistanceCalcEarth.METERS_PER_DEGREE, calc.calcDenormalizedDist(dist), 1e-4);
+        // just change lon
+        dist = calc.calcNormalizedEdgeDistance3D(0, 1, 0,
+                0, 0, 0,
+                0, 0, 1);
+        assertEquals(DistanceCalcEarth.METERS_PER_DEGREE, calc.calcDenormalizedDist(dist), 1e-4);
+    }
+
     @Test
     public void testValidEdgeDistance() {
         assertTrue(dc.validEdgeDistance(49.94241, 11.544356, 49.937964, 11.541824, 49.942272, 11.555643));
@@ -257,4 +297,38 @@ public void testDistance3dPlaneNaN() {
                 0, 0, Double.NaN
         ), 1e-6);
     }
+
+    @Test
+    public void testIntermediatePoint() {
+        DistanceCalc distCalc = new DistanceCalcEarth();
+        GHPoint point = distCalc.intermediatePoint(0, 0, 0, 0, 0);
+        assertEquals(0, point.getLat(), 1e-5);
+        assertEquals(0, point.getLon(), 1e-5);
+
+        point = distCalc.intermediatePoint(0.5, 0, 0, 10, 0);
+        assertEquals(5, point.getLat(), 1e-5);
+        assertEquals(0, point.getLon(), 1e-5);
+
+        point = distCalc.intermediatePoint(0.5, 0, 0, 0, 10);
+        assertEquals(0, point.getLat(), 1e-5);
+        assertEquals(5, point.getLon(), 1e-5);
+
+        // cross international date line going west
+        point = distCalc.intermediatePoint(0.5, 45, -179, 45, 177);
+        assertEquals(45, point.getLat(), 1);
+        assertEquals(179, point.getLon(), 1e-5);
+
+        // cross international date line going east
+        point = distCalc.intermediatePoint(0.5, 45, 179, 45, -177);
+        assertEquals(45, point.getLat(), 1);
+        assertEquals(-179, point.getLon(), 1e-5);
+
+        // cross north pole
+        point = distCalc.intermediatePoint(0.25, 45, -90, 45, 90);
+        assertEquals(67.5, point.getLat(), 1e-1);
+        assertEquals(-90, point.getLon(), 1e-5);
+        point = distCalc.intermediatePoint(0.75, 45, -90, 45, 90);
+        assertEquals(67.5, point.getLat(), 1e-1);
+        assertEquals(90, point.getLon(), 1e-5);
+    }
 }

config-example.yml

@@ -92,6 +92,19 @@ graphhopper:
   # graph.elevation.dataaccess: RAM_STORE
 
 
+  # To enable bilinear interpolation when sampling elevation at points (default uses nearest neighbor):
+  # graph.elevation.interpolate: bilinear
+
+
+  # To increase elevation profile resolution, use the following two parameters to tune the extra resolution you need
+  # against the additional storage space used for edge geometries. You should enable bilinear interpolation when using
+  # these features (see #1953 for details).
+  # - first, set the distance (in meters) at which elevation samples should be taken on long edges
+  # graph.elevation.long_edge_sampling_distance: 60
+  # - second, set the elevation tolerance (in meters) to use when simplifying polylines since the default ignores
+  #   elevation and will remove the extra points that long edge sampling added
+  # graph.elevation.way_point_max_distance: 10
+
 
   #### Speed, hybrid and flexible mode ####
 

core/src/main/java/com/graphhopper/GraphHopper.java

@@ -86,6 +86,7 @@ public class GraphHopper implements GraphHopperAPI {
     private boolean allowWrites = true;
     private boolean fullyLoaded = false;
     private boolean smoothElevation = false;
+    private double longEdgeSamplingDistance = Double.MAX_VALUE;
     // for routing
     private final RouterConfig routerConfig = new RouterConfig();
     // for index
@@ -328,6 +329,22 @@ public GraphHopper setElevation(boolean includeElevation) {
         return this;
     }
 
+    /**
+     * Sets the distance distance between elevation samples on long edges
+     */
+    public GraphHopper setLongEdgeSamplingDistance(double longEdgeSamplingDistance) {
+        this.longEdgeSamplingDistance = longEdgeSamplingDistance;
+        return this;
+    }
+
+    /**
+     * Sets the max elevation discrepancy between way points and the simplified polyline in meters
+     */
+    public GraphHopper setElevationWayPointMaxDistance(double elevationWayPointMaxDistance) {
+        this.routerConfig.setElevationWayPointMaxDistance(elevationWayPointMaxDistance);
+        return this;
+    }
+
     public String getGraphHopperLocation() {
         return ghLocation;
     }
@@ -485,9 +502,14 @@ public GraphHopper init(GraphHopperConfig ghConfig) {
 
         // elevation
         this.smoothElevation = ghConfig.getBool("graph.elevation.smoothing", false);
+        this.longEdgeSamplingDistance = ghConfig.getDouble("graph.elevation.long_edge_sampling_distance", Double.MAX_VALUE);
+        setElevationWayPointMaxDistance(ghConfig.getDouble("graph.elevation.way_point_max_distance", Double.MAX_VALUE));
         ElevationProvider elevationProvider = createElevationProvider(ghConfig);
         setElevationProvider(elevationProvider);
 
+        if (longEdgeSamplingDistance < Double.MAX_VALUE && !elevationProvider.getInterpolate())
+            logger.warn("Long edge sampling enabled, but bilinear interpolation disabled. See #1953");
+
         // optimizable prepare
         minNetworkSize = ghConfig.getInt("prepare.min_network_size", minNetworkSize);
 
@@ -687,7 +709,9 @@ protected DataReader initDataReader(DataReader reader) {
                 setElevationProvider(eleProvider).
                 setWorkerThreads(dataReaderWorkerThreads).
                 setWayPointMaxDistance(dataReaderWayPointMaxDistance).
-                setSmoothElevation(this.smoothElevation);
+                setWayPointElevationMaxDistance(routerConfig.getElevationWayPointMaxDistance()).
+                setSmoothElevation(smoothElevation).
+                setLongEdgeSamplingDistance(longEdgeSamplingDistance);
     }
 
     /**

core/src/main/java/com/graphhopper/reader/DataReader.java

@@ -36,8 +36,12 @@ public interface DataReader {
 
     DataReader setWayPointMaxDistance(double wayPointMaxDistance);
 
+    DataReader setWayPointElevationMaxDistance(double elevationWayPointMaxDistance);
+
     DataReader setSmoothElevation(boolean smoothElevation);
 
+    DataReader setLongEdgeSamplingDistance(double longEdgeSamplingDistance);
+
     /**
      * This method triggers reading the underlying data to create a graph
      */

core/src/main/java/com/graphhopper/reader/dem/AbstractElevationProvider.java

@@ -59,6 +59,11 @@ public void setInterpolate(boolean interpolate) {
         this.interpolate = interpolate;
     }
 
+    @Override
+    public boolean getInterpolate() {
+        return this.interpolate;
+    }
+
     void setSleep(long sleep) {
         this.sleep = sleep;
     }