/// <summary> /// Matches the given GPX track to the map /// </summary> /// <param name="gpx">The GPS track log</param> /// <returns>List of the CandidatePoints that match GPS log the best</returns> public IList <CandidatePoint> Match(GPXTrackSegment gpx) { _candidatesGraph = new CandidatesGraph(); CreateTrackCutout(gpx); //Find candidate points + ObservationProbability foreach (var gpxPoint in gpx.Nodes) { var candidates = FindCandidatePoints(gpxPoint); _candidatesGraph.CreateLayer(gpxPoint, candidates.OrderByDescending(c => c.ObservationProbability).Take(Math.Min(candidates.Count(), TMM.MaxCandidatesCount))); } // Calculate transmission probability _candidatesGraph.ConnectLayers(); AssignTransmissionProbability(); //Evaluates paths in the graph EvaluateGraph(); //Extract result List <CandidatePoint> result = new List <CandidatePoint>(); CandidatePoint current = _candidatesGraph.Layers[_candidatesGraph.Layers.Count - 1].Candidates.OrderByDescending(c => c.HighestProbability).FirstOrDefault(); while (current != null) { result.Add(current); current = current.HighesScoreParent; } result.Reverse(); return(result); }
/// <summary> /// Creates connection between two points /// </summary> /// <param name="from">The point, where the connections starts</param> /// <param name="to">The point, where the connection ends</param> void ConnectPoints(CandidatePoint from, CandidatePoint to) { CandidatesConnection c = new CandidatesConnection() { From = from, To = to }; from.OutgoingConnections.Add(c); to.IncomingConnections.Add(c); }
/// <summary> /// Removes temporary objects after search /// </summary> /// <param name="to">The destination point</param> void Finalize(CandidatePoint to) { foreach (var connection in _temporaryConnections) { connection.From.Connections.Remove(connection); } // Remove temporary connections //foreach (var targetConnections in to.Road.Connections) { // var connection = targetConnections.From.Connections.Where(c => c.To.MapPoint == to.MapPoint).Single(); // targetConnections.From.Connections.Remove(connection); //} }
/// <summary> /// Finds shortest path between two points along routes /// </summary> /// <param name="from">Start point</param> /// <param name="to">Destination point</param> /// <returns>length of the path in meters</returns> double FindShortestPath(CandidatePoint from, CandidatePoint to) { if (from.Road == to.Road) { return(Calculations.GetPathLength(from.MapPoint, to.MapPoint, from.Road)); } else { double length = double.PositiveInfinity; _pathfinder.FindPath(from, to, ref length); return(length); } }
/// <summary> /// Initializes internal properties before search /// </summary> /// <param name="from">The start point</param> /// <param name="to">The destination point</param> void Initialize(CandidatePoint from, CandidatePoint to) { _open.Clear(); _close.Clear(); // Add nodes reachable from the From point to the open list foreach (var connection in from.Road.Connections) { PartialPath path = new PartialPath() { End = connection.To, PathFromPrevious = connection.Geometry, Length = Calculations.GetPathLength(from.MapPoint, connection.To.MapPoint, connection.Geometry), EstimationToEnd = Calculations.GetDistance2D(connection.To.MapPoint, to.MapPoint), Id = connection.Id }; if (_open.Contains(path)) { if (_open[path.End].Length > path.Length) { _open.Remove(_open[path.End]); _open.Add(path); } } else { _open.Add(path); } } _temporaryConnections.Clear(); // Add temporary connections to the TO point foreach (var targetConnections in to.Road.Connections) { Connection destinationConnection = new Connection(targetConnections.From, new Node(to.MapPoint)) { Geometry = to.Road }; _temporaryConnections.Add(destinationConnection); } }
/// <summary> /// Builds result path /// </summary> /// <param name="lastPathPart"></param> /// <param name="from"></param> /// <returns></returns> IList <PathSegment> BuildPath(PartialPath lastPathPart, CandidatePoint from) { List <PathSegment> result = new List <PathSegment>(); while (lastPathPart.PreviousNode != null) { result.Add(new PathSegment() { From = lastPathPart.PreviousNode, To = lastPathPart.End, Road = lastPathPart.PathFromPrevious, Id = lastPathPart.Id }); lastPathPart = _close[lastPathPart.PreviousNode]; } result.Add(new PathSegment() { From = new Node(from.MapPoint), To = lastPathPart.End, Road = lastPathPart.PathFromPrevious, Id = lastPathPart.Id }); result.Reverse(); return(result); }
/// <summary> /// Finds path between From and To points /// </summary> /// <param name="from">The start point</param> /// <param name="to">The destination point</param> /// <param name="length">Length of the path in meters</param> /// <returns>Path as list of PathSegments beteewn two point. If path wasn't found returns null.</returns> public IList <PathSegment> FindPath(CandidatePoint from, CandidatePoint to, ref double length) { Initialize(from, to); while (_open.Count > 0) { PartialPath current = _open.RemoveTop(); _close.Add(current.End, current); // Path found if (current.End.MapPoint == to.MapPoint) { length = current.Length; var result = BuildPath(current, from); Finalize(to); return(result); } foreach (var link in current.End.Connections) { if (link.From != current.End) { continue; } double distance; if (link.To.MapPoint == to.MapPoint) { distance = current.Length + Calculations.GetPathLength(current.End.MapPoint, to.MapPoint, link.Geometry); } else { distance = current.Length + link.Geometry.Length; } if (_open.Contains(link.To)) { // Update previously found path in the open list (if this one is shorter) PartialPath p = _open[link.To]; if (p.Length > distance) { p.PreviousNode = current.End; p.PathFromPrevious = link.Geometry; _open.Update(p, distance); p.Id = link.Id; } } else if (_close.ContainsKey(link.To)) { // Update previously found path in the close list (if this one is shorter) if (_close[link.To].Length > distance) { _close[link.To].Length = distance; _close[link.To].End = current.End; _close[link.To].PathFromPrevious = link.Geometry; _close[link.To].Id = link.Id; } } else { // Expand path to new node PartialPath expanded = new PartialPath() { Length = distance, EstimationToEnd = Calculations.GetDistance2D(link.To.MapPoint, to.MapPoint), End = link.To, PreviousNode = current.End, PathFromPrevious = link.Geometry, Id = link.Id }; _open.Add(expanded); } } } Finalize(to); return(null); }