/// <summary> /// Get initial candidates, i.e., adjacent vertices /// </summary> /// <param name="head"></param> /// <param name="openTable"></param> /// <param name="currentIdx"></param> /// <returns>the currentIdx, larger than or equal to the given value</returns> private int getInitialCands(SortedSet <TrjTreeNode> openTable, int currentIdx) { double baseDist = GPS_ERROR_DISTANCE; bool initialVertexFound = false; int trjCount = trj.Count; while (openTable.Count == 0 && currentIdx < trjCount - 1) { GeoPoint currentPoint = trj[currentIdx].point; GeoPoint anotherPoint = trj[currentIdx + 1].point; HashSet <Vertex> currentVertices = graph.VertexRangeQuery(currentPoint, baseDist); foreach (Vertex v in currentVertices) { double tmpDist = Polyline.DistFrom(currentPoint, anotherPoint, v.Point); if (tmpDist <= baseDist) { TrjTreeNode node = new TrjTreeNode() { v = v, path = null, parent = null, idx = currentIdx, score = 0 }; initialVertexFound = true; openTable.Add(node); } } if (!initialVertexFound) { currentIdx++; } } return(currentIdx); }
public override RoadNetwork.Trajectory match(RoadNetwork.Trajectory trajectory) { this.trj = trajectory; double cellSize = 50; GridTrj trjIndex = new GridTrj(trj, cellSize); SortedSet <TrjTreeNode> openTable = new SortedSet <TrjTreeNode>(new TrjTreeNodeComparer()); List <TrjTreeNode> closeTable = new List <TrjTreeNode>(); int trjCount = trj.Count; int currentIdx = 0; while (currentIdx < trjCount - 1) { //1. get initial candidates if (openTable.Count == 0) { currentIdx = getInitialCands(openTable, currentIdx); } //2. Create candidate paths TrjTreeNode currentNode = null; while (openTable.Count > 0) { Debug.Assert(currentIdx < trjCount); //2.1.get the first node currentNode = openTable.Min; openTable.Remove(currentNode); closeTable.Add(currentNode); //2.2. create candidate paths List <TrjTreeNode> cands = createCands(trjIndex, currentNode); foreach (var cand in cands) { //2.3. check parallel paths if (openTable.Contains(cand)) { var tempCand = openTable.Where((n) => n == cand); Trace.Assert(tempCand.Count() == 1, "More than two parallel paths were found!"); TrjTreeNode bestCand = getBestPath(cand, tempCand.First()); openTable.Add(bestCand); } else { openTable.Add(cand); } } //2.3 prune and confirm if (openTable.Count > 3) { openTable = pruneConfirm(openTable); } currentIdx = currentNode.idx + 1; //for breaking the loop } } //3. Get the matched result Trajectory newTrj = getMatchedResult(closeTable); return(newTrj); }
/// <summary> /// Build paths within the error ellipse /// </summary> /// <param name="currentNode"></param> /// <returns></returns> private List <EdgePath> buildPath(TrjTreeNode currentNode) { Vertex startV = currentNode.v; int predIdx = currentNode.idx; int currentIdx = currentNode.idx + 1; GeoPoint currentPoint = trj[currentIdx].point; GeoPoint nextPoint = trj[currentIdx + 1].point; int interval = (int)(trj[currentIdx + 1].t - trj[currentIdx].t); double maxSpeed = Constants.MAX_SPEED; Queue <EdgePath> rawCandidatePaths = new Queue <EdgePath>(); List <EdgePath> candidatePaths = new List <EdgePath>(); //define the maximum allowed distance, i.e., the 2c of the error ellipse double maxDistance = Math.Min(interval * maxSpeed, Math.Max(GeoPoint.GetDistance(currentPoint, nextPoint) * 2, GeoPoint.GetDistance(currentPoint, nextPoint) + GPS_ERROR_DISTANCE)); //get initial candidate if (currentNode.path != null && currentNode.path.Count > 0) { rawCandidatePaths.Enqueue(new EdgePath() { currentNode.path.LastEdge }); } else { Trace.Assert(currentNode.v.InEdges.Count > 0); //Choose the first element of the InEdges EdgePath path = new EdgePath(currentNode.v); rawCandidatePaths.Enqueue(path); } while (rawCandidatePaths.Count > 0) { EdgePath path = rawCandidatePaths.Dequeue(); //extend it foreach (Edge e in path.End.OutEdges) { if (path.Contains(e.End)) //avoid duplicate vertices { continue; } EdgePath newPath = new EdgePath(path); newPath.Add(e); GeoPoint p = e.End.Point; GeoPoint result; int projectType = e.projectFrom(nextPoint, out result); double distance = GeoPoint.GetDistance(result, nextPoint); if (projectType == 0 && distance < GPS_ERROR_DISTANCE) { candidatePaths.Add(newPath); } else if (GeoPoint.GetDistance(p, currentPoint) + GeoPoint.GetDistance(p, nextPoint) < maxDistance) { //need further extension rawCandidatePaths.Enqueue(newPath); } } } return(candidatePaths); }
/// <summary> /// Choose the best path from the two parallel paths by considering the cost similarity <br/> /// However, if the speed limit information is not available, only the distance and difference between the trajectory and the real path will be used. /// </summary> /// <param name="cand"></param> /// <param name="tempCand"></param> /// <returns></returns> private TrjTreeNode getBestPath(TrjTreeNode cand, TrjTreeNode tempCand) { //1. get lowest common ancestor //2. get the two paths //3. evaluate the two paths return(cand); }
/// <summary> /// Create candidate paths /// </summary> /// <param name="trjIndex"></param> /// <param name="currentNode"></param> /// <returns></returns> private List <TrjTreeNode> createCands(GridTrj trjIndex, TrjTreeNode currentNode) { //1.get candidate paths for the current node List <EdgePath> paths = buildPath(currentNode); List <TrjTreeNode> cands = new List <TrjTreeNode>(); List <EdgePath> newPaths = new List <EdgePath>(); double baseDist = GPS_ERROR_DISTANCE; int currentIdx = currentNode.idx; //2.find the trajectory point near the end vertex of the path foreach (EdgePath path in paths) { GeoPoint endPoint = path.End.Point; HashSet <int> idxs = trjIndex.RangeQuery(endPoint, baseDist); if (idxs.Count > 0) { double minDist = baseDist; int bestIdx = -1; foreach (int idx in idxs) { if (idx >= currentIdx) { //Filter by real distance double distance = Polyline.DistFrom(trj[idx].point, trj[idx + 1].point, endPoint); if (distance <= minDist) { minDist = distance; bestIdx = idx; } } } if (bestIdx >= 0) { //Create new node TrjTreeNode node = new TrjTreeNode(currentNode); node.idx = bestIdx; node.path = path; node.v = path.End; cands.Add(node); } } } return(cands); }