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> /// 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); }