diff --git a/matching-core/src/main/java/com/graphhopper/matching/LocationIndexMatch.java b/matching-core/src/main/java/com/graphhopper/matching/LocationIndexMatch.java index 789bb171..7cd69c89 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/LocationIndexMatch.java +++ b/matching-core/src/main/java/com/graphhopper/matching/LocationIndexMatch.java @@ -25,6 +25,9 @@ import com.graphhopper.storage.index.QueryResult; import com.graphhopper.util.EdgeExplorer; import com.graphhopper.util.EdgeIteratorState; +import com.graphhopper.util.shapes.BBox; +import com.graphhopper.util.shapes.GHPoint; + import gnu.trove.procedure.TIntProcedure; import gnu.trove.set.hash.TIntHashSet; import java.util.ArrayList; @@ -52,95 +55,130 @@ public LocationIndexMatch(GraphHopperStorage graph, LocationIndexTree index) { this.index = index; } - public List findNClosest(final double queryLat, final double queryLon, final EdgeFilter edgeFilter, - double gpxAccuracyInMetern) { - // Return ALL results which are very close and e.g. within the GPS signal accuracy. - // Also important to get all edges if GPS point is close to a junction. - final double returnAllResultsWithin = distCalc.calcNormalizedDist(gpxAccuracyInMetern); + /** + * This method finds all the edges which are within a radial distance from a given point. + * + * @param maxDistance edges must be maxDistance or less from (queryLat, queryLon) * + * @return a list of such edges, sorted by the distance from the query point (closest first). + */ + public List findEdgesWithinRadius(final double queryLat, final double queryLon, final EdgeFilter edgeFilter, double maxDistance) { + return findEdgesWithinRadius(queryLat, queryLon, edgeFilter, 0, maxDistance); + } + /** + * This method finds all the edges which are within a radial distance from a given point. + * + * @param minDistance edges must be minDistance or further from (queryLat, queryLon) + * @param maxDistance edges must be maxDistance or less from (queryLat, queryLon) * + * @return a list of such edges, sorted by the distance from the query point (closest first). + */ + public List findEdgesWithinRadius(final double queryLat, final double queryLon, final EdgeFilter edgeFilter, double minDistance, double maxDistance) { + + final double dLat = index.deltaLat; + final double dLon = index.deltaLon; + // implement a cheap priority queue via List, sublist and Collections.sort final List queryResults = new ArrayList(); - TIntHashSet set = new TIntHashSet(); - - for (int iteration = 0; iteration < 2; iteration++) { - // should we use the return value of earlyFinish? - index.findNetworkEntries(queryLat, queryLon, set, iteration); - - final GHBitSet exploredNodes = new GHTBitSet(new TIntHashSet(set)); - final EdgeExplorer explorer = graph.createEdgeExplorer(edgeFilter); - - set.forEach(new TIntProcedure() { - - @Override - public boolean execute(int node) { - new XFirstSearchCheck(queryLat, queryLon, exploredNodes, edgeFilter) { - @Override - protected double getQueryDistance() { - // do not skip search if distance is 0 or near zero (equalNormedDelta) - return Double.MAX_VALUE; - } - - @Override - protected boolean check(int node, double normedDist, int wayIndex, EdgeIteratorState edge, QueryResult.Position pos) { - if (normedDist < returnAllResultsWithin - || queryResults.isEmpty() - || queryResults.get(0).getQueryDistance() > normedDist) { - - int index = -1; - for (int qrIndex = 0; qrIndex < queryResults.size(); qrIndex++) { - QueryResult qr = queryResults.get(qrIndex); - // overwrite older queryResults which are potentially more far away than returnAllResultsWithin - if (qr.getQueryDistance() > returnAllResultsWithin) { - index = qrIndex; - break; - } - - // avoid duplicate edges - if (qr.getClosestEdge().getEdge() == edge.getEdge()) { - if (qr.getQueryDistance() < normedDist) { - // do not add current edge - return true; - } else { - // overwrite old edge with current - index = qrIndex; - break; - } - } - } - - QueryResult qr = new QueryResult(queryLat, queryLon); - qr.setQueryDistance(normedDist); - qr.setClosestNode(node); - qr.setClosestEdge(edge.detach(false)); - qr.setWayIndex(wayIndex); - qr.setSnappedPosition(pos); - - if (index < 0) { - queryResults.add(qr); - } else { - queryResults.set(index, qr); + final TIntHashSet found = new TIntHashSet(); + + // get boundaries: + final GHPoint outerNorth = distCalc.projectCoordinate(queryLat, queryLon, maxDistance, 0); + final GHPoint outerEast = distCalc.projectCoordinate(queryLat, queryLon, maxDistance, 90); + final GHPoint outerSouth = distCalc.projectCoordinate(queryLat, queryLon, maxDistance, 180); + final GHPoint outerWest = distCalc.projectCoordinate(queryLat, queryLon, maxDistance, 270); + final double latMin = outerSouth.lat - dLat; + final double latMax = outerNorth.lat + dLat; + final double lonMin = outerWest.lon - dLon; + final double lonMax = outerEast.lon + dLon; + + // get the radii in normed units: + final double normedMinDistance = distCalc.calcNormalizedDist(minDistance); + final double normedMaxDistance = distCalc.calcNormalizedDist(maxDistance); + + // get the min/max allowed radii: we add/remove the max tile dimension (i.e. it's diagonal), as this means that + // we still get x, even in this case: + // + // |-------|-------| + // | . | x| + // | . | | + // | |. | + // |-------|--.----| + // | | . | + // + // where the dots represent the radius (with center somewhere down to left), and the x represents a valid point. That is, we still + // include the tile containing x, even though it's outside the radius. + final double deltaR = distCalc.calcDist(queryLat, queryLon, queryLat + dLat, queryLon + dLon); + final double lowerBoundNormedMinDistance = distCalc.calcNormalizedDist(Math.max(0, minDistance - deltaR)); + final double upperBoundNormedMaxDistance = distCalc.calcNormalizedDist(maxDistance + deltaR); + + // loop through tiles, and only consider those within minInner/maxOuter radius. If they are, add all + // the entries from that tile. + for (double lat = latMin; lat <= latMax; lat += dLat) { + for (double lon = lonMin; lon <= lonMax; lon += dLon) { + // find the points here if they're in bounds (including tolerance): + double d = distCalc.calcNormalizedDist(queryLat, queryLon, lat, lon); + if (lowerBoundNormedMinDistance < d && d < upperBoundNormedMaxDistance) { + index.findNetworkEntriesSingleRegion(found, lat, lon); + } + } + } + + // now loop through and filter to only include those which match the edgeFilter, and are (exactly) + // within the inner/outer radius. + final GHBitSet exploredNodes = new GHTBitSet(new TIntHashSet(found)); + final EdgeExplorer explorer = graph.createEdgeExplorer(edgeFilter); + + found.forEach(new TIntProcedure() { + + @Override + public boolean execute(int node) { + new XFirstSearchCheck(queryLat, queryLon, exploredNodes, edgeFilter) { + @Override + protected double getQueryDistance() { + // do not skip search if distance is 0 or near zero (equalNormedDelta) + return Double.MAX_VALUE; + } + + @Override + protected boolean check(int node, double normedDist, int wayIndex, EdgeIteratorState edge, QueryResult.Position pos) { + if (normedMinDistance <= normedDist && normedDist <= normedMaxDistance) { + + // check we don't already have it: + for (int qrIndex = 0; qrIndex < queryResults.size(); qrIndex++) { + QueryResult qr = queryResults.get(qrIndex); + if (qr.getClosestEdge().getEdge() == edge.getEdge()) { + return true; } } - return true; + + // cool, let's add it: + QueryResult qr = new QueryResult(queryLat, queryLon); + qr.setQueryDistance(normedDist); + qr.setClosestNode(node); + qr.setClosestEdge(edge.detach(false)); + qr.setWayIndex(wayIndex); + qr.setSnappedPosition(pos); + queryResults.add(qr); } - }.start(explorer, node); - return true; - } - }); - } + return true; + } + }.start(explorer, node); + return true; + } + }); + // sorted list: Collections.sort(queryResults, QR_COMPARATOR); + // denormalize distances and calculate the snapped point: for (QueryResult qr : queryResults) { if (qr.isValid()) { - // denormalize distance qr.setQueryDistance(distCalc.calcDenormalizedDist(qr.getQueryDistance())); qr.calcSnappedPoint(distCalc); } else { throw new IllegalStateException("Invalid QueryResult should not happen here: " + qr); } } - return queryResults; } -} +} \ No newline at end of file diff --git a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java index 8b95581b..aaa7cb95 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java +++ b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java @@ -145,7 +145,7 @@ public MatchResult doWork(List gpxList) { || distanceCalc.calcDist(previous.getLat(), previous.getLon(), entry.getLat(), entry.getLon()) > 2 * measurementErrorSigma // always include last point || indexGPX == gpxList.size() - 1) { - List candidates = locationIndex.findNClosest(entry.lat, entry.lon, edgeFilter, measurementErrorSigma); + List candidates = locationIndex.findEdgesWithinRadius(entry.lat, entry.lon, edgeFilter, measurementErrorSigma); allCandidates.addAll(candidates); List gpxExtensions = new ArrayList(); for (QueryResult candidate : candidates) { diff --git a/matching-core/src/test/java/com/graphhopper/matching/LocationIndexMatchTest.java b/matching-core/src/test/java/com/graphhopper/matching/LocationIndexMatchTest.java index 0aa323d7..0aa4800a 100644 --- a/matching-core/src/test/java/com/graphhopper/matching/LocationIndexMatchTest.java +++ b/matching-core/src/test/java/com/graphhopper/matching/LocationIndexMatchTest.java @@ -84,7 +84,7 @@ public void testFindNClosest() { LocationIndexMatch index = new LocationIndexMatch(ghStorage, tmpIndex); // query node 4 => get at least 4-5, 4-7 - List result = index.findNClosest(0.0004, 0.0006, EdgeFilter.ALL_EDGES, 15); + List result = index.findEdgesWithinRadius(0.0004, 0.0006, EdgeFilter.ALL_EDGES, 15); List ids = new ArrayList(); for (QueryResult qr : result) { ids.add(qr.getClosestEdge().getEdge()); diff --git a/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java b/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java index eab6e1ff..47019208 100644 --- a/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java +++ b/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java @@ -29,6 +29,7 @@ import com.graphhopper.storage.index.LocationIndex; import com.graphhopper.storage.index.LocationIndexTree; import com.graphhopper.util.BreadthFirstSearch; +import com.graphhopper.util.DistanceCalcEarth; import com.graphhopper.util.EdgeExplorer; import com.graphhopper.util.EdgeIteratorState; import com.graphhopper.util.GPXEntry; @@ -81,9 +82,10 @@ public void testDoWork() { (LocationIndexTree) HOPPER.getLocationIndex()); MapMatching mapMatching = new MapMatching(graph, locationIndex, ENCODER); - // mapMatching.setMeasurementErrorSigma(5); + mapMatching.setMeasurementErrorSigma(5); // printOverview(graph, hopper.getLocationIndex(), 51.358735, 12.360574, 500); + // https://graphhopper.com/maps/?point=51.358735%2C12.360574&point=51.358594%2C12.360032&layer=Lyrk List inputGPXEntries = createRandomGPXEntries( new GHPoint(51.358735, 12.360574), new GHPoint(51.358594, 12.360032)); @@ -110,6 +112,7 @@ public void testDoWork() { assertEquals(il.toString(), 2, il.size()); assertEquals("Platnerstraße", il.get(0).getName()); + // https://graphhopper.com/maps/?point=51.33099%2C12.380267&point=51.330689%2C12.380776&layer=Lyrk inputGPXEntries = createRandomGPXEntries( new GHPoint(51.33099, 12.380267), new GHPoint(51.330689, 12.380776)); @@ -131,16 +134,15 @@ public void testDoWork() { assertEquals("Bayrischer Platz", il.get(1).getName()); // full path + // https://graphhopper.com/maps/?point=51.377781%2C12.338333&point=51.323317%2C12.387085&layer=Lyrk inputGPXEntries = createRandomGPXEntries( new GHPoint(51.377781, 12.338333), new GHPoint(51.323317, 12.387085)); mapMatching = new MapMatching(graph, locationIndex, ENCODER); - mapMatching.setMeasurementErrorSigma(20); + mapMatching.setMeasurementErrorSigma(5); // new GPXFile(inputGPXEntries).doExport("test-input.gpx"); mr = mapMatching.doWork(inputGPXEntries); // new GPXFile(mr).doExport("test.gpx"); - - // System.out.println(fetchStreets(mr.getEdgeMatches())); assertEquals(mr.getGpxEntriesLength(), mr.getMatchLength(), 0.5); assertEquals(mr.getGpxEntriesMillis(), mr.getMatchMillis(), 200); assertEquals(138, mr.getEdgeMatches().size()); @@ -155,18 +157,29 @@ public void testSmallSeparatedSearchDistance() { LocationIndexMatch locationIndex = new LocationIndexMatch(graph, (LocationIndexTree) HOPPER.getLocationIndex()); + MapMatching mapMatching = new MapMatching(graph, locationIndex, ENCODER); + // import sample where two GPX entries are on one edge which is longer than 'separatedSearchDistance' aways (66m) - // https://graphhopper.com/maps/?point=51.359723%2C12.360108&point=51.358748%2C12.358798&point=51.358001%2C12.357597&point=51.358709%2C12.356511&layer=Lyrk + // https://graphhopper.com/maps/?point=51.359723%2C12.360108&point=51.359621%2C12.360243&point=51.358591%2C12.358584&point=51.358189%2C12.357876&point=51.358007%2C12.357403&point=51.358627%2C12.356612&point=51.358709%2C12.356511&locale=en-GB&vehicle=car&weighting=fastest&elevation=true&use_miles=false&layer=Lyrk List inputGPXEntries = new GPXFile().doImport("./src/test/resources/tour3-with-long-edge.gpx").getEntries(); - // TODO match at Weinlingstraße instead of following small part of Marbachstraße - MapMatching mapMatching = new MapMatching(graph, locationIndex, ENCODER); - mapMatching.setMeasurementErrorSigma(20); + + // fuzzy match: we exclude the endpoints with large sigma: + mapMatching.setMeasurementErrorSigma(50); MatchResult mr = mapMatching.doWork(inputGPXEntries); - assertEquals(Arrays.asList("Weinligstraße", "Weinligstraße", - "Weinligstraße", "Fechnerstraße", "Fechnerstraße"), + assertEquals(Arrays.asList("Weinligstraße", "Weinligstraße", "Fechnerstraße"), + fetchStreets(mr.getEdgeMatches())); + assertEquals(mr.getGpxEntriesLength(), mr.getMatchLength(), 16); + assertEquals(mr.getGpxEntriesMillis(), mr.getMatchMillis(), 4500); + + // more exact match: include the endpoints: + mapMatching.setMeasurementErrorSigma(5); + mr = mapMatching.doWork(inputGPXEntries); + assertEquals(Arrays.asList("Marbachstraße", "Weinligstraße", "Weinligstraße", + "Fechnerstraße", "Fechnerstraße"), fetchStreets(mr.getEdgeMatches())); assertEquals(mr.getGpxEntriesLength(), mr.getMatchLength(), 11); assertEquals(mr.getGpxEntriesMillis(), mr.getMatchMillis(), 3000); + } @Test @@ -179,10 +192,11 @@ public void testLoop() { // printOverview(graph, hopper.getLocationIndex(), 51.345796,12.360681, 1000); // https://graphhopper.com/maps/?point=51.343657%2C12.360708&point=51.344982%2C12.364066&point=51.344841%2C12.361223&point=51.342781%2C12.361867&layer=Lyrk List inputGPXEntries = new GPXFile().doImport("./src/test/resources/tour2-with-loop.gpx").getEntries(); + mapMatching.setMeasurementErrorSigma(20); MatchResult mr = mapMatching.doWork(inputGPXEntries); // new GPXFile(mr).doExport("testLoop-matched.gpx"); - // Expected is ~800m. If too short like 166m then the loop was skipped + // Expected is ~800m. If too short like 166m then the loop was skipped assertEquals(Arrays.asList("Gustav-Adolf-Straße", "Gustav-Adolf-Straße", "Gustav-Adolf-Straße", "Leibnizstraße", "Hinrichsenstraße", "Hinrichsenstraße", "Tschaikowskistraße", "Tschaikowskistraße"), @@ -198,8 +212,8 @@ public void testLoop2() { LocationIndexMatch locationIndex = new LocationIndexMatch(graph, (LocationIndexTree) HOPPER.getLocationIndex()); MapMatching mapMatching = new MapMatching(graph, locationIndex, ENCODER); - // TODO smaller sigma like 40m leads to U-turn at Tschaikowskistraße - mapMatching.setMeasurementErrorSigma(50); + // NOTE: larger sigma leads to odd route - it looks like the map is incorrect when compared to Google Maps. + mapMatching.setMeasurementErrorSigma(20); // https://graphhopper.com/maps/?point=51.342439%2C12.361615&point=51.343719%2C12.362784&point=51.343933%2C12.361781&point=51.342325%2C12.362607&layer=Lyrk List inputGPXEntries = new GPXFile().doImport("./src/test/resources/tour-with-loop.gpx").getEntries(); MatchResult mr = mapMatching.doWork(inputGPXEntries); @@ -219,13 +233,13 @@ public void testUTurns() { // https://graphhopper.com/maps/?point=51.343618%2C12.360772&point=51.34401%2C12.361776&point=51.343977%2C12.362886&point=51.344734%2C12.36236&point=51.345233%2C12.362055&layer=Lyrk List inputGPXEntries = new GPXFile().doImport("./src/test/resources/tour4-with-uturn.gpx").getEntries(); + + // exclude U-turn mapMatching.setMeasurementErrorSigma(50); MatchResult mr = mapMatching.doWork(inputGPXEntries); - assertEquals(Arrays.asList("Gustav-Adolf-Straße", "Gustav-Adolf-Straße", - "Funkenburgstraße", "Funkenburgstraße"), - fetchStreets(mr.getEdgeMatches())); - - // inclusive U-turn + assertEquals(Arrays.asList("Gustav-Adolf-Straße", "Gustav-Adolf-Straße", "Funkenburgstraße"), fetchStreets(mr.getEdgeMatches())); + + // include U-turn mapMatching.setMeasurementErrorSigma(10); mr = mapMatching.doWork(inputGPXEntries); assertEquals(Arrays.asList("Gustav-Adolf-Straße", "Gustav-Adolf-Straße",