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