/// <summary>
/// calculate route distance by search the shortest route between the from vertex of the two edges
/// this method may be not accurate.
/// <returns></returns>
private Edgespp RouteDistance(Point p1, Edge e1, Point p2, Edge e2, Graph g)
{
double routeDis = 0;
Point p1_prj = e1.projectFrom(p1);
Vertex e1_to = e1.To;
double dis1 = p1_prj.DistanceFrom(e1_to.toPoint());
Point p2_prj = e2.projectFrom(p2);
Vertex e2_from = e2.From;
double dis2 = p2_prj.DistanceFrom(e2_from.toPoint());
if (e1 == e2)
{
return(new Edgespp(p1_prj.DistanceFrom(p2_prj), new List <Edge>()
{
e1
}));
}
else if (e1_to.ID == e2_from.ID)
{
routeDis = dis1 + dis2;
return(new Edgespp(routeDis, new List <Edge>()
{
e1, e2
}));
}
else
{
Dijkstra dij = new Dijkstra(g);
LinkedList <Vertex> shortestPath_vertices = dij.ShortestPath_ScoreValue(e1_to, e2_from, _distance_limit);
//Route rs = getminRoute(F, e1.ID, e2.ID, p1, p2);
if (shortestPath_vertices == null)
{
return(null);
}
List <Edge> shortestPath_edges = dij.MakeShortestPath_EdgeList();
//List<Edge> shortestPath_edges = rs.getPath_EdgeList(m_graph);
for (int i = 0; i < shortestPath_edges.Count; i++)
{
routeDis += shortestPath_edges[i].Length;
}
if (shortestPath_edges.Contains(e1))
{
routeDis -= dis1;
shortestPath_edges.Remove(e1);
}
else
{
routeDis += dis1;
}
if (shortestPath_edges.Contains(e2))
{
routeDis -= dis2;
shortestPath_edges.Remove(e2);
}
else
{
routeDis += dis2;
}
return(new Edgespp(routeDis, shortestPath_edges));
}
}
private void GetCompletePath(List <Road_Time> cands, ref List <Road_Time> result)
{
result.Add(cands[0]);
Road_Time rt = new Road_Time();
bool flag = true;
for (int i = 0; i < cands.Count - 1; i++)
{
if (flag == true)
{
rt = cands[i];
}
Road_Time rt_next = cands[i + 1];
Edge e = cands[i].road;
Edge next = cands[i + 1].road;
int eid = e.ID;
int nextid = next.ID;
if (eid == nextid)
{
flag = false;
continue;
}
flag = true;
if (e.isConnectedTo(next) == true)
{
result.Add(rt_next);
continue;
}
else
{
//all possible roads in the iterative search procedure.
//int: the roadID
//int: the last roadID adjacent to the roadID
Dictionary <Edge, Edge> possibles = new Dictionary <Edge, Edge>();
//the roads are searched out by adjacency in the last round
Dictionary <Edge, Edge> lastNewEdges = new Dictionary <Edge, Edge>();
possibles.Add(e, null);
lastNewEdges.Add(e, null);
Dictionary <Edge, Edge> newEdges = new Dictionary <Edge, Edge>();
int times = 0; //record the loop times.
while (true)
{
times++;
newEdges.Clear();
foreach (Edge key in lastNewEdges.Keys)
{
SearchEdgesForEdge1(key, possibles, ref newEdges);
}
if (newEdges.ContainsKey(next))
{
Edge theE = null;
newEdges.TryGetValue(next, out theE);
possibles.Add(next, theE);
break;
}
lastNewEdges.Clear();
foreach (var item in newEdges)
{
lastNewEdges.Add(item.Key, item.Value);
possibles.Add(item.Key, item.Value);
}
}
//if there are n roads between e and next, then divide the timespan (between e and next) into n intervals averagely
TimeSpan ts = rt_next.T - rt.T;
int seconds = ts.Seconds;
TimeSpan time_interval = TimeSpan.FromSeconds(seconds / times);
List <Road_Time> newConnectedEdges = new List <Road_Time>();
//Edge edge = nextid;
//DateTime lastT = next.T;
Edge current_edge = next;
DateTime lastT = rt_next.T;
while (true)
{
Edge next_edge;
possibles.TryGetValue(current_edge, out next_edge);
if (next_edge == e)
{
break;
}
else
{
DateTime t = lastT.Subtract(time_interval);
Road_Time theRT = new Road_Time();
theRT.road = next_edge;
theRT.T = t;
theRT.strength = 0;
newConnectedEdges.Add(theRT);
current_edge = next_edge;
lastT = t;
}
}
newConnectedEdges.Reverse();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////connected mapped edges of two adjacent gps points can not exceed the max distance the vehicle traveled
double total_distance = 0;
for (int j = 0; j < newConnectedEdges.Count; j++)
{
Road_Time theRT = newConnectedEdges[j];
Edge theRoad = theRT.road;
total_distance += theRoad.Length;
}
if ((rt_next.T - rt.T).TotalSeconds * 80 < total_distance)
{
if (i == 0) //the first gps point may be mapped wrong, need the further check
{
}
else
{
cands.RemoveAt(i + 1);
i = i - 1;
continue;
}
}
else
{
result.AddRange(newConnectedEdges);
result.Add(rt_next);
}
}
}
}
public void Mapmatching_HMM(Graph G)
{
m_graph = G;
string patha = @"F:\map matching\frechet dictance\trajectories\traindata\clean.txt";
Program.writetxt(_trj, patha);
if (_trj == null || _trj.Count == 0)
{
throw new ArgumentOutOfRangeException("trajectory",
"Number of points in trajectory should be higher than zero.");
}
int edge_num = G.getEdge().Count;
//the first edge is from edge of the transition, the second edge is the to edge
Dictionary <int, Dictionary <Edge, Dictionary <Edge, double> > > transition_from_to = new Dictionary <int, Dictionary <Edge, Dictionary <Edge, double> > >();
//the first edge is to edge of the transition, the second edge is the from edge
Dictionary <int, Dictionary <Edge, Dictionary <Edge, double> > > transition_to_from = new Dictionary <int, Dictionary <Edge, Dictionary <Edge, double> > >();
//存储相邻GPS点的候选路径之间的路径
Dictionary <Edge, Dictionary <Edge, List <Edge> > > e1_e2_edges = new Dictionary <Edge, Dictionary <Edge, List <Edge> > >();
Dictionary <Edge, Dictionary <int, double> > emission = new Dictionary <Edge, Dictionary <int, double> >();
Dictionary <int, List <Edge> > id_candidate = new Dictionary <int, List <Edge> >();
Dictionary <Edge, double> initial = new Dictionary <Edge, double>();
double minmum_radius = 40; // GPS point must be within the 40m of any road segment, or it is regarded as noise
double maximum_radius = 100; //we calculate transition probability, for any point, we find at least 3 road segments for candidate
//if we can not find 3 points within 100 meters, stop searching
double search_radius = 30; //we start search within 20 meters, if the start search radius is too large, there are two many edges are selected
//calculating transition probability will cost a lot of time.
double step = 10;
double angle_threshod = 360;
//regard points far away from edges as noise, although it may be due to the low accuracy of the road networks
#region delete points which are far away from road segments
for (int i = _trj.Count - 1; i >= 0; i--)
{
Point p = _trj[i];
HashSet <Edge> cands_check_good_point = new HashSet <Edge>();
searchEdgeWithinRadiusOneStep(G._rtree_edge, p, cands_check_good_point, minmum_radius);
if (cands_check_good_point.Count == 0)
{
_trj.Remove(p);
}
}
if (_trj.Count <= 1) //must have least two points to calculate the transition prob;
{
return;
}
#endregion
#region calculate initial probability
foreach (int id in G.getEdge().Keys)
{
Edge e = G.getEdge(id);
initial.Add(e, 1);
}
#endregion
#region calculate emission probability
for (int i = 0; i < _trj.Count; i++)
{
HashSet <Edge> cands = new HashSet <Edge>();
Point p = _trj[i];
searchEdgeWithMaximumRadiusAngle(G._rtree_edge, p, cands, search_radius, step, leastNum, maximum_radius, angle_threshod);
foreach (Edge e in cands)
{
Stopwatch st1 = new Stopwatch();
st1.Start();//开始计时
double measurementProb = MeasurementProbability(p, e, 0, 20, 0, 30);
st1.Stop();//终止计时
emis_time += st1.ElapsedMilliseconds;
if (emission.ContainsKey(e))
{
Dictionary <int, double> pt_prob = emission[e];
if (pt_prob.ContainsKey(i))
{
pt_prob[i] = measurementProb;
}
else
{
pt_prob.Add(i, measurementProb);
}
}
else
{
Dictionary <int, double> pt_prob = new Dictionary <int, double>();
pt_prob.Add(i, measurementProb);
emission.Add(e, pt_prob);
}
//存储每个GPS点搜索到的candidate
if (id_candidate.ContainsKey(i))
{
id_candidate[i].Add(e);
}
else
{
List <Edge> candis = new List <Edge>();
candis.Add(e);
id_candidate.Add(i, candis);
}
}
}
#endregion
#region calculate transition probability
//the transition matrix is alway changed with time t (p1.t, p2.t)
//when at t = p1.t, based on krum_HMM, there is a transition probability in the matrix
//when at t = p2.t, based on krum_HMM, there is another transition probability in the matrix
for (int i = 0; i < _trj.Count - 1; i++)
{
Point p1 = _trj[i];
Point p2 = _trj[i + 1];
List <Edge> cands1 = null;
List <Edge> cands2 = null;
if (id_candidate.ContainsKey(i))
{
cands1 = id_candidate[i];
}
if (id_candidate.ContainsKey(i + 1))
{
cands2 = id_candidate[i + 1];
}
//searchEdgeWithMaximumRadiusAngle(G._rtree_edge, p1, cands1, search_radius, step, leastNum, maximum_radius, angle_threshod);
//searchEdgeWithMaximumRadiusAngle(G._rtree_edge, p2, cands2, search_radius, step, leastNum, maximum_radius, angle_threshod);
if (cands1 == null || cands2 == null)
{
continue;
}
bool transition_prob_flag = false;
foreach (Edge e1 in cands1)
{
foreach (Edge e2 in cands2)
{
Stopwatch st2 = new Stopwatch();
st2.Start();//开始计时
double transitionProb = TransitionProbability(p1, e1, p2, e2, G, 0.894, e1_e2_edges);
st2.Stop();//终止计时
tran_time += st2.ElapsedMilliseconds;
if (transitionProb == 0)
{
continue;
}
else
{
transition_prob_flag = true;
}
#region containner for to from transition
if (transition_to_from.ContainsKey(i))
{
Dictionary <Edge, Dictionary <Edge, double> > e_e_prob = transition_to_from[i];
if (e_e_prob.ContainsKey(e2))
{
Dictionary <Edge, double> e_prob = e_e_prob[e2];
if (e_prob.ContainsKey(e1))
{
e_prob[e1] = transitionProb;
}
else
{
e_prob.Add(e1, transitionProb);
}
}
else
{
Dictionary <Edge, double> e_prob = new Dictionary <Edge, double>();
e_prob.Add(e1, transitionProb);
e_e_prob.Add(e2, e_prob);
}
}
else
{
Dictionary <Edge, Dictionary <Edge, double> > e_e_prob = new Dictionary <Edge, Dictionary <Edge, double> >();
Dictionary <Edge, double> e_prob = new Dictionary <Edge, double>();
e_prob.Add(e1, transitionProb);
e_e_prob.Add(e2, e_prob);
transition_to_from.Add(i, e_e_prob);
}
#endregion
}
}
if (transition_prob_flag == false)
{
break;
}
}
#endregion
Stopwatch st3 = new Stopwatch();
st3.Start();//开始计时
HMM hmm = new HMM(transition_to_from, emission, initial);
double prob;
Edge[] sequence = hmm.Viertbi(false, out prob);
st3.Stop();//终止计时
viterbi_time = st3.ElapsedMilliseconds;
if (sequence == null)
{
return;
}
for (int i = 0; i < sequence.GetLength(0) - 1; i++)
{
if (sequence[i].Equals(sequence[i + 1]))
{
continue;
}
complete_route.Add(sequence[i]);
if (!sequence[i].isConnectedTo(sequence[i + 1]))
{
if (e1_e2_edges.ContainsKey(sequence[i]))
{
if (e1_e2_edges[sequence[i]].ContainsKey(sequence[i + 1]))
{
if (e1_e2_edges[sequence[i]][sequence[i + 1]].Count > 0)
{
complete_route.AddRange(e1_e2_edges[sequence[i]][sequence[i + 1]]);
}
}
}
}
}
complete_route.Add(sequence[sequence.GetLength(0) - 1]);
#region convert sequence to road segment with time
//complete_road_t = new List<Road_Time>();
//if (sequence != null)
//{
// List<Road_Time> final_road_t = new List<Road_Time>();
// List<Road_Time> road_t = new List<Road_Time>();
// for (int i = 0; i < sequence.GetLength(0); i++)
// {
// Road_Time rt = new Road_Time();
// rt.road = sequence[i];
// rt.T = _trj[i].T;
// road_t.Add(rt);
// }
// GetCompletePath(road_t, ref complete_road_t);
// if (complete_road_t.Count > 0)
// {
// int the_last_road_id = complete_road_t[0].road.ID;
// Road_Time rt1 = new Road_Time();
// rt1.road = complete_road_t[0].road;
// rt1.T = complete_road_t[0].T;
// final_road_t.Add(rt1);
// for (int i = 1; i < complete_road_t.Count; i++)
// {
// if (complete_road_t[i].road.ID != the_last_road_id)
// {
// Road_Time rt = new Road_Time();
// rt.road = complete_road_t[i].road;
// rt.T = complete_road_t[i].T;
// the_last_road_id = complete_road_t[i].road.ID;
// final_road_t.Add(rt);
// }
// }
// }
// complete_road_t.Clear();
// complete_road_t.AddRange(final_road_t);
//}
#endregion
}
