public void ConnectLayrsCreatesConnectionsAmongPoints()
        {
            CandidatesGraph target = new CandidatesGraph();

            CandidatePoint pt11 = new CandidatePoint() { MapPoint = new PointGeo() { Latitude = 1, Longitude = 1 } };
            CandidatePoint pt21 = new CandidatePoint() { MapPoint = new PointGeo() { Latitude = 2.1, Longitude = 2.1} };
            CandidatePoint pt22 = new CandidatePoint() { MapPoint = new PointGeo() { Latitude = 2.2, Longitude = 2.2} };

            CandidateGraphLayer layer1 = new CandidateGraphLayer();
            layer1.Candidates.Add(pt11);
            target.Layers.Add(layer1);

            CandidateGraphLayer layer2 = new CandidateGraphLayer();
            layer2.Candidates.AddRange(new CandidatePoint[] { pt21, pt22 });
            target.Layers.Add(layer2);

            target.ConnectLayers();

            Assert.Equal(0, target.Layers[0].Candidates[0].IncomingConnections.Count);
            Assert.Equal(1, target.Layers[0].Candidates[0].OutgoingConnections.Where(c => c.From == pt11 && c.To == pt21).Count());
            Assert.Equal(1, target.Layers[0].Candidates[0].OutgoingConnections.Where(c => c.From == pt11 && c.To == pt22).Count());

            Assert.Equal(0, target.Layers[1].Candidates[0].OutgoingConnections.Count);
            Assert.Equal(0, target.Layers[1].Candidates[1].OutgoingConnections.Count);

            Assert.Equal(1, target.Layers[1].Candidates[0].IncomingConnections.Where(c => c.From == pt11 && c.To == pt21).Count());
            Assert.Equal(1, target.Layers[1].Candidates[1].IncomingConnections.Where(c => c.From == pt11 && c.To == pt22).Count());
        }
예제 #2
0
        public static void SaveCandidateIncomingConnections(CandidatePoint candidate, string filename)
        {
            int   counter = -1;
            OSMDB result  = new OSMDB();

            OSMNode osmCandidate = new OSMNode(counter--, candidate.MapPoint.Latitude, candidate.MapPoint.Longitude);

            osmCandidate.Tags.Add(new OSMTag("observation", candidate.ObservationProbability.ToString()));
            osmCandidate.Tags.Add(new OSMTag("time", candidate.Layer.TrackPoint.Time.ToString()));
            result.Nodes.Add(osmCandidate);
            foreach (var connection in candidate.IncomingConnections)
            {
                OSMNode from = new OSMNode(counter--, connection.From.MapPoint.Latitude, connection.From.MapPoint.Longitude);
                from.Tags.Add(new OSMTag("observation", connection.From.ObservationProbability.ToString()));
                from.Tags.Add(new OSMTag("time", connection.From.Layer.TrackPoint.Time.ToString()));
                result.Nodes.Add(from);

                OSMWay osmConnection = new OSMWay(counter--, new long[] { from.ID, osmCandidate.ID });
                osmConnection.Tags.Add(new OSMTag("transmission", connection.TransmissionProbability.ToString()));

                result.Ways.Add(osmConnection);
            }

            result.Save(filename);
        }
예제 #3
0
        /// <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(), STMatching.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);
        }
예제 #4
0
        /// <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);
                        }
                    }
                    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;
                        }
                    }
                    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 };
                        _open.Add(expanded);
                    }
                }
            }

            Finalize(to);
            return null;
        }
예제 #5
0
        /// <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);
        }
예제 #6
0
파일: Astar.cs 프로젝트: wsgan001/TPM
 /// <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);
     //}
 }
        public void CreateLayerAddsLayerIntoGraph()
        {
            CandidatesGraph target = new CandidatesGraph();

            GPXPoint originalPt = new GPXPoint(1, 1.1);
            CandidatePoint pt1 = new CandidatePoint() { MapPoint = new PointGeo() { Latitude = 1, Longitude = 1 } };
            CandidatePoint pt2 = new CandidatePoint() { MapPoint = new PointGeo() { Latitude = 2.1, Longitude = 2.1 } };

            target.CreateLayer(originalPt, new CandidatePoint[] { pt1, pt2 });

            Assert.Equal(1, target.Layers.Count);
            Assert.Equal(2, target.Layers[0].Candidates.Count);
        }
예제 #8
0
 /// <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);
     }
 }
예제 #9
0
파일: Astar.cs 프로젝트: wsgan001/TPM
        /// <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
                });
                lastPathPart = _close[lastPathPart.PreviousNode];
            }

            result.Add(new PathSegment()
            {
                From = new Node(from.MapPoint), To = lastPathPart.End, Road = lastPathPart.PathFromPrevious
            });
            result.Reverse();

            return(result);
        }
예제 #10
0
파일: Astar.cs 프로젝트: wsgan001/TPM
        /// <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)
                };

                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);
            }
        }
예제 #11
0
        public static void SaveCandidateIncomingConnections(CandidatePoint candidate, string filename)
        {
            int counter = -1;
            OSMDB result = new OSMDB();

            OSMNode osmCandidate = new OSMNode(counter--, candidate.MapPoint.Latitude, candidate.MapPoint.Longitude);
            osmCandidate.Tags.Add(new OSMTag("observation", candidate.ObservationProbability.ToString()));
            osmCandidate.Tags.Add(new OSMTag("time", candidate.Layer.TrackPoint.Time.ToString()));
            result.Nodes.Add(osmCandidate);
            foreach (var connection in candidate.IncomingConnections) {
                OSMNode from = new OSMNode(counter--, connection.From.MapPoint.Latitude, connection.From.MapPoint.Longitude);
                from.Tags.Add(new OSMTag("observation", connection.From.ObservationProbability.ToString()));
                from.Tags.Add(new OSMTag("time", connection.From.Layer.TrackPoint.Time.ToString()));
                result.Nodes.Add(from);

                OSMWay osmConnection = new OSMWay(counter--, new int[] { from.ID, osmCandidate.ID });
                osmConnection.Tags.Add(new OSMTag("transmission", connection.TransmissionProbability.ToString()));

                result.Ways.Add(osmConnection);
            }

            result.Save(filename);
        }
예제 #12
0
        /// <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)
                };

                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);
            }
        }
예제 #13
0
 /// <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);
     //}
 }
예제 #14
0
        /// <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 });
                lastPathPart = _close[lastPathPart.PreviousNode];
            }

            result.Add(new PathSegment() { From = new Node(from.MapPoint), To = lastPathPart.End, Road = lastPathPart.PathFromPrevious });
            result.Reverse();

            return result;
        }
예제 #15
0
 /// <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;
     }
 }
예제 #16
0
 /// <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);
 }
예제 #17
0
파일: Astar.cs 프로젝트: wsgan001/TPM
        /// <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);
                        }
                    }
                    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;
                        }
                    }
                    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
                        };
                        _open.Add(expanded);
                    }
                }
            }

            Finalize(to);
            return(null);
        }