diff --git a/src/algorithm/geom_algorithm.cpp b/src/algorithm/geom_algorithm.cpp index 885ef4f..b0ad16b 100644 --- a/src/algorithm/geom_algorithm.cpp +++ b/src/algorithm/geom_algorithm.cpp @@ -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 @@ -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 @@ -393,7 +393,7 @@ 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) { @@ -401,15 +401,14 @@ FMM::CORE::LineString FMM::ALGORITHM::cutoffseg_unique( 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; } diff --git a/src/mm/stmatch/stmatch_algorithm.cpp b/src/mm/stmatch/stmatch_algorithm.cpp index 8b1018e..e797657 100644 --- a/src/mm/stmatch/stmatch_algorithm.cpp +++ b/src/mm/stmatch/stmatch_algorithm.cpp @@ -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; @@ -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 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 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_probcumu_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 STMATCH::shortest_path_upperbound( int level, const CompositeGraph &cg, NodeIndex source, const std::vector &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 unreached_targets; for (auto &node:targets) { unreached_targets.insert(node); @@ -331,12 +333,12 @@ std::vector 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; @@ -345,14 +347,14 @@ std::vector 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); @@ -360,8 +362,8 @@ std::vector STMATCH::shortest_path_upperbound( } 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}); @@ -370,7 +372,7 @@ std::vector STMATCH::shortest_path_upperbound( } } // Update distances - SPDLOG_TRACE(" Update distances"); + // SPDLOG_TRACE(" Update distances"); std::vector distances; for (int i = 0; i < targets.size(); ++i) { if (dmap.find(targets[i]) != dmap.end()) { @@ -379,7 +381,7 @@ std::vector STMATCH::shortest_path_upperbound( distances.push_back(std::numeric_limits::max()); } } - SPDLOG_TRACE(" Distance value {}", distances); + // SPDLOG_TRACE(" Distance value {}", distances); return distances; } @@ -393,38 +395,31 @@ C_Path STMATCH::build_cpath(const TGOpath &opath, std::vector *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); } } diff --git a/src/mm/transition_graph.cpp b/src/mm/transition_graph.cpp index 3bf9104..da04b87 100644 --- a/src/mm/transition_graph.cpp +++ b/src/mm/transition_graph.cpp @@ -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){ diff --git a/src/network/network_graph.cpp b/src/network/network_graph.cpp index a09a35d..14f9e00 100644 --- a/src/network/network_graph.cpp +++ b/src/network/network_graph.cpp @@ -160,7 +160,7 @@ std::vector NetworkGraph::shortest_path_astar( std::vector 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 { @@ -169,9 +169,9 @@ std::vector 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); @@ -187,19 +187,19 @@ std::vector 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; } }