Skip to content
This repository has been archived by the owner on Mar 1, 2021. It is now read-only.

added find radial method to address #65 #67

Closed
wants to merge 1 commit into from
Closed

added find radial method to address #65 #67

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
184 changes: 111 additions & 73 deletions matching-core/src/main/java/com/graphhopper/matching/LocationIndexMatch.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -52,95 +55,130 @@ public LocationIndexMatch(GraphHopperStorage graph, LocationIndexTree index) {
this.index = index;
}

public List<QueryResult> 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<QueryResult> 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<QueryResult> 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<QueryResult> queryResults = new ArrayList<QueryResult>();
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;
}
}
}
2 changes: 1 addition & 1 deletion matching-core/src/main/java/com/graphhopper/matching/MapMatching.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ public MatchResult doWork(List<GPXEntry> gpxList) {
|| distanceCalc.calcDist(previous.getLat(), previous.getLon(), entry.getLat(), entry.getLon()) > 2 * measurementErrorSigma
// always include last point
|| indexGPX == gpxList.size() - 1) {
List<QueryResult> candidates = locationIndex.findNClosest(entry.lat, entry.lon, edgeFilter, measurementErrorSigma);
List<QueryResult> candidates = locationIndex.findEdgesWithinRadius(entry.lat, entry.lon, edgeFilter, measurementErrorSigma);
allCandidates.addAll(candidates);
List<GPXExtension> gpxExtensions = new ArrayList<GPXExtension>();
for (QueryResult candidate : candidates) {
Expand Down
2 changes: 1 addition & 1 deletion matching-core/src/test/java/com/graphhopper/matching/LocationIndexMatchTest.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ public void testFindNClosest() {
LocationIndexMatch index = new LocationIndexMatch(ghStorage, tmpIndex);

// query node 4 => get at least 4-5, 4-7
List<QueryResult> result = index.findNClosest(0.0004, 0.0006, EdgeFilter.ALL_EDGES, 15);
List<QueryResult> result = index.findEdgesWithinRadius(0.0004, 0.0006, EdgeFilter.ALL_EDGES, 15);
List<Integer> ids = new ArrayList<Integer>();
for (QueryResult qr : result) {
ids.add(qr.getClosestEdge().getEdge());
Expand Down
50 changes: 32 additions & 18 deletions matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<GPXEntry> inputGPXEntries = createRandomGPXEntries(
new GHPoint(51.358735, 12.360574),
new GHPoint(51.358594, 12.360032));
Expand All @@ -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));
Expand All @@ -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());
Expand All @@ -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<GPXEntry> 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
Expand All @@ -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<GPXEntry> 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"),
Expand All @@ -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<GPXEntry> inputGPXEntries = new GPXFile().doImport("./src/test/resources/tour-with-loop.gpx").getEntries();
MatchResult mr = mapMatching.doWork(inputGPXEntries);
Expand All @@ -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<GPXEntry> 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",
Expand Down