Skip to content

Commit

Permalink
Update transition probability calculation and refactor the log of stm…
Browse files Browse the repository at this point in the history 
…atch (cyang-kth#131)

Address this issue cyang-kth#129
  • Loading branch information
@cyang-kth
cyang-kth authored Oct 28, 2020
1 parent f164dac commit 22322db
Show file tree
Hide file tree
Showing 4 changed files with 48 additions and 54 deletions.
13 changes: 6 additions & 7 deletions src/algorithm/geom_algorithm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -351,7 +351,7 @@ void FMM::ALGORITHM::locate_point_by_offset(
FMM::CORE::LineString FMM::ALGORITHM::cutoffseg_unique(
const FMM::CORE::LineString &linestring, double offset1, double offset2) {
FMM::CORE::LineString cutoffline;
SPDLOG_TRACE("Offset1 {} Offset2 {}", offset1, offset2);
// SPDLOG_TRACE("Offset1 {} Offset2 {}", offset1, offset2);
int Npoints = linestring.get_num_points();
if (Npoints == 2) {
// A single segment
Expand Down Expand Up @@ -381,10 +381,10 @@ FMM::CORE::LineString FMM::ALGORITHM::cutoffseg_unique(
double deltaL = std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
l2 = l1 + deltaL;
// Insert p1
SPDLOG_TRACE(" L1 {} L2 {} ", l1, l2);
// SPDLOG_TRACE(" L1 {} L2 {} ", l1, l2);
if (l1 >= offset1 && l1 <= offset2) {
cutoffline.add_point(x1, y1);
SPDLOG_TRACE(" add p1 {} {}", x1, y1);
// SPDLOG_TRACE(" add p1 {} {}", x1, y1);
}

// Insert p between p1 and p2
Expand All @@ -393,23 +393,22 @@ FMM::CORE::LineString FMM::ALGORITHM::cutoffseg_unique(
double px = x1 + ratio1 * (x2 - x1);
double py = y1 + ratio1 * (y2 - y1);
cutoffline.add_point(px, py);
SPDLOG_TRACE(" add p {} {} between p1 p2", px, py);
// SPDLOG_TRACE(" add p {} {} between p1 p2", px, py);
}

if (offset2 > l1 && offset2 < l2) {
double ratio2 = (offset2 - l1) / deltaL;
double px = x1 + ratio2 * (x2 - x1);
double py = y1 + ratio2 * (y2 - y1);
cutoffline.add_point(px, py);
SPDLOG_TRACE(" add p {} {} between p1 p2", px, py);
// SPDLOG_TRACE(" add p {} {} between p1 p2", px, py);
}

// last point
if (i == Npoints - 2 && offset2 >= l2) {
cutoffline.add_point(x2, y2);
SPDLOG_TRACE(" add p2 {} {} for last point", x2, y2);
// SPDLOG_TRACE(" add p2 {} {} for last point", x2, y2);
}

l1 = l2;
++i;
}
Expand Down
69 changes: 32 additions & 37 deletions src/mm/stmatch/stmatch_algorithm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,7 +264,6 @@ void STMATCH::update_tg(TransitionGraph *tg,
int N = layers.size();
for (int i = 0; i < N - 1; ++i) {
// Routing from current_layer to next_layer
SPDLOG_DEBUG("Update layer {} ", i);
double delta = 0;
if (traj.timestamps.size() != N) {
delta = eu_dists[i] * config.factor * 4;
Expand All @@ -282,40 +281,43 @@ void STMATCH::update_layer(int level, TGLayer *la_ptr, TGLayer *lb_ptr,
const CompositeGraph &cg,
double eu_dist,
double delta) {
// SPDLOG_TRACE("Update layer");
SPDLOG_DEBUG("Update layer {} starts", level);
TGLayer &lb = *lb_ptr;
for (auto iter = la_ptr->begin(); iter != la_ptr->end(); ++iter) {
NodeIndex source = iter->c->index;
SPDLOG_TRACE(" Calculate distance from source {}", source);
for (auto iter_a = la_ptr->begin(); iter_a != la_ptr->end(); ++iter_a) {
NodeIndex source = iter_a->c->index;
// SPDLOG_TRACE(" Calculate distance from source {}", source);
// single source upper bound routing
std::vector<NodeIndex> targets(lb.size());
std::transform(lb.begin(), lb.end(), targets.begin(),
[](TGNode &a) {
return a.c->index;
});
SPDLOG_TRACE(" Upperbound shortest path {} ", delta);
std::vector<double> distances = shortest_path_upperbound(
level, cg, source, targets, delta);
SPDLOG_TRACE(" Update property of transition graph ");
for (int i = 0; i < distances.size(); ++i) {
for (auto iter_b = lb_ptr->begin(); iter_b != lb_ptr->end(); ++iter_b) {
int i = std::distance(lb_ptr->begin(),iter_b);
double tp = TransitionGraph::calc_tp(distances[i], eu_dist);
double temp = iter->cumu_prob + log(tp) + log(lb[i].ep);
if (lb[i].cumu_prob<temp) {
lb[i].cumu_prob = temp;
lb[i].prev = &(*iter);
lb[i].sp_dist = distances[i];
lb[i].tp = tp;
double temp = iter_a->cumu_prob + log(tp) + log(iter_b->ep);
SPDLOG_TRACE("L {} f {} t {} sp {} dist {} tp {} ep {} fcp {} tcp {}",
level, iter_a->c->edge->id,iter_b->c->edge->id,
distances[i], eu_dist, tp, iter_b->ep, iter_a->cumu_prob,
temp);
if (temp >= iter_b->cumu_prob) {
iter_b->cumu_prob = temp;
iter_b->prev = &(*iter_a);
iter_b->sp_dist = distances[i];
iter_b->tp = tp;
}
}
}
SPDLOG_TRACE("Update layer done");
SPDLOG_DEBUG("Update layer done");
}

std::vector<double> STMATCH::shortest_path_upperbound(
int level, const CompositeGraph &cg, NodeIndex source,
const std::vector<NodeIndex> &targets, double delta) {
SPDLOG_TRACE("Upperbound shortest path source {}", source);
SPDLOG_TRACE("Upperbound shortest path targets {}", targets);
// SPDLOG_TRACE("Upperbound shortest path source {}", source);
// SPDLOG_TRACE("Upperbound shortest path targets {}", targets);
std::unordered_set<NodeIndex> unreached_targets;
for (auto &node:targets) {
unreached_targets.insert(node);
Expand All @@ -331,12 +333,12 @@ std::vector<double> STMATCH::shortest_path_upperbound(
while (!Q.empty() && !unreached_targets.empty()) {
HeapNode node = Q.top();
Q.pop();
SPDLOG_TRACE(" Node u {} dist {}", node.index, node.value);
// SPDLOG_TRACE(" Node u {} dist {}", node.index, node.value);
NodeIndex u = node.index;
auto iter = unreached_targets.find(u);
if (iter != unreached_targets.end()) {
// Remove u
SPDLOG_TRACE(" Remove target {}", u);
// SPDLOG_TRACE(" Remove target {}", u);
unreached_targets.erase(iter);
}
if (node.value > delta) break;
Expand All @@ -345,23 +347,23 @@ std::vector<double> STMATCH::shortest_path_upperbound(
++node_iter) {
NodeIndex v = node_iter->v;
temp_dist = node.value + node_iter->cost;
SPDLOG_TRACE(" Examine node v {} temp dist {}", v, temp_dist);
// SPDLOG_TRACE(" Examine node v {} temp dist {}", v, temp_dist);
auto v_iter = dmap.find(v);
if (v_iter != dmap.end()) {
// dmap contains node v
if (v_iter->second - temp_dist > 1e-6) {
// a smaller distance is found for v
SPDLOG_TRACE(" Update key {} {} in pdmap prev dist {}",
v, temp_dist, v_iter->second);
// SPDLOG_TRACE(" Update key {} {} in pdmap prev dist {}",
// v, temp_dist, v_iter->second);
pmap[v] = u;
dmap[v] = temp_dist;
Q.decrease_key(v, temp_dist);
}
} else {
// dmap does not contain v
if (temp_dist <= delta) {
SPDLOG_TRACE(" Insert key {} {} into pmap and dmap",
v, temp_dist);
// SPDLOG_TRACE(" Insert key {} {} into pmap and dmap",
// v, temp_dist);
Q.push(v, temp_dist);
pmap.insert({v, u});
dmap.insert({v, temp_dist});
Expand All @@ -370,7 +372,7 @@ std::vector<double> STMATCH::shortest_path_upperbound(
}
}
// Update distances
SPDLOG_TRACE(" Update distances");
// SPDLOG_TRACE(" Update distances");
std::vector<double> distances;
for (int i = 0; i < targets.size(); ++i) {
if (dmap.find(targets[i]) != dmap.end()) {
Expand All @@ -379,7 +381,7 @@ std::vector<double> STMATCH::shortest_path_upperbound(
distances.push_back(std::numeric_limits<double>::max());
}
}
SPDLOG_TRACE(" Distance value {}", distances);
// SPDLOG_TRACE(" Distance value {}", distances);
return distances;
}

Expand All @@ -393,38 +395,31 @@ C_Path STMATCH::build_cpath(const TGOpath &opath, std::vector<int> *indices,
int N = opath.size();
cpath.push_back(opath[0]->c->edge->id);
int current_idx = 0;
SPDLOG_TRACE("Insert index {}", current_idx);
// SPDLOG_TRACE("Insert index {}", current_idx);
indices->push_back(current_idx);
for (int i = 0; i < N - 1; ++i) {
const Candidate *a = opath[i]->c;
const Candidate *b = opath[i + 1]->c;
SPDLOG_DEBUG("Check a {} b {}", a->edge->id, b->edge->id);
// SPDLOG_TRACE("Check a {} b {}", a->edge->id, b->edge->id);
if ((a->edge->id != b->edge->id) ||
(a->offset-b->offset>a->edge->length * reverse_tolerance)) {
auto segs = graph_.shortest_path_dijkstra(a->edge->target,
b->edge->source);
// No transition found
if (segs.empty() && a->edge->target != b->edge->source) {
SPDLOG_DEBUG("Edges not found connecting a b");
SPDLOG_TRACE("Candidate {} has disconnected edge {} to {}",
i, a->edge->id, b->edge->id);
indices->clear();
return {};
}
if (segs.empty()) {
SPDLOG_DEBUG("Edges ab are adjacent");
} else {
SPDLOG_DEBUG("Edges connecting ab are {}", segs);
}

for (int e:segs) {
cpath.push_back(edges[e].id);
++current_idx;
}
cpath.push_back(b->edge->id);
++current_idx;
SPDLOG_TRACE("Insert index {}", current_idx);
indices->push_back(current_idx);
} else {
SPDLOG_TRACE("Insert index {}", current_idx);
indices->push_back(current_idx);
}
}
Expand Down
2 changes: 1 addition & 1 deletion src/mm/transition_graph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ TransitionGraph::TransitionGraph(const Traj_Candidates &tc, double gps_error){
}

double TransitionGraph::calc_tp(double sp_dist,double eu_dist){
return eu_dist>=sp_dist ? (sp_dist+1e-6)/(eu_dist+1e-6) : eu_dist/sp_dist;
return eu_dist>=sp_dist ? 1.0 : eu_dist/sp_dist;
}

double TransitionGraph::calc_ep(double dist,double error){
Expand Down
18 changes: 9 additions & 9 deletions src/network/network_graph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,7 +160,7 @@ std::vector<EdgeIndex> NetworkGraph::shortest_path_astar(
std::vector<EdgeIndex> NetworkGraph::back_track(
NodeIndex source, NodeIndex target, const PredecessorMap &pmap,
const DistanceMap &dmap) const {
SPDLOG_TRACE("Backtrack starts");
// SPDLOG_TRACE("Backtrack starts");
if (dmap.find(target) == dmap.end()) {
return {};
} else {
Expand All @@ -169,9 +169,9 @@ std::vector<EdgeIndex> NetworkGraph::back_track(
NodeIndex u = pmap.at(v);
while (v != source) {
double cost = dmap.at(v) - dmap.at(u);
SPDLOG_TRACE("u {} d {} v {} d {} cost {}",
get_node_id(u), dmap.at(u),
get_node_id(v), dmap.at(v), cost);
// SPDLOG_TRACE("u {} d {} v {} d {} cost {}",
// get_node_id(u), dmap.at(u),
// get_node_id(v), dmap.at(v), cost);
path.push_back(get_edge_index(u, v, cost));
v = u;
u = pmap.at(v);
Expand All @@ -187,19 +187,19 @@ std::vector<EdgeIndex> NetworkGraph::back_track(
*/
int NetworkGraph::get_edge_index(NodeIndex source, NodeIndex target,
double cost) const {
SPDLOG_TRACE("Find edge from {} to {} cost {}", source, target, cost);
// SPDLOG_TRACE("Find edge from {} to {} cost {}", source, target, cost);
if (source >= num_vertices || target >= num_vertices) return -1;
EdgeDescriptor e;
OutEdgeIterator out_i, out_end;
for (boost::tie(out_i, out_end) = boost::out_edges(source, g);
out_i != out_end; ++out_i) {
e = *out_i;
SPDLOG_TRACE(" Check Edge from {} to {} cost {}",
source, boost::target(e, g), g[e].length);
// SPDLOG_TRACE(" Check Edge from {} to {} cost {}",
// source, boost::target(e, g), g[e].length);
if (target == boost::target(e, g) &&
(std::abs(g[e].length - cost) <= DOUBLE_MIN)) {
SPDLOG_TRACE(" Found edge idx {} id {}",
g[e].index, get_edge_id(g[e].index));
// SPDLOG_TRACE(" Found edge idx {} id {}",
// g[e].index, get_edge_id(g[e].index));
return g[e].index;
}
}
Expand Down

0 comments on commit 22322db

Please sign in to comment.