From d9b8195b8528bde53222c328ee409b97d425f7e6 Mon Sep 17 00:00:00 2001 From: Can Yang Date: Mon, 3 Feb 2020 16:46:02 +0100 Subject: [PATCH 1/6] :construction: Start to handle unmatched point --- src/network.hpp | 7 +------ src/transition_graph.hpp | 11 ++++++----- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/network.hpp b/src/network.hpp index 5cc792a..301b01d 100644 --- a/src/network.hpp +++ b/src/network.hpp @@ -255,15 +255,10 @@ class Network if (dist<=radius) { Candidate c = {offset,dist,Network::emission_prob(dist,gps_error), - edge,NULL,0,0}; + edge,nullptr,0,0}; pcs.push_back(c); } } - // If no candidate is found, return an empty Traj_Candidates - if (pcs.empty()) - { - return Traj_Candidates(); - }; // KNN part if (pcs.size()<=k) { diff --git a/src/transition_graph.hpp b/src/transition_graph.hpp index cbb6d3d..78d4d16 100644 --- a/src/transition_graph.hpp +++ b/src/transition_graph.hpp @@ -50,8 +50,7 @@ class TransitionGraph */ O_Path viterbi(double pf=0) { - O_Path opath; - if (m_traj_candidates->empty()) return opath; + if (m_traj_candidates->empty()) return O_Path{}; int N = m_traj_candidates->size(); /* Update transition probabilities */ Traj_Candidates::iterator csa = m_traj_candidates->begin(); @@ -99,8 +98,11 @@ class TransitionGraph ++csa; ++csb; } + return back_track(); + }; - // Back track to find optimal path + O_Path back_track(){ + O_Path opath; Candidate *track_cand=nullptr; double final_prob = -0.001; Point_Candidates& last_candidates = m_traj_candidates->back(); @@ -117,10 +119,9 @@ class TransitionGraph track_cand = &(*c); } } - int i = N-1; opath.push_back(track_cand); // Iterate from tail to head to assign path - while ((track_cand=track_cand->prev)!=NULL) + while ((track_cand=track_cand->prev)!=nullptr) { opath.push_back(track_cand); } From d227713359aab35e999ba7c97534cd7f02daeed0 Mon Sep 17 00:00:00 2001 From: Can Yang Date: Tue, 4 Feb 2020 04:00:00 +0100 Subject: [PATCH 2/6] :tada: Implement segmentation and tpath geometry --- app/fmm.cpp | 15 ++- app/fmm_omp.cpp | 7 +- example/fmm_config.xml | 16 ++-- example/mr.txt | 8 +- example/segment/fmm_config_csv.xml | 36 +++++++ example/segment/mr_outlier.txt | 5 + example/segment/trips_outlier.csv | 5 + src/config.hpp | 14 ++- src/geometry_type.hpp | 23 +++++ src/network.hpp | 34 +++++++ src/transition_graph.hpp | 118 ++++++++++++++--------- src/ubodt.hpp | 132 ++++++++++++++++++-------- src/util.hpp | 23 ++++- src/writer.hpp | 147 +++++++++++++++++------------ 14 files changed, 424 insertions(+), 159 deletions(-) create mode 100644 example/segment/fmm_config_csv.xml create mode 100644 example/segment/mr_outlier.txt create mode 100644 example/segment/trips_outlier.csv diff --git a/app/fmm.cpp b/app/fmm.cpp index d383b92..06cc0e2 100644 --- a/app/fmm.cpp +++ b/app/fmm.cpp @@ -89,19 +89,32 @@ void run(int argc, char **argv) { Trajectory trajectory = gps_reader.read_next_trajectory(); int points_in_tr = trajectory.geom.getNumPoints(); + SPDLOG_TRACE("Match trajectory {} points {}",trajectory.id,points_in_tr); // Candidate search Traj_Candidates traj_candidates = network.search_tr_cs_knn( trajectory,config.k,config.radius,config.gps_error); + SPDLOG_TRACE("Candidate search done"); TransitionGraph tg( &traj_candidates,trajectory.geom,*ubodt,config.delta); + SPDLOG_TRACE("TG creation done"); // Optimal path inference O_Path o_path = tg.viterbi(config.penalty_factor); + SPDLOG_TRACE("Viterbi inference done"); T_Path t_path = ubodt->construct_traversed_path(o_path,network); + SPDLOG_TRACE("Tpath creation done"); LineString m_geom; + MultiLineString t_geom; // if (result_config.write_mgeom) { m_geom = network.complete_path_to_geometry(o_path,t_path.cpath); } - rw.write_result(trajectory.id,trajectory.geom,o_path,t_path,m_geom); + SPDLOG_TRACE("Cpath geometry done"); + if (result_config.write_tgeom) { + t_geom = network.ot_path_to_multilinestring(o_path,t_path); + } + SPDLOG_TRACE("Tpath geometry done"); + rw.write_result( + trajectory.id,trajectory.geom,o_path,t_path,m_geom,t_geom); + SPDLOG_TRACE("Write result done"); // update statistics total_points+=points_in_tr; if (!t_path.cpath.empty()) points_matched+=points_in_tr; diff --git a/app/fmm_omp.cpp b/app/fmm_omp.cpp index 1cab0ff..fb7a651 100644 --- a/app/fmm_omp.cpp +++ b/app/fmm_omp.cpp @@ -108,10 +108,15 @@ void run(int argc, char **argv) O_Path o_path = tg.viterbi(config.penalty_factor); T_Path t_path = ubodt->construct_traversed_path(o_path,network); LineString m_geom; + MultiLineString t_geom; // if (result_config.write_mgeom) { m_geom = network.complete_path_to_geometry(o_path,t_path.cpath); } - rw.write_result(trajectory.id,trajectory.geom,o_path,t_path,m_geom); + if (result_config.write_tgeom) { + t_geom = network.ot_path_to_multilinestring(o_path,t_path); + } + rw.write_result( + trajectory.id,trajectory.geom,o_path,t_path,m_geom,t_geom); // update statistics #pragma omp critical total_points+=points_in_tr; diff --git a/example/fmm_config.xml b/example/fmm_config.xml index 6faff15..1c896aa 100644 --- a/example/fmm_config.xml +++ b/example/fmm_config.xml @@ -21,14 +21,16 @@ - - + + + + + + mr.txt + + 0 + diff --git a/example/mr.txt b/example/mr.txt index 5621712..4e896f8 100644 --- a/example/mr.txt +++ b/example/mr.txt @@ -1,4 +1,4 @@ -id;ogeom;opath;error;offset;spdist;pgeom;cpath;tpath;mgeom;ep;tp -1;LINESTRING(1.65889830508 0.250988700565,1.65494350282 0.701836158192,2.49336158192 1.7656779661,3.54929378531 1.88827683616,4.13064971751 2.45776836158);2,2,13,14,23;0.341101694915,0.345056497175,0.234322033898,0.111723163842,0.130649717514;0.250988700565,0.701836158192,0.493361581921,0.549293785311,0.457768361582;0,0.450847457627,1.79152542373,1.05593220339,0.908474576271;LINESTRING(2 0.250988700565,2 0.701836158192,2.49336158192 2,3.54929378531 2,4 2.45776836158);2,5,13,14,23;2|2,5,13|13,14|14,23;LINESTRING(2 0.250988700565,2 1,2 2,3 2,4 2,4 2.45776836158);0.792390674782,0.78810182929,0.896001106441,0.975344896361,0.966437453005;0.99996137619,0.75606662035,0.993327021599,0.89580398798 -2;LINESTRING(4.15042372881 1.60353107345,3.47019774011 0.923305084746,2.4063559322 0.923305084746,2.14533898305 1.53234463277,2.08601694915 2.57641242938,2.49731638418 2.98771186441);25,4,3,5,17,19;0.150423728814,0.0766949152542,0.0766949152542,0.145338983051,0.0860169491525,0.0122881355932;0.396468926554,0.529802259887,0.593644067797,0.532344632768,0.576412429379,0.497316384181;0,1.13333333333,1.06384180791,0.938700564972,1.04406779661,0.920903954802;LINESTRING(4 1.60353107345,3.47019774011 1,2.4063559322 1,2 1.53234463277,2 2.57641242938,2.49731638418 3);25,4,3,5,17,19;25,4|4,3|3,5|5,17|17,19;LINESTRING(4 1.60353107345,4 1,3 1,2 1,2 2,2 3,2.49731638418 3);0.955754119316,0.988304707826,0.988304707826,0.958633122376,0.98531111875,0.999698049044;0.848810076714,0.999999821186,0.705885767937,0.998389482498,0.631624162197 -3;LINESTRING(0.200812146893 2.14088983051,1.4426200565 2.14879943503,3.06408898305 2.16066384181,3.06408898305 2.71038135593,3.70872175141 2.9793079096,4.11606638418 2.62337570621);8,11,18,18,20,24;0.140889830508,0.148799435028,0.0640889830508,0.0640889830508,0.0206920903955,0.116066384181;0.200812146893,0.442620056497,0.160663841808,0.710381355932,0.708721751412,0.376624293785;0,1.2418079096,1.71804378531,0.549717514124,0.99834039548,0.667902542373;LINESTRING(0.200812146893 2,1.4426200565 2,3 2.16066384181,3 2.71038135593,3.70872175141 3,4 2.62337570621);8,11,13,18,20,24;8,11|11,13,18|18|18,20|20,24;LINESTRING(0.200812146893 2,1 2,2 2,3 2,3 3,4 3,4 2.62337570621);0.96107782621,0.956683614327,0.991818853732,0.991818853732,0.999144041332,0.9734169086;0.999979615211,0.943813025951,0.999999940395,0.699640154839,0.809909999371 +id;opath;cpath;tpath;mgeom;ep;tp +1;2,2,13,14,23;2,5,13,14,23;2|2,5,13|13,14|14,23;LINESTRING(2 0.250988700565,2 1,2 2,3 2,4 2,4 2.45776836158);0.792390674782,0.78810182929,0.896001106441,0.975344896361,0.966437453005;0.99996137619,0.75606662035,0.993327021599,0.89580398798 +2;25,4,3,5,17,19;25,4,3,5,17,19;25,4|4,3|3,5|5,17|17,19;LINESTRING(4 1.60353107345,4 1,3 1,2 1,2 2,2 3,2.49731638418 3);0.955754119316,0.988304707826,0.988304707826,0.958633122376,0.98531111875,0.999698049044;0.848810076714,0.999999821186,0.705885767937,0.998389482498,0.631624162197 +3;8,11,18,18,20,24;8,11,13,18,20,24;8,11|11,13,18|18|18,20|20,24;LINESTRING(0.200812146893 2,1 2,2 2,3 2,3 3,4 3,4 2.62337570621);0.96107782621,0.956683614327,0.991818853732,0.991818853732,0.999144041332,0.9734169086;0.999979615211,0.943813025951,0.999999940395,0.699640154839,0.809909999371 diff --git a/example/segment/fmm_config_csv.xml b/example/segment/fmm_config_csv.xml new file mode 100644 index 0000000..d2dabe5 --- /dev/null +++ b/example/segment/fmm_config_csv.xml @@ -0,0 +1,36 @@ + + + + + ../ubodt.txt + + + ../data/edges.shp + id + + + trips_outlier.csv + id + + + + 4 + 0.4 + 0 + 0.5 + + + + + + + + + + + mr_outlier.txt + + + 0 + + diff --git a/example/segment/mr_outlier.txt b/example/segment/mr_outlier.txt new file mode 100644 index 0000000..8a461c7 --- /dev/null +++ b/example/segment/mr_outlier.txt @@ -0,0 +1,5 @@ +id;opath;cpath;tpath;mgeom;ep;tp +1;2,2,13,-1,14,23;2,5,13,-1,14,23;2|2,5,13|13,-1|-1,14|14,23;;0.792391,0.788102,0.896001,0,0.975345,0.966437;0.999961,0.756067,0,0,0.895804 +2;2,2,13,-1,29,14,23;2,5,13,-1,29,-1,14,23;2|2,5,13|13,-1|-1,29|29,-1,14|14,23;;0.792391,0.788102,0.896001,0,0.923116,0.975345,0.966437;0.999961,0.756067,0,0,0,0.895804 +3;2,2,13,30,29,14,23;2,5,13,-1,30,29,-1,14,23;2|2,5,13|13,-1,30|30,29|29,-1,14|14,23;;0.792391,0.788102,0.896001,0.980199,0.923116,0.975345,0.966437;0.999961,0.756067,0,0.948683,0,0.895804 +4;-1,2,2,13,14,23;-1,2,5,13,14,23;-1,2|2|2,5,13|13,14|14,23;;0,0.792391,0.788102,0.896001,0.975345,0.966437;0,0.999961,0.756067,0.993327,0.895804 diff --git a/example/segment/trips_outlier.csv b/example/segment/trips_outlier.csv new file mode 100644 index 0000000..b82e20a --- /dev/null +++ b/example/segment/trips_outlier.csv @@ -0,0 +1,5 @@ +id;geom +1;LINESTRING(1.65889830508474 0.25098870056497,1.65494350282486 0.701836158192091,2.4933615819209 1.76567796610169,5 5,3.54929378531073 1.88827683615819,4.13064971751412 2.45776836158192) +2;LINESTRING(1.65889830508474 0.25098870056497,1.65494350282486 0.701836158192091,2.4933615819209 1.76567796610169,5 5,3.7 3.8,3.54929378531073 1.88827683615819,4.13064971751412 2.45776836158192) +3;LINESTRING(1.65889830508474 0.25098870056497,1.65494350282486 0.701836158192091,2.4933615819209 1.76567796610169,3.4 3.9,3.7 3.8,3.54929378531073 1.88827683615819,4.13064971751412 2.45776836158192) +4;LINESTRING(5 5,1.65889830508474 0.25098870056497,1.65494350282486 0.701836158192091,2.4933615819209 1.76567796610169,3.54929378531073 1.88827683615819,4.13064971751412 2.45776836158192) diff --git a/src/config.hpp b/src/config.hpp index 8ec4148..e864914 100644 --- a/src/config.hpp +++ b/src/config.hpp @@ -53,6 +53,8 @@ struct ResultConfig { bool write_ep = false; // The transition probability of each matched point. bool write_tp = false; + // Traversed path geometry, which is a multilinestring + bool write_tgeom = false; }; static const std::vector @@ -161,6 +163,9 @@ class FMM_Config if (tree.get_child_optional("fmm_config.output.fields.tp")) { result_config.write_tp = true; } + if (tree.get_child_optional("fmm_config.output.fields.tgeom")) { + result_config.write_tgeom = true; + } if (tree.get_child_optional("fmm_config.output.fields.all")) { result_config.write_ogeom= true; result_config.write_opath = true; @@ -173,6 +178,7 @@ class FMM_Config result_config.write_tpath = true; result_config.write_ep = true; result_config.write_tp = true; + result_config.write_tgeom = true; } } else { std::cout<<"Default output fields used.\n"; @@ -276,6 +282,9 @@ class FMM_Config if (dict.find("tp")!=dict.end()) { result_config.write_tp = true; } + if (dict.find("tgeom")!=dict.end()) { + result_config.write_tgeom = true; + } if (dict.find("all")!=dict.end()) { result_config.write_ogeom= true; result_config.write_opath = true; @@ -288,6 +297,7 @@ class FMM_Config result_config.write_tpath = true; result_config.write_ep = true; result_config.write_tp = true; + result_config.write_tgeom = true; } } std::cout<<"Finish with reading FMM configuration\n"; @@ -315,7 +325,7 @@ class FMM_Config std::cout<<"--log_level (optional) : log level (2)\n"; std::cout<<"--output_fields (optional) : Output fields\n"; std::cout<<" opath,cpath,tpath,ogeom,mgeom,pgeom,\n"; - std::cout<<" offset,error,spdist,tp,ep,all\n"; + std::cout<<" offset,error,spdist,tp,ep,tgeom,all\n"; std::cout<<"For xml configuration, check example folder\n"; }; @@ -365,6 +375,8 @@ class FMM_Config std::cout << " ep "; if (result_config.write_tp) std::cout << " tp "; + if (result_config.write_tgeom) + std::cout << " tgeom "; std::cout << "\n"; std::cout << "------------------------------------------\n"; }; diff --git a/src/geometry_type.hpp b/src/geometry_type.hpp index c61c6c5..8428137 100644 --- a/src/geometry_type.hpp +++ b/src/geometry_type.hpp @@ -16,6 +16,8 @@ namespace bg = boost::geometry; // Point for rtree box typedef bg::model::point boost_point; typedef bg::model::linestring linestring_t; +typedef bg::model::multi_linestring mlinestring_t; + /** * Boost Geometry Linestring, compatible with OGRGeometry */ @@ -64,6 +66,27 @@ class LineString { linestring_t line; }; // LineString +class MultiLineString { +public: + MultiLineString(){}; + MultiLineString(const std::vector &lines){ + mline.resize(lines.size()); + for (int i=0; i exportToWkt() const { + return bg::wkt(mline); + }; +private: + mlinestring_t mline; +}; /** * Convert an OGRLineString to Boost geometry, the caller is responsible to diff --git a/src/network.hpp b/src/network.hpp index 301b01d..530810a 100644 --- a/src/network.hpp +++ b/src/network.hpp @@ -300,7 +300,13 @@ class Network const C_Path &complete_path) { LineString line; + SPDLOG_TRACE("Opath {}", opath2string(o_path)); + SPDLOG_TRACE("Complete path {}", vec2string(complete_path)); if (complete_path.empty()) return line; + // If complete path contains -1, export empty line + for (auto &item:complete_path){ + if (item == -1) return line; + } int NOsegs = o_path.size(); int NCsegs = complete_path.size(); if (NCsegs ==1) @@ -332,6 +338,34 @@ class Network return line; }; + MultiLineString ot_path_to_multilinestring( + const O_Path &o_path,const T_Path &t_path) + { + SPDLOG_TRACE("Opath {}", opath2string(o_path)); + SPDLOG_TRACE("T path index {}", vec2string(t_path.indices)); + SPDLOG_TRACE("Complete path {}", vec2string(t_path.cpath)); + if (t_path.cpath.empty()) return {}; + std::vector lines; + // Iterate through consecutive indexes and write the traversed path + int J = t_path.indices.size(); + for (int j=0; j0.00001 ? 0 : 1.0; + // } else { + // tran_prob =eu_dist>sp_dist ? sp_dist/eu_dist : eu_dist/sp_dist; + // } + return eu_dist>=sp_dist ? (sp_dist+1e-6)/(eu_dist+1e-6) : eu_dist/sp_dist; + }; /** * Viterbi algorithm, infer an optimal path in the transition * graph @@ -50,83 +63,92 @@ class TransitionGraph */ O_Path viterbi(double pf=0) { + SPDLOG_TRACE("Viterbi start"); if (m_traj_candidates->empty()) return O_Path{}; int N = m_traj_candidates->size(); /* Update transition probabilities */ Traj_Candidates::iterator csa = m_traj_candidates->begin(); /* Initialize the cumu probabilities of the first layer */ - Point_Candidates::iterator ca = csa->begin(); - while (ca != csa->end()) - { - ca->cumu_prob = ca->obs_prob; - ++ca; - } + reset_cs(&(*csa)); /* Updating the cumu probabilities of subsequent layers */ Traj_Candidates::iterator csb = m_traj_candidates->begin(); ++csb; while (csb != m_traj_candidates->end()) { - Point_Candidates::iterator ca = csa->begin(); - double eu_dist=eu_distances[ + double dist_ab=eu_distances[ std::distance(m_traj_candidates->begin(),csa)]; - while (ca != csa->end()) - { - Point_Candidates::iterator cb = csb->begin(); - while (cb != csb->end()) - { - // Calculate transition probability - double sp_dist = get_sp_dist_penalized(ca,cb,pf); - // A degenerate case is that the same point - // is reported multiple times where both eu_dist and sp_dist = 0 - double tran_prob = 1.0; - if (eu_dist<0.00001) { - tran_prob =sp_dist>0.00001 ? 0 : 1.0; - } else { - tran_prob =eu_dist>sp_dist ? sp_dist/eu_dist : eu_dist/sp_dist; - } - + // Iterate through candidates for two points + bool connected = false; + for (auto ca = csa->begin();ca!=csa->end();++ca){ + for(auto cb = csb->begin();cb!=csb->end();++cb){ + bool connected_ab = false; + double sp_dist = get_sp_dist_penalized(ca,cb,pf,&connected_ab); + double tran_prob = calc_tp(sp_dist,dist_ab); if (ca->cumu_prob + tran_prob * cb->obs_prob >= cb->cumu_prob) { cb->cumu_prob = ca->cumu_prob + tran_prob * cb->obs_prob; cb->prev = &(*ca); cb->sp_dist = sp_dist; } - ++cb; + if (connected_ab) connected = connected_ab; } - ++ca; } + if (!connected) reset_cs(&(*csb)); ++csa; ++csb; } return back_track(); }; + // Reset the properties of a candidate set + void reset_cs(Point_Candidates *cs){ + for (auto iter=cs->begin();iter!=cs->end();++iter){ + iter->cumu_prob = iter->obs_prob; + iter->prev = nullptr; + } + }; + + /** + * Backtrack to get the optimal path. If any point has no candidate, nullptr + * will be returned at that place for the pointer. + * + * @return a vector of pointers to optimal candidate + */ O_Path back_track(){ + SPDLOG_TRACE("Backtrack start"); O_Path opath; - Candidate *track_cand=nullptr; + Candidate *opt_c=nullptr; + double final_prob = -0.001; + int N = m_traj_candidates->size(); + for (int i=N-1;i>=0;--i){ + opt_c = find_optimal_candidate((*m_traj_candidates)[i]); + opath.push_back(opt_c); + if (opt_c!=nullptr){ + while ((opt_c=opt_c->prev)!=nullptr) + { + opath.push_back(opt_c); + --i; + } + } + } + std::reverse(opath.begin(), opath.end()); + SPDLOG_TRACE("Backtrack done"); + return opath; + }; + + Candidate *find_optimal_candidate(Point_Candidates &cs){ + Candidate *opt_c=nullptr; double final_prob = -0.001; - Point_Candidates& last_candidates = m_traj_candidates->back(); - for ( - Point_Candidates::iterator c = last_candidates.begin(); - c!=last_candidates.end(); - ++c - ) + for (auto c = cs.begin();c!=cs.end();++c) { // One more step here to filter out these with equal final probability if(final_prob < c->cumu_prob) { final_prob = c->cumu_prob; - track_cand = &(*c); + opt_c = &(*c); } } - opath.push_back(track_cand); - // Iterate from tail to head to assign path - while ((track_cand=track_cand->prev)!=nullptr) - { - opath.push_back(track_cand); - } - std::reverse(opath.begin(), opath.end()); - return opath; + return opt_c; }; /** @@ -160,10 +182,11 @@ class TransitionGraph }; /** * Get the penalized shortest path (SP) distance from Candidate ca to cb + * if a and b are not connected, connected_ab will be set as false */ double get_sp_dist_penalized(Point_Candidates::iterator& ca, Point_Candidates::iterator& cb, - double pf) + double pf, bool *connected_ab) { double sp_dist=0; // Transition on the same edge @@ -180,7 +203,10 @@ class TransitionGraph { Record *r = m_ubodt.look_up(ca->edge->target,cb->edge->source); // No sp path exist from O to D. - if (r==NULL) return DISTANCE_NOT_FOUND; + if (r==NULL) { + *connected_ab = false; + return DISTANCE_NOT_FOUND; + }; // calculate original SP distance sp_dist = r->cost + ca->edge->length - ca->offset + cb->offset; // Two penalized cases @@ -192,8 +218,8 @@ class TransitionGraph { sp_dist+=pf*ca->edge->length; } - } + *connected_ab = true; return sp_dist; }; /** @@ -224,7 +250,7 @@ class TransitionGraph std::vector &get_eu_distances(){ return eu_distances; - } + }; private: // a pointer to trajectory candidates diff --git a/src/ubodt.hpp b/src/ubodt.hpp index c3c8528..e982dbd 100644 --- a/src/ubodt.hpp +++ b/src/ubodt.hpp @@ -37,7 +37,7 @@ class UBODT UBODT(int buckets_arg,int multiplier_arg) : buckets(buckets_arg),multiplier(multiplier_arg) { SPDLOG_TRACE("Intialization UBODT with buckets {} multiplier {}", - buckets, multiplier); + buckets, multiplier); hashtable = (Record **) malloc(sizeof(Record*)*buckets); for (int i = 0; i < buckets; i++) { hashtable[i] = NULL; @@ -109,28 +109,9 @@ class UBODT * path implying complete path cannot be found in UBDOT, * an empty path is returned */ - C_Path construct_complete_path(O_Path &path, std::vector &edges){ - C_Path cpath; - if (path.empty()) return cpath; - int N = path.size(); - cpath.push_back(path[0]->edge->id); - for(int i=0; iedge->id!=b->edge->id) || (a->offset>b->offset)) { - // segs stores edge index - auto segs = look_sp_path(a->edge->target,b->edge->source); - // No transition exist in UBODT - if (segs.empty() && a->edge->target!=b->edge->source) { - return C_Path(); - } - for (int e:segs) { - cpath.push_back(edges[e].id); - } - cpath.push_back(b->edge->id); - } - } - return cpath; + C_Path construct_complete_path(O_Path &path, Network &network){ + T_Path tpath = construct_traversed_path(path,network); + return tpath.cpath; }; /** @@ -141,35 +122,104 @@ class UBODT * matched edge for each point in the GPS trajectory. */ T_Path construct_traversed_path(O_Path &path, Network &network){ + SPDLOG_TRACE("Opath {}", opath2string(path)); if (path.empty()) return T_Path(); std::vector &edges = network.get_edges(); int N = path.size(); // T_Path *edges= new T_Path(); T_Path t_path; - t_path.cpath.push_back(path[0]->edge->id); - int current_idx = 0; - t_path.indices.push_back(current_idx); + int current_idx = -1; + // if (path[0]==nullptr){ + // t_path.cpath.push_back(-1); + // } else { + // t_path.cpath.push_back(path[0]->edge->id); + // } + // int current_idx = 0; + // t_path.indices.push_back(current_idx); + bool prev_connected = false; for(int i=0; iedge->id!=b->edge->id) || (a->offset>b->offset)) { - // segs stores edge index - auto segs = look_sp_path(a->edge->target,b->edge->source); - // No transition exist in UBODT - if (segs.empty() && a->edge->target!=b->edge->source) { - return T_Path(); - } - for (int e:segs) { - t_path.cpath.push_back(edges[e].id); + if (a==nullptr && b==nullptr ) { + SPDLOG_TRACE("a null b null"); + t_path.cpath.push_back(-1); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + t_path.indices.push_back(current_idx); + if (i==N-2) { + t_path.cpath.push_back(-1); ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + t_path.indices.push_back(current_idx); } - t_path.cpath.push_back(b->edge->id); + prev_connected = false; + } + if (a==nullptr && b!=nullptr ) { + SPDLOG_TRACE("a null b {}",b->edge->id); + t_path.cpath.push_back(-1); ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); t_path.indices.push_back(current_idx); - } else { - // b stays on the same edge - t_path.indices.push_back(current_idx); + if (i==N-2) { + t_path.cpath.push_back(b->edge->id); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + t_path.indices.push_back(current_idx); + } + prev_connected = false; + } + if (a!=nullptr && b==nullptr) { + SPDLOG_TRACE("a {} b nullptr",a->edge->id); + if (!prev_connected) { + t_path.cpath.push_back(a->edge->id); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + t_path.indices.push_back(current_idx); + } + if (i==N-2) { + t_path.cpath.push_back(-1); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + t_path.indices.push_back(current_idx); + } + prev_connected = false; + } + if (a!=nullptr && b !=nullptr) { + SPDLOG_TRACE("a {} b {}",a->edge->id,b->edge->id); + if (!prev_connected) { + t_path.cpath.push_back(a->edge->id); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + t_path.indices.push_back(current_idx); + } + if ((a->edge->id!=b->edge->id) || (a->offset>b->offset)) { + // segs stores edge index + auto segs = look_sp_path(a->edge->target,b->edge->source); + // No transition exist in UBODT + if (segs.empty() && a->edge->target!=b->edge->source) { + t_path.cpath.push_back(-1); + ++current_idx; + prev_connected = false; + } else { + for (int e:segs) { + t_path.cpath.push_back(edges[e].id); + ++current_idx; + } + t_path.cpath.push_back(b->edge->id); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + t_path.indices.push_back(current_idx); + prev_connected = true; + } + } else { + // b stays on the same edge + SPDLOG_TRACE("Push back current index {}",current_idx); + t_path.indices.push_back(current_idx); + prev_connected = true; + } } + SPDLOG_TRACE("Iterate i {} done",i); } return t_path; }; @@ -244,7 +294,9 @@ int estimate_ubodt_rows(const std::string &filename){ std::transform(fn_extension.begin(), fn_extension.end(), fn_extension.begin(), - [](unsigned char c){ return std::tolower(c);}); + [](unsigned char c){ + return std::tolower(c); + }); if (fn_extension == "csv" || fn_extension == "txt") { int row_size = 36; return file_bytes/row_size; diff --git a/src/util.hpp b/src/util.hpp index 6f48e8e..b734e18 100644 --- a/src/util.hpp +++ b/src/util.hpp @@ -23,6 +23,27 @@ namespace MM { + +template std::string vec2string(const std::vector& v) { + std::stringstream ss; + for (int i = 0; i < v.size(); ++i) { + ss << v[i]; + if (i != v.size() - 1) + ss << ","; + } + return ss.str(); +}; + +std::string opath2string(const O_Path &opath){ + std::stringstream ss; + for (int i = 0; i < opath.size(); ++i) { + ss << (opath[i]==nullptr?-1:opath[i]->edge->id); + if (i != opath.size() - 1) + ss << ","; + } + return ss.str(); +}; + namespace UTIL { /** @@ -66,7 +87,7 @@ int get_file_extension(std::string &fn) { std::string get_file_directory(std::string &fn){ std::size_t found = fn.find_last_of("/"); - if (found!=std::string::npos){ + if (found!=std::string::npos) { return fn.substr(0,found); } return {}; diff --git a/src/writer.hpp b/src/writer.hpp index 50f3bb0..e91694a 100644 --- a/src/writer.hpp +++ b/src/writer.hpp @@ -57,7 +57,8 @@ class ResultWriter * first and last edge are removed.) */ void write_result(int tr_id, const LineString &ogeom, const O_Path &o_path, - const T_Path &t_path, LineString &mgeom){ + const T_Path &t_path, LineString &mgeom, + MultiLineString &tgeom){ std::stringstream buf; buf << tr_id; if (config.write_ogeom) { @@ -106,6 +107,10 @@ class ResultWriter buf << ";"; write_tp(buf,o_path); } + if (config.write_tgeom) { + buf << ";"; + write_multilinestring(buf,tgeom); + } buf << '\n'; // Ensure that fstream is called corrected in OpenMP #pragma omp critical @@ -127,9 +132,9 @@ class ResultWriter buf << ";"; write_pgeom(buf,o_path); buf << ";"; - write_cpath_network(buf,t_path,network); + write_cpath(buf,t_path); buf << ";"; - write_tpath_network(buf,t_path,network); + write_tpath(buf,t_path); buf << ";"; } if (!mgeom.isEmpty()) { @@ -137,6 +142,7 @@ class ResultWriter } return buf.str(); }; + void write_header() { std::string header = "id"; if (config.write_ogeom) header+=";ogeom"; @@ -150,6 +156,7 @@ class ResultWriter if (config.write_mgeom) header+=";mgeom"; if (config.write_ep) header+=";ep"; if (config.write_tp) header+=";tp"; + if (config.write_tgeom) header+=";tgeom"; fstream << header << '\n'; }; static void write_geometry(std::stringstream &buf, const LineString &line){ @@ -157,6 +164,14 @@ class ResultWriter buf << std::setprecision(12) << line.exportToWkt(); } }; + + static void write_multilinestring(std::stringstream &buf, + const MultiLineString &mline){ + if (!mline.isEmpty()) { + buf << std::setprecision(12) << mline.exportToWkt(); + } + }; + // Write the optimal path static void write_o_path(std::stringstream &buf,const O_Path &o_path) { @@ -166,9 +181,17 @@ class ResultWriter int N = o_path.size(); for (int i = 0; i < N - 1; ++i) { - buf << o_path[i]->edge->id << ","; + if (o_path[i]==nullptr){ + buf << "-1,"; + } else { + buf << o_path[i]->edge->id << ","; + } + } + if (o_path[N-1]==nullptr){ + buf << "-1"; + } else { + buf << o_path[N - 1]->edge->id; } - buf << o_path[N - 1]->edge->id; }; static void write_offset(std::stringstream &buf,const O_Path &o_path) @@ -179,9 +202,17 @@ class ResultWriter int N = o_path.size(); for (int i = 0; i < N - 1; ++i) { - buf << o_path[i]->offset<< ","; + if (o_path[i]==nullptr){ + buf << "-1,"; + } else { + buf << o_path[i]->offset << ","; + } + } + if (o_path[N-1]==nullptr){ + buf << -1; + } else { + buf << o_path[N - 1]->offset; } - buf << o_path[N - 1]->offset; }; static void write_spdist(std::stringstream &buf,const O_Path &o_path) @@ -192,9 +223,17 @@ class ResultWriter int N = o_path.size(); for (int i = 0; i < N - 1; ++i) { - buf << o_path[i]->sp_dist<< ","; + if (o_path[i]==nullptr){ + buf <<"-1,"; + } else { + buf << o_path[i]->sp_dist << ","; + } + } + if (o_path[N-1]==nullptr){ + buf << -1; + } else { + buf << o_path[N - 1]->sp_dist; } - buf << o_path[N - 1]->sp_dist; }; static void write_ep(std::stringstream &buf,const O_Path &o_path) @@ -205,9 +244,17 @@ class ResultWriter int N = o_path.size(); for (int i = 0; i < N - 1; ++i) { - buf << o_path[i]->obs_prob<< ","; + if (o_path[i]==nullptr){ + buf << "0,"; + } else { + buf << o_path[i]->obs_prob << ","; + } + } + if (o_path[N-1]==nullptr){ + buf << 0; + } else { + buf << o_path[N - 1]->obs_prob; } - buf << o_path[N - 1]->obs_prob; }; static void write_tp(std::stringstream &buf,const O_Path &o_path) @@ -216,9 +263,15 @@ class ResultWriter int N = o_path.size(); for (int i = 0; i < N - 1; ++i) { - float ca = o_path[i]->cumu_prob; - float cb = o_path[i+1]->cumu_prob; - float tp = (cb - ca)/(o_path[i+1]->obs_prob+1e-7); + float tp = 0; + if (o_path[i]!=nullptr && o_path[i+1]!=nullptr){ + float ca = o_path[i]->cumu_prob; + float cb = o_path[i+1]->cumu_prob; + if (o_path[i+1]->prev==o_path[i]){ + // a and b are connected + tp = (cb - ca)/(o_path[i+1]->obs_prob+1e-7); + } + } if (i==N-2) { buf << tp; } else { @@ -235,9 +288,17 @@ class ResultWriter int N = o_path.size(); for (int i = 0; i < N - 1; ++i) { - buf << o_path[i]->dist<< ","; + if (o_path[i]==nullptr){ + buf << "-1,"; + } else { + buf << o_path[i]->dist << ","; + } + } + if (o_path[N-1]==nullptr){ + buf << -1; + } else { + buf << o_path[N - 1]->dist; } - buf << o_path[N - 1]->dist; }; // Write the optimal path @@ -251,12 +312,16 @@ class ResultWriter // Create a linestring from matched point for (int i = 0; i < N; ++i) { - LineString &edge_geom = o_path[i]->edge->geom; - double px = 0; - double py = 0; - ALGORITHM::locate_point_by_offset( - edge_geom,o_path[i]->offset,&px,&py); - pline.addPoint(px,py); + if (o_path[i]==nullptr){ + pline.addPoint(-1,-1); + } else { + LineString &edge_geom = o_path[i]->edge->geom; + double px = 0; + double py = 0; + ALGORITHM::locate_point_by_offset( + edge_geom,o_path[i]->offset,&px,&py); + pline.addPoint(px,py); + } } if (!pline.isEmpty()) { write_geometry(buf,pline); @@ -264,7 +329,7 @@ class ResultWriter }; // Write the complete path - void write_cpath(std::stringstream &buf,const T_Path &t_path) { + static void write_cpath(std::stringstream &buf,const T_Path &t_path) { if (t_path.cpath.empty()) return; const C_Path &c_path = t_path.cpath; int N = c_path.size(); @@ -275,10 +340,8 @@ class ResultWriter buf << c_path[N - 1]; }; - - // Write the traversed path separated by vertical bar - void write_tpath(std::stringstream &buf,const T_Path &t_path) { + static void write_tpath(std::stringstream &buf,const T_Path &t_path) { if (t_path.cpath.empty()) return; // Iterate through consecutive indexes and write the traversed path int J = t_path.indices.size(); @@ -297,38 +360,6 @@ class ResultWriter } }; - static void write_cpath_network( - std::stringstream &buf, const T_Path &t_path, const Network &network) { - if (t_path.cpath.empty()) return; - const C_Path &c_path = t_path.cpath; - int N = c_path.size(); - for (int i = 0; i < N - 1; ++i) - { - buf << network.get_edge_id(c_path[i]) << ","; - } - buf << network.get_edge_id(c_path[N - 1]); - }; - - // Write the traversed path separated by vertical bar - static void write_tpath_network( - std::stringstream &buf,const T_Path &t_path,const Network &network) { - if (t_path.cpath.empty()) return; - // Iterate through consecutive indexes and write the traversed path - int J = t_path.indices.size(); - for (int j=0; j Date: Sat, 16 May 2020 02:38:11 +0200 Subject: [PATCH 3/6] Add partial match result class and multilinestring --- src/core/geometry.cpp | 6 ++ src/core/geometry.hpp | 60 +++++++++++++----- src/io/mm_writer.cpp | 138 ++++++++++++++++++++++++++---------------- src/io/mm_writer.hpp | 12 ++++ src/mm/mm_type.hpp | 18 ++++-- 5 files changed, 164 insertions(+), 70 deletions(-) diff --git a/src/core/geometry.cpp b/src/core/geometry.cpp index 1024821..2c92172 100644 --- a/src/core/geometry.cpp +++ b/src/core/geometry.cpp @@ -16,6 +16,12 @@ std::ostream& FMM::CORE::operator<<(std::ostream& os, return os; }; +std::ostream& FMM::CORE::operator<<(std::ostream& os, + const FMM::CORE::MultiLineString& rhs){ + os<< std::setprecision(12) << boost::geometry::wkt(rhs.mline); + return os; +}; + FMM::CORE::LineString FMM::CORE::ogr2linestring(const OGRLineString *line){ int binary_size = line->WkbSize(); std::vector wkb(binary_size); diff --git a/src/core/geometry.hpp b/src/core/geometry.hpp index a73f236..1344812 100644 --- a/src/core/geometry.hpp +++ b/src/core/geometry.hpp @@ -19,13 +19,13 @@ namespace FMM { /** * Core data types */ -namespace CORE{ +namespace CORE { /** * Point class */ typedef boost::geometry::model::point Point; // Point for rtree box + boost::geometry::cs::cartesian> Point; // Point for rtree box /** * Linestring geometry class * @@ -43,7 +43,7 @@ class LineString { * @param i point index * @return x coordinate */ - inline double get_x(int i) const{ + inline double get_x(int i) const { return boost::geometry::get<0>(line.at(i)); }; /** @@ -52,7 +52,7 @@ class LineString { * number of points in the line * @return y coordinate */ - inline double get_y(int i) const{ + inline double get_y(int i) const { return boost::geometry::get<1>(line.at(i)); }; /** @@ -95,9 +95,9 @@ class LineString { * Manipulating the returned point will not change the * original line. */ - inline Point get_point(int i) const{ + inline Point get_point(int i) const { return Point(boost::geometry::get<0>( - line.at(i)),boost::geometry::get<1>(line.at(i))); + line.at(i)),boost::geometry::get<1>(line.at(i))); }; /** * Get a constance reference of the i-th point in the line @@ -105,14 +105,14 @@ class LineString { * @return A constant reference to the ith point of line, which * avoids create a new point. */ - inline const Point &at(int i) const{ + inline const Point &at(int i) const { return line.at(i); } /** * Get the number of points in a line * @return the point number */ - inline int get_num_points() const{ + inline int get_num_points() const { return boost::geometry::num_points(line); }; /** @@ -141,7 +141,7 @@ class LineString { * * Example: LINESTRING (30 10, 10 30, 40 40) */ - inline std::string export_wkt() const{ + inline std::string export_wkt() const { std::ostringstream ss; ss << boost::geometry::wkt(line); return ss.str(); @@ -150,14 +150,14 @@ class LineString { * Export a string containing GeoJSON representation of the line. * @return The GeoJSON of the line */ - inline std::string export_json() const{ + inline std::string export_json() const { std::ostringstream ss; int N = get_num_points(); - if (N>0){ + if (N>0) { ss << "{\"type\":\"LineString\",\"coordinates\": ["; - for (int i=0;i1e-6) result = false; } @@ -211,6 +211,36 @@ class LineString { std::ostream& operator<<(std::ostream& os,const FMM::CORE::LineString& rhs); +class MultiLineString { +public: + typedef boost::geometry::model::multi_linestring + mlinestring_t; + + MultiLineString(){ + }; + MultiLineString(const std::vector &lines){ + mline.resize(lines.size()); + for (int i=0; i exportToWkt() const { + return bg::wkt(mline); + }; + friend std::ostream& operator<<(std::ostream& os,const MultiLineString& rhs); +private: + mlinestring_t mline; +}; + +std::ostream& operator<<(std::ostream& os, + const FMM::CORE::MultiLineString& rhs); + /** * Convert a OGRLineString to a linestring * @param line a pointer to OGRLineString diff --git a/src/io/mm_writer.cpp b/src/io/mm_writer.cpp index 16525cf..f8aeea9 100644 --- a/src/io/mm_writer.cpp +++ b/src/io/mm_writer.cpp @@ -19,8 +19,8 @@ namespace FMM { namespace IO { CSVMatchResultWriter::CSVMatchResultWriter( - const std::string &result_file, const CONFIG::OutputConfig &config_arg) : - m_fstream(result_file), config_(config_arg) { + const std::string &result_file, const CONFIG::OutputConfig &config_arg) : + m_fstream(result_file), config_(config_arg) { write_header(); } @@ -43,50 +43,93 @@ void CSVMatchResultWriter::write_header() { } void CSVMatchResultWriter::write_result( - const FMM::CORE::Trajectory &traj, - const FMM::MM::MatchResult &result) { + const FMM::CORE::Trajectory &traj, + const FMM::MM::MatchResult &result){ std::stringstream buf; - buf << result.id; + write_result_(buf,traj,result.opt_candidate_path, + result.opath, result.cpath, result.indices); + write_linestring_(buf,result.mgeom); + buf << '\n'; + // Ensure that fstream is called corrected in OpenMP + #pragma omp critical + m_fstream << buf.rdbuf(); +}; + +void CSVMatchResultWriter::write_result( + const FMM::CORE::Trajectory &traj, + const FMM::MM::PartialMatchResult &result){ + std::stringstream buf; + write_result_(buf,traj,result.opt_candidate_path, + result.opath, result.cpath, result.indices); + write_multilinestring_(buf,result.mgeom); + buf << '\n'; + // Ensure that fstream is called corrected in OpenMP + #pragma omp critical + m_fstream << buf.rdbuf(); +}; + +void CSVMatchResultWriter::write_linestring_( + std::stringstream &buf,const FMM::CORE::LineString &line){ + if (config_.write_mgeom) { + buf << ";" << line; + } +}; + +void CSVMatchResultWriter::write_multilinestring_( + std::stringstream &buf,const FMM::CORE::MultiLineString &mline){ + if (config_.write_mgeom) { + buf << ";" << mline; + } +}; + +void CSVMatchResultWriter::write_result_( + std::stringstream &buf, + const FMM::CORE::Trajectory &traj, + const FMM::MM::MatchedCandidatePath &opt_candidate_path, + const FMM::MM::O_Path &opath, + const FMM::MM::C_Path &cpath, + const std::vector &indices) { + buf << traj.id; if (config_.write_opath) { - buf << ";" << result.opath; + buf << ";" << opath; } if (config_.write_error) { buf << ";"; - if (!result.opt_candidate_path.empty()) { - int N = result.opt_candidate_path.size(); + if (!opt_candidate_path.empty()) { + int N = opt_candidate_path.size(); for (int i = 0; i < N - 1; ++i) { - buf << result.opt_candidate_path[i].c.dist << ","; + buf << opt_candidate_path[i].c.dist << ","; } - buf << result.opt_candidate_path[N - 1].c.dist; + buf << opt_candidate_path[N - 1].c.dist; } } if (config_.write_offset) { buf << ";"; - if (!result.opt_candidate_path.empty()) { - int N = result.opt_candidate_path.size(); + if (!opt_candidate_path.empty()) { + int N = opt_candidate_path.size(); for (int i = 0; i < N - 1; ++i) { - buf << result.opt_candidate_path[i].c.offset << ","; + buf << opt_candidate_path[i].c.offset << ","; } - buf << result.opt_candidate_path[N - 1].c.offset; + buf << opt_candidate_path[N - 1].c.offset; } } if (config_.write_spdist) { buf << ";"; - if (!result.opt_candidate_path.empty()) { - int N = result.opt_candidate_path.size(); + if (!opt_candidate_path.empty()) { + int N = opt_candidate_path.size(); for (int i = 1; i < N; ++i) { - buf << result.opt_candidate_path[i].sp_dist - << (i==N-1?"":","); + buf << opt_candidate_path[i].sp_dist + << (i==N-1 ? "" : ","); } } } if (config_.write_pgeom) { buf << ";"; - if (!result.opt_candidate_path.empty()) { - int N = result.opt_candidate_path.size(); + if (!opt_candidate_path.empty()) { + int N = opt_candidate_path.size(); FMM::CORE::LineString pline; for (int i = 0; i < N; ++i) { - const FMM::CORE::Point &point = result.opt_candidate_path[i].c.point; + const FMM::CORE::Point &point = opt_candidate_path[i].c.point; pline.add_point(point); } buf << pline; @@ -94,21 +137,21 @@ void CSVMatchResultWriter::write_result( } // Write fields related with cpath if (config_.write_cpath) { - buf << ";" << result.cpath; + buf << ";" << cpath; } if (config_.write_tpath) { buf << ";"; - if (!result.cpath.empty()) { + if (!cpath.empty()) { // Iterate through consecutive indexes and write the traversed path - int J = result.indices.size(); + int J = indices.size(); for (int j = 0; j < J - 1; ++j) { - int a = result.indices[j]; - int b = result.indices[j + 1]; + int a = indices[j]; + int b = indices[j + 1]; for (int i = a; i < b; ++i) { - buf << result.cpath[i]; + buf << cpath[i]; buf << ","; } - buf << result.cpath[b]; + buf << cpath[b]; if (j < J - 2) { // Last element should not have a bar buf << "|"; @@ -116,40 +159,37 @@ void CSVMatchResultWriter::write_result( } } } - if (config_.write_mgeom) { - buf << ";" << result.mgeom; - } if (config_.write_ep) { buf << ";"; - if (!result.opt_candidate_path.empty()) { - int N = result.opt_candidate_path.size(); + if (!opt_candidate_path.empty()) { + int N = opt_candidate_path.size(); for (int i = 0; i < N - 1; ++i) { - buf << result.opt_candidate_path[i].ep << ","; + buf << opt_candidate_path[i].ep << ","; } - buf << result.opt_candidate_path[N - 1].ep; + buf << opt_candidate_path[N - 1].ep; } } if (config_.write_tp) { buf << ";"; - if (!result.opt_candidate_path.empty()) { - int N = result.opt_candidate_path.size(); + if (!opt_candidate_path.empty()) { + int N = opt_candidate_path.size(); for (int i = 0; i < N - 1; ++i) { - buf << result.opt_candidate_path[i].tp << ","; + buf << opt_candidate_path[i].tp << ","; } - buf << result.opt_candidate_path[N - 1].tp; + buf << opt_candidate_path[N - 1].tp; } } if (config_.write_length) { buf << ";"; - if (!result.opt_candidate_path.empty()) { - int N = result.opt_candidate_path.size(); + if (!opt_candidate_path.empty()) { + int N = opt_candidate_path.size(); SPDLOG_TRACE("Write length for {} edges",N); for (int i = 0; i < N - 1; ++i) { // SPDLOG_TRACE("Write length {}",i); - buf << result.opt_candidate_path[i].c.edge->length << ","; + buf << opt_candidate_path[i].c.edge->length << ","; } // SPDLOG_TRACE("Write length {}",N-1); - buf << result.opt_candidate_path[N - 1].c.edge->length; + buf << opt_candidate_path[N - 1].c.edge->length; } } if (config_.write_duration) { @@ -160,25 +200,21 @@ void CSVMatchResultWriter::write_result( for (int i = 1; i < N; ++i) { // SPDLOG_TRACE("Write length {}",i); buf << traj.timestamps[i] - traj.timestamps[i-1] - << (i==N-1?"":","); + << (i==N-1 ? "" : ","); } } } if (config_.write_speed) { buf << ";"; - if (!result.opt_candidate_path.empty() && !traj.timestamps.empty()) { + if (!opt_candidate_path.empty() && !traj.timestamps.empty()) { int N = traj.timestamps.size(); for (int i = 1; i < N; ++i) { double duration = traj.timestamps[i] - traj.timestamps[i-1]; - buf << (duration>0?(result.opt_candidate_path[i].sp_dist/duration):0) - << (i==N-1?"":","); + buf << (duration>0 ? (opt_candidate_path[i].sp_dist/duration) : 0) + << (i==N-1 ? "" : ","); } } } - buf << '\n'; - // Ensure that fstream is called corrected in OpenMP - #pragma omp critical - m_fstream << buf.rdbuf(); } } //IO diff --git a/src/io/mm_writer.hpp b/src/io/mm_writer.hpp index d49a43d..ec1939f 100644 --- a/src/io/mm_writer.hpp +++ b/src/io/mm_writer.hpp @@ -68,7 +68,19 @@ class CSVMatchResultWriter : public MatchResultWriter { */ void write_result(const FMM::CORE::Trajectory &traj, const FMM::MM::MatchResult &result); + void write_result(const FMM::CORE::Trajectory &traj, + const FMM::MM::PartialMatchResult &result); private: + void write_result_(std::stringstream &buf, + const FMM::CORE::Trajectory &traj, + const MatchedCandidatePath &opt_candidate_path, + const O_Path &opath, + const C_Path &cpath, + const std::vector &indices); + void write_linestring_(std::stringstream &buf, + const LineString &line); + void write_multilinestring_(std::stringstream &buf, + const MultiLineString &mline); std::ofstream m_fstream; const CONFIG::OutputConfig &config_; }; // CSVMatchResultWriter diff --git a/src/mm/mm_type.hpp b/src/mm/mm_type.hpp index aa78d2f..686cfd2 100644 --- a/src/mm/mm_type.hpp +++ b/src/mm/mm_type.hpp @@ -81,10 +81,20 @@ struct MatchResult { CORE::LineString mgeom; /**< the geometry of the matched path */ }; -// struct PartialMatchResult{ -// const MatchResult &result; -// std::vector t_geom; -// }; +struct PartialMatchResult { + int id; /**< id of the trajectory to be matched */ + MatchedCandidatePath opt_candidate_path; /**< A vector of candidate matched + to each point of a trajectory. It is stored in order to export more + detailed map matching information. */ + O_Path opath; /**< the optimal path, + containing id of edges matched to each + point in a trajectory */ + C_Path cpath; /**< the complete path, containing ids of a sequence of + topologically connected edges traversed by the + trajectory. */ + std::vector indices; /**< index of opath edge in cpath */ + CORE::MultiLineString mgeom; /**< the geometry of the matched path */ +}; }; From ef93b598359001cc6b6ce332b2c286e2ae80fd86 Mon Sep 17 00:00:00 2001 From: Can Yang Date: Sat, 16 May 2020 03:25:08 +0200 Subject: [PATCH 4/6] Add partial match function --- src/mm/fmm/fmm_algorithm.cpp | 156 +++++++++++++++++++++++++---------- src/mm/fmm/fmm_algorithm.hpp | 45 +++++++--- src/mm/transition_graph.cpp | 34 ++++---- src/network/network.hpp | 2 +- 4 files changed, 164 insertions(+), 73 deletions(-) diff --git a/src/mm/fmm/fmm_algorithm.cpp b/src/mm/fmm/fmm_algorithm.cpp index 490d2b1..8173a21 100644 --- a/src/mm/fmm/fmm_algorithm.cpp +++ b/src/mm/fmm/fmm_algorithm.cpp @@ -14,8 +14,8 @@ using namespace FMM::PYTHON; using namespace FMM::MM; FastMapMatchConfig::FastMapMatchConfig(int k_arg, double r_arg, - double gps_error) : - k(k_arg), radius(r_arg), gps_error(gps_error) { + double gps_error) : + k(k_arg), radius(r_arg), gps_error(gps_error) { }; void FastMapMatchConfig::print() const { @@ -24,7 +24,7 @@ void FastMapMatchConfig::print() const { }; FastMapMatchConfig FastMapMatchConfig::load_from_xml( - const boost::property_tree::ptree &xml_data) { + const boost::property_tree::ptree &xml_data) { int k = xml_data.get("config.parameters.k", 8); double radius = xml_data.get("config.parameters.r", 300.0); double gps_error = xml_data.get("config.parameters.gps_error", 50.0); @@ -32,7 +32,7 @@ FastMapMatchConfig FastMapMatchConfig::load_from_xml( }; FastMapMatchConfig FastMapMatchConfig::load_from_arg( - const cxxopts::ParseResult &arg_data) { + const cxxopts::ParseResult &arg_data) { int k = arg_data["candidates"].as(); double radius = arg_data["radius"].as(); double gps_error = arg_data["error"].as(); @@ -47,14 +47,14 @@ void FastMapMatchConfig::register_arg(cxxopts::Options &options){ cxxopts::value()->default_value("300.0")) ("e,error","GPS error", cxxopts::value()->default_value("50.0")); -} +}; void FastMapMatchConfig::register_help(std::ostringstream &oss){ oss<<"-k/--candidates (optional) : Number of candidates (8)\n"; oss<<"-r/--radius (optional) : search " - "radius (network data unit) (300)\n"; + "radius (network data unit) (300)\n"; oss<<"-e/--error (optional) : GPS error " - "(network data unit) (50)\n"; + "(network data unit) (50)\n"; }; bool FastMapMatchConfig::validate() const { @@ -64,14 +64,14 @@ bool FastMapMatchConfig::validate() const { return false; } return true; -} +}; MatchResult FastMapMatch::match_traj(const Trajectory &traj, - const FastMapMatchConfig &config) { + const FastMapMatchConfig &config) { SPDLOG_TRACE("Count of points in trajectory {}", traj.geom.get_num_points()); SPDLOG_TRACE("Search candidates"); Traj_Candidates tc = network_.search_tr_cs_knn( - traj.geom, config.k, config.radius); + traj.geom, config.k, config.radius); SPDLOG_TRACE("Trajectory candidate {}", tc); if (tc.empty()) return MatchResult{}; SPDLOG_TRACE("Generate transition graph"); @@ -86,16 +86,16 @@ MatchResult FastMapMatch::match_traj(const Trajectory &traj, std::transform(tg_opath.begin(), tg_opath.end(), matched_candidate_path.begin(), [](const TGNode *a) { - return MatchedCandidate{ - *(a->c), a->ep, a->tp, a->sp_dist - }; - }); + return MatchedCandidate{ + *(a->c), a->ep, a->tp, a->sp_dist + }; + }); O_Path opath(tg_opath.size()); std::transform(tg_opath.begin(), tg_opath.end(), opath.begin(), [](const TGNode *a) { - return a->c->edge->id; - }); + return a->c->edge->id; + }); std::vector indices; const std::vector &edges = network_.get_edges(); C_Path cpath = ubodt_->construct_complete_path(tg_opath, edges, @@ -103,14 +103,58 @@ MatchResult FastMapMatch::match_traj(const Trajectory &traj, SPDLOG_TRACE("Cpath {}", cpath); SPDLOG_TRACE("Complete path inference"); LineString mgeom = network_.complete_path_to_geometry( - traj.geom, cpath); + traj.geom, cpath); SPDLOG_TRACE("Complete path inference done"); return MatchResult{ - traj.id, matched_candidate_path, opath, cpath, indices, mgeom}; -} + traj.id, matched_candidate_path, opath, cpath, indices, mgeom}; +}; + +PartialMatchResult partial_match_traj( + const Trajectory &traj,const FastMapMatchConfig &config){ + SPDLOG_TRACE("Count of points in trajectory {}", traj.geom.get_num_points()); + SPDLOG_TRACE("Search candidates"); + Traj_Candidates tc = network_.search_tr_cs_knn( + traj.geom, config.k, config.radius); + SPDLOG_TRACE("Trajectory candidate {}", tc); + if (tc.empty()) return MatchResult{}; + SPDLOG_TRACE("Generate transition graph"); + TransitionGraph tg(tc, config.gps_error); + SPDLOG_TRACE("Update cost in transition graph"); + // The network will be used internally to update transition graph + update_tg(&tg, traj, config); + SPDLOG_TRACE("Optimal path inference"); + TGOpath tg_opath = tg.backtrack(); + SPDLOG_TRACE("Optimal path size {}", tg_opath.size()); + MatchedCandidatePath matched_candidate_path(tg_opath.size()); + std::transform(tg_opath.begin(), tg_opath.end(), + matched_candidate_path.begin(), + [](const TGNode *a) { + return MatchedCandidate{ + *(a->c), a->ep, a->tp, a->sp_dist + }; + }); + O_Path opath(tg_opath.size()); + std::transform(tg_opath.begin(), tg_opath.end(), + opath.begin(), + [](const TGNode *a) { + return a->c->edge->id; + }); + std::vector indices; + const std::vector &edges = network_.get_edges(); + C_Path cpath; + ubodt_->construct_traversed_path(tg_opath, edges, + &cpath, &indices); + SPDLOG_TRACE("Cpath {}", cpath); + SPDLOG_TRACE("Complete path inference"); + MultiLineString mgeom = network_.oc_path_to_multiple_geometry( + traj.geom, tg_opath, cpath); + SPDLOG_TRACE("Complete path inference done"); + return PartialMatchResult{ + traj.id, matched_candidate_path, opath, cpath, indices, mgeom}; +}; PyMatchResult FastMapMatch::match_wkt( - const std::string &wkt, const FastMapMatchConfig &config) { + const std::string &wkt, const FastMapMatchConfig &config) { LineString line = wkt2linestring(wkt); std::vector timestamps; Trajectory traj{0, line, timestamps}; @@ -124,17 +168,17 @@ PyMatchResult FastMapMatch::match_wkt( for (int i = 0; i < result.opt_candidate_path.size(); ++i) { const MatchedCandidate &mc = result.opt_candidate_path[i]; output.candidates.push_back( - {i, - mc.c.edge->id, - graph_.get_node_id(mc.c.edge->source), - graph_.get_node_id(mc.c.edge->target), - mc.c.dist, - mc.c.offset, - mc.c.edge->length, - mc.ep, - mc.tp, - mc.sp_dist} - ); + {i, + mc.c.edge->id, + graph_.get_node_id(mc.c.edge->source), + graph_.get_node_id(mc.c.edge->target), + mc.c.dist, + mc.c.offset, + mc.c.edge->length, + mc.ep, + mc.tp, + mc.sp_dist} + ); output.pgeom.add_point(mc.c.point); } return output; @@ -155,40 +199,68 @@ double FastMapMatch::get_sp_dist(const Candidate *ca, const Candidate *cb) { sp_dist = r->cost + ca->edge->length - ca->offset + cb->offset; } return sp_dist; -} +}; + +double FastMapMatch::get_sp_dist(const Candidate *ca, const Candidate *cb, + bool *connected) { + double sp_dist = 0; + if (ca->edge->id == cb->edge->id && ca->offset <= cb->offset) { + sp_dist = cb->offset - ca->offset; + } else if (ca->edge->target == cb->edge->source) { + // Transition on the same OD nodes + sp_dist = ca->edge->length - ca->offset + cb->offset; + } else { + Record *r = ubodt_->look_up(ca->edge->target, cb->edge->source); + // No sp path exist from O to D. + if (r == nullptr) { + *connected = false; + return sp_dist; + }; + // calculate original SP distance + sp_dist = r->cost + ca->edge->length - ca->offset + cb->offset; + } + *connected = true; + return sp_dist; +}; void FastMapMatch::update_tg( - TransitionGraph *tg, - const Trajectory &traj, const FastMapMatchConfig &config) { + TransitionGraph *tg, + const Trajectory &traj, const FastMapMatchConfig &config) { SPDLOG_TRACE("Update transition graph"); std::vector &layers = tg->get_layers(); std::vector eu_dists = ALGORITHM::cal_eu_dist(traj.geom); int N = layers.size(); for (int i = 0; i < N - 1; ++i) { + bool connected = false; update_layer(i, &(layers[i]), &(layers[i + 1]), - eu_dists[i]); + eu_dists[i],&connected); + if (!connected) tg->reset_layer(&(layers[i + 1])); } SPDLOG_TRACE("Update transition graph done"); -} +}; void FastMapMatch::update_layer(int level, - TGLayer *la_ptr, - TGLayer *lb_ptr, - double eu_dist) { + TGLayer *la_ptr, + TGLayer *lb_ptr, + double eu_dist, + bool *connected) { SPDLOG_TRACE("Update layer"); TGLayer &lb = *lb_ptr; for (auto iter_a = la_ptr->begin(); iter_a != la_ptr->end(); ++iter_a) { NodeIndex source = iter_a->c->index; for (auto iter_b = lb_ptr->begin(); iter_b != lb_ptr->end(); ++iter_b) { - double sp_dist = get_sp_dist(iter_a->c, iter_b->c); + bool connected_ab = false; + double sp_dist = get_sp_dist(iter_a->c, iter_b->c, &connected_ab); double tp = TransitionGraph::calc_tp(sp_dist, eu_dist); - if (iter_a->cumu_prob + tp * iter_b->ep >= iter_b->cumu_prob) { + if (iter_a->cumu_prob + tp * iter_b->ep >= iter_b->cumu_prob + && connected_ab) { iter_b->cumu_prob = iter_a->cumu_prob + tp * iter_b->ep; iter_b->prev = &(*iter_a); iter_b->tp = tp; iter_b->sp_dist = sp_dist; } + if (connected_ab) *connected = connected_ab; } } SPDLOG_TRACE("Update layer done"); -} +}; diff --git a/src/mm/fmm/fmm_algorithm.hpp b/src/mm/fmm/fmm_algorithm.hpp index c0c2ec9..bf58274 100644 --- a/src/mm/fmm/fmm_algorithm.hpp +++ b/src/mm/fmm/fmm_algorithm.hpp @@ -23,11 +23,11 @@ #include "cxxopts/cxxopts.hpp" namespace FMM { -namespace MM{ +namespace MM { /** * Configuration class for fmm algorithm */ -struct FastMapMatchConfig{ +struct FastMapMatchConfig { /** * Constructor of FastMapMatch configuration * @param k_arg the number of candidates @@ -55,14 +55,14 @@ struct FastMapMatchConfig{ * @return a %FastMapMatchConfig object */ static FastMapMatchConfig load_from_xml( - const boost::property_tree::ptree &xml_data); + const boost::property_tree::ptree &xml_data); /** * Load configuration from argument data * @param arg_data argument data * @return a %FastMapMatchConfig object */ static FastMapMatchConfig load_from_arg( - const cxxopts::ParseResult &arg_data); + const cxxopts::ParseResult &arg_data); /** * Register arguments to an option object */ @@ -79,7 +79,7 @@ struct FastMapMatchConfig{ * */ class FastMapMatch { - public: +public: /** * Constructor of Fast map matching model * @param network road network @@ -87,9 +87,9 @@ class FastMapMatch { * @param ubodt Upperbounded origin destination table */ FastMapMatch(const NETWORK::Network &network, - const NETWORK::NetworkGraph &graph, - std::shared_ptr ubodt) - : network_(network), graph_(graph), ubodt_(ubodt) { + const NETWORK::NetworkGraph &graph, + std::shared_ptr ubodt) + : network_(network), graph_(graph), ubodt_(ubodt) { }; /** * Match a trajectory to the road network @@ -99,6 +99,15 @@ class FastMapMatch { */ MatchResult match_traj(const CORE::Trajectory &traj, const FastMapMatchConfig &config); + /** + * Partial match a trajectory to the road network + * @param traj input trajector data + * @param config configuration of map matching algorithm + * @return A partial map matching result, which may contain multiple + * paths matched to segments of a trajectory. + */ + PartialMatchResult partial_match_traj( + const CORE::Trajectory &traj,const FastMapMatchConfig &config); /** * Match a wkt linestring to the road network. * @param wkt WKT representation of a trajectory @@ -106,8 +115,8 @@ class FastMapMatch { * @return Map matching result in POD format used in Python API */ PYTHON::PyMatchResult match_wkt( - const std::string &wkt,const FastMapMatchConfig &config); - protected: + const std::string &wkt,const FastMapMatchConfig &config); +protected: /** * Get shortest path distance between two candidates * @param ca from candidate @@ -116,6 +125,16 @@ class FastMapMatch { */ double get_sp_dist(const Candidate *ca, const Candidate *cb); + /** + * Get shortest path distance between two candidates + * @param ca from candidate + * @param cb to candidate + * @param connected if ca and cb are connected (found in UBODT), this + * value is set to true. Otherwise, it is set to false + * @return shortest path value + */ + double get_sp_dist(const Candidate *ca, + const Candidate *cb, bool* connected); /** * Update probabilities in a transition graph * @param tg transition graph @@ -131,10 +150,12 @@ class FastMapMatch { * @param la_ptr layer a * @param lb_ptr layer b next to a * @param eu_dist Euclidean distance between two observed point + * @param connected If the two layer are connected by a path, this value is + * set to true */ void update_layer(int level, TGLayer *la_ptr, TGLayer *lb_ptr, - double eu_dist); - private: + double eu_dist, bool *connected); +private: const NETWORK::Network &network_; const NETWORK::NetworkGraph &graph_; std::shared_ptr ubodt_; diff --git a/src/mm/transition_graph.cpp b/src/mm/transition_graph.cpp index dc97f61..2cc6598 100644 --- a/src/mm/transition_graph.cpp +++ b/src/mm/transition_graph.cpp @@ -60,29 +60,27 @@ const TGNode *TransitionGraph::find_optimal_candidate(const TGLayer &layer){ } TGOpath TransitionGraph::backtrack(){ - SPDLOG_TRACE("Backtrack on transition graph"); - TGNode* track_cand=nullptr; - double final_prob = -0.001; - std::vector& last_layer = layers.back(); - for (auto c = last_layer.begin(); c!=last_layer.end(); ++c) { - if(final_prob < c->cumu_prob) { - final_prob = c->cumu_prob; - track_cand = &(*c); - } - } + SPDLOG_TRACE("Backtrack start"); TGOpath opath; - if (final_prob>0) { - opath.push_back(track_cand); - // Iterate from tail to head to assign path - while ((track_cand=track_cand->prev)!=nullptr) { - opath.push_back(track_cand); + TGNode *opt_c=nullptr; + double final_prob = -0.001; + int N = layers.size(); + for (int i=N-1; i>=0; --i) { + opt_c = find_optimal_candidate(layers[i]); + opath.push_back(opt_c); + if (opt_c!=nullptr) { + while ((opt_c=opt_c->prev)!=nullptr) + { + opath.push_back(opt_c); + --i; + } } - std::reverse(opath.begin(), opath.end()); } - SPDLOG_TRACE("Backtrack on transition graph done"); + std::reverse(opath.begin(), opath.end()); + SPDLOG_TRACE("Backtrack done"); return opath; } std::vector &TransitionGraph::get_layers(){ return layers; -} \ No newline at end of file +} diff --git a/src/network/network.hpp b/src/network/network.hpp index 20c076f..9327c70 100644 --- a/src/network/network.hpp +++ b/src/network/network.hpp @@ -165,7 +165,7 @@ class Network { * @param complete_path A vector edge of IDs that are topologically connected * @param indices Indices of each edge of the opath in complete path */ - std::vector oc_path_to_multiple_geometry( + FMM::CORE::MultiLineString oc_path_to_multiple_geometry( const MM::O_Path &o_path, const MM::C_Path &complete_path, const std::vector &indices) const; /** From 930a52ff2584b8153508f4fa9591d1d0ccaff553 Mon Sep 17 00:00:00 2001 From: Can Yang Date: Sat, 16 May 2020 13:24:15 +0200 Subject: [PATCH 5/6] In progress with multiple path construction in fmm --- src/mm/fmm/fmm_algorithm.cpp | 49 +++++------------ src/mm/fmm/ubodt.cpp | 100 +++++++++++++++++++++++++++++++++++ src/mm/fmm/ubodt.hpp | 6 +-- src/mm/mm_type.hpp | 21 +++++--- src/mm/transition_graph.cpp | 30 ++++++++++- src/mm/transition_graph.hpp | 11 ++-- src/network/network.cpp | 56 +++++++++++++++++--- src/network/network.hpp | 4 +- 8 files changed, 219 insertions(+), 58 deletions(-) diff --git a/src/mm/fmm/fmm_algorithm.cpp b/src/mm/fmm/fmm_algorithm.cpp index 8173a21..31c4ff8 100644 --- a/src/mm/fmm/fmm_algorithm.cpp +++ b/src/mm/fmm/fmm_algorithm.cpp @@ -82,29 +82,19 @@ MatchResult FastMapMatch::match_traj(const Trajectory &traj, SPDLOG_TRACE("Optimal path inference"); TGOpath tg_opath = tg.backtrack(); SPDLOG_TRACE("Optimal path size {}", tg_opath.size()); - MatchedCandidatePath matched_candidate_path(tg_opath.size()); - std::transform(tg_opath.begin(), tg_opath.end(), - matched_candidate_path.begin(), - [](const TGNode *a) { - return MatchedCandidate{ - *(a->c), a->ep, a->tp, a->sp_dist - }; - }); - O_Path opath(tg_opath.size()); - std::transform(tg_opath.begin(), tg_opath.end(), - opath.begin(), - [](const TGNode *a) { - return a->c->edge->id; - }); + MatchedCandidatePath matched_candidate_path = + TransitionGraph::extract_matched_candidates(network,tg_opath); + O_Path opath = TransitionGraph::extract_opath(tg_opath); + SPDLOG_TRACE("Complete path inference"); std::vector indices; const std::vector &edges = network_.get_edges(); C_Path cpath = ubodt_->construct_complete_path(tg_opath, edges, &indices); SPDLOG_TRACE("Cpath {}", cpath); - SPDLOG_TRACE("Complete path inference"); + SPDLOG_TRACE("Geometry creation"); LineString mgeom = network_.complete_path_to_geometry( traj.geom, cpath); - SPDLOG_TRACE("Complete path inference done"); + SPDLOG_TRACE("Match trajectory done"); return MatchResult{ traj.id, matched_candidate_path, opath, cpath, indices, mgeom}; }; @@ -125,30 +115,19 @@ PartialMatchResult partial_match_traj( SPDLOG_TRACE("Optimal path inference"); TGOpath tg_opath = tg.backtrack(); SPDLOG_TRACE("Optimal path size {}", tg_opath.size()); - MatchedCandidatePath matched_candidate_path(tg_opath.size()); - std::transform(tg_opath.begin(), tg_opath.end(), - matched_candidate_path.begin(), - [](const TGNode *a) { - return MatchedCandidate{ - *(a->c), a->ep, a->tp, a->sp_dist - }; - }); - O_Path opath(tg_opath.size()); - std::transform(tg_opath.begin(), tg_opath.end(), - opath.begin(), - [](const TGNode *a) { - return a->c->edge->id; - }); + MatchedCandidatePath matched_candidate_path = + TransitionGraph::extract_matched_candidates(network,tg_opath); + O_Path opath = TransitionGraph::extract_opath(tg_opath); std::vector indices; const std::vector &edges = network_.get_edges(); C_Path cpath; - ubodt_->construct_traversed_path(tg_opath, edges, - &cpath, &indices); + C_Path cpath = ubodt_->construct_complete_path_partial_match( + tg_opath, edges, &indices); SPDLOG_TRACE("Cpath {}", cpath); - SPDLOG_TRACE("Complete path inference"); + SPDLOG_TRACE("Geometry construction"); MultiLineString mgeom = network_.oc_path_to_multiple_geometry( - traj.geom, tg_opath, cpath); - SPDLOG_TRACE("Complete path inference done"); + tg_opath, cpath, indices); + SPDLOG_TRACE("Partial match trajectory done"); return PartialMatchResult{ traj.id, matched_candidate_path, opath, cpath, indices, mgeom}; }; diff --git a/src/mm/fmm/ubodt.cpp b/src/mm/fmm/ubodt.cpp index 706afc0..3bb28a0 100644 --- a/src/mm/fmm/ubodt.cpp +++ b/src/mm/fmm/ubodt.cpp @@ -74,6 +74,10 @@ C_Path UBODT::construct_complete_path(const TGOpath &path, if (!indices->empty()) indices->clear(); if (path.empty()) return cpath; int N = path.size(); + for (int i = 0; i < N; ++i){ + if (path[i]==nullptr) + return cpath; + } cpath.push_back(path[0]->c->edge->id); int current_idx = 0; indices->push_back(current_idx); @@ -102,6 +106,102 @@ C_Path UBODT::construct_complete_path(const TGOpath &path, return cpath; } +C_Path UBODT::construct_complete_path_partial_match( + const TGOpath &path,const std::vector &edges, + std::vector *indices) const{ + C_Path cpath; + if (path.empty()) return cpath; + std::vector &edges = network.get_edges(); + int N = path.size(); + int current_idx = -1; + bool prev_connected = false; + for(int i=0; ipush_back(current_idx); + if (i==N-2) { + cpath.push_back(-1); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + indices->push_back(current_idx); + } + prev_connected = false; + } + if (a==nullptr && b!=nullptr ) { + SPDLOG_TRACE("a null b {}",b->edge->id); + cpath.push_back(-1); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + indices->push_back(current_idx); + if (i==N-2) { + cpath.push_back(b->edge->id); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + indices->push_back(current_idx); + } + prev_connected = false; + } + if (a!=nullptr && b==nullptr) { + SPDLOG_TRACE("a {} b nullptr",a->edge->id); + if (!prev_connected) { + cpath.push_back(a->edge->id); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + indices->push_back(current_idx); + } + if (i==N-2) { + cpath.push_back(-1); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + indices->push_back(current_idx); + } + prev_connected = false; + } + if (a!=nullptr && b !=nullptr) { + SPDLOG_TRACE("a {} b {}",a->edge->id,b->edge->id); + if (!prev_connected) { + cpath.push_back(a->edge->id); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + indices->push_back(current_idx); + } + if ((a->edge->id!=b->edge->id) || (a->offset>b->offset)) { + // segs stores edge index + auto segs = look_sp_path(a->edge->target,b->edge->source); + // No transition exist in UBODT + if (segs.empty() && a->edge->target!=b->edge->source) { + cpath.push_back(-1); + ++current_idx; + prev_connected = false; + } else { + for (int e:segs) { + cpath.push_back(edges[e].id); + ++current_idx; + } + cpath.push_back(b->edge->id); + ++current_idx; + SPDLOG_TRACE("Push back current index {}",current_idx); + indices->push_back(current_idx); + prev_connected = true; + } + } else { + // b stays on the same edge + SPDLOG_TRACE("Push back current index {}",current_idx); + indices->push_back(current_idx); + prev_connected = true; + } + } + SPDLOG_TRACE("Iterate i {} done",i); + } + return cpath; +} + double UBODT::get_delta() const { return delta; } diff --git a/src/mm/fmm/ubodt.hpp b/src/mm/fmm/ubodt.hpp index 37b6ce7..495a77d 100644 --- a/src/mm/fmm/ubodt.hpp +++ b/src/mm/fmm/ubodt.hpp @@ -80,9 +80,9 @@ class UBODT { const std::vector &edges, std::vector *indices) const; - void construct_traversed_path( - const TGOpath &path, const std::vector &edges, - C_Path *cpath, std::vector *indices) const; + C_Path construct_complete_path_partial_match( + const TGOpath &path,const std::vector &edges, + std::vector *indices) const; /** * Get the upperbound of the UBODT diff --git a/src/mm/mm_type.hpp b/src/mm/mm_type.hpp index 686cfd2..ba6a5c5 100644 --- a/src/mm/mm_type.hpp +++ b/src/mm/mm_type.hpp @@ -48,13 +48,22 @@ typedef std::vector C_Path; /**< Complete path, ids of a sequence of topologically connected edges.*/ /** - * A candidate matched to a point + * A candidate matched to a point, the fields are stored to facilitate the + * output of detailed MM information. */ struct MatchedCandidate { - Candidate c; /**< Candidate matched to a point */ - double ep; /**< emission probability */ - double tp; /**< transition probability to previous matched candidate */ - double sp_dist; /**< shortest path distance to previous matched candidate */ + int index; /**< Index of point index in trajectory */ + EdgeID edge_id; /**< Edge id matched */ + NodeID source; /**< Edge source node ID */ + NodeID target; /**< Edge target node ID */ + double error; /**< Error of matching */ + double offset; /**< Matched point distance to start node of edge */ + double length; /**< Length of edge matched */ + double ep; /**< emission probability */ + double tp; /**< transition proability from previous matched candidate */ + double spdist; /**< shortest path distance from previous matched candidate */ + double px; /**< Projected point x */ + double py; /**< Projected point y */ }; /** @@ -66,7 +75,7 @@ typedef std::vector MatchedCandidatePath; /** * Map matched result representation */ -struct MatchResult { +struct MatchResult{ int id; /**< id of the trajectory to be matched */ MatchedCandidatePath opt_candidate_path; /**< A vector of candidate matched to each point of a trajectory. It is stored in order to export more diff --git a/src/mm/transition_graph.cpp b/src/mm/transition_graph.cpp index 2cc6598..1a883e7 100644 --- a/src/mm/transition_graph.cpp +++ b/src/mm/transition_graph.cpp @@ -57,7 +57,7 @@ const TGNode *TransitionGraph::find_optimal_candidate(const TGLayer &layer){ } } return opt_c; -} +}; TGOpath TransitionGraph::backtrack(){ SPDLOG_TRACE("Backtrack start"); @@ -84,3 +84,31 @@ TGOpath TransitionGraph::backtrack(){ std::vector &TransitionGraph::get_layers(){ return layers; } + +MatchedCandidatePath TransitionGraph::extract_matched_candidates( + const Network &graph, + const TGOpath &tg_opath){ + MatchedCandidatePath matched_candidate_path; + for (int i = 0; i < tg_opath.size(); ++i) { + const TGNode *a = tg_opath[i]; + if (a==nullptr) { + matched_candidate_path.push_back( + {i,-1,-1,-1,0,0,0,0,0,0} + ); + } else { + matched_candidate_path.push_back( + {i, + a->c->edge->id, + network.get_node_id(a->c->edge->source), + network.get_node_id(a->c->edge->target), + a->c->dist, + a->c->offset, + a->c->edge->length, + a->ep, + a->tp, + a->sp_dist} + ); + } + } + return matched_candidate_path; +}; diff --git a/src/mm/transition_graph.hpp b/src/mm/transition_graph.hpp index e152fba..7cc3c68 100644 --- a/src/mm/transition_graph.hpp +++ b/src/mm/transition_graph.hpp @@ -15,6 +15,7 @@ #include "network/type.hpp" #include "mm/mm_type.hpp" +#include "network/network.hpp" #include @@ -96,15 +97,19 @@ class TransitionGraph */ const TGNode *find_optimal_candidate(const TGLayer &layer); /** - * Backtrack the transition graph to find an optimal path - * @return An optimal path connecting the first layer with last layer and - * has the highest accumulative probability value. + * Backtrack the transition graph to find an optimal path. Some parts may + * be unmatched (e.g., not connected or no candidates) + * @return An optimal path connecting the candidate found for each point. + * if some points are not matched, nullptr will be returned. */ TGOpath backtrack(); /** * Get a reference to the inner layers of the transition graph. */ std::vector &get_layers(); + static MatchedCandidatePath extract_matched_candidates( + const Network &network, const TGOpath &tg_opath); + static O_Path extract_opath(const TGOpath &tg_opath); private: // candidates of a trajectory std::vector layers; diff --git a/src/network/network.cpp b/src/network/network.cpp index 1fad960..034d5c0 100644 --- a/src/network/network.cpp +++ b/src/network/network.cpp @@ -297,24 +297,64 @@ LineString Network::complete_path_to_geometry( return line; }; -std::vector Network::oc_path_to_multiple_geometry( - const O_Path &o_path, const C_Path &complete_path, +LineString Network::ocpath_to_geometry(const MM::TGOpath &tg_opath + const C_Path &complete_path) +{ + LineString line; + if (complete_path.empty()) return line; + // If complete path contains -1, export empty line + for (auto &item:complete_path) { + if (item == -1) return line; + } + int NOsegs = o_path.size(); + int NCsegs = complete_path.size(); + if (NCsegs ==1) + { + LineString &firstseg = get_edge_geom(complete_path[0]); + double firstoffset = tg_opath[0]->c->offset; + double lastoffset = tg_opath[NOsegs-1]->c->offset; + LineString firstlineseg= ALGORITHM::cutoffseg_unique( + firstoffset,lastoffset,firstseg); + append_segs_to_line(&line,firstlineseg,0); + } else { + LineString &firstseg = get_edge_geom(complete_path[0]); + LineString &lastseg = get_edge_geom(complete_path[NCsegs-1]); + double firstoffset = tg_opath[0]->c->offset; + double lastoffset = tg_opath[NOsegs-1]->c->offset; + LineString firstlineseg= ALGORITHM::cutoffseg(firstoffset, firstseg, 0); + LineString lastlineseg= ALGORITHM::cutoffseg(lastoffset, lastseg, 1); + append_segs_to_line(&line,firstlineseg,0); + if (NCsegs>2) + { + for(int i=1; i &indices) const { - if (t_path.cpath.empty()) return {}; + if (cpath.empty()) return {}; std::vector lines; // Iterate through consecutive indexes and write the traversed path int J = indices.size(); for (int j=0; j &indices) const; /** * Get all node geometry From d500cba4785d2061787fd512222088f8d16e73c8 Mon Sep 17 00:00:00 2001 From: Can Yang Date: Sat, 16 May 2020 13:34:45 +0200 Subject: [PATCH 6/6] Add px,py to the matched candidate --- src/core/geometry.hpp | 3 ++- src/mm/transition_graph.cpp | 9 ++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/core/geometry.hpp b/src/core/geometry.hpp index 1344812..f10c324 100644 --- a/src/core/geometry.hpp +++ b/src/core/geometry.hpp @@ -25,7 +25,8 @@ namespace CORE { * Point class */ typedef boost::geometry::model::point Point; // Point for rtree box + boost::geometry::cs::cartesian> Point; + /** * Linestring geometry class * diff --git a/src/mm/transition_graph.cpp b/src/mm/transition_graph.cpp index 1a883e7..a93ceb6 100644 --- a/src/mm/transition_graph.cpp +++ b/src/mm/transition_graph.cpp @@ -93,8 +93,8 @@ MatchedCandidatePath TransitionGraph::extract_matched_candidates( const TGNode *a = tg_opath[i]; if (a==nullptr) { matched_candidate_path.push_back( - {i,-1,-1,-1,0,0,0,0,0,0} - ); + {i,-1,-1,-1,0,0,0,0,0,0,0,0} + ); } else { matched_candidate_path.push_back( {i, @@ -106,7 +106,10 @@ MatchedCandidatePath TransitionGraph::extract_matched_candidates( a->c->edge->length, a->ep, a->tp, - a->sp_dist} + a->sp_dist, + a->c->point.get<0>(), + a->c->point.get<1>() + } ); } }