-
Notifications
You must be signed in to change notification settings - Fork 40
/
HMM_mapmatching.cpp
186 lines (178 loc) · 7.95 KB
/
HMM_mapmatching.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
double MapMatcher::cal_logEmiProb(GeoPoint* p, int s)
{
//////////////////////////////////////////////////////////////////////////
/// Compute the emission probability like the GIS'09 paper
/// p: a observation
/// s: a candidate state, i.e., a road id
//////////////////////////////////////////////////////////////////////////
double dist = roadNet->dist(p->lat, p->lon, roadNet->edges[s]); //compute the vertical distance from a point to an edge, in meter, should be implemented by yourself
return -0.5 * ((dist - mu) / sigma) * ((dist - mu) / sigma);
}
double MapMatcher::cal_logTransProb(GeoPoint* p_pre, GeoPoint* p, int s1, int s2)
{
//////////////////////////////////////////////////////////////////////////
/// Compute the transition probability like the GIS'09 paper
/// p_pre: the previous observation
/// p: the current observation
/// s1: the candidate state of p_pre, a road id
/// s2: the candidate state of p, a road id
//////////////////////////////////////////////////////////////////////////
Edge* e1 = roadNet->edges[s1], *e2 = roadNet->edges[s2];
if (e1 == NULL || e2 == NULL)
{
cout << "Error@MapMatcher::cal_logTransProb: edge is NULL" << endl;
system("pause");
exit(0);
}
double dist_G = 0.0;
if (s1 == s2)
{
double start2p1, start2p2; // the road network distance from the start node of e1/e2 to the projection of p1/p2 on e1/e2
start2p1 = roadNet->start2projection(p_pre->lat, p_pre->lon, e1); //compute the road network distance from the start node of an edge to the projection of a given point onto a given edge, should be implemented by yourself
start2p2 = roadNet->start2projection(p->lat, p->lon, e1);
if (start2p2 >= start2p1)
dist_G += (start2p2 - start2p1); //p1->p2 is along direction of the road
else
dist_G += (start2p1 - start2p2); //p2->p1 is along direction of the road
}
else
{
double e1start2p1, e2start2p2; // the road network distance from the start node of e1/e2 to the projection of p1/p2 on e1/e2
e1start2p1 = roadNet->start2projection(p_pre->lat, p_pre->lon, e1);
e2start2p2 = roadNet->start2projection(p->lat, p->lon, e2);
dist_G += (e1->lengthM - e1start2p1 + e2start2p2);
if (e1->endNodeId != e2->startNodeId)
{
dist_G += roadNet->cal_SP(e1->endNodeId, e2->startNodeId); // compute the shortest path given two nodes in the road network, should be implemented by yourself
}
}
double dist = GeoPoint::distM(p_pre, p);
return -fabs(dist - dist_G) / beta;
}
bool MapMatcher::MapMatching_kernel(list<GeoPoint*>& traj, double candidateRangeM)
{
//////////////////////////////////////////////////////////////////////////
/// Map matching for a trajectory with HMM
/// traj: here refers to the point trajectory (raw trajectory), which is a list of points/coordinates.
/// candidateRangeM: the range to find candidate road segments, in meter, for GPS, 50m is enough
/// Ensure that you have dropped out all the points which have no road nearby within `candidateRangeM` meters.
///
/// Note: things you have to implement by your own
/// - Design a `GeoPoint` class which has the field `lat` and `lon` (should be already converted into the rectangular coordinate) as well as `mmRoadId`, which will be overwritten by the answer after calling this function.
/// - Design a `RoadNet` class which
/// - support indexing edge given an edge id, i.e., `roadNet->edges[]`
/// - support range query given a point and a range, i.e., `roadNet->getNearEdges_s()`
/// - support compute the vertical distance form a given point to a given edge, i.e., `roadNet->dist()`
/// - support compute the road network distance from a given edge's start node to a given point's projection onto the given edge, i.e. `roadNet->start2projection()`
/// - support shortest path algorithm which will be used in computing transition probability as well as connecting inadjacent road segments to form a vaild route, i.e., `roadNet->calSP()`
/// If you want to get the legal route (i.e., a edge sequence with each edge being adjacent), just insert the shortest path into the edges which are not adjacent, to form a continuous route
//////////////////////////////////////////////////////////////////////////
int maxEdgeNum = roadNet->edges.size(); // #edges in a road network
int seqLen = traj.size(); // #points in a given trajectory
vector<pair<int, double> > logProbPrev; //records in the previous time step, if the state is `logProbPrev[i].first`, its corresponding log probability is `logProbPrev[t].second`
vector<vector<pair<int, int> > > pathMat; //records in time step t, if the state is `pathMat[t][i].first`, its corresponding best state in t-1 is `pathMat[t-1][pathMat[t][i].second].first`
list<GeoPoint*>::iterator ptIter = traj.begin();
list<GeoPoint*>::iterator pre_ptIter = traj.begin();
list<vector<Edge*>>::iterator candidates_traj_iter = candidates_traj.begin();
bool firstFlag = true;
int t = 0; //record for time step
while (ptIter != traj.end())
{
GeoPoint* pt = (*ptIter);
GeoPoint* pt_pre = (*pre_ptIter);
vector<Edge*> candidates;
roadNet->getNearEdges_s(pt->lat, pt->lon, (double)candidateRangeM, candidates); //find the candidate roads given `pt`, should be implemented by yourself
if (candidates.size() == 0)
{
cout << "Please drop out those points having no candidate within the given range" << endl;
return false;
}
vector<pair<int, double> > logProbCurrent;
vector<pair<int, int> > currentStates_for_pathMat;
for each (Edge* edge in candidates)
{
int s = edge->id;
double logEmiProb = cal_logEmiProb(pt, s); //compute the log emission probability, should be implemented by yourself
currentStates_for_pathMat.push_back(make_pair(s, -1));
if (firstFlag)
{
//initialize logProbCurrent[i].second = P(s|p_0), logProbCurrent[i].first = s
logProbCurrent.push_back(make_pair(s, logEmiProb));
}
else
{
//M[i, s] = max{M[i-1, s'] + P(s|s') + P(o|s)} where M[i-1, s'] > -INF
double maxProb = -INF;
for (int i = 0; i < logProbPrev.size(); i++)
{
int s_prev = logProbPrev[i].first;
if (roadNet->edges[s_prev] == NULL)
{
cout << "error: edge is null" << endl;
return false;
}
double logTransProb = cal_logTransProb(pt_pre, pt, s_prev, s, use_prune); // compute the log transition probability, should be implemented by yourself
if (logProbPrev[i].second + logTransProb + logEmiProb > maxProb)
{
maxProb = logProbPrev[i].second + logTransProb + logEmiProb;
currentStates_for_pathMat.back().second = i; //save path
}
}
if (maxProb > -INF)
{
logProbCurrent.push_back(make_pair(s, maxProb));
}
else
{
printf("maxProb is INF\n");
return false;
}
} // END: if (firstFlag) else
} //END for each (Edge* edge in candidates)
logProbPrev = logProbCurrent;
pathMat.push_back(currentStates_for_pathMat);
//update iterate info
if (firstFlag)
firstFlag = false;
else
pre_ptIter++;
ptIter++;
if (candidates_traj.size() == traj.size())
candidates_traj_iter++;
t++;
} //END: while (ptIter != traj.end())
//retrieve the path
//find the state of the last time step having the highest log prob
double maxProb = -INF;
int last_state = -1;
for (int i = 0; i < logProbPrev.size();
int last_idx = -1; i++)
{
if (logProbPrev[i].second > maxProb)
{
maxProb = logProbPrev[i].second;
last_state = logProbPrev[i].first;
last_idx = i;
}
}
//then go back till the head
vector<int> rev_path;
int succ_state = last_state; //record the best state of time step t+1
int succ_idx = last_idx; //record the position of `succ_state` in pathMat[t+1]
ptIter = traj.end(); ptIter--;
for (t = traj.size()-1; t >=0; ptIter--, t--)
{
if (t == traj.size() - 1)
{
(*ptIter)->mmRoadId = last_state; //`mmRoadId` now records the matched road segment, just implement your GeoPoint class by your own.
}
else
{
int cur_idx = pathMat[t + 1][succ_idx].second;
int cur_state = pathMat[t][cur_idx].first;
(*ptIter)->mmRoadId = cur_state;
succ_idx = cur_idx;
}
}
return true;
}