diff --git a/app/fmm.cpp b/app/fmm.cpp index cfe3186..fff82ee 100644 --- a/app/fmm.cpp +++ b/app/fmm.cpp @@ -20,110 +20,115 @@ #include "../src/gps.hpp" #include "../src/reader.hpp" #include "../src/writer.hpp" -#include "../src/multilevel_debug.h" +#include "../src/debug.h" #include "../src/config.hpp" using namespace std; using namespace MM; using namespace MM::IO; int main (int argc, char **argv) { - std::cout<<"------------ Fast map matching (FMM) ------------"<get_delta(); - std::cout<<" Delta inferred from ubodt as "<< config.delta <<'\n'; - } - TrajectoryReader tr_reader(config.gps_file,config.gps_id); - ResultConfig result_config = config.get_result_config(); - ResultWriter rw(config.result_file,&network,result_config); - int progress=0; - int points_matched=0; - int total_points=0; - int num_trajectories = tr_reader.get_num_trajectories(); - int step_size = num_trajectories/10; - if (step_size<10) step_size=10; - std::chrono::steady_clock::time_point corrected_begin = std::chrono::steady_clock::now(); - std::cout<<"Start to map match trajectories with total number "<< num_trajectories <<'\n'; - // The header is moved to constructor of result writer - // rw.write_header(); + ubodt = read_ubodt_csv(config.ubodt_file,multiplier); + } + if (!config.delta_defined) { + config.delta = ubodt->get_delta(); + std::cout<<" Delta inferred from ubodt as "<< config.delta <<'\n'; + } + TrajectoryReader tr_reader(config.gps_file,config.gps_id); + ResultConfig result_config = config.get_result_config(); + ResultWriter rw(config.result_file,network,result_config); + int progress=0; + int points_matched=0; + int total_points=0; + int num_trajectories = tr_reader.get_num_trajectories(); + int step_size = num_trajectories/10; + if (step_size<10) step_size=10; + std::chrono::steady_clock::time_point corrected_begin = + std::chrono::steady_clock::now(); + std::cout<<"Start to map match trajectories with total number " + << num_trajectories <<'\n'; + // The header is moved to constructor of result writer + // rw.write_header(); - while (tr_reader.has_next_feature()) - { - DEBUG(2) std::cout<<"Start of the loop"<<'\n'; - Trajectory trajectory = tr_reader.read_next_trajectory(); - int points_in_tr = trajectory.geom->getNumPoints(); - if (progress%step_size==0) std::cout<<"Progress "<construct_traversed_path(o_path_ptr); - // C_Path *c_path_ptr = ubodt->construct_complete_path(o_path_ptr); - if (result_config.write_mgeom) { - LineString *m_geom = network.complete_path_to_geometry(o_path_ptr,&(t_path_ptr->cpath)); - rw.write_result(trajectory.id,trajectory.geom,o_path_ptr,t_path_ptr,m_geom); - delete m_geom; - } else { - rw.write_result(trajectory.id,trajectory.geom,o_path_ptr,t_path_ptr,nullptr); - } - // update statistics - total_points+=points_in_tr; - if (t_path_ptr!=nullptr) points_matched+=points_in_tr; - DEBUG(1) std::cout<<"Free memory of o_path and c_path"<<'\n'; - ++progress; - delete o_path_ptr; - delete t_path_ptr; - DEBUG(1) std::cout<<"============================="<<'\n'; - } - std::cout<<"\n============================="<<'\n'; - std::cout<<"MM process finished"<<'\n'; - std::chrono::steady_clock::time_point end= std::chrono::steady_clock::now(); - // clock_t end_time = clock(); // program end time - // Unit is second - // double time_spent = (double)(end_time - begin_time) / CLOCKS_PER_SEC; - double time_spent = std::chrono::duration_cast(end - begin).count()/1000.; - double time_spent_exclude_input = std::chrono::duration_cast(end - corrected_begin).count()/1000.; - std::cout<<"Time takes "<construct_traversed_path(o_path,network); + LineString m_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); + // update statistics + total_points+=points_in_tr; + if (!t_path.cpath.empty()) points_matched+=points_in_tr; + ++progress; } - std::cout<<"------------ Program finished ------------"< + (end - begin).count()/1000.; + double time_spent_exclude_input = + std::chrono::duration_cast + (end - corrected_begin).count()/1000.; + std::cout<<"Time takes "< trajectories = tr_reader.read_next_N_trajectories(buffer_trajectories_size); - int trajectories_fetched = trajectories.size(); + ubodt = read_ubodt_csv(config.ubodt_file,multiplier); + } + if (!config.delta_defined) { + config.delta = ubodt->get_delta(); + std::cout<<" Delta inferred from ubodt as "<< config.delta <<'\n'; + } + TrajectoryReader tr_reader(config.gps_file, config.gps_id); + ResultConfig result_config = config.get_result_config(); + ResultWriter rw(config.result_file,network,result_config); + int progress = 0; + int points_matched = 0; + int total_points = 0; + int num_trajectories = tr_reader.get_num_trajectories(); + int buffer_trajectories_size = 100000; + int step_size = num_trajectories / 10; + if (step_size < 10) step_size = 10; + std::cout << "Start to map match trajectories with total number " + << num_trajectories << '\n'; + // No geometry output + while (tr_reader.has_next_feature()) { + std::vector trajectories = + tr_reader.read_next_N_trajectories(buffer_trajectories_size); + int trajectories_fetched = trajectories.size(); #pragma omp parallel for - for (int i = 0; i < trajectories_fetched; ++i) { - Trajectory &trajectory = trajectories[i]; - int points_in_tr = trajectory.geom->getNumPoints(); - DEBUG(1) std::cout << "\n=============================" << '\n'; - DEBUG(1) std::cout << "Process trips with id : " << trajectory.id << '\n'; - // Candidate search - Traj_Candidates traj_candidates = network.search_tr_cs_knn(trajectory, config.k, config.radius,config.gps_error); - TransitionGraph tg = TransitionGraph(&traj_candidates,trajectory.geom, ubodt,config.delta); - // Optimal path inference - O_Path *o_path_ptr = tg.viterbi(config.penalty_factor); - T_Path *t_path_ptr = ubodt->construct_traversed_path(o_path_ptr); - if (result_config.write_mgeom) { - LineString *m_geom = network.complete_path_to_geometry(o_path_ptr,&(t_path_ptr->cpath)); - rw.write_result(trajectory.id,trajectory.geom,o_path_ptr,t_path_ptr,m_geom); - delete m_geom; - } else { - rw.write_result(trajectory.id,trajectory.geom,o_path_ptr,t_path_ptr,nullptr); - } - // update statistics - total_points+=points_in_tr; - if (t_path_ptr!=nullptr) points_matched+=points_in_tr; - DEBUG(1) std::cout<<"Free memory of o_path and c_path"<<'\n'; - ++progress; - if (progress % step_size == 0) { - std::stringstream buf; - buf << "Progress " << progress << " / " << num_trajectories << '\n'; - std::cout << buf.rdbuf(); - } - delete o_path_ptr; - delete t_path_ptr; - } + for (int i = 0; i < trajectories_fetched; ++i) { + Trajectory &trajectory = trajectories[i]; + int points_in_tr = trajectory.geom.getNumPoints(); + // Candidate search + Traj_Candidates traj_candidates = network.search_tr_cs_knn( + trajectory, config.k, config.radius,config.gps_error); + TransitionGraph tg = TransitionGraph( + &traj_candidates,trajectory.geom,*ubodt,config.delta); + // Optimal path inference + O_Path o_path = tg.viterbi(config.penalty_factor); + T_Path t_path = ubodt->construct_traversed_path(o_path,network); + LineString m_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); + // update statistics + #pragma omp critical + total_points+=points_in_tr; + if (!t_path.cpath.empty()) points_matched+=points_in_tr; + ++progress; + if (progress % step_size == 0) { + std::stringstream buf; + buf << "Progress " << progress << " / " << num_trajectories << '\n'; + std::cout << buf.rdbuf(); } - std::cout << "\n=============================" << '\n'; - std::cout << "MM process finished" << '\n'; - // clock_t end_time = clock(); // program end time - std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); - // Unit is second - // std::cout << "Time takes" << <(end - begin).count() / 1000.; - //double time_spent = (double)(end_time - begin_time) / CLOCKS_PER_SEC; - std::cout << "Time takes " << time_spent << '\n'; - std::cout << "Finish map match total points " << total_points - << " and points matched " << points_matched << '\n'; - std::cout << "Matched percentage: " << points_matched / (double)total_points << '\n'; - std::cout << "Point match speed:" << points_matched / time_spent << "pt/s" << '\n'; - delete ubodt; + } } - std::cout << "------------ Program finished ------------" << endl; - return 0; + std::cout << "\n=============================" << '\n'; + std::cout << "MM process finished" << '\n'; + // clock_t end_time = clock(); // program end time + std::chrono::steady_clock::time_point end = + std::chrono::steady_clock::now(); + // Unit is second + // std::cout << "Time takes" << < + (end - begin).count() / 1000.; + //double time_spent = (double)(end_time - begin_time) / CLOCKS_PER_SEC; + std::cout << "Time takes " << time_spent << '\n'; + std::cout << "Finish map match total points " << total_points + << " and points matched " << points_matched << '\n'; + std::cout << "Matched percentage: " + << points_matched / (double)total_points << '\n'; + std::cout << "Point match speed:" + << points_matched / time_spent << "pt/s" << '\n'; + delete ubodt; + } + std::cout << "------------ Program finished ------------" << endl; + return 0; }; diff --git a/app/ubodt_gen.cpp b/app/ubodt_gen.cpp index 6fa9883..861405f 100644 --- a/app/ubodt_gen.cpp +++ b/app/ubodt_gen.cpp @@ -3,7 +3,7 @@ * Precomputation of UBODT (Optimized) * * @author: Can Yang - * @version: 2018.03.09 + * @version: 2020.01.31 */ #include "../src/network_graph.hpp" #include "../src/config.hpp" @@ -13,43 +13,48 @@ using namespace std; using namespace MM; int main(int argc, char* argv[]) { - std::cout<<"------------ Fast map matching (FMM) ------------"<(end - begin).count() / 1000.; - //double time_spent = (double)(end_time - begin_time) / CLOCKS_PER_SEC; - std::cout << "Time takes " << time_spent << '\n'; - // clock_t end_time = clock(); // program end time - // // Unit is second - // double time_spent = (double)(end_time - begin_time) / CLOCKS_PER_SEC; - // std::cout<<"Time takes "< + (end - begin).count() / 1000.; + //double time_spent = (double)(end_time - begin_time) / CLOCKS_PER_SEC; + std::cout << "Time takes " << time_spent << '\n'; + // clock_t end_time = clock(); // program end time + // // Unit is second + // double time_spent = (double)(end_time - begin_time) / CLOCKS_PER_SEC; + // std::cout<<"Time takes "< #include @@ -13,47 +13,50 @@ using namespace std; using namespace MM; int main(int argc, char* argv[]) { - std::cout<<"------------ Fast map matching (FMM) ------------"<(end - begin).count() / 1000.; - //double time_spent = (double)(end_time - begin_time) / CLOCKS_PER_SEC; - std::cout << "Time takes " << time_spent << '\n'; - } - std::cout<<"------------ Program finished ------------"< + (end - begin).count() / 1000.; + //double time_spent = (double)(end_time - begin_time) / CLOCKS_PER_SEC; + std::cout << "Time takes " << time_spent << '\n'; + } + std::cout<<"------------ Program finished ------------"< + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. \ No newline at end of file diff --git a/include/fiboheap/README.md b/include/fiboheap/README.md new file mode 100644 index 0000000..ed083c1 --- /dev/null +++ b/include/fiboheap/README.md @@ -0,0 +1,16 @@ +fiboheap +======== + +C++ STL-like Fibonacci heap and queue for fast priority queues with mutable keys. + +This is a header-only implementation of: +* Fibonacci Heap: a fast heap with mutable keys; + Implementation follows Cormen et al. (2009) "Fibonacci Heaps," in Introduction to Algorithms, 3rd ed. Cambridge: MIT Press, pp. 505-530. +* Fibonacci Queue: a priority queue based on Fibonacci heap. This is basically a Fibonacci heap with an added fast store for retrieving nodes, and decrease their key as needed. Useful for search algorithms (e.g. Dijkstra, heuristic, ...). + +The heap and queues are targeted at projects that are relunctant to rely on Boost for a simple Fibonacci heap or queue. + +Compile test exe with +``` +g++ -g -std=c++11 test_fiboheap.cc -o tf +``` diff --git a/include/fiboheap/fiboheap.h b/include/fiboheap/fiboheap.h new file mode 100644 index 0000000..cafb9a6 --- /dev/null +++ b/include/fiboheap/fiboheap.h @@ -0,0 +1,590 @@ +/** + * Fibonacci Heap + * Copyright (c) 2014, Emmanuel Benazera beniz@droidnik.fr, All rights reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 3.0 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library. + */ + +#ifndef FIBOHEAP_H +#define FIBOHEAP_H + +#include +#include +#include +#include + +template> +class FibHeap +{ + public: + + // node + class FibNode + { + public: + FibNode(T k, void *pl) + :key(std::move(k)),mark(false),p(nullptr),left(nullptr),right(nullptr),child(nullptr),degree(-1),payload(pl) + { + } + + ~FibNode() + { + } + + T key; + bool mark; + FibNode *p; + FibNode *left; + FibNode *right; + FibNode *child; + int degree; + void *payload; + }; // end FibNode + + FibHeap() : FibHeap(std::less()) + { + } + + FibHeap(Comp comp) + :n(0), min(nullptr), comp(comp) + { + } + + ~FibHeap() + { + clear(); + } + + void clear() { + // delete all nodes. + delete_fibnodes(min); + min = nullptr; + n = 0; + } + + void delete_fibnodes(FibNode *x) + { + if (!x) + return; + + FibNode *cur = x; + while(true) + { + /*std::cerr << "cur: " << cur << std::endl; + std::cerr << "x: " << x << std::endl;*/ + if (cur->left && cur->left != x) + { + //std::cerr << "cur left: " << cur->left << std::endl; + FibNode *tmp = cur; + cur = cur->left; + if (tmp->child) + delete_fibnodes(tmp->child); + delete tmp; + } + else + { + if (cur->child) + delete_fibnodes(cur->child); + delete cur; + break; + } + } + } + + /* + * insert(x) + * 1. x.degree = 0 + * 2. x.p = NIL + * 3. x.child = NIL + * 4. x.mark = FALSE + * 5. if H.min == NIL + * 6. create a root list for H containing just x + * 7. H.min = x + * 8. else insert x into H's root list + * 9. if x.key < H.min.key + *10. H.min = x + *11. H.n = H.n + 1 + */ + void insert(FibNode *x) + { + // 1 + x->degree = 0; + // 2 + x->p = nullptr; + // 3 + x->child = nullptr; + // 4 + x->mark = false; + // 5 + if ( min == nullptr) + { + // 6, 7 + min = x->left = x->right = x; + } + else + { + // 8 + min->left->right = x; + x->left = min->left; + min->left = x; + x->right = min; + // 9 + if ( comp(x->key, min->key) ) + { + // 10 + min = x; + } + } + // 11 + ++n; + } + + /* + * The minimum node of the heap. + */ + FibNode* minimum() + { + return min; + } + + /* + * union_fibheap(H1,H2) + * 1. H = MAKE-FIB-HEAP() + * 2. H.min = H1.min + * 3. concatenate the root list of H2 with the root list of H + * 4. if (H1.min == NIL) or (H2.min != NIL and H2.min.key < H1.min.key) + * 5. H.min = H2.min + * 6. H.n = H1.n + H2.n + * 7. return H + */ + static FibHeap* union_fibheap(FibHeap *H1, FibHeap *H2) + { + // 1 + FibHeap* H = new FibHeap(); + // 2 + H->min = H1->min; + // 3 + if ( H->min != nullptr && H2->min != nullptr ) + { + H->min->right->left = H2->min->left; + H2->min->left->right = H->min->right; + H->min->right = H2->min; + H2->min->left = H->min; + } + // 4 + if ( H1->min == nullptr || ( H2->min != nullptr && H1->comp(H2->min->key, H1->min->key) ) ) + { + // 5 + H->min = H2->min; + } + // 6 + H->n = H1->n + H2->n; + // 7 + return H; + } + + /* + * extract_min + * 1. z = H.min + * 2. if z != NIL + * 3. for each child x of z + * 4. add x to the root list of H + * 5. x.p = NIL + * 6. remove z from the root list of H + * 7. if z == z.right + * 8. H.min = NIL + * 9. else H.min = z.right + *10. CONSOLIDATE(H) + *11. H.n = H.n - 1 + *12. return z + */ + FibNode* extract_min() + { + FibNode *z, *x, *next; + FibNode ** childList; + + // 1 + z = min; + // 2 + if ( z != nullptr ) + { + // 3 + x = z->child; + if ( x != nullptr ) + { + childList = new FibNode*[z->degree]; + next = x; + for ( int i = 0; i < (int)z->degree; i++ ) + { + childList[i] = next; + next = next->right; + } + for ( int i = 0; i < (int)z->degree; i++ ) + { + x = childList[i]; + // 4 + min->left->right = x; + x->left = min->left; + min->left = x; + x->right = min; + // 5 + x->p = nullptr; + } + delete [] childList; + } + // 6 + z->left->right = z->right; + z->right->left = z->left; + // 7 + if ( z == z->right ) + { + // 8 + min = nullptr; + } + else + { + // 9 + min = z->right; + // 10 + consolidate(); + } + // 11 + n--; + } + // 12 + return z; + } + + /* + * consolidate + * 1. let A[0 . . D(H.n)] be a new array + * 2. for i = 0 to D(H.n) + * 3. A[i] = NIL + * 4. for each node w in the root list of H + * 5. x = w + * 6. d = x.degree + * 7. while A[d] != NIL + * 8. y = A[d] + * 9. if x.key > y.key + *10. exchange x with y + *11. FIB-HEAP-LINK(H,y,x) + *12. A[d] = NIL + *13. d = d + 1 + *14. A[d] = x + *15. H.min = NIL + *16. for i = 0 to D(H.n) + *17. if A[i] != NIL + *18. if H.min == NIL + *19. create a root list for H containing just A[i] + *20. H.min = A[i] + *21. else insert A[i] into H's root list + *22. if A[i].key < H.min.key + *23. H.min = A[i] + */ + void consolidate() + { + FibNode* w, * next, * x, * y, * temp; + FibNode** A, ** rootList; + // Max degree <= log base golden ratio of n + int d, rootSize; + int max_degree = static_cast(floor(log(static_cast(n))/log(static_cast(1 + sqrt(static_cast(5)))/2))); + + // 1 + A = new FibNode*[max_degree+2]; // plus two both for indexing to max degree and so A[max_degree+1] == NIL + // 2, 3 + std::fill_n(A, max_degree+2, nullptr); + // 4 + w = min; + rootSize = 0; + next = w; + do + { + rootSize++; + next = next->right; + } while ( next != w ); + rootList = new FibNode*[rootSize]; + for ( int i = 0; i < rootSize; i++ ) + { + rootList[i] = next; + next = next->right; + } + for ( int i = 0; i < rootSize; i++ ) + { + w = rootList[i]; + // 5 + x = w; + // 6 + d = x->degree; + // 7 + while ( A[d] != nullptr ) + { + // 8 + y = A[d]; + // 9 + if ( comp(y->key, x->key) ) + { + // 10 + temp = x; + x = y; + y = temp; + } + // 11 + fib_heap_link(y,x); + // 12 + A[d] = nullptr; + // 13 + d++; + } + // 14 + A[d] = x; + } + delete [] rootList; + // 15 + min = nullptr; + // 16 + for ( int i = 0; i < max_degree+2; i++ ) + { + // 17 + if ( A[i] != nullptr ) + { + // 18 + if ( min == nullptr ) + { + // 19, 20 + min = A[i]->left = A[i]->right = A[i]; + } + else + { + // 21 + min->left->right = A[i]; + A[i]->left = min->left; + min->left = A[i]; + A[i]->right = min; + // 22 + if ( comp(A[i]->key, min->key) ) + { + // 23 + min = A[i]; + } + } + } + } + delete [] A; + } + +/* + * fib_heap_link(y,x) + * 1. remove y from the root list of heap + * 2. make y a child of x, incrementing x.degree + * 3. y.mark = FALSE + */ + void fib_heap_link( FibNode* y, FibNode* x ) + { + // 1 + y->left->right = y->right; + y->right->left = y->left; + // 2 + if ( x->child != nullptr ) + { + x->child->left->right = y; + y->left = x->child->left; + x->child->left = y; + y->right = x->child; + } + else + { + x->child = y; + y->right = y; + y->left = y; + } + y->p = x; + x->degree++; + // 3 + y->mark = false; + } + + + /* + * decrease_key(x,k) + * 1. if k > x.key + * 2. error "new key is greater than current key" + * 3. x.key = k + * 4. y = x.p + * 5. if y != NIL and x.key < y.key + * 6. CUT(H,x,y) + * 7. CASCADING-CUT(H,y) + * 8. if x.key < H.min.key + * 9. H.min = x + */ + void decrease_key( FibNode* x, T k ) + { + FibNode* y; + + // 1 + if ( comp(x->key, k) ) + { + // 2 + // error( "new key is greater than current key" ); + return; + } + // 3 + x->key = std::move(k); + // 4 + y = x->p; + // 5 + if ( y != nullptr && comp(x->key, y->key) ) + { + // 6 + cut(x,y); + // 7 + cascading_cut(y); + } + // 8 + if ( comp(x->key, min->key) ) + { + // 9 + min = x; + } + } + + /* + * cut(x,y) + * 1. remove x from the child list of y, decrementing y.degree + * 2. add x to the root list of H + * 3. x.p = NIL + * 4. x.mark = FALSE + */ + void cut( FibNode* x, FibNode* y ) + { + // 1 + if ( x->right == x ) + { + y->child = nullptr; + } + else + { + x->right->left = x->left; + x->left->right = x->right; + if ( y->child == x ) + { + y->child = x->right; + } + } + y->degree--; + // 2 + min->right->left = x; + x->right = min->right; + min->right = x; + x->left = min; + // 3 + x->p = nullptr; + // 4 + x->mark = false; + } + +/* + * cascading_cut(y) + * 1. z = y.p + * 2. if z != NIL + * 3. if y.mark == FALSE + * 4. y.mark = TRUE + * 5. else CUT(H,y,z) + * 6. CASCADING-CUT(H,z) + */ + void cascading_cut( FibNode* y ) + { + FibNode* z; + + // 1 + z = y->p; + // 2 + if ( z != nullptr ) + { + // 3 + if ( y->mark == false ) + { + // 4 + y->mark = true; + } + else + { + // 5 + cut(y,z); + // 6 + cascading_cut(z); + } + } + } + + /* + * set to infinity so that it hits the top of the heap, then easily remove. + */ + void remove_fibnode( FibNode* x ) + { + decrease_key(x,std::numeric_limits::min()); + FibNode *fn = extract_min(); + delete fn; + } + + /* + * mapping operations to STL-compatible signatures. + */ + bool empty() const + { + return n == 0; + } + + FibNode* topNode() + { + return minimum(); + } + + T& top() + { + return minimum()->key; + } + + void pop() + { + if (empty()) + return; + FibNode *x = extract_min(); + if (x) + delete x; + } + + FibNode* push(T k, void *pl) + { + FibNode *x = new FibNode(std::move(k),pl); + insert(x); + return x; + } + + FibNode* push(T k) + { + return push(std::move(k),nullptr); + } + + unsigned int size() + { + return (unsigned int) n; + } + + int n; + FibNode *min; + Comp comp; + +}; + +#endif diff --git a/include/fiboheap/fiboqueue.h b/include/fiboheap/fiboqueue.h new file mode 100644 index 0000000..e62e507 --- /dev/null +++ b/include/fiboheap/fiboqueue.h @@ -0,0 +1,119 @@ +/** + * Fibonacci Queue + * Copyright (c) 2014, Emmanuel Benazera beniz@droidnik.fr, All rights reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 3.0 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library. + */ + +/** + * This is basically a Fibonacci heap with an added fast store for retrieving + * nodes, and decrease their key as needed. Useful for search algorithms (e.g. + * Dijstra, heuristic, ...). + */ + +#ifndef FIBOQUEUE_H +#define FIBOQUEUE_H + +#include "fiboheap.h" +#include +#include + +template> +class FibQueue : public FibHeap +{ + public: + using Heap = FibHeap; + using Node = typename Heap::FibNode; + using KeyNodeIter = typename std::unordered_map::iterator; + + FibQueue() + : Heap() + { + } + + FibQueue(Comp comp) + : Heap(comp) + { + } + + ~FibQueue() + { + } + + void decrease_key(Node *x, T k) + { + KeyNodeIter mit = find(x->key); + fstore.erase(mit); + fstore.insert({ k, x }); + Heap::decrease_key(x,std::move(k)); + } + + Node* push(T k, void *pl) + { + Node *x = Heap::push(std::move(k),pl); + fstore.insert({ k, x }); + return x; + } + + Node* push(T k) + { + return push(std::move(k),NULL); + } + + KeyNodeIter find(const T& k) + { + KeyNodeIter mit = fstore.find(k); + return mit; + } + + int count(const T& k) + { + KeyNodeIter mit = fstore.find(k); + return mit != fstore.end(); + } + + Node* findNode(const T& k) + { + KeyNodeIter mit = find(k); + return mit->second; + } + + void pop() + { + if (Heap::empty()) + return; + Node *x = Heap::extract_min(); + if (!x) + return; // should not happen. + auto range = fstore.equal_range(x->key); + auto mit = std::find_if(range.first, range.second, + [x](const std::pair &ele){ + return ele.second == x; + } + ); + if (mit != range.second) + fstore.erase(mit); + else std::cerr << "[Error]: key " << x->key << " cannot be found in FiboQueue fast store\n"; + delete x; + } + + void clear() { + Heap::clear(); + fstore.clear(); + } + + std::unordered_multimap fstore; +}; + +#endif diff --git a/include/fiboheap/test_fiboheap.cc b/include/fiboheap/test_fiboheap.cc new file mode 100644 index 0000000..2704be6 --- /dev/null +++ b/include/fiboheap/test_fiboheap.cc @@ -0,0 +1,124 @@ +/** + * Copyright (c) 2014, Emmanuel Benazera beniz@droidnik.fr, All rights reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 3.0 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library. + */ + +#include "fiboheap.h" +#include "fiboqueue.h" +#include +#include +#include + +struct lowerI +{ + bool operator()(const int &d1,const int &d2) + { + return d1>d2; + } +}; + +void fill_heaps(FibHeap &fh, + std::priority_queue,lowerI> &pqueue, + const int &n) +{ + for (int i=0;i &fh, + std::priority_queue,lowerI> &pqueue) +{ + while(!pqueue.empty()) + { + int i1 = pqueue.top(); + int i2 = fh.top(); + //std::cerr << "i1: " << i1 << " -- i2: " << i2 << std::endl; + assert(i1 == i2); + pqueue.pop(); + fh.pop(); + } + assert(fh.empty()); +} + +void fill_queues(FibQueue &fh, + std::priority_queue,lowerI> &pqueue, + const int &n) +{ + for (int i=0;i &fh, + std::priority_queue,lowerI> &pqueue) +{ + while(!pqueue.empty()) + { + int i1 = pqueue.top(); + int i2 = fh.top(); + //std::cerr << "i1: " << i1 << " -- i2: " << i2 << std::endl; + assert(i1 == i2); + pqueue.pop(); + fh.pop(); + } + assert(fh.empty()); +} + +int main(int argc, char *argv[]) +{ + FibHeap fh; + int n = 10; + std::priority_queue,lowerI> pqueue; + + //srand(time(0)); + + fill_heaps(fh,pqueue,n); + assert(match_heaps(fh,pqueue)); + + fill_heaps(fh,pqueue,n); + //std::cerr << "top pqueue: " << pqueue.top() << " -- top fh: " << fh.top() << std::endl; + int r = pqueue.top()-1; + //std::cerr << "old val: " << pqueue.top() << " -- new val: " << r << std::endl; + pqueue.pop(); + pqueue.push(r); + std::make_heap(const_cast(&pqueue.top()),const_cast(&pqueue.top())+pqueue.size(),lowerI()); + fh.decrease_key(fh.topNode(),r); + assert(match_heaps(fh,pqueue)); + + FibQueue fq; + fill_queues(fq,pqueue,n); + match_queues(fq,pqueue); + + fill_queues(fq,pqueue,n); + r = rand(); + fq.push(r); + FibHeap::FibNode *x = fq.findNode(r); + assert(x!=NULL); + int nr = r-rand()/2; + fq.decrease_key(x,nr); + pqueue.push(nr); + match_queues(fq,pqueue); +} diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ebdad42..d8b8c2e 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -12,7 +12,7 @@ set(CMAKE_CXX_STANDARD 11) # Make sure the swig package is loaded. find_package(SWIG REQUIRED) find_package(GDAL 2.2 REQUIRED) -find_package(PythonLibs REQUIRED) +find_package(PythonLibs 2.7 REQUIRED) find_package(Boost 1.54.0 REQUIRED serialization) include(${SWIG_USE_FILE}) @@ -24,7 +24,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) # Set the flags. -set(CMAKE_CXX_FLAGS "-O3 -DUSE_BG_GEOMETRY") +set(CMAKE_CXX_FLAGS "-O3") # Set the properties for the interface file. set_source_files_properties(fmm.i PROPERTIES CPLUSPLUS ON) diff --git a/python/fmm.h b/python/fmm.h index 1ce7cdb..cd0af27 100644 --- a/python/fmm.h +++ b/python/fmm.h @@ -15,217 +15,304 @@ #include "../src/transition_graph.hpp" #include "../src/config.hpp" #include "../src/writer.hpp" -#include "../src/python_types.hpp" -namespace MM{ +#include "python_types.hpp" -class MapMatcherConfig{ +namespace MM { + +class MapMatcherConfig { public: - MapMatcherConfig(const std::string &config_file){ - std::cout << "Start reading FMM configuration \n"; - // Create empty property tree object - boost::property_tree::ptree tree; - boost::property_tree::read_xml(config_file, tree); - // Parse the XML into the property tree. - // Without default value, the throwing version of get to find attribute. - // If the path cannot be resolved, an exception is thrown. - // UBODT - ubodt_file = tree.get("fmm_config.input.ubodt.file"); - // multiplier = tree.get("fmm_config.input.ubodt.multiplier", 37); // multiplier=30000 - // nhash = tree.get("fmm_config.input.ubodt.nhash", 127); // 5178049 - // Check if delta is specified or not - if (!tree.get_optional("fmm_config.input.ubodt.delta").is_initialized()){ - delta_defined = false; - delta = 0.0; - } else { - delta = tree.get("fmm_config.input.ubodt.delta",5000.0); // - delta_defined = true; - } - binary_flag = MM::get_file_extension(ubodt_file); - // Network - network_file = tree.get("fmm_config.input.network.file"); - network_id = tree.get("fmm_config.input.network.id", "id"); - network_source = tree.get("fmm_config.input.network.source", "source"); - network_target = tree.get("fmm_config.input.network.target", "target"); - - // Other parameters - k = tree.get("fmm_config.parameters.k", 8); - radius = tree.get("fmm_config.parameters.r", 300.0); - gps_error = tree.get("fmm_config.parameters.gps_error", 50.0); - std::cout << "Finish with reading FMM configuration \n"; - - std::cout << "------------------------------------------" << '\n'; - std::cout << "Configuration parameters for map matching application: " << '\n'; - std::cout << "Network_file: " << network_file << '\n';; - std::cout << "Network id: " << network_id << '\n'; - std::cout << "Network source: " << network_source << '\n'; - std::cout << "Network target: " << network_target << '\n'; - std::cout << "ubodt_file: " << ubodt_file << '\n'; - // std::cout << "multiplier: " << multiplier << '\n'; - // std::cout << "nhash: " << nhash << '\n'; - if (delta_defined) { - std::cout << "delta: " << delta << '\n'; - } else { - std::cout << "delta: " << "undefined, to be inferred from ubodt file\n"; - } - std::cout << "ubodt format(1 binary, 0 csv): " << binary_flag << '\n'; - std::cout << "k: " << k << '\n'; - std::cout << "radius: " << radius << '\n'; - std::cout << "gps_error: " << gps_error << '\n'; - std::cout << "------------------------------------------" << '\n'; + MapMatcherConfig(const std::string &config_file){ + std::cout << "Start reading FMM configuration \n"; + // Create empty property tree object + boost::property_tree::ptree tree; + boost::property_tree::read_xml(config_file, tree); + // Parse the XML into the property tree. + // Without default value, the throwing version of get to find attribute. + // If the path cannot be resolved, an exception is thrown. + // UBODT + ubodt_file = tree.get("fmm_config.input.ubodt.file"); + // nhash = tree.get("fmm_config.input.ubodt.nhash", 127); // 5178049 + // Check if delta is specified or not + if (!tree.get_optional + ("fmm_config.input.ubodt.delta").is_initialized()) { + delta_defined = false; + delta = 0.0; + } else { + delta = tree.get("fmm_config.input.ubodt.delta",5000.0); // + delta_defined = true; + } + binary_flag = MM::UTIL::get_file_extension(ubodt_file); + // Network + network_file = tree.get("fmm_config.input.network.file"); + network_id = tree.get("fmm_config.input.network.id", "id"); + network_source = tree.get("fmm_config.input.network.source", "source"); + network_target = tree.get("fmm_config.input.network.target", "target"); - }; - std::string network_file; - std::string network_id; - std::string network_source; - std::string network_target; - - // UBODT configurations - std::string ubodt_file; - // int multiplier; - // int nhash; - double delta; - bool delta_defined = true; - int binary_flag; - - // Parameters - double gps_error; - // Used by hashtable in UBODT - - // Used by Rtree search - int k; - double radius; + // Other parameters + k = tree.get("fmm_config.parameters.k", 8); + radius = tree.get("fmm_config.parameters.r", 300.0); + gps_error = tree.get("fmm_config.parameters.gps_error", 50.0); + std::cout << "Finish with reading FMM configuration \n"; + + std::cout << "------------------------------------------" << '\n'; + std::cout << "Configuration parameters for map matching application:\n "; + std::cout << "Network_file: " << network_file << '\n';; + std::cout << "Network id: " << network_id << '\n'; + std::cout << "Network source: " << network_source << '\n'; + std::cout << "Network target: " << network_target << '\n'; + std::cout << "ubodt_file: " << ubodt_file << '\n'; + // std::cout << "multiplier: " << multiplier << '\n'; + // std::cout << "nhash: " << nhash << '\n'; + if (delta_defined) { + std::cout << "delta: " << delta << '\n'; + } else { + std::cout << "delta: " << "undefined, to be inferred from ubodt file\n"; + } + std::cout << "ubodt format(1 binary, 0 csv): " << binary_flag << '\n'; + std::cout << "k: " << k << '\n'; + std::cout << "radius: " << radius << '\n'; + std::cout << "gps_error: " << gps_error << '\n'; + std::cout << "------------------------------------------" << '\n'; + + }; + std::string network_file; + std::string network_id; + std::string network_source; + std::string network_target; + + // UBODT configurations + std::string ubodt_file; + // int multiplier; + // int nhash; + double delta; + bool delta_defined = true; + int binary_flag; + + // Parameters + double gps_error; + // Used by hashtable in UBODT + + // Used by Rtree search + int k; + double radius; }; class MapMatcher { public: - MapMatcher(const std::string &config_file):config(MapMatcherConfig(config_file)){ - std::cout << "Loading model from file" << config_file <<'\n'; - network = new MM::Network(config.network_file,config.network_id, - config.network_source,config.network_target); - network->build_rtree_index(); - int multipler = network->get_node_count(); - if (config.binary_flag==1){ - ubodt = MM::read_ubodt_binary(config.ubodt_file,multipler); - } else { - ubodt = MM::read_ubodt_csv(config.ubodt_file,multipler); - } - if (!config.delta_defined){ - config.delta = ubodt->get_delta(); - std::cout<<" Delta inferred from ubodt as "<< config.delta <<'\n'; - } - std::cout << "Loading model finished" << '\n'; - }; - MatchResult match_wkt(const std::string &wkt){ - std::cout << "Perform map matching" << '\n'; - LineString line; - bg::read_wkt(wkt,*(line.get_geometry())); - int points_in_tr = line.getNumPoints(); - // Candidate search - MM::Traj_Candidates traj_candidates = network->search_tr_cs_knn(&line,config.k,config.radius,config.gps_error); - MM::TransitionGraph tg = MM::TransitionGraph(&traj_candidates,&line,ubodt,config.delta); - // Optimal path inference - MM::O_Path *o_path_ptr = tg.viterbi(); - // Complete path construction as an array of indices of edges vector - MM::T_Path *t_path_ptr = ubodt->construct_traversed_path(o_path_ptr); - MM::LineString *m_geom = network->complete_path_to_geometry(o_path_ptr,&(t_path_ptr->cpath)); - MatchResult result = generate_result(network,o_path_ptr,t_path_ptr,m_geom); - delete o_path_ptr; - delete t_path_ptr; - delete m_geom; - std::cout << "Perform map matching success" << '\n'; - return result; - }; - /** - * Search the network for candidates matched to a trajectory - */ - CandidateSet search_candidate(const std::string &wkt){ - LineString line; - bg::read_wkt(wkt,*(line.get_geometry())); - int points_in_tr = line.getNumPoints(); - Traj_Candidates traj_candidates = network->search_tr_cs_knn(&line,config.k,config.radius,config.gps_error); - CandidateSet result; - for (int i = 0;i < traj_candidates.size();++i){ - Point_Candidates & point_candidates = traj_candidates[i]; - for (int j = 0;j < point_candidates.size();++j){ - Candidate c = point_candidates[j]; - result.push_back({i, - std::stoi(c.edge->id_attr.c_str()),c.edge->source,c.edge->target,c.dist, - c.edge->length,c.offset,c.obs_prob}); - }; - }; - return result; - }; - /** - * Build a transition lattice for the trajectory containing - * index,from,to,tp,ep,cp - */ - TransitionLattice build_transition_lattice(const std::string &wkt){ - std::cout << "Perform map matching" << '\n'; - LineString line; - bg::read_wkt(wkt,*(line.get_geometry())); - int points_in_tr = line.getNumPoints(); - // Candidate search - MM::Traj_Candidates traj_candidates = network->search_tr_cs_knn(&line,config.k,config.radius,config.gps_error); - MM::TransitionGraph tg = MM::TransitionGraph(&traj_candidates,&line,ubodt,config.delta); - return tg.generate_transition_lattice(); - }; - // Getter and setter to change the configuration in Python interactively. - void set_gps_error(double error){ - config.gps_error = error; - }; - double get_gps_error(){ - return config.gps_error; - }; - void set_k(int k){ - config.k = k; - }; - int get_k(){ - return config.k; - }; - double get_radius(){ - return config.radius; - }; - void set_radius(double r){ - config.radius = r; - }; - ~MapMatcher(){ - delete network; - delete ubodt; + MapMatcher(const std::string &config_file) : + config(MapMatcherConfig(config_file)){ + std::cout << "Loading model from file" << config_file <<'\n'; + network = new Network(config.network_file,config.network_id, + config.network_source,config.network_target); + network->build_rtree_index(); + int multipler = network->get_node_count(); + if (config.binary_flag==1) { + ubodt = MM::read_ubodt_binary(config.ubodt_file,multipler); + } else { + ubodt = MM::read_ubodt_csv(config.ubodt_file,multipler); + } + if (!config.delta_defined) { + config.delta = ubodt->get_delta(); + std::cout<<" Delta inferred from ubodt as "<< config.delta <<'\n'; + } + std::cout << "Loading model finished" << '\n'; + }; + ~MapMatcher(){ + delete network; + delete ubodt; + }; + MatchResult match_wkt(const std::string &wkt){ + SPDLOG_INFO("Perform map matching"); + LineString line; + bg::read_wkt(wkt,line.get_geometry()); + SPDLOG_INFO("Linestring WKT {}",line.exportToWkt()); + int points_in_tr = line.getNumPoints(); + SPDLOG_INFO("Search candidates"); + // Candidate search + MM::Traj_Candidates traj_candidates = network->search_tr_cs_knn( + line,config.k,config.radius,config.gps_error); + SPDLOG_INFO("Build transition graph"); + MM::TransitionGraph tg = MM::TransitionGraph( + &traj_candidates,line,*ubodt,config.delta); + SPDLOG_INFO("Viterbi Optimal inference"); + // Optimal path inference + MM::O_Path o_path = tg.viterbi(); + SPDLOG_INFO("Construct complete path"); + // Complete path construction as an array of indices of edges vector + MM::T_Path t_path = ubodt->construct_traversed_path(o_path,*network); + MM::LineString m_geom = network->complete_path_to_geometry( + o_path,t_path.cpath); + MatchResult result = generate_result(*network,o_path,t_path,m_geom); + SPDLOG_INFO("Perform map matching success"); + return result; + }; + /** + * Search the network for candidates matched to a trajectory + */ + CandidateSet search_candidate(const std::string &wkt){ + LineString line; + bg::read_wkt(wkt,line.get_geometry()); + int points_in_tr = line.getNumPoints(); + Traj_Candidates traj_candidates = network->search_tr_cs_knn( + line,config.k,config.radius,config.gps_error); + CandidateSet result; + for (int i = 0; i < traj_candidates.size(); ++i) { + Point_Candidates & point_candidates = traj_candidates[i]; + for (int j = 0; j < point_candidates.size(); ++j) { + Candidate c = point_candidates[j]; + result.push_back({i, + c.edge->id, + network->get_node_id(c.edge->source), + network->get_node_id(c.edge->target), + c.dist,c.edge->length,c.offset,c.obs_prob}); + }; }; + return result; + }; + /** + * Build a transition lattice for the trajectory containing + * index,from,to,tp,ep,cp + */ + TransitionLattice build_transition_lattice(const std::string &wkt){ + std::cout << "Building transition lattice" << '\n'; + LineString line; + bg::read_wkt(wkt,line.get_geometry()); + int points_in_tr = line.getNumPoints(); + std::cout << "Number of points " <search_tr_cs_knn( + line,config.k,config.radius,config.gps_error); + MM::TransitionGraph tg = MM::TransitionGraph( + &traj_candidates,line,*ubodt,config.delta); + return generate_transition_lattice(tg); + }; + // Getter and setter to change the configuration in Python interactively. + void set_gps_error(double error){ + config.gps_error = error; + }; + double get_gps_error(){ + return config.gps_error; + }; + void set_k(int k){ + config.k = k; + }; + int get_k(){ + return config.k; + }; + double get_radius(){ + return config.radius; + }; + void set_radius(double r){ + config.radius = r; + }; private: - static MatchResult generate_result(Network *network_ptr,O_Path *o_path_ptr, T_Path *t_path_ptr, LineString *mgeom){ - MatchResult result; - // Opath - if (o_path_ptr != nullptr) { - int N = o_path_ptr->size(); - for (int i = 0; i < N; ++i) - { - result.opath.push_back(std::stoi((*o_path_ptr)[i]->edge->id_attr)); - } - }; - // Cpath - if (t_path_ptr != nullptr) { - C_Path *c_path_ptr = &(t_path_ptr->cpath); - int N = c_path_ptr->size(); - for (int i = 0; i < N; ++i) - { - result.cpath.push_back(std::stoi(network_ptr->get_edge_id_attr((*c_path_ptr)[i]))); - } - }; - if (mgeom!=nullptr){ - std::stringstream buf; - MM::IO::ResultWriter::write_geometry(buf,mgeom); - result.mgeom = buf.str(); + /** + * Generate transition lattice for the transition graph, used in + * Python extension for verification of the result + */ + TransitionLattice generate_transition_lattice(MM::TransitionGraph &tg){ + TransitionLattice tl; + Traj_Candidates *m_traj_candidates = tg.get_traj_candidates(); + if (m_traj_candidates->empty()) return tl; + std::vector &eu_distances = tg.get_eu_distances(); + 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; + } + /* 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[std::distance( + m_traj_candidates->begin(),csa)]; + while (ca != csa->end()) + { + Point_Candidates::iterator cb = csb->begin(); + while (cb != csb->end()) + { + + int step =std::distance(m_traj_candidates->begin(),csa); + // Calculate transition probability + double sp_dist = tg.get_sp_dist_penalized(ca,cb,0); + /* + 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; + } + + 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; + } + tl.push_back( + {step, + ca->edge->id, + cb->edge->id, + sp_dist, + eu_dist, + tran_prob, + cb->obs_prob, + ca->cumu_prob + tran_prob * cb->obs_prob}); + ++cb; } - std::stringstream pgeom_buf; - MM::IO::ResultWriter::write_pgeom(pgeom_buf,o_path_ptr); - result.pgeom = pgeom_buf.str(); - return result; + ++ca; + } + ++csa; + ++csb; + } + return tl; + }; + + static MatchResult generate_result( + const Network &network,const O_Path &o_path, + const T_Path &t_path, const LineString &mgeom){ + MatchResult result; + // Opath + if (!o_path.empty()) { + int N = o_path.size(); + for (int i = 0; i < N; ++i) + { + result.opath.push_back(o_path[i]->edge->id); + } + }; + // Cpath + if (!t_path.cpath.empty()) { + const C_Path &c_path = t_path.cpath; + int N = c_path.size(); + for (int i = 0; i < N; ++i) + { + result.cpath.push_back(c_path[i]); + } }; - MM::UBODT *ubodt; - MM::Network *network; - MapMatcherConfig config; + if (!mgeom.isEmpty()) { + std::stringstream buf; + MM::IO::ResultWriter::write_geometry(buf,mgeom); + result.mgeom = buf.str(); + } + std::stringstream pgeom_buf; + MM::IO::ResultWriter::write_pgeom(pgeom_buf,o_path); + result.pgeom = pgeom_buf.str(); + return result; + }; + MM::UBODT *ubodt; + MM::Network *network; + MapMatcherConfig config; }; } // MM diff --git a/python/fmm.i b/python/fmm.i index 4f04ec0..7675bcb 100644 --- a/python/fmm.i +++ b/python/fmm.i @@ -15,7 +15,7 @@ namespace std { // %template(IntSet) set; } -%include "../src/python_types.hpp" +%include "python_types.hpp" %template (CandidateVector) std::vector; %template (TransitionLattice) std::vector; @@ -38,7 +38,8 @@ public: double get_radius(); void set_radius(double r); private: - static MatchResult generate_result(Network *network_ptr,O_Path *o_path_ptr, T_Path *t_path_ptr, LineString *mgeom); + static MatchResult generate_result(Network *network_ptr,O_Path *o_path_ptr, + T_Path *t_path_ptr, LineString *mgeom); MM::UBODT *ubodt; MM::Network *network; MapMatcherConfig config; diff --git a/python/python_types.hpp b/python/python_types.hpp new file mode 100644 index 0000000..82caca7 --- /dev/null +++ b/python/python_types.hpp @@ -0,0 +1,54 @@ +/** + * Content + * Definition of Data types used in the FMM python extension + * + * @author: Can Yang + * @version: 2019.06.14 + */ + +#ifndef MM_PYTHON_TYPES_HPP +#define MM_PYTHON_TYPES_HPP + +#include +#include +#include "../src/geometry_type.hpp" + +namespace MM { + +struct MatchResult { + std::vector opath; + std::vector cpath; + std::string mgeom; + std::string pgeom; +}; + +/** + * Candidate class, used for verifying the result in Python + */ +struct CandidateElement { + int index; // point index in trajectory + int edge_id; // edge id + int source; + int target; + double distance; // distance to that edge + double length; + double offset; + double ep; // emission probability +}; + +typedef std::vector CandidateSet; + +struct TLElement { + int index; // point index in trajectory + int from_edge; + int to_edge; + double sp_dist; + double eu_dist; + double tp; // transition probability + double e_prob; // emission probability + double cumu_prob; // cumulative emission probability +}; + +typedef std::vector TransitionLattice; +} +#endif /* MM_PYTHON_TYPES_HPP */ diff --git a/src/algorithm.hpp b/src/algorithm.hpp index 3fbef3f..586ba69 100644 --- a/src/algorithm.hpp +++ b/src/algorithm.hpp @@ -4,6 +4,7 @@ * a polyline given an input point. * * @author: Can Yang + * @version: 2020.01.23 - LineString pointer removed. * @version: 2017.11.11 */ @@ -11,7 +12,7 @@ #define MM_ALGORITHM_HPP #include #include "types.hpp" -#include "multilevel_debug.h" +#include "debug.h" #include "util.hpp" namespace MM { @@ -21,30 +22,27 @@ namespace ALGORITHM { * Compute the boundary of an LineString and returns the result in * the passed x1,y1,x2,y2 variables. * - * @param linestring: input, which is a pointer to a - * linestring object + * @param linestring: line geometry * @param x1,y1,x2,y2: the coordinates of the boundary */ -void boundingbox_geometry(LineString *linestring,double *x1,double *y1,double *x2,double *y2) +void boundingbox_geometry(const LineString &linestring, + double *x1,double *y1,double *x2,double *y2) { - CS_DEBUG(3) std::cout<<"EXECUTE "<<__LINE__<<'\n'; - CS_DEBUG(3) MM::UTIL::print_geometry(linestring); - int Npoints = linestring->getNumPoints(); - CS_DEBUG(3) std::cout<<"Number points "<getX(i); - y = linestring->getY(i); - if (x<*x1) *x1 = x; - if (y<*y1) *y1 = y; - if (x>*x2) *x2 = x; - if (y>*y2) *y2 = y; - }; + int Npoints = linestring.getNumPoints(); + *x1 = DBL_MAX; + *y1 = DBL_MAX; + *x2 = DBL_MIN; + *y2 = DBL_MIN; + double x,y; + for(int i=0; i*x2) *x2 = x; + if (y>*y2) *y2 = y; + }; }; // boundingbox_geometry /** @@ -56,30 +54,57 @@ void boundingbox_geometry(LineString *linestring,double *x1,double *y1,double *x * @param offset the distance from the start of the segement (x1,y1) * to p' */ -void closest_point_on_segment(double x,double y,double x1,double y1,double x2,double y2,double *dist,double *offset) +void closest_point_on_segment(double x,double y,double x1,double y1, + double x2,double y2,double *dist,double *offset) { - double L2 = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1); - if (L2 == 0.0) - { - *dist=std::sqrt((x-x1)*(x-x1)+(y-y1)*(y-y1)); - *offset=0.0; - return; - } - double x1_x = x-x1; - double y1_y = y-y1; - double x1_x2 = x2-x1; - double y1_y2 = y2-y1; - double ratio = (x1_x*x1_x2+y1_y*y1_y2)/L2; - ratio=(ratio>1)?1:ratio; - ratio=(ratio<0)?0:ratio; - double prj_x = x1+ ratio*(x1_x2); - double prj_y = y1+ ratio*(y1_y2); - *offset = std::sqrt((prj_x-x1)*(prj_x-x1)+(prj_y-y1)*(prj_y-y1)); - *dist = std::sqrt((prj_x-x)*(prj_x-x)+(prj_y-y)*(prj_y-y)); - // std::cout<<"Ratio is "<1) ? 1 : ratio; + ratio=(ratio<0) ? 0 : ratio; + double prj_x = x1+ ratio*(x1_x2); + double prj_y = y1+ ratio*(y1_y2); + *offset = std::sqrt((prj_x-x1)*(prj_x-x1)+(prj_y-y1)*(prj_y-y1)); + *dist = std::sqrt((prj_x-x)*(prj_x-x)+(prj_y-y)*(prj_y-y)); }; // closest_point_on_segment +void closest_point_on_segment(double x, double y, double x1, double y1, + double x2, double y2, double *dist, + double *offset, double *closest_x, + double *closest_y) +{ + double L2 = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1); + if (L2 == 0.0) + { + *dist=std::sqrt((x-x1)*(x-x1)+(y-y1)*(y-y1)); + *offset=0.0; + *closest_x=x1; + *closest_y=y1; + return; + } + double x1_x = x-x1; + double y1_y = y-y1; + double x1_x2 = x2-x1; + double y1_y2 = y2-y1; + double ratio = (x1_x*x1_x2+y1_y*y1_y2)/L2; + ratio=(ratio>1) ? 1 : ratio; + ratio=(ratio<0) ? 0 : ratio; + double prj_x = x1+ ratio*(x1_x2); + double prj_y = y1+ ratio*(y1_y2); + *offset = std::sqrt((prj_x-x1)*(prj_x-x1)+(prj_y-y1)*(prj_y-y1)); + *dist = std::sqrt((prj_x-x)*(prj_x-x)+(prj_y-y)*(prj_y-y)); + *closest_x = prj_x; + *closest_y = prj_y; +}; // closest_point_on_segment /** * A linear referencing function @@ -92,37 +117,78 @@ void closest_point_on_segment(double x,double y,double x1,double y1,double x2,do * @param result_offset output offset distance from the start of the * polyline */ -void linear_referencing(double px, double py, LineString *linestring,double *result_dist,float *result_offset) +void linear_referencing(double px, double py, + const LineString &linestring, double *result_dist, + double *result_offset) { - int Npoints = linestring->getNumPoints(); - double min_dist=DBL_MAX; - double final_offset=DBL_MAX; - double length_parsed=0; - int i=0; - // Iterating to check p(i) == p(i+2) - // int seg_idx=0; - while(igetX(i); - double y1 = linestring->getY(i); - double x2 = linestring->getX(i+1); - double y2 = linestring->getY(i+1); - double temp_min_dist; - double temp_min_offset; - CS_DEBUG(3) std::cout<<"\nProcess segment "<getNumPoints(); - // Geometry construction debug - GC_DEBUG(2) std::cout<< "offset_1: "<getX(0); - double y1 = linestring->getY(0); - double x2 = linestring->getX(1); - double y2 = linestring->getY(1); - double L = std::sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)); - double ratio1 = offset1/L; + double x1 = linestring.getX(i); + double y1 = linestring.getY(i); + double x2 = linestring.getX(i+1); + double y2 = linestring.getY(i+1); + double deltaL = std::sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)); + // If L <= offset1 <= L + deltaL + if (L< offset1 && offset1addPoint(new_x1, new_y1); - cutoffline->addPoint(new_x2, new_y2); - } - else // Multiple segments - { - double L = 0; // current length parsed - int i = 0; - while(igetX(i); - double y1 = linestring->getY(i); - double x2 = linestring->getX(i+1); - double y2 = linestring->getY(i+1); - double deltaL = std::sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)); // current segment length - // If L <= offset1 <= L + deltaL - if (L< offset1 && offset1addPoint(new_x1, new_y1); - } - // If offset1 < L < offset2 - if (offset1addPoint(x1,y1); - } - // If L <= offset2 <= L + deltaL - if (L< offset2 && offset2addPoint(new_x2, new_y2); - } - L = L + deltaL; - ++i; - }; - } - return cutoffline; -};//cutoffseg_twoparameters + cutoffline.addPoint(new_x2, new_y2); + } + L = L + deltaL; + ++i; + }; + } + return cutoffline; +}; //cutoffseg_twoparameters /** * Locate the point on a linestring according to the input of offset * The two pointer's target value will be updated. */ -void locate_point_by_offset(LineString * linestring, double offset, double *x, double *y){ - int Npoints = linestring->getNumPoints(); - if (offset<=0.0) { - *x = linestring->getX(0); - *y = linestring->getY(0); - return; - } - double L_processed=0; // length parsed - int i = 0; - double px=0; - double py=0; - bool found = false; - // Find the idx of the point to be exported close to p - while(i=L_processed && offset<=L_processed+deltaL) { - double x1 = linestring->getX(i); - double y1 = linestring->getY(i); - double x2 = linestring->getX(i+1); - double y2 = linestring->getY(i+1); - double deltaL = std::sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)); // length of current segment - double ratio = (offset-L_processed)/deltaL; - if(offset>=L_processed && offset<=L_processed+deltaL) - { - // double ratio = (offset-L)/deltaL; - // Think - px = x1+ratio*(x2-x1); - py = y1+ratio*(y2-y1); - // cutoffline->addPoint(new_x, new_y); - found = true; - break; - } - ++i; - L_processed += deltaL; - }; - if (found) { - *x = px; - *y = py; - } else { - // Assign the last value, offset is slightly bigger than length - *x = linestring->getX(Npoints-1); - *y = linestring->getY(Npoints-1); + px = x1+ratio*(x2-x1); + py = y1+ratio*(y2-y1); + found = true; + break; } -}; // calculate_offset_point + ++i; + L_processed += deltaL; + }; + if (found) { + *x = px; + *y = py; + } else { + *x = linestring.getX(Npoints-1); + *y = linestring.getY(Npoints-1); + } +}; // locate_point_by_offset /** * added by Diao 18.01.17 @@ -248,95 +307,86 @@ void locate_point_by_offset(LineString * linestring, double offset, double *x, d * * @param offset input offset(from start node) * @param linestring input linestring - * @param mode input mode, 0 represent cutoff from start node, namely export the part p->end , 1 from endnode + * @param mode input mode, 0 represent cutoff from start node, + * namely export the part p->end , 1 from endnode * export the part of start-> p - * @return cutoffline output cutoff linstring, the caller should take care of freeing the memory + * @return cutoffline output cutoff linstring, the caller + * should take care of freeing the memory */ -LineString * cutoffseg(double offset, LineString * linestring, int mode) +LineString cutoffseg(double offset, const LineString &linestring, int mode) { - LineString* cutoffline = new LineString(); - int Npoints = linestring->getNumPoints(); - GC_DEBUG(3) std::cout<< " Npoints " << Npoints <<'\n'; - if (Npoints==2) - { - double x1 = linestring->getX(0); - double y1 = linestring->getY(0); - double x2 = linestring->getX(1); - double y2 = linestring->getY(1); - double deltaL = std::sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)); - double ratio = offset/deltaL; - double new_x = x1+ratio*(x2-x1); - double new_y = y1+ratio*(y2-y1); - if (mode==0){ - // export p -> end - if (1-ratio>0.0001) cutoffline->addPoint(new_x, new_y); - cutoffline->addPoint(x2, y2); - } else { - // export start -> p - cutoffline->addPoint(x1, y1); - if (1-ratio>0.0001) cutoffline->addPoint(new_x, new_y); - } + LineString cutoffline; + int Npoints = linestring.getNumPoints(); + if (Npoints==2) + { + double x1 = linestring.getX(0); + double y1 = linestring.getY(0); + double x2 = linestring.getX(1); + double y2 = linestring.getY(1); + double deltaL = std::sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)); + double ratio = offset/deltaL; + double new_x = x1+ratio*(x2-x1); + double new_y = y1+ratio*(y2-y1); + if (mode==0) { + // export p -> end + //if (1-ratio>0.0001) cutoffline.addPoint(new_x, new_y); + if (1-ratio>0) cutoffline.addPoint(new_x, new_y); + cutoffline.addPoint(x2, y2); } else { - double L_processed=0; // length parsed - int i = 0; - int p_idx = 0; - double px=0; - double py=0; - // Find the idx of the point to be exported close to p - while(igetX(i); - double y1 = linestring->getY(i); - double x2 = linestring->getX(i+1); - double y2 = linestring->getY(i+1); - double deltaL = std::sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)); // length of current segment - double ratio = (offset-L_processed)/deltaL; - GC_DEBUG(3) std::cout<< " deltaL " << deltaL <<'\n'; - GC_DEBUG(3) std::cout<< " Offset " << offset <<'\n'; - GC_DEBUG(3) std::cout<< " L_processed " << L_processed <<'\n'; - GC_DEBUG(3) std::cout<< " Ratio " << ratio <<'\n'; - //if - if(offset>=L_processed && offset<=L_processed+deltaL) - { - // double ratio = (offset-L)/deltaL; - // Think - px= x1+ratio*(x2-x1); - py = y1+ratio*(y2-y1); - // cutoffline->addPoint(new_x, new_y); - break; - } - ++i; - L_processed += deltaL; - }; - if (offset>L_processed) { - // The offset value is slightly bigger than the length because - // of precision - // implies that px and py are still 0 - px = linestring->getX(i); - py = linestring->getY(i); - } - GC_DEBUG(3) std::cout<< " px " << px <<'\n'; - GC_DEBUG(3) std::cout<< " py " << py <<'\n'; - GC_DEBUG(3) std::cout<< " i " << py <<'\n'; - p_idx = i; - if (mode==0){ // export p -> end - cutoffline->addPoint(px,py); - for(int j=p_idx+1; jaddPoint(linestring->getX(j), linestring->getY(j)); - } - } else { // export start -> p - for(int j=0; jaddPoint(linestring->getX(j), linestring->getY(j)); - } - cutoffline->addPoint(px,py); - } + // export start -> p + cutoffline.addPoint(x1, y1); + //if (1-ratio>0.0001) cutoffline.addPoint(new_x, new_y); + if (1-ratio>=0) cutoffline.addPoint(new_x, new_y); } - return cutoffline; -};//cutoffseg - - + } else { + double L_processed=0; // length parsed + int i = 0; + int p_idx = 0; + double px=0; + double py=0; + // Find the idx of the point to be exported close to p + while(i=L_processed && offset<=L_processed+deltaL) + { + px= x1+ratio*(x2-x1); + py = y1+ratio*(y2-y1); + break; + } + ++i; + L_processed += deltaL; + }; + if (offset>L_processed) { + // The offset value is slightly bigger than the length because + // of precision + // implies that px and py are still 0 + px = linestring.getX(i); + py = linestring.getY(i); + } + p_idx = i; + if (mode==0) { // export p -> end + cutoffline.addPoint(px,py); + for(int j=p_idx+1; j p + for(int j=0; j -#include -#include -#include -#include -#include -#include - -namespace boost { -/* -namespace detail{ - //template - //struct my_vertex_property_map_generator_helper {}; - template - struct my_vertex_property_map_generator_helper{ - typedef boost::iterator_property_map type; - static type build(const Graph& g, const IndexMap& index, boost::array& array_holder) { - // array_holder.reset(new Value[num_vertices(g)]); - std::fill(array_holder.get(), array_holder.get() + num_vertices(g), Value()); - return make_iterator_property_map(array_holder.get(), index); - } - }; -}*/ - -// template -// static boost::iterator_property_map build(const Graph& g, const IndexMap& index, boost::array& array_holder){ -// array_holder.reset(new Value[num_vertices(g)]); -// std::fill(array_holder.get(), array_holder.get() + num_vertices(g), Value()); -// // Vertex index to fetch the pointer address at array holder. -// // It maps the indexmap (or index vector) to the values stored in the iteratior, which is the array holder. -// return make_iterator_property_map(array_holder.get(), index); -// }; - - -/** - * This is an optimized driving distance function based on dijkstra_shortest_paths_no_color_map_no_init - * in BGL. - * - * Input: - * graph,...,distance_zero: graph definitions - * delta: upper bound distance for early stopping - * Note: the predecessor map must be initialized with the vertex at each position - * and the distance map initialized with infinity - * - * Returns: - * predecessor_map: similar to Dijkstra - * distance_map: similar to Dijkstra - * examined_vertex_map: examined vertexes, that will be used - * to clean the predecessor_map and distance_map in repeated queries. - * - */ -template -void dijkstra_shortest_paths_upperbound -(const Graph& graph, - typename graph_traits::vertex_descriptor start_vertex, - PredecessorMap predecessor_map, - DistanceMap distance_map, - WeightMap weight_map, - VertexIndexMap index_map, - DistanceCompare distance_compare, - DistanceWeightCombine distance_weight_combine, - DistanceInfinity distance_infinity, - DistanceZero distance_zero, double distance_goal, - std::vector &nodes_within_goal, - std::vector &examined_vertices) -{ - typedef typename graph_traits::vertex_descriptor Vertex; - typedef typename property_traits::value_type Distance; - - typedef indirect_cmp DistanceIndirectCompare; - DistanceIndirectCompare - distance_indirect_compare(distance_map, distance_compare); - - // Default - use d-ary heap (d = 4) - // typedef - // detail::vertex_property_map_generator - // IndexInHeapMapHelper; - // typedef typename IndexInHeapMapHelper::type IndexInHeapMap; - typedef boost::iterator_property_map IndexInHeapMap; - typedef - d_ary_heap_indirect - VertexQueue; - // 80% - // boost::scoped_array index_in_heap_map_holder; - // // IndexInHeapMap is actually a boost::iterator_property_map - // IndexInHeapMap index_in_heap = - // IndexInHeapMapHelper::build(graph, index_map, - // index_in_heap_map_holder); - // typedef boost::array array; - // *dheap_on_stack = new boost::array(std::size_t,num_vertices(g)); - std::size_t * dheap_on_stack = new std::size_t[num_vertices(graph)]; - boost::iterator_property_map index_in_heap_map = - make_iterator_property_map(dheap_on_stack, index_map); - - // The first two arguments in the template of VertexQueue are already defined - VertexQueue vertex_queue(distance_map, index_in_heap_map, distance_compare); - - double m_distance_goal; //Delta - - // Add vertex to the queue - vertex_queue.push(start_vertex); - - // Starting vertex will always be the first discovered vertex - // visitor.discover_vertex(start_vertex, graph); - - while (!vertex_queue.empty()) { - Vertex min_vertex = vertex_queue.top(); - vertex_queue.pop(); - // visitor.examine_vertex(min_vertex, graph); - nodes_within_goal.push_back(min_vertex); - if (distance_map[min_vertex] > distance_goal) { - nodes_within_goal.pop_back(); - delete[] dheap_on_stack; - return; - } - - // Check if any other vertices can be reached - Distance min_vertex_distance = get(distance_map, min_vertex); - - if (!distance_compare(min_vertex_distance, distance_infinity)) { - // This is the minimum vertex, so all other vertices are unreachable - delete[] dheap_on_stack; - return; - } - - // Examine neighbors of min_vertex - BGL_FORALL_OUTEDGES_T(min_vertex, current_edge, graph, Graph) { - // visitor.examine_edge(current_edge, graph); - - // Check if the edge has a negative weight - if (distance_compare(get(weight_map, current_edge), distance_zero)) { - boost::throw_exception(negative_edge()); - } - - // Extract the neighboring vertex and get its distance - Vertex neighbor_vertex = target(current_edge, graph); - Distance neighbor_vertex_distance = get(distance_map, neighbor_vertex); - bool is_neighbor_undiscovered = - !distance_compare(neighbor_vertex_distance, distance_infinity); - - // Attempt to relax the edge - bool was_edge_relaxed = relax(current_edge, graph, weight_map, - predecessor_map, distance_map, - distance_weight_combine, distance_compare); - - if (was_edge_relaxed) { - // visitor.edge_relaxed(current_edge, graph); - examined_vertices.push_back(neighbor_vertex); - if (is_neighbor_undiscovered) { - // visitor.discover_vertex(neighbor_vertex, graph); - vertex_queue.push(neighbor_vertex); - } else { - vertex_queue.update(neighbor_vertex); - } - } else { - // visitor.edge_not_relaxed(current_edge, graph); - } - - } // end out edge iteration - - // visitor.finish_vertex(min_vertex, graph); - } // end while queue not empty - delete[] dheap_on_stack; -} // dijkstra_shortest_paths_upperbound - -} // namespace boost - -#endif // BOOST_GRAPH_DIJKSTRA_NO_COLOR_MAP_HPP diff --git a/src/config.hpp b/src/config.hpp index 1ff22d6..73d72a3 100644 --- a/src/config.hpp +++ b/src/config.hpp @@ -3,7 +3,7 @@ * Configuration Class defined for the two application * * @author: Can Yang - * @version: 2019.03.27 + * @version: 2020.01.31 */ #ifndef MM_CONFIG_HPP #define MM_CONFIG_HPP @@ -18,278 +18,294 @@ #include #include +#include "debug.h" +#include "util.hpp" + namespace MM { -bool fileExists(std::string &filename) -{ - struct stat buf; - if (stat(filename.c_str(), &buf) != -1) - { - return true; - } - return false; -}; -// Check extension of the file, 0 for CSV and 1 for Binary -int get_file_extension(std::string &fn) { - std::string fn_extension = fn.substr(fn.find_last_of(".") + 1); - if (fn_extension == "csv" || fn_extension == "txt") { - return 0; - } else if (fn_extension == "bin" || fn_extension == "binary") { - return 1; - } - return 2; -}; - - /** * The configuration defined for output of the program */ struct ResultConfig { - bool write_ogeom = false; // Optimal path, the edge matched to each point - bool write_opath = false; // Optimal path, the edge matched to each point - bool write_offset = false; // The distance from the source node of an edge - bool write_error = false; // The distance from a raw GPS point to a matched GPS point - bool write_cpath = true; // Complete path, a path traversed by the trajectory - bool write_tpath = false; // Complete path, a path traversed by the trajectory - bool write_mgeom = true; // The geometry of the complete path - bool write_wkt = true; // mgeom in WKT or WKB format - bool write_spdist = false; // The distance travelled between two GPS observations - bool write_pgeom = false; // A linestring connecting the point matched for each edge. - bool write_ep = false; // The emission probability of each matched point. - bool write_tp = false; // The transition probability of each matched point. + // Original geometry + bool write_ogeom = false; + // Optimal path, the edge matched to each point + bool write_opath = false; + // The distance from the source node of an edge to the matched point + bool write_offset = false; + // The distance from a raw GPS point to a matched GPS point + bool write_error = false; + // Complete path, a path traversed by the trajectory + bool write_cpath = true; + // Traversed path, the path traversed between + // each two consecutive observations + bool write_tpath = false; + // The geometry of the complete path + bool write_mgeom = true; + // The distance travelled between two GPS observations + bool write_spdist = false; + // A linestring connecting the point matched for each edge. + bool write_pgeom = false; + // The emission probability of each matched point. + bool write_ep = false; + // The transition probability of each matched point. + bool write_tp = false; }; +static const std::vector +LOG_LEVESLS {"0-trace","1-debug","2-info", + "3-warn","4-err","5-critical","6-off"}; + /** * Configuration class for map matching */ class FMM_Config { public: - /** - * FILETYPE 0 for ini and 1 for XML - */ - FMM_Config(const std::string &file) - { - std::cout << "Start reading FMM configuration \n"; - // Create empty property tree object - boost::property_tree::ptree tree; - boost::property_tree::read_xml(file, tree); - // Parse the XML into the property tree. - // Without default value, the throwing version of get to find attribute. - // If the path cannot be resolved, an exception is thrown. + /** + * FILETYPE 0 for ini and 1 for XML + */ + FMM_Config(const std::string &file) + { + SPDLOG_INFO("Start reading FMM configuration"); + // Create empty property tree object + boost::property_tree::ptree tree; + boost::property_tree::read_xml(file, tree); + // Parse the XML into the property tree. + // Without default value, the throwing version of get to find attribute. + // If the path cannot be resolved, an exception is thrown. - // UBODT - ubodt_file = tree.get("fmm_config.input.ubodt.file"); - // Check if delta is specified or not - if (!tree.get_optional("fmm_config.input.ubodt.delta").is_initialized()){ - delta_defined = false; - delta = 0.0; - } else { - delta = tree.get("fmm_config.input.ubodt.delta",5000.0); // - delta_defined = true; - } - binary_flag = get_file_extension(ubodt_file); + // UBODT + ubodt_file = tree.get("fmm_config.input.ubodt.file"); + // Check if delta is specified or not + if (!tree.get_optional("fmm_config.input.ubodt.delta") + .is_initialized()) { + delta_defined = false; + delta = 0.0; + } else { + delta = tree.get("fmm_config.input.ubodt.delta",5000.0); // + delta_defined = true; + } + binary_flag = UTIL::get_file_extension(ubodt_file); - // Network - network_file = tree.get("fmm_config.input.network.file"); - network_id = tree.get("fmm_config.input.network.id", "id"); - network_source = tree.get("fmm_config.input.network.source", "source"); - network_target = tree.get("fmm_config.input.network.target", "target"); + // Network + network_file = tree.get("fmm_config.input.network.file"); + network_id = tree.get("fmm_config.input.network.id", "id"); + network_source = tree.get("fmm_config.input.network.source", "source"); + network_target = tree.get("fmm_config.input.network.target", "target"); - // GPS - gps_file = tree.get("fmm_config.input.gps.file"); - gps_id = tree.get("fmm_config.input.gps.id", "id"); + // GPS + gps_file = tree.get("fmm_config.input.gps.file"); + gps_id = tree.get("fmm_config.input.gps.id", "id"); - // Other parameters - k = tree.get("fmm_config.parameters.k", 8); - radius = tree.get("fmm_config.parameters.r", 300.0); + // Other parameters + k = tree.get("fmm_config.parameters.k", 8); + radius = tree.get("fmm_config.parameters.r", 300.0); - // HMM - gps_error = tree.get("fmm_config.parameters.gps_error", 50.0); - penalty_factor = tree.get("fmm_config.parameters.pf", 0.0); + // HMM + gps_error = tree.get("fmm_config.parameters.gps_error", 50.0); + penalty_factor = tree.get("fmm_config.parameters.pf", 0.0); - // Output - result_file = tree.get("fmm_config.output.file"); - mode = tree.get("fmm_config.output.mode", 0); + // Output + result_file = tree.get("fmm_config.output.file"); + log_level = tree.get("fmm_config.other.log_level",2); - if (tree.get_child_optional("fmm_config.output.fields")){ - // Fields specified - // close the default output fields (cpath,mgeom are true by default) - result_config.write_cpath = false; - result_config.write_mgeom = false; - if (tree.get_child_optional("fmm_config.output.fields.ogeom")){ - result_config.write_ogeom = true; - } - if (tree.get_child_optional("fmm_config.output.fields.opath")){ - result_config.write_opath = true; - } - if (tree.get_child_optional("fmm_config.output.fields.cpath")){ - result_config.write_cpath = true; - } - if (tree.get_child_optional("fmm_config.output.fields.tpath")){ - result_config.write_tpath = true; - } - if (tree.get_child_optional("fmm_config.output.fields.mgeom")){ - result_config.write_mgeom = true; - } - if (tree.get_child_optional("fmm_config.output.fields.pgeom")){ - result_config.write_pgeom = true; - } - if (tree.get_child_optional("fmm_config.output.fields.offset")){ - result_config.write_offset = true; - } - if (tree.get_child_optional("fmm_config.output.fields.error")){ - result_config.write_error = true; - } - if (tree.get_child_optional("fmm_config.output.fields.spdist")){ - result_config.write_spdist = true; - } - if (tree.get_child_optional("fmm_config.output.fields.ep")){ - result_config.write_ep = true; - } - if (tree.get_child_optional("fmm_config.output.fields.tp")){ - result_config.write_tp = true; - } - if (tree.get_child_optional("fmm_config.output.fields.all")){ - result_config.write_ogeom= true; - result_config.write_opath = true; - result_config.write_pgeom = true; - result_config.write_offset = true; - result_config.write_error = true; - result_config.write_spdist = true; - result_config.write_cpath = true; - result_config.write_mgeom = true; - result_config.write_tpath = true; - result_config.write_ep = true; - result_config.write_tp = true; - } - } else { - std::cout << " Default output fields used.\n"; - } - std::cout << "Finish with reading FMM configuration \n"; - }; + if (tree.get_child_optional("fmm_config.output.fields")) { + // Fields specified + // close the default output fields (cpath,mgeom are true by default) + result_config.write_cpath = false; + result_config.write_mgeom = false; + if (tree.get_child_optional("fmm_config.output.fields.ogeom")) { + result_config.write_ogeom = true; + } + if (tree.get_child_optional("fmm_config.output.fields.opath")) { + result_config.write_opath = true; + } + if (tree.get_child_optional("fmm_config.output.fields.cpath")) { + result_config.write_cpath = true; + } + if (tree.get_child_optional("fmm_config.output.fields.tpath")) { + result_config.write_tpath = true; + } + if (tree.get_child_optional("fmm_config.output.fields.mgeom")) { + result_config.write_mgeom = true; + } + if (tree.get_child_optional("fmm_config.output.fields.pgeom")) { + result_config.write_pgeom = true; + } + if (tree.get_child_optional("fmm_config.output.fields.offset")) { + result_config.write_offset = true; + } + if (tree.get_child_optional("fmm_config.output.fields.error")) { + result_config.write_error = true; + } + if (tree.get_child_optional("fmm_config.output.fields.spdist")) { + result_config.write_spdist = true; + } + if (tree.get_child_optional("fmm_config.output.fields.ep")) { + result_config.write_ep = true; + } + if (tree.get_child_optional("fmm_config.output.fields.tp")) { + result_config.write_tp = true; + } + if (tree.get_child_optional("fmm_config.output.fields.all")) { + result_config.write_ogeom= true; + result_config.write_opath = true; + result_config.write_pgeom = true; + result_config.write_offset = true; + result_config.write_error = true; + result_config.write_spdist = true; + result_config.write_cpath = true; + result_config.write_mgeom = true; + result_config.write_tpath = true; + result_config.write_ep = true; + result_config.write_tp = true; + } + } else { + SPDLOG_INFO("Default output fields used."); + } + SPDLOG_INFO("Finish with reading FMM configuration"); + }; - ResultConfig get_result_config(){ - return result_config; - }; - void print() - { - std::cout << "------------------------------------------" << '\n'; - std::cout << "Configuration parameters for map matching application: " << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "Network_file: " << network_file << '\n';; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "Network id: " << network_id << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "Network source: " << network_source << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "Network target: " << network_target << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "ubodt_file: " << ubodt_file << '\n'; - if (delta_defined) { - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "delta: " << delta << '\n'; - } else { - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "delta: " << "undefined, to be inferred from ubodt file\n"; - } - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "ubodt format(1 binary, 0 csv): " << binary_flag << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "gps_file: " << gps_file << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "gps_id: " << gps_id << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "k: " << k << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "radius: " << radius << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "gps_error: " << gps_error << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "penalty_factor: " << penalty_factor << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "result_file:" << result_file << '\n'; - // std::cout << std::left << std::setw(4) << "" << std::setw(20) << "geometry mode: " << mode << "(0 no geometry, 1 wkb, 2 wkt)" << '\n'; + ResultConfig get_result_config(){ + return result_config; + }; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "Output fields:"<<'\n'; - if (result_config.write_ogeom) std::cout << std::left << std::setw(8) << "" << "ogeom"<<'\n'; - if (result_config.write_opath) std::cout << std::left << std::setw(8) << "" << "opath"<<'\n'; - if (result_config.write_pgeom) std::cout << std::left << std::setw(8) << "" << "pgeom"<<'\n'; - if (result_config.write_offset) std::cout << std::left << std::setw(8) << "" << "offset"<<'\n'; - if (result_config.write_error) std::cout << std::left << std::setw(8) << "" << "error"<<'\n'; - if (result_config.write_spdist) std::cout << std::left << std::setw(8) << "" << "spdist"<<'\n'; - if (result_config.write_cpath) std::cout << std::left << std::setw(8) << "" << "cpath"<<'\n'; - if (result_config.write_tpath) std::cout << std::left << std::setw(8) << "" << "tpath"<<'\n'; - if (result_config.write_mgeom) std::cout << std::left << std::setw(8) << "" << "mgeom"<<'\n'; + void print() + { + std::cout << "------------------------------------------\n"; + std::cout << "Configuration parameters for map matching application: \n"; + std::cout << "Network_file: " << network_file << '\n';; + std::cout << "Network id: " << network_id << '\n'; + std::cout << "Network source: " << network_source << '\n'; + std::cout << "Network target: " << network_target << '\n'; + std::cout << "ubodt_file: " << ubodt_file << '\n'; + if (delta_defined) { + std::cout << "delta: " << delta << '\n'; + } else { + std::cout << "delta: " << "undefined, to be inferred from ubodt file\n"; + } + std::cout << "ubodt format(1 binary, 0 csv): " << binary_flag << '\n'; + std::cout << "gps_file: " << gps_file << '\n'; + std::cout << "gps_id: " << gps_id << '\n'; + std::cout << "k: " << k << '\n'; + std::cout << "radius: " << radius << '\n'; + std::cout << "gps_error: " << gps_error << '\n'; + std::cout << "penalty_factor: " << penalty_factor << '\n'; + std::cout << "log_level:" << LOG_LEVESLS[log_level] << '\n'; + std::cout << "result_file:" << result_file << '\n'; + std::cout << "Output fields:"<<'\n'; + if (result_config.write_ogeom) + std::cout << std::left << std::setw(8) << "" << "ogeom"<<'\n'; + if (result_config.write_opath) + std::cout << std::left << std::setw(8) << "" << "opath"<<'\n'; + if (result_config.write_pgeom) + std::cout << std::left << std::setw(8) << "" << "pgeom"<<'\n'; + if (result_config.write_offset) + std::cout << std::left << std::setw(8) << "" << "offset"<<'\n'; + if (result_config.write_error) + std::cout << std::left << std::setw(8) << "" << "error"<<'\n'; + if (result_config.write_spdist) + std::cout << std::left << std::setw(8) << "" << "spdist"<<'\n'; + if (result_config.write_cpath) + std::cout << std::left << std::setw(8) << "" << "cpath"<<'\n'; + if (result_config.write_tpath) + std::cout << std::left << std::setw(8) << "" << "tpath"<<'\n'; + if (result_config.write_mgeom) + std::cout << std::left << std::setw(8) << "" << "mgeom"<<'\n'; + if (result_config.write_ep) + std::cout << std::left << std::setw(8) << "" << "ep"<<'\n'; + if (result_config.write_tp) + std::cout << std::left << std::setw(8) << "" << "tp"<<'\n'; - std::cout << "------------------------------------------" << '\n'; + std::cout << "------------------------------------------\n"; + }; + bool validate_mm() + { + std::cout << "Validating configuration for map match application:\n"; + if (!UTIL::fileExists(gps_file)) + { + SPDLOG_CRITICAL("GPS file {} not found",gps_file); + return false; }; - bool validate_mm() + if (!UTIL::fileExists(network_file)) { - std::cout << "Validating configuration for map match application:" << '\n'; - if (!fileExists(gps_file)) - { - std::cout << std::setw(12) << "" << "Error, GPS_file not found. Program stop." << '\n'; - return false; - }; - if (!fileExists(network_file)) - { - std::cout << std::setw(12) << "" << "Error, Network file not found. Program stop." << '\n'; - return false; - }; - if (!fileExists(ubodt_file)) - { - std::cout << std::setw(12) << "" << "Error, UBODT file not found. Program stop." << '\n'; - return false; - }; - if (binary_flag==2){ - std::cout << std::setw(12) << "" << "Error, UBODT file extension not recognized, which should be csv or binary. Program stop." << '\n'; - return false; - } - if (fileExists(result_file)) - { - std::cout << std::setw(4) << "" << "Warning, overwrite existing result file." << result_file << '\n'; - }; - if (gps_error <= 0 || radius <= 0 || k <= 0) - { - std::cout << std::setw(12) << "" << "Error, Algorithm parameters invalid." << '\n'; - return false; - } - // Check the definition of parameters search radius and gps error - if (radius / gps_error > 10) { - std::cout << std::setw(12) << "" << "Error, the gps error " << gps_error - << " is too small compared with search radius " << radius << '\n'; - std::cout << std::setw(12) << "It may cause underflow, try to increase gps error or descrease search radius" << '\n'; - return false; - } - std::cout << "Validating success." << '\n'; - return true; + SPDLOG_CRITICAL("Network file {} not found",network_file); + return false; }; + if (!UTIL::fileExists(ubodt_file)) + { + SPDLOG_CRITICAL("UBODT file {} not found",ubodt_file); + return false; + }; + if (binary_flag==2) { + SPDLOG_CRITICAL("UBODT file extension not recognized"); + return false; + } + if (log_level<0 || log_level>LOG_LEVESLS.size()) { + SPDLOG_CRITICAL("Invalid log_level {}, which should be 0 - 6",log_level); + SPDLOG_INFO("0-trace,1-debug,2-info,3-warn,4-err,5-critical,6-off"); + return false; + } + if (UTIL::fileExists(result_file)) + { + SPDLOG_WARN("Overwrite existing result file {}",result_file); + }; + std::string output_folder = UTIL::get_file_directory(result_file); + if (!UTIL::folderExists(output_folder)) { + SPDLOG_CRITICAL("Output folder {} not exists",output_folder); + return false; + } + if (gps_error <= 0 || radius <= 0 || k <= 0) + { + SPDLOG_CRITICAL("Invalid mm parameter k {} r {} gps error {}", + k,radius,gps_error); + return false; + } + // Check the definition of parameters search radius and gps error + if (radius / gps_error > 10) { + SPDLOG_CRITICAL("Too large radius {} compared with gps error {}", + radius,gps_error); + return false; + } + std::cout << "Validating done.\n"; + return true; + }; - /* Input files */ - // Network file - std::string network_file; - std::string network_id; - std::string network_source; - std::string network_target; + /* Input files */ + // Network file + std::string network_file; + std::string network_id; + std::string network_source; + std::string network_target; - // UBODT configurations - std::string ubodt_file; - double delta; - bool delta_defined = true; - int binary_flag; + // UBODT configurations + std::string ubodt_file; + double delta; + bool delta_defined = true; + int binary_flag; - // GPS file - std::string gps_file; - std::string gps_id; - // Result file - std::string result_file; - /* - 0 for no geometry construction, only optimal path and complete - path will be outputed - 1 for wkb geometry output - 2 for wkt geometry output - */ - int mode; - // Parameters - double gps_error; - // Used by hashtable in UBODT + // GPS file + std::string gps_file; + std::string gps_id; + // Result file + std::string result_file; + // Parameters + double gps_error; + // Used by hashtable in UBODT - // Used by Rtree search - int k; - double radius; + // Used by Rtree search + int k; + double radius; - // PF for reversed movement - double penalty_factor; + // PF for reversed movement + double penalty_factor; - // Configuration of output format - ResultConfig result_config; + // Configuration of output format + ResultConfig result_config; + // 0-trace,1-debug,2-info,3-warn,4-err,5-critical,6-off + int log_level; }; // FMM_Config @@ -299,79 +315,91 @@ class FMM_Config class UBODT_Config { public: - /** - * FILETYPE 0 for ini and 1 for XML - */ - UBODT_Config(const std::string &file) - { + /** + * FILETYPE 0 for ini and 1 for XML + */ + UBODT_Config(const std::string &file) + { - // Create empty property tree object - boost::property_tree::ptree tree; - std::cout << "Read configuration from xml file: " << file << '\n'; - boost::property_tree::read_xml(file, tree); - // Parse the XML into the property tree. - // Without default value, the throwing version of get to find attribute. - // If the path cannot be resolved, an exception is thrown. + // Create empty property tree object + boost::property_tree::ptree tree; + std::cout << "Read configuration from xml file: " << file << '\n'; + boost::property_tree::read_xml(file, tree); + // Parse the XML into the property tree. - // UBODT configuration - delta = tree.get("ubodt_config.parameters.delta", 5000.0); + // UBODT configuration + delta = tree.get("ubodt_config.parameters.delta", 5000.0); - // Network - network_file = tree.get("ubodt_config.input.network.file"); - network_id = tree.get("ubodt_config.input.network.id", "id"); - network_source = tree.get("ubodt_config.input.network.source", "source"); - network_target = tree.get("ubodt_config.input.network.target", "target"); - // int temp = tree.get("ubodt_config.input.network.nid_index",0); - // nid_index= temp>0; - // Output - result_file = tree.get("ubodt_config.output.file"); - binary_flag = get_file_extension(result_file); - // binary_flag = tree.get("ubodt_config.output.binary", 1); - }; - void print() + // Network + network_file = tree.get("ubodt_config.input.network.file"); + network_id = tree.get("ubodt_config.input.network.id", "id"); + network_source = tree.get("ubodt_config.input.network.source", "source"); + network_target = tree.get("ubodt_config.input.network.target", "target"); + + // Output + result_file = tree.get("ubodt_config.output.file"); + binary_flag = UTIL::get_file_extension(result_file); + + // 0-trace,1-debug,2-info,3-warn,4-err,5-critical,6-off + log_level = tree.get("ubodt_config.other.log_level",2); + }; + void print() + { + std::cout << "------------------------------------------\n"; + std::cout << "Configuration parameters for UBODT construction: \n"; + std::cout << "Network_file: " << network_file << '\n';; + std::cout << "Network id: " << network_id << '\n'; + std::cout << "Network source: " << network_source << '\n'; + std::cout << "Network target: " << network_target << '\n'; + std::cout << "delta: " << delta << '\n'; + std::cout << "Output file:" << result_file << '\n'; + std::cout << "Output format(1 binary, 0 csv): " << binary_flag << '\n'; + std::cout << "log_level:" << LOG_LEVESLS[log_level] << '\n'; + std::cout << "------------------------------------------\n"; + }; + bool validate() + { + std::cout << "Validating configuration for UBODT construction:\n"; + if (!UTIL::fileExists(network_file)) { - std::cout << "------------------------------------------" << '\n'; - std::cout << "Configuration parameters for UBODT construction: " << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "Network_file: " << network_file << '\n';; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "Network id: " << network_id << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "Network source: " << network_source << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "Network target: " << network_target << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "delta: " << delta << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "Output file:" << result_file << '\n'; - std::cout << std::left << std::setw(4) << "" << std::setw(20) << "Output format(1 binary, 0 csv): " << binary_flag << '\n'; - std::cout << "------------------------------------------" << '\n'; - }; - bool validate() + SPDLOG_CRITICAL("Network file {} not found",network_file); + return false; + } + if (UTIL::fileExists(result_file)) { - std::cout << "Validating configuration for UBODT construction:" << '\n'; - if (!fileExists(network_file)) - { - std::cout << std::setw(12) << "" << "Error,Network file not found" << '\n'; - return false; - } - if (fileExists(result_file)) - { - std::cout << std::setw(4) << "" << "Warning, overwrite existing result file." << result_file << '\n'; - } - if (binary_flag==2){ - std::cout << std::setw(12) << "" << "Error, UBODT file extension not recognized, which should be csv or binary. Program stop." << '\n'; - return false; - } - if (delta <= 0) - { - std::cout << std::setw(12) << "" << "Error,Delta value should be positive." << '\n'; - return false; - } - std::cout << "Validating success." << '\n'; - return true; - }; - std::string network_file; - std::string network_id; - std::string network_source; - std::string network_target; - int binary_flag; - double delta; - std::string result_file; + SPDLOG_WARN("Overwrite result file {}",result_file); + } + std::string output_folder = UTIL::get_file_directory(result_file); + if (!UTIL::folderExists(output_folder)) { + SPDLOG_CRITICAL("Output folder {} not exists",output_folder); + return false; + } + if (log_level<0 || log_level>LOG_LEVESLS.size()) { + SPDLOG_CRITICAL("Invalid log_level {}, which should be 0 - 6",log_level); + SPDLOG_INFO("0-trace,1-debug,2-info,3-warn,4-err,5-critical,6-off"); + return false; + } + if (binary_flag==2) { + SPDLOG_CRITICAL("UBODT file extension not recognized"); + return false; + } + if (delta <= 0) + { + SPDLOG_CRITICAL("Delta {} should be positive"); + return false; + } + std::cout << "Validating done.\n"; + return true; + }; + std::string network_file; + std::string network_id; + std::string network_source; + std::string network_target; + int binary_flag; + double delta; + std::string result_file; + // 0-trace,1-debug,2-info,3-warn,4-err,5-critical,6-off + int log_level; }; // UBODT_Config } // MM diff --git a/src/debug.h b/src/debug.h new file mode 100644 index 0000000..873fa75 --- /dev/null +++ b/src/debug.h @@ -0,0 +1,18 @@ +/** + * Content + * Debug information used in development + * + * @author: Can Yang + * @version: 2020.01.31 + */ + +#ifndef MM_DEBUG_HPP +#define MM_DEBUG_HPP + +#define SPDLOG_ACTIVE_LEVEL SPDLOG_LEVEL_TRACE + +#include "spdlog/spdlog.h" +// must be included for custom operator +#include "spdlog/fmt/ostr.h" + +#endif // MM_DEBUG_HPP diff --git a/src/geometry_type.hpp b/src/geometry_type.hpp index 6ed7958..c61c6c5 100644 --- a/src/geometry_type.hpp +++ b/src/geometry_type.hpp @@ -1,89 +1,98 @@ #ifndef MM_GEOMTYPES_HPP #define MM_GEOMTYPES_HPP -#ifdef USE_BG_GEOMETRY - #include // C++ API for GDAL #include #include -#include #include #include -#else // USE_BG_GEOMETRY not defined - -#include // C++ API for GDAL - -#endif +#include "boost/geometry/extensions/gis/io/wkb/read_wkb.hpp" +#include "boost/geometry/extensions/gis/io/wkb/write_wkb.hpp" namespace MM { -#ifdef USE_BG_GEOMETRY - namespace bg = boost::geometry; -typedef bg::model::point bg_point_t; -typedef bg::model::linestring linestring_t; +// Point for rtree box +typedef bg::model::point boost_point; +typedef bg::model::linestring linestring_t; /** * Boost Geometry Linestring, compatible with OGRGeometry */ -class BGLineString { +class LineString { public: -inline double getX(int i){ + inline double getX(int i) const { return bg::get<0>(line.at(i)); -}; -inline double getY(int i){ + }; + inline double getY(int i) const { return bg::get<1>(line.at(i)); -}; -inline void addPoint(double x,double y){ - bg::append(line, bg_point_t(x,y)); -}; -inline int getNumPoints(){ + }; + inline void setX(int i, double v) { + bg::set<0>(line.at(i),v); + }; + inline void setY(int i, double v) { + bg::set<1>(line.at(i),v); + }; + inline void addPoint(double x,double y){ + bg::append(line, boost_point(x,y)); + }; + inline void addPoint(const boost_point& point){ + bg::append(line, point); + }; + inline boost_point getPoint(int i) const { + return boost_point(bg::get<0>(line.at(i)),bg::get<1>(line.at(i))); + }; + inline int getNumPoints() const { return bg::num_points(line); -}; -inline bool IsEmpty(){ + }; + inline bool isEmpty() const { return bg::num_points(line)==0; -}; -bg::wkt_manipulator exportToWkt(){ + }; + bg::wkt_manipulator exportToWkt() const { return bg::wkt(line); -}; -linestring_t *get_geometry(){ - return &line; -}; -inline double get_Length(){ + }; + linestring_t &get_geometry(){ + return line; + }; + inline void clear(){ + bg::clear(line); + }; + inline double getLength() const { return bg::length(line); -}; + }; private: -linestring_t line; -}; // BGLineString + linestring_t line; +}; // LineString -typedef BGLineString LineString; /** * Convert an OGRLineString to Boost geometry, the caller is responsible to * freeing the memory. - * */ -LineString *ogr2linestring(OGRLineString *line){ - int binary_size = line->WkbSize(); - std::vector wkb(binary_size); - // http://www.gdal.org/ogr__core_8h.html#a36cc1f4d807ba8f6fb8951f3adf251e2 - line->exportToWkb(wkbNDR,&wkb[0]); - BGLineString *l = new BGLineString(); - bg::read_wkb(wkb.begin(),wkb.end(),*(l->get_geometry())); - return l; +LineString ogr2linestring(OGRLineString *line){ + int binary_size = line->WkbSize(); + std::vector wkb(binary_size); + line->exportToWkb(wkbNDR,&wkb[0]); + LineString l; + bg::read_wkb(wkb.begin(),wkb.end(),l.get_geometry()); + return l; }; -#else // USE_BG_GEOMETRY not defined - -typedef OGRLineString LineString; - -LineString *ogr2linestring(OGRLineString *line){ - LineString *linestring = (OGRLineString*) line->clone(); - return linestring; +OGRLineString *linestring2ogr(LineString &line, int srid=4326){ + std::vector wkb; + bg::write_wkb(line.get_geometry(),std::back_inserter(wkb)); + OGRGeometry *poGeometry; + OGRGeometryFactory::createFromWkb(&wkb[0], NULL, &poGeometry); + return (OGRLineString *) poGeometry; }; - -#endif //USE_BG_GEOMETRY +OGRPoint *bg2ogr_point(boost_point &p, int srid=4326){ + std::vector wkb; + bg::write_wkb(p,std::back_inserter(wkb)); + OGRGeometry *poGeometry; + OGRGeometryFactory::createFromWkb(&wkb[0], NULL, &poGeometry); + return (OGRPoint *) poGeometry; +}; }; // MM diff --git a/src/gps.hpp b/src/gps.hpp index ca2b2c1..6540529 100644 --- a/src/gps.hpp +++ b/src/gps.hpp @@ -1,39 +1,21 @@ /** * Content * Definition of input trajectory format - * + * * @author: Can Yang * @version: 2017.11.11 */ #ifndef MM_GPS_HPP #define MM_GPS_HPP + #include "geometry_type.hpp" -#include -#include namespace MM { -/** - * A trajectory stores two fields: - * id: int - * geom: a pointer to OGRLineString - * - * It is constructed with ID and a pointer to an OGRLineString object. - * When it is destructed, the geom pointer will be freed. - */ -class Trajectory{ -public: - Trajectory():id(0),geom(NULL){}; // Default constructor for vector of trajectories - Trajectory(int e_id,LineString *e_geom):id(e_id),geom(e_geom){}; - ~Trajectory(){ -#ifdef USE_BG_GEOMETRY - delete geom; -#else - if (geom!=NULL) OGRGeometryFactory::destroyGeometry(geom); -#endif - }; - int id; // Trip id - LineString *geom; // It will be freed by the destructor of the trajectory -}; // Trajectory +struct Trajectory { + int id; + LineString geom; +}; + } // MM #endif /* MM_GPS_HPP */ diff --git a/src/graph_type.hpp b/src/graph_type.hpp new file mode 100644 index 0000000..3174a51 --- /dev/null +++ b/src/graph_type.hpp @@ -0,0 +1,40 @@ +/** + * Definition of graph types + * + * @author: Can Yang + * @version: 2020.01.31 + */ + +#ifndef MM_GRAPH_TYPE_HPP +#define MM_GRAPH_TYPE_HPP + +#include +#include +#include + +#include "types.hpp" + +namespace MM{ + +struct EdgeProperty +{ + EdgeIndex index; + double length; +}; + +typedef boost::adjacency_list Graph_T; + +// The EdgeDescriptor is different from EdgeIndex, it can be used +// to access the edge property of a graph as g[e].property +typedef Graph_T::edge_descriptor EdgeDescriptor; +typedef boost::graph_traits::vertex_iterator NodeIterator; +typedef boost::graph_traits::out_edge_iterator OutEdgeIterator; + +// Data types used for routing +typedef std::unordered_map PredecessorMap; +typedef std::unordered_map DistanceMap; + +} + +#endif // MM_GRAPH_TYPE_HPP diff --git a/src/heap.hpp b/src/heap.hpp new file mode 100644 index 0000000..d375b98 --- /dev/null +++ b/src/heap.hpp @@ -0,0 +1,66 @@ +/** + * Definition of heap types + * + * @author: Can Yang + * @version: 2020.01.31 + */ + + +#ifndef MM_HEAP_HPP +#define MM_HEAP_HPP + +#include "fiboheap/fiboheap.h" + +namespace MM { + +struct HeapNode +{ + NodeIndex index; + double dist; + bool operator<(const HeapNode &rhs) const { + if (dist == rhs.dist) return (index < rhs.index); + return dist < rhs.dist; + } +}; + +// This class is a wrapper of the fiboheap to facilitate calling of +// insert, pop and update functions. +class Heap { +public: + inline void push(NodeIndex index, double dist){ + HeapNodeHandle handle = heap.push({index,dist}); + handle_data.insert({index,handle}); + } + + inline void pop(){ + HeapNode &node = heap.top(); + handle_data.erase(node.index); + heap.pop(); + } + + inline HeapNode top(){ + return heap.top(); + } + + inline bool empty(){ + return heap.empty(); + } + + inline unsigned int size(){ + return heap.size(); + } + + inline void decrease_key(NodeIndex index,double dist){ + HeapNodeHandle handle = handle_data[index]; + heap.decrease_key(handle,{index,dist}); + } + +private: + typedef FibHeap::FibNode *HeapNodeHandle; + FibHeap heap; + std::unordered_map handle_data; +}; // Heap + +}; //MM + +#endif diff --git a/src/multilevel_debug.h b/src/multilevel_debug.h deleted file mode 100644 index 21f9cea..0000000 --- a/src/multilevel_debug.h +++ /dev/null @@ -1,65 +0,0 @@ -/** - * Content - * Debug information used in development - * - * @author: Can Yang - * @version: 2017.11.11 - */ - -#ifndef MM_DEBUG_HPP -#define MM_DEBUG_HPP -#include - -#include "spdlog/spdlog.h" -#include "spdlog/fmt/ostr.h" // must be included for custom operator - -/** - * Debug level from 1 to 3 - * that will deal with various details of debug setting - * default debug level is 0 - */ - -#ifndef DEBUG_LEVEL -#define DEBUG_LEVEL 0 -#endif - -#ifndef CS_DEBUG_LEVEL -#define CS_DEBUG_LEVEL 0 -#endif - -#ifndef OPI_DEBUG_LEVEL -#define OPI_DEBUG_LEVEL 0 -#endif - -#ifndef CPC_DEBUG_LEVEL -#define CPC_DEBUG_LEVEL 0 -#endif - -#ifndef GC_DEBUG_LEVEL -#define GC_DEBUG_LEVEL 0 -#endif - -#ifndef UTIL_DEBUG_LEVEL -#define UTIL_DEBUG_LEVEL 0 -#endif - -#define DEBUG(level) if(level <= DEBUG_LEVEL) - -/* Candidate search debug */ -#define CS_DEBUG(level) if(level <= CS_DEBUG_LEVEL) - -/* Optimal path inference debug */ -#define OPI_DEBUG(level) if(level <= OPI_DEBUG_LEVEL) - -/* Complete path construction debug */ -#define CPC_DEBUG(level) if(level <= CPC_DEBUG_LEVEL) - -/* Geometry construction debug*/ -#define GC_DEBUG(level) if(level <=GC_DEBUG_LEVEL) - -/* Geometry construction debug*/ -#define UTIL_DEBUG(level) if(level <=UTIL_DEBUG_LEVEL) - -// #define ROUTING_RECORD_DEBUG - -#endif // MM_DEBUG_HPP diff --git a/src/network.hpp b/src/network.hpp index 4845f78..26119b2 100644 --- a/src/network.hpp +++ b/src/network.hpp @@ -3,24 +3,26 @@ * Definition of the Network class * * @author: Can Yang + * @version: 2020.01.23 + * Reformat indentation + * Change linestring pointer to reference * @version: 2017.11.11 */ #ifndef MM_NETWORK_HPP #define MM_NETWORK_HPP -#include // C++ API for GDAL +#include #include -#include // Calulating probability -#include -#include // Partial sort copy -#include // Partial sort copy +#include +#include + // Data structures for Rtree #include #include #include -#include "multilevel_debug.h" #include "types.hpp" +#include "debug.h" #include "util.hpp" #include "algorithm.hpp" #include "gps.hpp" @@ -30,14 +32,12 @@ namespace MM // Define a type alias for the rtree used in map matching /* Edge format in the network */ -typedef boost::geometry::model::point - boost_point; typedef boost::geometry::model::box boost_box; // Item stored in rtree typedef std::pair Item; typedef boost::geometry::index::rtree - > Rtree; // Rtree definition + > Rtree; // Rtree definition // This function is used for KNN sort static bool candidate_compare(const Candidate &a, const Candidate &b) @@ -48,7 +48,7 @@ static bool candidate_compare(const Candidate &a, const Candidate &b) } else { - return a.edge->idid; + return a.edge->indexindex; } }; @@ -69,152 +69,152 @@ class Network Network(const std::string &filename, const std::string &id_name="id", const std::string &source_name="source", - const std::string &target_name="target") { - std::cout<<"Read network shp file from " << filename <<'\n'; - std::cout<<"Id column "<GetLayer(0); int NUM_FEATURES = ogrlayer->GetFeatureCount(); - std::cout<< "\tNumber of edges in file: " << NUM_FEATURES << '\n'; - network_edges= std::vector(NUM_FEATURES); - // Initialize network edges + SPDLOG_INFO("Number of edges in file: {}",NUM_FEATURES); + OGRFeatureDefn *ogrFDefn = ogrlayer->GetLayerDefn(); OGRFeature *ogrFeature; + // Fetch the field index given field name. int id_idx=ogrFDefn->GetFieldIndex(id_name.c_str()); int source_idx=ogrFDefn->GetFieldIndex(source_name.c_str()); int target_idx=ogrFDefn->GetFieldIndex(target_name.c_str()); - std::cout<< "\tSHP ID idx: " <GetGeomType()) != wkbLineString) { - std::cout<GetGeomType())<<'\n'; - std::cout<GetGeomType())); GDALClose( poDS ); - std::cout<<"Program stop"<< '\n'; std::exit(EXIT_FAILURE); } else { - std::cout<< "\tGeometry type is " << OGRGeometryTypeToName( - ogrFDefn->GetGeomType())<<'\n'; + SPDLOG_INFO("Geometry type of network is {}", + OGRGeometryTypeToName(ogrFDefn->GetGeomType())); } - OGRSpatialReference *ogrsr = ogrFDefn->GetGeomFieldDefn(0)->GetSpatialRef(); + OGRSpatialReference *ogrsr = + ogrFDefn->GetGeomFieldDefn(0)->GetSpatialRef(); if (ogrsr != nullptr) { srid = ogrsr->GetEPSGGeogCS(); if (srid==-1) { srid= 4326; - std::cout<< "\t---- Warning: srid is not found, set to 4326 for default"<< '\n'; + SPDLOG_WARN("SRID is not found, set to 4326 by default"); } else { - std::cout<< "\tSRID is "< nodeSet; + // Read data from shapefile + EdgeIndex index = 0; while( (ogrFeature = ogrlayer->GetNextFeature()) != NULL) { - int id = ogrFeature->GetFID(); - Edge *e = &network_edges[id]; - e->id = id; - e->id_attr = std::string(ogrFeature->GetFieldAsString(id_idx)); - e->source = ogrFeature->GetFieldAsInteger(source_idx); - e->target = ogrFeature->GetFieldAsInteger(target_idx); + EdgeID id = ogrFeature->GetFieldAsInteger(id_idx); + NodeID source = ogrFeature->GetFieldAsInteger(source_idx); + NodeID target = ogrFeature->GetFieldAsInteger(target_idx); OGRGeometry *rawgeometry = ogrFeature->GetGeometryRef(); - e->geom = ogr2linestring((OGRLineString*) rawgeometry); - e->length = e->geom->get_Length(); - if (e->source>max_node_id) - { - max_node_id = e->source; - } - if (e->target>max_node_id) - { - max_node_id = e->target; - } - if (nodeSet.find(e->source)==nodeSet.end()) { - nodeSet.insert(e->source); + LineString geom = ogr2linestring((OGRLineString*) rawgeometry); + NodeIndex s_idx,t_idx; + if (node_map.find(source)==node_map.end()) { + s_idx = node_id_vec.size(); + node_id_vec.push_back(source); + node_map.insert({source,s_idx}); + vertex_points.push_back(geom.getPoint(0)); + } else { + s_idx = node_map[source]; } - if (nodeSet.find(e->target)==nodeSet.end()) { - nodeSet.insert(e->target); + if (node_map.find(target)==node_map.end()) { + t_idx = node_id_vec.size(); + node_id_vec.push_back(target); + node_map.insert({target,t_idx}); + int npoints = geom.getNumPoints(); + vertex_points.push_back(geom.getPoint(npoints-1)); + } else { + t_idx = node_map[target]; } + edges.push_back({index,id,s_idx,t_idx,geom.getLength(),geom}); + edge_map.insert({id,index}); + ++index; OGRFeature::DestroyFeature(ogrFeature); } GDALClose( poDS ); - CS_DEBUG(3) std::cout<<"Line "<< __LINE__<< " Length "<get_Length()<<"\n"; - std::cout<<"Read network finish."<< '\n'; - node_count = nodeSet.size(); - std::cout<<"\tThe maximum node ID is "<< max_node_id << '\n'; - std::cout<<"Node count is "<< node_count << '\n'; - std::cout<<"\tTotal number of edges read "<< network_edges.size()<< '\n'; - }; // Network constructor - ~Network() - { - std::cout<< "Cleaning network" << '\n'; - for (auto &item:network_edges) - { -#ifdef USE_BG_GEOMETRY - delete item.geom; -#else - OGRGeometryFactory::destroyGeometry(item.geom); -#endif - } - std::cout<< "Cleaning network finished" << '\n'; - } + SPDLOG_INFO("Read network done."); + num_vertices = node_id_vec.size(); + SPDLOG_INFO("Node count is {}",num_vertices); + SPDLOG_INFO("Total number of edges read {}",edges.size()); + }; // Network constructor int get_node_count(){ - return node_count; - }; - // Get the edge vector - std::vector *get_edges() - { - return &network_edges; + return node_id_vec.size();; }; + // Get the ID attribute of an edge according to its index - std::string get_edge_id_attr(int eid) + EdgeID get_edge_id(EdgeIndex index) const { - return network_edges[eid].id_attr; + return edges[index].id; }; - int get_max_node_id() - { - return max_node_id; + + EdgeIndex get_edge_index(EdgeID id){ + return edge_map[id]; + }; + + inline std::vector &get_edges(){ + return edges; }; + + NodeID get_node_id(NodeIndex index){ + return indexgeom->getNumPoints()<<"\n"; - // boundary is returned is a multipoint geometry, but not the envelop - // instead, it is only the first and last point + Edge *edge = &edges[i]; double x1,y1,x2,y2; ALGORITHM::boundingbox_geometry(edge->geom,&x1,&y1,&x2,&y2); - CS_DEBUG(3) std::cout<<"Process trajectory: "<getNumPoints(); + int NumberPoints = geom.getNumPoints(); Traj_Candidates tr_cs(NumberPoints); for (int i=0; igetX(i); - double py = geom->getY(i); + double px = geom.getX(i); + double py = geom.getY(i); Point_Candidates pcs; - boost_box b(boost_point(geom->getX(i)-radius,geom->getY(i)-radius),boost_point(geom->getX(i)+radius,geom->getY(i)+radius)); + boost_box b(boost_point(geom.getX(i)-radius,geom.getY(i)-radius), + boost_point(geom.getX(i)+radius,geom.getY(i)+radius)); std::vector temp; - // Rtree can only detect intersect with a the bounding box of the geometry stored. - rtree.query(boost::geometry::index::intersects(b),std::back_inserter(temp)); + rtree.query(boost::geometry::index::intersects(b), + std::back_inserter(temp)); for (Item &i:temp) { - // Check for detailed intersection - // The two edges are all in OGR_linestring Edge *edge = i.second; - float offset; + double offset; double dist; ALGORITHM::linear_referencing(px,py,edge->geom,&dist,&offset); - CS_DEBUG(2) std::cout<<"Edge id: "<id<< '\n'; - CS_DEBUG(2) std::cout<<"Dist: "<empty()) return nullptr; - // if (complete_path->empty()) return nullptr; - LineString *line = new LineString(); - int NOsegs = o_path_ptr->size(); - int NCsegs = complete_path->size(); - GC_DEBUG(2) std::cout<< __FILE__ << __LINE__ <<" Optimal path size "<offset; - double lastoffset = (*o_path_ptr)[NOsegs-1]->offset; - GC_DEBUG(2) std::cout<< "first offset " << firstoffset <<'\n'; - GC_DEBUG(2) std::cout<< "last offset " << lastoffset <<'\n'; - LineString * firstseg = network_edges[(*complete_path)[0]].geom; - LineString * firstlineseg= ALGORITHM::cutoffseg_unique(firstoffset,lastoffset,firstseg); - append_segs_to_line(line,firstlineseg,0); - GC_DEBUG(2) UTIL::print_geometry(firstlineseg); - // Free the memory - delete firstlineseg; + LineString &firstseg = get_edge_geom(complete_path[0]); + double firstoffset = o_path[0]->offset; + double lastoffset = o_path[NOsegs-1]->offset; + LineString firstlineseg= ALGORITHM::cutoffseg_unique( + firstoffset,lastoffset,firstseg); + append_segs_to_line(&line,firstlineseg,0); } else { - double firstoffset = (*o_path_ptr)[0]->offset; - double lastoffset = (*o_path_ptr)[NOsegs-1]->offset; - LineString * firstseg = network_edges[(*complete_path)[0]].geom; - LineString * lastseg = network_edges[(*complete_path)[NCsegs-1]].geom; - LineString * firstlineseg= ALGORITHM::cutoffseg(firstoffset, firstseg, 0); - LineString * lastlineseg= ALGORITHM::cutoffseg(lastoffset, lastseg, 1); - GC_DEBUG(2) std::cout<< "First offset " << firstoffset <<'\n'; - GC_DEBUG(2) std::cout<< "First line " <<'\n'; - GC_DEBUG(2) UTIL::print_geometry(firstseg); - GC_DEBUG(2) std::cout<< "First line cutoff " <<'\n'; - GC_DEBUG(2) UTIL::print_geometry(firstlineseg); - GC_DEBUG(2) std::cout<< "last offset " << lastoffset <<'\n'; - GC_DEBUG(2) std::cout<< "Last line " <<'\n'; - GC_DEBUG(2) UTIL::print_geometry(lastseg); - GC_DEBUG(2) std::cout<< "Last line cutoff " <<'\n'; - GC_DEBUG(2) UTIL::print_geometry(lastlineseg); - append_segs_to_line(line,firstlineseg,0); + LineString &firstseg = get_edge_geom(complete_path[0]); + LineString &lastseg = get_edge_geom(complete_path[NCsegs-1]); + double firstoffset = o_path[0]->offset; + double lastoffset = o_path[NOsegs-1]->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; igetNumPoints(); - DEBUG(2) std::cout<< "Number of points "<< Npoints <<'\n'; + int Npoints = segs.getNumPoints(); for(int i=0; i=offset) { - line->addPoint(segs->getX(i),segs->getY(i)); + line->addPoint(segs.getX(i),segs.getY(i)); } } }; int srid; // Spatial reference id Rtree rtree; // Network rtree structure - std::vector network_edges; // all edges in the network - int max_node_id = 0; // a variable to record the maximum node ID - int node_count = 0; + std::vector edges; // all edges in the network + NodeIDVec node_id_vec; + unsigned int num_vertices; + NodeIndexMap node_map; + EdgeIndexMap edge_map; + std::vector vertex_points; }; // Network } // MM #endif /* MM_NETWORK_HPP */ diff --git a/src/network_graph.hpp b/src/network_graph.hpp index 9bd38cf..50a094d 100644 --- a/src/network_graph.hpp +++ b/src/network_graph.hpp @@ -15,354 +15,326 @@ * for nodes. * * @author: Can Yang + * @version: 2020.01.23 + * Reformat indentation + * Change linestring pointer to reference * @version: 2018.03.09 */ #ifndef MM_NETWORK_GRAPH_HPP #define MM_NETWORK_GRAPH_HPP #include #include +#include #include #include #include -#include -#include -#include +#include +#include +// OpenMP +#include + +// Binary output of UBODT +#include + #include #include -#include -#include "types.hpp" -#include "reader.hpp" -#include "float.h" + +#include "graph_type.hpp" #include "network.hpp" -#include // std::reverse -#include -#include // Binary output of UBODT -namespace MM{ +#include "heap.hpp" + +namespace MM { + class NetworkGraph { public: - // A infinity value used in the routing algorithm - static constexpr double DIST_NOT_FOUND = DBL_MAX; - /** - * Construct a network graph from a network object - */ - NetworkGraph(Network *network) { - std::vector *edges = network->get_edges(); - std::cout << "Construct graph from network edges start" << '\n'; - // Key is the external ID and value is the index of vertice - std::unordered_map vertex_map; - int current_idx=-1; - edge_descriptor e; - bool inserted; - g = Graph_T(); //18 - int N = edges->size(); - int source_idx = 0; - int target_idx = 0; - printf("Network edges :%d \n", N); - for (int i = 0; i < N; ++i) { - Edge &network_edge = (*edges)[i]; - auto search = vertex_map.find(network_edge.source); - // Search for source node idx - if(search != vertex_map.end()) { - // A node exists already - source_idx = search->second; - } else { - // A new node is found - ++current_idx; - vertex_map.insert({network_edge.source,current_idx}); - source_idx = current_idx; - vertex_id_vec.push_back(network_edge.source); - }; - // Search for target node idx - search = vertex_map.find(network_edge.target); - if(search != vertex_map.end()) { - // A node exists already - target_idx = search->second; - } else { - // A new node is found - ++current_idx; - vertex_map.insert({network_edge.target,current_idx}); - target_idx = current_idx; - vertex_id_vec.push_back(network_edge.target); - }; - // boost::tie(e, inserted) = add_edge(network_edge.source, network_edge.target, g); - boost::tie(e, inserted) = add_edge(source_idx,target_idx, g); - // id is the FID read, id_attr is the external property in SHP - g[e].id = network_edge.id; - g[e].length = network_edge.length; - //printf( "Edge read %d,%d,%d,%lf\n",network_edge.id,network_edge.source,network_edge.target,network_edge.length); - } - num_vertices = boost::num_vertices(g); - std::cout << "Graph nodes " << num_vertices << '\n'; - initialize_distances_predecessors(); - std::cout << "Construct graph from network edges end" << '\n'; - }; - /** - * Precompute an UBODT with delta and save it to the file - * @param filename [description] - * @param delta [description] - */ - void precompute_ubodt(const std::string &filename, double delta, bool binary=true) { - int step_size = num_vertices/10; - if (step_size<10) step_size=10; - std::ofstream myfile(filename); - std::cout << "Start to generate UBODT with delta " << delta << '\n'; - std::cout << "Output format " << (binary?"binary":"csv") << '\n'; - if (binary){ - boost::archive::binary_oarchive oa(myfile); - vertex_iterator vi, vend; - for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi) { - if (*vi%step_size==0) std::cout<<"Progress "<<*vi<< " / " << num_vertices <<'\n'; - driving_distance_binary(*vi, delta, oa); - } + /** + * Construct a network graph from a network object + */ + NetworkGraph(Network *network_arg) : network(network_arg) { + std::vector &edges = network->get_edges(); + SPDLOG_INFO("Construct graph from network edges start"); + EdgeDescriptor e; + bool inserted; + g = Graph_T(); //18 + int N = edges.size(); + // std::cout<< "Network edges : " << N <<"\n"; + for (int i = 0; i < N; ++i) { + Edge &edge = edges[i]; + boost::tie(e, inserted) = boost::add_edge(edge.source,edge.target,g); + // id is the FID read, id_attr is the external property in SHP + g[e].index = edge.index; + g[e].length = edge.length; + } + num_vertices = boost::num_vertices(g); + SPDLOG_INFO("Graph nodes {}",num_vertices); + SPDLOG_INFO("Graph edges {}",boost::num_edges(g)); + SPDLOG_INFO("Construct graph from network edges end"); + } + + /** + * Routing from a single source to all nodes within an upperbound + * Pmap and dmap are updated to record the result. + */ + void single_source_upperbound_dijkstra(NodeIndex s, + double delta, + PredecessorMap *pmap, + DistanceMap *dmap){ + Heap Q; + // Initialization + Q.push(s,0); + pmap->insert({s,s}); + dmap->insert({s,0}); + OutEdgeIterator out_i, out_end; + double temp_dist = 0; + // Dijkstra search + while (!Q.empty()) { + HeapNode node = Q.top(); + Q.pop(); + NodeIndex u = node.index; + if (node.dist>delta) break; + for (boost::tie(out_i, out_end) = boost::out_edges(u,g); + out_i != out_end; ++out_i) { + EdgeDescriptor e = *out_i; + NodeIndex v = boost::target(e,g); + temp_dist = node.dist + g[e].length; + auto iter = dmap->find(v); + if (iter!=dmap->end()) { + // dmap contains node v + if (iter->second>temp_dist) { + // a smaller distance is found for v + (*pmap)[v] = u; + (*dmap)[v] = temp_dist; + Q.decrease_key(v,temp_dist); + }; } else { - myfile << "source;target;next_n;prev_n;next_e;distance\n"; - vertex_iterator vi, vend; - for (boost::tie(vi, vend) = vertices(g); vi != vend ; ++vi) { - if (*vi%step_size==0) std::cout<<"Progress "<<*vi<< " / " << num_vertices <<'\n'; - driving_distance_csv(*vi, delta, myfile); - } + // dmap does not contain v + if (temp_dist<=delta){ + Q.push(v,temp_dist); + pmap->insert({v,u}); + dmap->insert({v,temp_dist}); + } } - myfile.close(); - }; -private: - /* Type definition for the property stored at each edge */ - struct Edge_Property - { - int id; - double length; - }; - // Boost graph type definition - typedef boost::adjacency_list Graph_T; - typedef Graph_T::vertex_descriptor vertex_descriptor; - typedef Graph_T::edge_descriptor edge_descriptor; - typedef boost::graph_traits::vertex_iterator vertex_iterator; - typedef boost::graph_traits::out_edge_iterator out_edge_iterator; - struct found_goals {}; // Used for driving distances - /** - * The visitor is an inner class whose function examine_vertex() - * is called whenever a new node is found in conventional Dijkstra - * algorithm. - * - * It is called in the driving_distance function. - */ - class driving_distance_visitor : public boost::default_dijkstra_visitor { - public: - // Create a visitor - explicit driving_distance_visitor( - double distance_goal, - std::deque< vertex_descriptor > &nodesInDistance, - std::vector< double > &distances, - std::vector< vertex_descriptor > &examined_vertices_ref - ) : m_distance_goal(distance_goal), m_nodes(nodesInDistance), m_dist(distances), - m_examined_vertices(examined_vertices_ref) {}; - template void examine_vertex(vertex_descriptor u, Graph &g) { - DEBUG (2) std::cout << "Examine node " << u << '\n'; - m_nodes.push_back(u); - if (m_dist[u] > m_distance_goal) { - m_nodes.pop_back(); - throw found_goals(); - } - }; - template void edge_relaxed(edge_descriptor e, Graph &g) { - // Add v to the examined vertices - DEBUG (2) std::cout << "Examine edge" << e << '\n'; - m_examined_vertices.push_back(boost::target(e, g)); - }; - private: - double m_distance_goal; //Delta - std::deque< vertex_descriptor > &m_nodes; //Precedessors - std::vector< double > &m_dist; // Distances - std::vector< vertex_descriptor > & m_examined_vertices; //Examined nodes - }; // driving_distance_visitor + } + } + } - Graph_T g; // The member storing a boost graph - /** - * Find the edge ID given a pair of nodes and its cost - */ - int get_edge_id(vertex_descriptor source, vertex_descriptor target, double cost) { - edge_descriptor e; - out_edge_iterator 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; // Can we directly get edge id here or latter from the graph - if (target == boost::target(e, g) && (g[e].length - cost <= DOUBLE_MIN)) { - return g[e].id; - } - } - std::cout << "Edge not found for source " << source << " target " << target - << " cost " << cost << '\n'; - return -1; - }; + /** + * Precompute an UBODT with upperbound and save it to the file + * @param filename output file to write the result + * @param delta upper bound + * @param binary format of saving ubodt as binary or not + */ + void precompute_ubodt(const std::string &filename, double delta, + bool binary=true) { + int step_size = num_vertices/10; + if (step_size<10) step_size=10; + std::ofstream myfile(filename); + SPDLOG_INFO("Start to generate UBODT with delta {}",delta); + SPDLOG_INFO("Output format {}", (binary ? "binary" : "csv")); + if (binary) { + boost::archive::binary_oarchive oa(myfile); + for(NodeIndex source = 0; source < num_vertices; ++source) { + if (source%step_size==0) + SPDLOG_INFO("Progress {} / {}", source, num_vertices); + PredecessorMap pmap; + DistanceMap dmap; + single_source_upperbound_dijkstra(source,delta,&pmap,&dmap); + write_result_binary(oa,source,pmap,dmap); + } + } else { + myfile << "source;target;next_n;prev_n;next_e;distance\n"; + for(NodeIndex source = 0; source < num_vertices; ++source) { + if (source%step_size==0) + SPDLOG_INFO("Progress {} / {}",source, num_vertices); + PredecessorMap pmap; + DistanceMap dmap; + single_source_upperbound_dijkstra(source,delta,&pmap,&dmap); + write_result_csv(myfile,source,pmap,dmap); + } + } + myfile.close(); + } - /** - * Get the successors (next node visited) for each node in a - * shortest path tree defined by a deque and a predecessor vector - */ - std::vector get_successors(std::deque &nodesInDistance, std::vector& predecessors) { - int N = nodesInDistance.size(); - std::vector successors = std::vector(N); - int i; - vertex_descriptor u, v; - vertex_descriptor source = nodesInDistance[0];// source node - for (i = 0; i < N; ++i) { - v = nodesInDistance[i]; - while ((u = predecessors[v]) != source) { - v = u; - } - successors[i] = v; - } - return successors; - }; - /** - * Given a source node and an upper bound distance delta - * write the UBODT rows to a file - */ - void driving_distance_csv(const vertex_descriptor& source, double delta, std::ostream& stream) { - DEBUG (2) std::cout << "Debug progress source " << source << '\n'; - std::deque nodesInDistance; - examined_vertices.push_back(source); - double inf = std::numeric_limits::max(); - distances_map[source]=0; - // In BGL, http://www.boost.org/doc/libs/1_66_0/boost/graph/dijkstra_shortest_paths_no_color_map.hpp - // The named parameter version is only defined for dijkstra_shortest_paths_no_color_map - // Therefore, we need to explicitly pass in the arguments - // boost::choose_param choose parameter with default value - try { - // This part to be fixed - dijkstra_shortest_paths_no_color_map_no_init( - g, - source, - //boost::predecessor_map(&predecessors_map[0]), - make_iterator_property_map(predecessors_map.begin(),get(boost::vertex_index, g),predecessors_map[0]), - //boost::distance_map(boost::make_iterator_property_map(distances_map.begin(), get(boost::vertex_index, g))), - //&distances_map[0], - make_iterator_property_map(distances_map.begin(),get(boost::vertex_index, g),distances_map[0]), - //make_iterator_property_map(distances_map.begin(), boost::vertex_index_map(get(boost::vertex_index, g)),distances_map[0]), - //boost::distance_map(boost::make_iterator_property_map(distances_map.begin(), get(boost::vertex_index, g))), - get(&Edge_Property::length, g), - get(boost::vertex_index, g), - std::less(), //DistanceCompare distance_compare, - boost::closed_plus(inf), - inf, - 0, - driving_distance_visitor( - delta, nodesInDistance, distances_map, examined_vertices - ) - ); - } catch (found_goals& goal) { - //std::cout << "Found goals" << '\n'; + // Parallelly generate ubodt using OpenMP + void precompute_ubodt_omp(const std::string &filename, double delta, + bool binary=true) { + int step_size = num_vertices/10; + if (step_size<10) step_size=10; + std::ofstream myfile(filename); + SPDLOG_INFO("Start to generate UBODT with delta {}",delta); + SPDLOG_INFO("Output format {}", (binary ? "binary" : "csv")); + if (binary) { + boost::archive::binary_oarchive oa(myfile); + int progress = 0; + #pragma omp parallel + { + #pragma omp for + for(int source = 0; source < num_vertices; ++source) { + ++progress; + if (progress % step_size == 0) { + SPDLOG_INFO("Progress {} / {}", progress, num_vertices); + } + PredecessorMap pmap; + DistanceMap dmap; + std::stringstream node_output_buf; + single_source_upperbound_dijkstra(source,delta,&pmap,&dmap); + write_result_binary(oa,source,pmap,dmap); } - // Get successors for each node reached - DEBUG (2) std::cout << "Find nodes in distance # "<< nodesInDistance.size() <<'\n'; - std::vector successors = - get_successors(nodesInDistance, predecessors_map); - double cost; - int edge_id; - int k = 0; - vertex_descriptor node; - std::stringstream node_output_buf; - while (k < nodesInDistance.size()) { - node = nodesInDistance[k]; - if (source != node) { - // The cost is need to identify the edge ID - cost = distances_map[successors[k]] - distances_map[source]; - edge_id = get_edge_id(source, successors[k], cost); - stream << vertex_id_vec[source] << ";" << vertex_id_vec[node] << ";" << vertex_id_vec[successors[k]] << ";" - << vertex_id_vec[predecessors_map[node]] << ";" << edge_id << ";" << distances_map[node] - << "\n"; - // stream << source << ";" << node << ";" << successors[k] << ";" - // << predecessors_map[node] << ";" << edge_id << ";" << distances_map[node] - // << "\n"; - } - ++k; + } + } else { + myfile << "source;target;next_n;prev_n;next_e;distance\n"; + int progress = 0; + #pragma omp parallel + { + #pragma omp for + for(int source = 0; source < num_vertices; ++source) { + ++progress; + if (progress % step_size == 0) { + SPDLOG_INFO("Progress {} / {}", progress, num_vertices); + } + PredecessorMap pmap; + DistanceMap dmap; + std::stringstream node_output_buf; + single_source_upperbound_dijkstra(source,delta,&pmap,&dmap); + write_result_csv(myfile,source,pmap,dmap); } - DEBUG (2) std::cout << "Clean examined vertices"<< examined_vertices.size() <<'\n'; - clean_distances_predecessors(); - }; - void driving_distance_binary(const vertex_descriptor& source, double delta, boost::archive::binary_oarchive& oa) { - DEBUG (2) std::cout << "Debug progress source " << source << '\n'; - std::deque nodesInDistance; - examined_vertices.push_back(source); - double inf = std::numeric_limits::max(); - distances_map[source]=0; - try { - // This part to be fixed - dijkstra_shortest_paths_no_color_map_no_init( - g, - source, - make_iterator_property_map(predecessors_map.begin(),get(boost::vertex_index, g),predecessors_map[0]), - make_iterator_property_map(distances_map.begin(),get(boost::vertex_index, g),distances_map[0]), - get(&Edge_Property::length, g), - get(boost::vertex_index, g), - std::less(), //DistanceCompare distance_compare, - boost::closed_plus(inf), - inf, - 0, - driving_distance_visitor( - delta, nodesInDistance, distances_map, examined_vertices - ) - ); - } catch (found_goals& goal) { - //std::cout << "Found goals" << '\n'; - } - // Get successors for each node reached - DEBUG (2) std::cout << "Find nodes in distance # "<< nodesInDistance.size() <<'\n'; - std::vector successors =get_successors(nodesInDistance, predecessors_map); - double cost; - int edge_id; - int k = 0; - vertex_descriptor node; - while (k < nodesInDistance.size()) { - node = nodesInDistance[k]; - if (source != node) { - // The cost is need to identify the edge ID - cost = distances_map[successors[k]] - distances_map[source]; - edge_id = get_edge_id(source, successors[k], cost); - oa << vertex_id_vec[source]; - oa << vertex_id_vec[node]; - oa << vertex_id_vec[successors[k]]; - oa << vertex_id_vec[predecessors_map[node]]; - oa << edge_id; - oa << distances_map[node]; - } - ++k; - } - DEBUG (2) std::cout << "Clean examined vertices"<< examined_vertices.size() <<'\n'; - clean_distances_predecessors(); - }; - /* - Clean the distance map and predecessor map - */ - void initialize_distances_predecessors(){ - // Need initialization - predecessors_map= std::vector(num_vertices); - distances_map = std::vector(num_vertices); - for (int i = 0; i < num_vertices; ++i) { - distances_map[i] = std::numeric_limits::max(); - predecessors_map[i] = i; + } + } + myfile.close(); + } + + /** + * Write the result of routing from a single source node + * @param stream output stream + * @param s source node + * @param pmap predecessor map + * @param dmap distance map + */ + void write_result_csv(std::ostream& stream, NodeIndex s, + PredecessorMap &pmap, DistanceMap &dmap){ + NodeIDVec &node_id_vec = network->get_node_id_vec(); + std::vector source_map; + for (auto iter = pmap.begin(); iter!=pmap.end(); ++iter) { + NodeIndex cur_node = iter->first; + if (cur_node!=s) { + NodeIndex prev_node = iter->second; + NodeIndex v = cur_node; + NodeIndex u; + while ((u = pmap[v]) != s) { + v = u; } + NodeIndex successor = v; + // Write the result to source map + double cost = dmap[successor]; + EdgeIndex edge_index = get_edge_index(s, successor, cost); + source_map.push_back( + {s, + cur_node, + successor, + prev_node, + edge_index, + dmap[cur_node], + nullptr}); + } + } + #pragma omp critical + for (Record &r:source_map) { + stream << r.source<<";" + << r.target<<";" + << r.first_n<<";" + << r.prev_n<<";" + << r.next_e<<";" + << r.cost<<"\n"; + } + } + /** + * Write the result of routing from a single source node in + * binary format + * + * @param stream output stream + * @param s source node + * @param pmap predecessor map + * @param dmap distance map + */ + void write_result_binary(boost::archive::binary_oarchive& stream, + NodeIndex s, + PredecessorMap &pmap, + DistanceMap &dmap){ + NodeIDVec &node_id_vec = network->get_node_id_vec(); + std::vector source_map; + for (auto iter = pmap.begin(); iter!=pmap.end(); ++iter) { + NodeIndex cur_node = iter->first; + if (cur_node!=s) { + NodeIndex prev_node = iter->second; + NodeIndex v = cur_node; + NodeIndex u; + // When u=s, v is the next node visited + while ((u = pmap[v]) != s) { + v = u; + } + NodeIndex successor = v; + // Write the result + double cost = dmap[successor]; + EdgeIndex edge_index = get_edge_index(s, successor, cost); + source_map.push_back( + {s, + cur_node, + successor, + prev_node, + edge_index, + dmap[cur_node], + nullptr}); + } + } + #pragma omp critical + for (Record &r:source_map) { + stream << r.source << r.target + << r.first_n << r.prev_n <::max(); - predecessors_map[v] = v; + } + + Graph_T &get_boost_graph(){ + return g; + } + + Network *get_network(){ + return network; + } + + unsigned int get_num_vertices(){ + return num_vertices; + } + + EdgeIndex get_edge_index(NodeIndex source, NodeIndex target, + double dist) { + EdgeDescriptor e; + OutEdgeIterator out_i, out_end; + bool found =false; + for (boost::tie(out_i, out_end) = boost::out_edges(source, g); + out_i != out_end; ++out_i) { + e = *out_i; + if (target == boost::target(e, g)) { + found = true; + if (abs(g[e].length - dist)<=DOUBLE_MIN) { + break; } - examined_vertices.clear(); - // Clear the examined vertices - }; - static constexpr double DOUBLE_MIN = 1.e-6; // This is used for comparing double values - // Two maps record the routing output - std::vector predecessors_map; - // a list of costs stored for one node to all nodes in the graph - std::vector distances_map; - std::vector vertex_id_vec; // stores the external ID of each vertex in G - std::vector examined_vertices; // Nodes whose distance in the dist_map is updated. - int num_vertices=0; -}; // NetworkGraph + } + } + if (found) return g[e].index; + SPDLOG_ERROR( + "Edge not found from source {} to target {} dist {}", + network->get_node_id(source), + network->get_node_id(target), dist); + return -1; + } + +private: + Graph_T g; + static constexpr double DOUBLE_MIN = 1.e-6; + Network *network; + unsigned int num_vertices=0; +}; // NetworkGraph } // MM #endif /* MM_NETWORK_GRAPH_HPP */ diff --git a/src/network_graph_omp.hpp b/src/network_graph_omp.hpp deleted file mode 100644 index f371900..0000000 --- a/src/network_graph_omp.hpp +++ /dev/null @@ -1,355 +0,0 @@ -/** - * Content - * An optimizated network graph class - * - * The optimization is achieved by recording the output - * of routing in two variables predecessor map and distance map - * and regularly updating and cleaning them. - * - * It avoids intialization of distances and predecessors vector - * in each iteration of the driving distance calculation. With a - * large road network, a small proportion of their nodes are visited - * in the precomputation. 2018.03.09 - * - * Add a property map for vertices in the graph to store discontinuous ID - * for nodes. - * - * @author: Can Yang - * @version: 2018.03.09 - */ -#ifndef MM_NETWORK_GRAPH_OMP_HPP -#define MM_NETWORK_GRAPH_OMP_HPP -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "bgl_driving_dist.hpp" -#include -#include -#include "types.hpp" -#include "reader.hpp" -#include "float.h" -#include "network.hpp" -#include -#include // std::reverse -#include -#include // Binary output of UBODT -namespace MM { -class NetworkGraphOmp -{ -public: - // A infinity value used in the routing algorithm - static constexpr double DIST_NOT_FOUND = DBL_MAX; - /** - * Construct a network graph from a network object - */ - NetworkGraphOmp(Network *network){ - std::vector *edges = network->get_edges(); - std::cout << "Construct graph from network edges start" << '\n'; - // Key is the external ID and value is the index of vertice - std::unordered_map vertex_map; - int current_idx = -1; - edge_descriptor e; - bool inserted; - g = Graph_T(); //18 - int N = edges->size(); - int source_idx = 0; - int target_idx = 0; - // printf("Network edges :%d \n", N); - for (int i = 0; i < N; ++i) { - Edge &network_edge = (*edges)[i]; - auto search = vertex_map.find(network_edge.source); - // Search for source node idx - if (search != vertex_map.end()) { - // A node exists already - source_idx = search->second; - } else { - // A new node is found - ++current_idx; - vertex_map.insert({network_edge.source, current_idx}); - source_idx = current_idx; - vertex_id_vec.push_back(network_edge.source); - }; - // Search for target node idx - search = vertex_map.find(network_edge.target); - if (search != vertex_map.end()) { - // A node exists already - target_idx = search->second; - } else { - // A new node is found - ++current_idx; - vertex_map.insert({network_edge.target, current_idx}); - target_idx = current_idx; - vertex_id_vec.push_back(network_edge.target); - }; - // boost::tie(e, inserted) = add_edge(network_edge.source, network_edge.target, g); - boost::tie(e, inserted) = add_edge(source_idx, target_idx, g); - // id is the FID read, id_attr is the external property in SHP - g[e].id = network_edge.id; - g[e].length = network_edge.length; - //// printf( "Edge read %d,%d,%d,%lf\n",network_edge.id,network_edge.source,network_edge.target,network_edge.length); - } - num_vertices = boost::num_vertices(g); - std::cout << "Graph nodes " << num_vertices << '\n'; - int num_threads = omp_get_num_procs(); - std::cout << "Number of thread available is " << num_threads << '\n'; - // initialize_distances_predecessors(num_threads); - std::cout << "Construct graph from network edges end" << '\n'; - }; - - /** - * Precompute an UBODT with delta and save it to the file - * @param filename [description] - * @param delta [description] - */ - void precompute_ubodt(const std::string &filename, double delta, bool binary=true) { - if (binary){ - precompute_ubodt_binary(filename,delta); - } else { - precompute_ubodt_csv(filename,delta); - } - }; - - /** - * Precompute an UBODT with delta and save it to the file - * @param delta [description] - */ - void precompute_ubodt_csv(const std::string &filename, double delta) { - int step_size = num_vertices / 10; - if (step_size < 10) step_size = 10; - std::ofstream m_fstream(filename); - std::cout << "Start to generate UBODT CSV file with delta " << delta << '\n'; - m_fstream << "source;target;next_n;prev_n;next_e;distance\n"; - int progress = 0; - // if (K>50000){ - // K =50000; - // } - // std::cout << "Start to generate UBODT with k " << K << '\n'; - #pragma omp parallel - { - double thread_start_time = omp_get_wtime(); - // The copy is not complete here - // boost::copy_graph(grid, graph, boost::vertex_copy(detail::grid_to_graph_vertex_copier(grid, graph)) - // .edge_copy(detail::grid_to_graph_edge_copier())); - // Graph_T g; - // boost::copy_graph(mg, g); - std::vector predecessors_map(num_vertices); - std::vector distances_map(num_vertices); - for (int i = 0; i < num_vertices; ++i) { - distances_map[i] = std::numeric_limits::max(); - predecessors_map[i] = i; - } - std::vector examined_vertices; // Nodes whose distance in the dist_map is updated. - int thread_process_count=0; - // If buf placed here, then the result almost doubles - // Position 1 - // std::stringstream node_output_buf; - #pragma omp for - for (int source = 0; source < num_vertices; ++source) { - std::vector nodesInDistance; - examined_vertices.push_back(source); - double inf = std::numeric_limits::max(); - distances_map[source] = 0; - // make_iterator_property_map maps the vertex indices vector to predecessors. - boost::dijkstra_shortest_paths_upperbound(g, - source, - make_iterator_property_map(predecessors_map.begin(), get(boost::vertex_index, g), predecessors_map[0]), - make_iterator_property_map(distances_map.begin(), get(boost::vertex_index, g), distances_map[0]), - get(&Edge_Property::length, g), - get(boost::vertex_index, g), - std::less(), //DistanceCompare distance_compare, - boost::closed_plus(inf), - inf, - 0, delta, nodesInDistance,examined_vertices - ); - std::vector successors = get_successors(nodesInDistance, predecessors_map); - double cost; - int edge_id; - int k = 0; - vertex_descriptor node; - std::stringstream node_output_buf; - // Position 2 - while (k < nodesInDistance.size()) { - node = nodesInDistance[k]; - if (source != node) { - cost = distances_map[successors[k]] - distances_map[source]; - edge_id = get_edge_id(source, successors[k], cost); - node_output_buf << vertex_id_vec[source]<< ";" << vertex_id_vec[node] - << ";" << vertex_id_vec[successors[k]] << ";" << vertex_id_vec[predecessors_map[node]] - << ";" << edge_id << ";" << distances_map[node] << "\n"; - } - ++k; - } - ++progress; - if (progress % step_size == 0) { - printf("Progress %d / %d \n",progress, num_vertices); - } - // Clean the result - int N = examined_vertices.size(); - for (int i = 0; i < N; ++i) { - vertex_descriptor v = examined_vertices[i]; - distances_map[v] = std::numeric_limits::max(); - predecessors_map[v] = v; - } - examined_vertices.clear(); - #pragma omp critical - m_fstream << node_output_buf.str(); - } // end of omp for - } // end of omp parallel - m_fstream.close(); - }; - /** - * Precompute an UBODT with delta and save it to the file - * @param delta [description] - */ - void precompute_ubodt_binary(const std::string &filename, double delta) { - int step_size = num_vertices / 10; - if (step_size < 10) step_size = 10; - std::ofstream myfile(filename); - boost::archive::binary_oarchive oa(myfile); - std::cout << "Start to generate UBODT binary file with delta " << delta << '\n'; - int progress = 0; - #pragma omp parallel - { - double thread_start_time = omp_get_wtime(); - // The copy is not complete here - // boost::copy_graph(grid, graph, boost::vertex_copy(detail::grid_to_graph_vertex_copier(grid, graph)) - // .edge_copy(detail::grid_to_graph_edge_copier())); - // Graph_T g; - // boost::copy_graph(mg, g); - std::vector predecessors_map(num_vertices); - std::vector distances_map(num_vertices); - for (int i = 0; i < num_vertices; ++i) { - distances_map[i] = std::numeric_limits::max(); - predecessors_map[i] = i; - } - std::vector examined_vertices; // Nodes whose distance in the dist_map is updated. - int thread_process_count=0; - // If buf placed here, then the result almost doubles - // Position 1 - #pragma omp for - for (int source = 0; source < num_vertices; ++source) { - std::vector nodesInDistance; - examined_vertices.push_back(source); - double inf = std::numeric_limits::max(); - distances_map[source] = 0; - // make_iterator_property_map maps the vertex indices vector to predecessors. - boost::dijkstra_shortest_paths_upperbound(g, - source, - make_iterator_property_map(predecessors_map.begin(), get(boost::vertex_index, g), predecessors_map[0]), - make_iterator_property_map(distances_map.begin(), get(boost::vertex_index, g), distances_map[0]), - get(&Edge_Property::length, g), - get(boost::vertex_index, g), - std::less(), //DistanceCompare distance_compare, - boost::closed_plus(inf), - inf, - 0, delta, nodesInDistance,examined_vertices - ); - std::vector successors = get_successors(nodesInDistance, predecessors_map); - double cost; - int edge_id; - int k = 0; - vertex_descriptor node; - std::vector source_map; - // Position 2 - while (k < nodesInDistance.size()) { - node = nodesInDistance[k]; - if (source != node) { - cost = distances_map[successors[k]] - distances_map[source]; - edge_id = get_edge_id(source, successors[k], cost); - source_map.push_back({vertex_id_vec[source],vertex_id_vec[node] - ,vertex_id_vec[successors[k]], vertex_id_vec[predecessors_map[node]] - , edge_id, distances_map[node],nullptr}); - } - ++k; - } - ++progress; - if (progress % step_size == 0) { - printf("Progress %d / %d \n",progress, num_vertices); - } - // Clean the result - int N = examined_vertices.size(); - for (int i = 0; i < N; ++i) { - vertex_descriptor v = examined_vertices[i]; - distances_map[v] = std::numeric_limits::max(); - predecessors_map[v] = v; - } - examined_vertices.clear(); - #pragma omp critical - for (Record &r:source_map){ - oa << r.source << r.target << r.first_n < Graph_T; - typedef Graph_T::vertex_descriptor vertex_descriptor; - typedef Graph_T::edge_descriptor edge_descriptor; - typedef boost::graph_traits::vertex_iterator vertex_iterator; - typedef boost::graph_traits::out_edge_iterator out_edge_iterator; - - Graph_T g; // The member storing a boost graph - /** - * Find the edge ID given a pair of nodes and its cost - */ - int get_edge_id(vertex_descriptor source, vertex_descriptor target, double cost) { - edge_descriptor e; - out_edge_iterator 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; // Can we directly get edge id here or latter from the graph - if (target == boost::target(e, g) && (g[e].length - cost <= DOUBLE_MIN)) { - return g[e].id; - } - } - std::cout << "Edge not found for source " << source << " target " << target - << " cost " << cost << '\n'; - return -1; - }; - - /** - * Get the successors (next node visited) for each node in a - * shortest path tree defined by a deque and a predecessor vector - */ - std::vector get_successors(std::vector &nodesInDistance, std::vector& predecessors) { - int N = nodesInDistance.size(); - std::vector successors = std::vector(N); - int i; - vertex_descriptor u, v; - vertex_descriptor source = nodesInDistance[0];// source node - for (i = 0; i < N; ++i) { - v = nodesInDistance[i]; - while ((u = predecessors[v]) != source) { - v = u; - } - successors[i] = v; - } - return successors; - }; - static constexpr double DOUBLE_MIN = 1.e-6; // This is used for comparing double values - // Two maps record the routing output - // The _pool variables are used by OpenMP - // std::vector> predecessors_map_pool; - // a list of costs stored for one node to all nodes in the graph - // std::vector> distances_map_pool; - std::vector vertex_id_vec; // stores the external ID of each vertex in G - // std::vector> examined_vertices_pool; // Nodes whose distance in the dist_map is updated. - //std::vector stream_pool; // Nodes whose distance in the dist_map is updated. - int num_vertices = 0; -}; // NetworkGraphOmp -} // MM -#endif /* MM_NETWORK_GRAPH_OPT_OMP_HPP */ diff --git a/src/python_types.hpp b/src/python_types.hpp deleted file mode 100644 index e170416..0000000 --- a/src/python_types.hpp +++ /dev/null @@ -1,57 +0,0 @@ -/** - * Content - * Definition of Data types used in the FMM python extension - * - * @author: Can Yang - * @version: 2019.06.14 - */ - -#ifndef MM_PYTHON_TYPES_HPP -#define MM_PYTHON_TYPES_HPP - -#include -#include -#include "geometry_type.hpp" - -namespace MM { - - /** - * MatchResult used for communicating with Python - */ - struct MatchResult{ - std::vector opath; // optimal path - std::vector cpath; - std::string mgeom; - std::string pgeom; - }; - - /** - * Candidate class, used for verifying the result in Python - */ - struct CandidateElement { - int index; // point index in trajectory - int edge_id; // edge id - int source; - int target; - double distance; // distance to that edge - double length; - double offset; - double ep; // emission probability - }; - - typedef std::vector CandidateSet; - - struct TLElement { - int index; // point index in trajectory - int from_edge; - int to_edge; - double sp_dist; - double eu_dist; - double tp; // transition probability - double e_prob; // emission probability - double cumu_prob; // cumulative emission probability - }; - - typedef std::vector TransitionLattice; -} -#endif /* MM_PYTHON_TYPES_HPP */ diff --git a/src/reader.hpp b/src/reader.hpp index 0149a0b..e14dccb 100644 --- a/src/reader.hpp +++ b/src/reader.hpp @@ -4,20 +4,22 @@ * the standard shapefile reader in GDAL. * * @author: Can Yang - * @version: 2017.11.11 + * @version: 2020.01.31 */ #ifndef MM_READER_HPP /* Currently not used */ #define MM_READER_HPP + #include #include #include #include -#include "multilevel_debug.h" + +#include "debug.h" #include "types.hpp" #include "gps.hpp" #include "util.hpp" -#include + namespace MM { @@ -48,43 +50,39 @@ class TrajectoryReader */ TrajectoryReader(const std::string & filename,const std::string & id_name) { - std::cout<<"Reading meta data of GPS trajectories from: " << filename << '\n'; + std::cout<<"Reading meta data of GPS trajectories from: "<GetLayer(0); _cursor=0; - // Get the number of features first OGRFeatureDefn *ogrFDefn = ogrlayer->GetLayerDefn(); - // This should be a local field rather than a new variable id_idx=ogrFDefn->GetFieldIndex(id_name.c_str()); NUM_FEATURES= ogrlayer->GetFeatureCount(); if (id_idx<0) { - std::cout<< "ERROR: id column not found with "<GetGeomType()) != wkbLineString) { - std::cout<GetGeomType())<<'\n'; - std::cout<GetGeomType())); GDALClose( poDS ); - std::cout<<"Program stop"<< '\n'; std::exit(EXIT_FAILURE); } else { - std::cout<< "\tGeometry type is " << - OGRGeometryTypeToName(ogrFDefn->GetGeomType())<<'\n'; + SPDLOG_INFO("Geometry type is {}", + OGRGeometryTypeToName(ogrFDefn->GetGeomType())); } - std::cout<<" Index of ID column: " << id_idx<< '\n'; - std::cout<<" Total number of trajectories: " << NUM_FEATURES << '\n'; - std::cout<<"Finish reading meta data" << '\n'; + SPDLOG_INFO("Index of id column {}",id_idx); + SPDLOG_INFO("Total number of trajectories {}",NUM_FEATURES); + SPDLOG_INFO("Finish reading meta data"); }; // If there are still features not read bool has_next_feature() @@ -96,28 +94,26 @@ class TrajectoryReader { OGRFeature *ogrFeature =ogrlayer->GetNextFeature(); int trid = ogrFeature->GetFieldAsInteger(id_idx); - DEBUG(2) std::cout<<"Read trajectory id : "<GetGeometryRef(); - LineString *linestring = ogr2linestring((OGRLineString*) rawgeometry); + LineString linestring = ogr2linestring((OGRLineString*) rawgeometry); OGRFeature::DestroyFeature(ogrFeature); ++_cursor; - return Trajectory(trid,linestring); + return Trajectory{trid,linestring}; }; // Read the next N trajectories in the shapefile std::vector read_next_N_trajectories(int N=30000) { int trajectories_size = NUM_FEATURES-_cursor trajectories(trajectories_size); + std::vector trajectories; int i=0; while(iGetNextFeature(); int trid = ogrFeature->GetFieldAsInteger(id_idx); OGRGeometry *rawgeometry = ogrFeature->GetGeometryRef(); - LineString *linestring = ogr2linestring((OGRLineString*) rawgeometry); + LineString linestring = ogr2linestring((OGRLineString*) rawgeometry); OGRFeature::DestroyFeature(ogrFeature); - trajectories[i].id = trid; - trajectories[i].geom = linestring; + trajectories.push_back({trid,linestring}); ++i; } _cursor+=trajectories_size; @@ -128,17 +124,16 @@ class TrajectoryReader std::vector read_all_trajectories() { std::cout<<"\t Read all trajectoires" << '\n'; - std::vector trajectories(NUM_FEATURES); + std::vector trajectories; int i=0; while(iGetNextFeature(); int trid = ogrFeature->GetFieldAsInteger(id_idx); OGRGeometry *rawgeometry = ogrFeature->GetGeometryRef(); - LineString *linestring = ogr2linestring((OGRLineString*) rawgeometry); + LineString linestring = ogr2linestring((OGRLineString*) rawgeometry); OGRFeature::DestroyFeature(ogrFeature); - trajectories[i].id = trid; - trajectories[i].geom = linestring; + trajectories.push_back({trid,linestring}); ++i; } std::cout<<"\t Read trajectory set size : "<< i <<'\n'; @@ -157,7 +152,7 @@ class TrajectoryReader int NUM_FEATURES=0; int id_idx; // Index of the id column in shapefile int _cursor=0; // Keep record of current features read - GDALDataset *poDS; // GDAL 2.1.0 + GDALDataset *poDS; OGRLayer *ogrlayer; }; // TrajectoryReader } // IO diff --git a/src/transition_graph.hpp b/src/transition_graph.hpp index 2d931fb..cbb6d3d 100644 --- a/src/transition_graph.hpp +++ b/src/transition_graph.hpp @@ -6,302 +6,236 @@ * Viterbi algorithm is implemented. * * @author: Can Yang - * @version: 2017.11.11 + * @version: 2020.01.31 */ #ifndef MM_TRANSITION_GRAPH_HPP #define MM_TRANSITION_GRAPH_HPP + +#include + #include "types.hpp" #include "network.hpp" #include "ubodt.hpp" -#include "float.h" -#include "multilevel_debug.h" -#include "python_types.hpp" +#include "debug.h" namespace MM { class TransitionGraph { public: - // static constexpr float DISTANCE_NOT_FOUND= 5000; // This is the value returned as the SP distance if it is not found in UBODT - /** - * Constructor of a TransitionGraph - * @param traj_candidates: a variational 2D vector - * of candidates - * @param traj: a pointer to raw trajectory - * @param ubodt: a pointer to UBODT table - */ - TransitionGraph(Traj_Candidates *traj_candidates,LineString *traj,UBODT *ubodt, double delta = 5000): - m_traj_candidates(traj_candidates), - m_traj(traj), - m_ubodt(ubodt), - eu_distances(cal_eu_dist(traj)), - DISTANCE_NOT_FOUND(delta) - {}; - /** - * Viterbi algorithm, infer an optimal path in the transition - * graph - * - * @param pf, penalty factor - * @return O_Path, a optimal path containing candidates - * matched for each point in a trajectory. In case that no - * path is found, nullptr is returned. - */ - O_Path *viterbi(double pf=0) + /** + * Constructor of a TransitionGraph + * @param traj_candidates: a variational 2D vector + * of candidates + * @param traj: raw trajectory + * @param ubodt: UBODT table + */ + TransitionGraph(Traj_Candidates *traj_candidates, const LineString &traj, + UBODT &ubodt, double delta = 5000) : + m_traj_candidates(traj_candidates), + m_traj(traj), + m_ubodt(ubodt), + eu_distances(cal_eu_dist(traj)), + DISTANCE_NOT_FOUND(delta){ + }; + /** + * Viterbi algorithm, infer an optimal path in the transition + * graph + * + * @param pf, penalty factor + * @return O_Path, a optimal path containing candidates + * matched for each point in a trajectory. In case that no + * path is found, an empty path is returned. + */ + O_Path viterbi(double pf=0) + { + O_Path opath; + if (m_traj_candidates->empty()) return opath; + 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()) { - OPI_DEBUG(2) std::cout<<"-----------------------"<<'\n'; - OPI_DEBUG(2) std::cout<<"Viterbi start!"<<'\n'; - if (m_traj_candidates->empty()) return nullptr; - int N = m_traj_candidates->size(); - O_Path *opt_path = new O_Path(N); - /* 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; - } - /* Updating the cumu probabilities of subsequent layers */ - Traj_Candidates::iterator csb = m_traj_candidates->begin(); - ++csb; - OPI_DEBUG(2) std::cout<<"Start to update cost in transition graph"<<'\n'; - OPI_DEBUG(2) std::cout<<"step;from;to;sp;eu;tran_prob;e_prob;cumu_prob"<<'\n'; - while (csb != m_traj_candidates->end()) + ca->cumu_prob = ca->obs_prob; + ++ca; + } + /* 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[ + std::distance(m_traj_candidates->begin(),csa)]; + while (ca != csa->end()) + { + Point_Candidates::iterator cb = csb->begin(); + while (cb != csb->end()) { - Point_Candidates::iterator ca = csa->begin(); - double eu_dist=eu_distances[std::distance(m_traj_candidates->begin(),csa)]; - OPI_DEBUG(2) std::cout<<"eu_dist calculated"<<'\n'; - while (ca != csa->end()) - { - Point_Candidates::iterator cb = csb->begin(); - while (cb != csb->end()) - { - // OPI_DEBUG(3) std::cout<<"Iterating cb start"<<'\n'; - // int step =std::distance(m_traj_candidates->begin(),csa); // The problem seems to be here - // Calculate transition probability - double sp_dist = get_sp_dist_penalized(ca,cb,pf); - // OPI_DEBUG(3) std::cout<<"sp_dist calculated"<<'\n'; - /* - 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; - } - // OPI_DEBUG(3) std::cout<<"tran_prob calculated"<<'\n'; - 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; // update the SP distance to previous point - } - // OPI_DEBUG(3) std::cout<edge->id_attr<<";"<edge->id_attr<<";"<obs_prob<<";"<cumu_prob + tran_prob * cb->obs_prob<<'\n'; - ++cb; - // OPI_DEBUG(3) std::cout<<"Iterating cb end"<<'\n'; - } - ++ca; - } - ++csa; - ++csb; - } // End of calculating transition probability + // 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; + } - // Back track to find optimal path - OPI_DEBUG(2) std::cout<<"Find last optimal candidate"<<'\n'; - Candidate *track_cand=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 - ) - { - // 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); - } - } - OPI_DEBUG(2) std::cout<<"Back tracking"<<'\n'; - int i = N-1; - (*opt_path)[i]=track_cand; - OPI_DEBUG(2) std::cout<<"Optimal Path candidate index "<edge->id_attr<<'\n'; - // Iterate from tail to head to assign path - while ((track_cand=track_cand->prev)!=NULL) - { - (*opt_path)[i-1]=track_cand; - OPI_DEBUG(2) std::cout<<"Optimal Path candidate index "<< i-1 <<" edge_id "<edge->id_attr<<'\n'; - --i; + 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; } - OPI_DEBUG(2) std::cout<<"Viterbi ends"<<'\n'; - return opt_path; - }; + ++ca; + } + ++csa; + ++csb; + } - /** - * Generate transition lattice for the transition graph, used in Python extension - * for verification of the result - */ - TransitionLattice generate_transition_lattice(){ - TransitionLattice tl; - if (m_traj_candidates->empty()) return tl; - 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; - } - /* 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[std::distance(m_traj_candidates->begin(),csa)]; - while (ca != csa->end()) - { - Point_Candidates::iterator cb = csb->begin(); - while (cb != csb->end()) - { - // OPI_DEBUG(3) std::cout<<"Iterating cb start"<<'\n'; - int step =std::distance(m_traj_candidates->begin(),csa); // The problem seems to be here - // Calculate transition probability - double sp_dist = get_sp_dist_penalized(ca,cb,0); - // OPI_DEBUG(3) std::cout<<"sp_dist calculated"<<'\n'; - /* - 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; - } - // OPI_DEBUG(3) std::cout<<"tran_prob calculated"<<'\n'; - 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; // update the SP distance to previous point - } - tl.push_back( - {step,std::atoi(ca->edge->id_attr.c_str()),std::atoi(cb->edge->id_attr.c_str()), - sp_dist,eu_dist,tran_prob,cb->obs_prob,ca->cumu_prob + tran_prob * cb->obs_prob}); - ++cb; - } - ++ca; - } - ++csa; - ++csb; - } // End of calculating transition probability - return tl; - }; + // Back track to find optimal path + Candidate *track_cand=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 + ) + { + // 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); + } + } + int i = N-1; + opath.push_back(track_cand); + // Iterate from tail to head to assign path + while ((track_cand=track_cand->prev)!=NULL) + { + opath.push_back(track_cand); + } + std::reverse(opath.begin(), opath.end()); + return opath; + }; - /** - * Get the shortest path (SP) distance from Candidate ca to cb - * @param ca - * @param cb - * @return the SP from ca to cb - */ - double get_sp_dist(Point_Candidates::iterator& ca,Point_Candidates::iterator& cb) + /** + * Get the shortest path (SP) distance from Candidate ca to cb + * @param ca + * @param cb + * @return the SP from ca to cb + */ + double get_sp_dist(Point_Candidates::iterator& ca, + Point_Candidates::iterator& cb) + { + double sp_dist=0; + // Transition on the same edge + if ( ca->edge->id == cb->edge->id && ca->offset <= cb->offset ) { - double sp_dist=0; - // Transition on the same edge - 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 - { - // No sp path exist from O to D. - Record *r = m_ubodt->look_up(ca->edge->target,cb->edge->source); - sp_dist = r==NULL? DISTANCE_NOT_FOUND:r->cost + ca->edge->length - ca->offset + cb->offset; - } - return sp_dist; - }; - /** - * Get the penalized shortest path (SP) distance from Candidate ca to cb - */ - double get_sp_dist_penalized(Point_Candidates::iterator& ca,Point_Candidates::iterator& cb,double pf) + sp_dist = cb->offset - ca->offset; + } + else if(ca->edge->target == cb->edge->source) { - double sp_dist=0; - // Transition on the same edge - 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 - { - OPI_DEBUG(3) std::cout<<"Query long distance routing started"<<'\n'; - OPI_DEBUG(3) std::cout<<"Looking for path for edges "<edge->id <<" to "<edge->id <<'\n'; - OPI_DEBUG(3) std::cout<<"Looking for path for edges id attr "<edge->id_attr <<" to "<edge->id_attr <<'\n'; - OPI_DEBUG(3) std::cout<<"Looking for path from "<edge->target<<" to "<edge->source<<'\n'; - 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; - // calculate original SP distance - sp_dist = r->cost + ca->edge->length - ca->offset + cb->offset; - // Two penalized cases - if(r->prev_n==cb->edge->target) - { - sp_dist+=pf*cb->edge->length; - } - if(r->first_n==ca->edge->source) - { - sp_dist+=pf*ca->edge->length; - } - OPI_DEBUG(3) std::cout<<"Query long distance routing finished"<<'\n'; - } - return sp_dist; - }; - /** - * Calculate the Euclidean distances of all segments in a linestring - */ - static std::vector cal_eu_dist(LineString *trajectory) + // Transition on the same OD nodes + sp_dist = ca->edge->length - ca->offset + cb->offset; + } + else { - OPI_DEBUG(2) std::cout<<"Calculating lengths of segments"<<'\n'; - int N = trajectory->getNumPoints(); - std::vector lengths(N-1); - double x0 = trajectory->getX(0); - double y0 = trajectory->getY(0); - for(int i = 1; i < N; ++i) - { - double x1 = trajectory->getX(i); - double y1 = trajectory->getY(i); - double dx = x1 - x0; - double dy = y1 - y0; - lengths[i-1]=sqrt(dx * dx + dy * dy); // Not push_back which will insert new elements. - x0 = x1; - y0 = y1; - } - return lengths; - }; + // No sp path exist from O to D. + Record *r = m_ubodt.look_up(ca->edge->target,cb->edge->source); + sp_dist = r==NULL ? DISTANCE_NOT_FOUND : r->cost + + ca->edge->length - ca->offset + cb->offset; + } + return sp_dist; + }; + /** + * Get the penalized shortest path (SP) distance from Candidate ca to cb + */ + double get_sp_dist_penalized(Point_Candidates::iterator& ca, + Point_Candidates::iterator& cb, + double pf) + { + double sp_dist=0; + // Transition on the same edge + 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 = 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; + // calculate original SP distance + sp_dist = r->cost + ca->edge->length - ca->offset + cb->offset; + // Two penalized cases + if(r->prev_n==cb->edge->target) + { + sp_dist+=pf*cb->edge->length; + } + if(r->first_n==ca->edge->source) + { + sp_dist+=pf*ca->edge->length; + } + + } + return sp_dist; + }; + /** + * Calculate the length of all segments in a linestring + */ + static std::vector cal_eu_dist(const LineString &trajectory) + { + int N = trajectory.getNumPoints(); + std::vector lengths(N-1); + double x0 = trajectory.getX(0); + double y0 = trajectory.getY(0); + for(int i = 1; i < N; ++i) + { + double x1 = trajectory.getX(i); + double y1 = trajectory.getY(i); + double dx = x1 - x0; + double dy = y1 - y0; + lengths[i-1]=sqrt(dx * dx + dy * dy); + x0 = x1; + y0 = y1; + } + return lengths; + }; + + Traj_Candidates *get_traj_candidates(){ + return m_traj_candidates; + }; + + std::vector &get_eu_distances(){ + return eu_distances; + } + private: - Traj_Candidates *m_traj_candidates; // a pointer trajectory candidates - LineString *m_traj; // a pointer to GPS trajectory - UBODT *m_ubodt; // UBODT - std::vector eu_distances; // Euclidean distances of segments in the trajectory - float DISTANCE_NOT_FOUND; // This is the value returned as the SP distance if it is not found in UBODT + // a pointer to trajectory candidates + Traj_Candidates *m_traj_candidates; + // reference to GPS trajectory + const LineString &m_traj; + // UBODT + UBODT &m_ubodt; + // Euclidean distances of segments in the trajectory + std::vector eu_distances; + // This is the value returned as the SP distance if it is not found in UBODT + float DISTANCE_NOT_FOUND; }; } #endif /* MM_TRANSITION_GRAPH_ROUTING_HPP */ diff --git a/src/types.hpp b/src/types.hpp index b800647..cc2cd32 100644 --- a/src/types.hpp +++ b/src/types.hpp @@ -3,69 +3,88 @@ * Definition of Data types used in the FMM algorithm * * @author: Can Yang - * @version: 2017.11.11 + * @version: 2020.01.31 */ #ifndef MM_TYPES_HPP #define MM_TYPES_HPP #include -#include +#include #include "geometry_type.hpp" namespace MM { +typedef int NodeID; +typedef int EdgeID; +typedef unsigned int NodeIndex; +typedef unsigned int EdgeIndex; + +typedef std::vector NodeIDVec; +typedef std::unordered_map NodeIndexMap; +typedef std::unordered_map EdgeIndexMap; + struct Edge { - int id; // This is the id, which is continuous distributed - std::string id_attr; // This is the external ID attribute, which does not have to be continuous - int source; // source node ID - int target; // target node ID - double length; // length of the edge polyline - LineString *geom; // a pointer to the edge geometry + // This is the index of an edge, which is continuous [0,N-1] + EdgeIndex index; + // Edge ID, can be discontinuous integers + EdgeID id; + NodeIndex source; // source node index + NodeIndex target; // target node index + double length; // length of the edge polyline + LineString geom; // edge geometry }; struct Candidate { - float offset; // offset distance from the start of polyline to p' - double dist; // distance from original point p to map matched point p' - double obs_prob; // this is the emission probability - Edge *edge; // candidate edge - Candidate* prev; // optimal previous candidate used in Viterbi algorithm - float cumu_prob; // used in Viterbi, initialized to be 0 - float sp_dist; // sp distance to previous point, initialized to be 0 + double offset; // offset distance from the start of polyline to p' + double dist; // distance from original point p to map matched point p' + double obs_prob; // this is the emission probability + Edge *edge; // candidate edge + Candidate* prev; // optimal previous candidate used in Viterbi algorithm + double cumu_prob; // used in Viterbi, initialized to be 0 + double sp_dist; // sp distance to previous point, initialized to be 0 }; -/* Record type in UBODT */ +// Record type in UBODT +// Every column stores index rather than ID. +// For verification of the result, run ubodt to generate the ID map. struct Record { - int source; - int target; - int first_n; // next_n in the paper - int prev_n; - int next_e; - double cost; - Record *next; // the next Record used in Hashtable + NodeIndex source; + NodeIndex target; + NodeIndex first_n; // next_n in the paper + NodeIndex prev_n; + EdgeIndex next_e; + double cost; + Record *next; // the next Record used in Hashtable }; /* Transitiong graph*/ - -typedef std::vector Point_Candidates; // candidates of a point -typedef std::vector Traj_Candidates; // candidates of a trajectory +// candidates of a point +typedef std::vector Point_Candidates; +// candidates of a trajectory +typedef std::vector Traj_Candidates; /* Result of map matching */ // Optimal path containing candidates matched to each point in a trajectory typedef std::vector O_Path; -// Complete path, a contiguous sequence of edges traversed -typedef std::vector C_Path; +// Complete path, a sequence of spatially contiguous edges traversed +typedef std::vector C_Path; + +// Complete path, a sequence of spatially contiguous edges traversed, +// with edge index stored +typedef std::vector C_PathIndex; -// The traversed path stores also the location of GPS point inside the C_Path, thus -// edges traversed between two GPS observations can be found. -struct T_Path{ - C_Path cpath; - std::vector indices; +// The traversed path stores also the location of GPS point +// inside the C_Path, thus edges traversed between two GPS +// observations can be accessed. +struct T_Path { + C_Path cpath; + std::vector indices; }; } diff --git a/src/ubodt.hpp b/src/ubodt.hpp index 6dc3c1f..0d77425 100644 --- a/src/ubodt.hpp +++ b/src/ubodt.hpp @@ -1,10 +1,10 @@ /** * Content - * Definition of UBODT, which is a hashtable containing the precomputed shortest path - * routing results. + * Definition of UBODT, which is a hashtable containing the precomputed + * shortest path routing results. * * @author: Can Yang - * @version: 2017.11.11 + * @version: 2020.01.31 */ #ifndef MM_UBODT_HPP @@ -14,222 +14,218 @@ #include #include #include -#include /* used for statistics */ +#include #include #include + #include "types.hpp" -#include "multilevel_debug.h" -// #include +#include "network.hpp" +#include "debug.h" + namespace MM { class UBODT { public: - /** - * Constructor of UBODT - * @param buckets: the number of buckets in the hashtable - * @param multipler: multiplier to calculate hashcode of an OD pair as n_o * multiplier + n_d - */ - UBODT(int buckets_arg,int multiplier_arg):buckets(buckets_arg),multiplier(multiplier_arg) { - std::cout<<"Creating UBODT with buckets "<< buckets << " muliplier "<< multiplier <<"\n"; - hashtable = (Record **) malloc(sizeof(Record*)*buckets); - /* This initialization is required to later free the memory, to figure out the problem */ - for (int i = 0; i < buckets; i++){ - hashtable[i] = NULL; - } - std::cout<<"Creating UBODT finished\n"; - }; + /** + * Constructor of UBODT + * @param buckets: the number of buckets in the hashtable + * @param multipler: multiplier to calculate hashcode of an OD pair as + * n_o * multiplier + n_d + */ + UBODT(int buckets_arg,int multiplier_arg) : + buckets(buckets_arg),multiplier(multiplier_arg) { + std::cout <<"Creating UBODT with buckets "<< buckets + << " muliplier "<< multiplier <<"\n"; + hashtable = (Record **) malloc(sizeof(Record*)*buckets); + for (int i = 0; i < buckets; i++) { + hashtable[i] = NULL; + } + std::cout<<"Creating UBODT finished\n"; + }; - Record *look_up(int source,int target) + ~UBODT(){ + /* Clean hashtable */ + std::cout<< "Clean UBODT" << '\n'; + int i; + for (i=0; inext; + free(curr); + } + } + // Destory hash table pointer + free(hashtable); + std::cout<< "Clean UBODT finished" << '\n'; + }; + + Record *look_up(NodeIndex source,NodeIndex target) + { + unsigned int h = cal_bucket_index(source,target); + Record *r = hashtable[h]; + while (r != NULL) { - //int h = (source*multiplier+target)%buckets; - int h = cal_bucket_index(source,target); - Record *r = hashtable[h]; - while (r != NULL) - { - if (r->source==source && r->target==target) - { - return r; - } - else - { - r=r->next; - } - } - //printf("Not found s %d t %d h %d\n",source,target,h); NULL will be returned. + if (r->source==source && r->target==target) + { return r; - }; - /** - * Return a shortest path (SP) containing edges from source to target. - * In case that SP is not found, empty is returned. - */ - std::vector look_sp_path(int source,int target){ - CPC_DEBUG(4) std::cout<<"Look shortest path from "<< source <<" to "< edges; - if (source==target) {return edges;} - Record *r=look_up(source,target); - // No transition exist from source to target - if (r==NULL){return edges;} - while(r->first_n!=target){ - edges.push_back(r->next_e); - r=look_up(r->first_n,target); - } - edges.push_back(r->next_e); - return edges; - }; + } + else + { + r=r->next; + } + } + return r; + }; - /** - * Construct the complete path (a vector of edge ID) from an optimal path (a vector of candidates) - * - * @param path, an optimal path - * @return a pointer to a complete path, which should be freed by the caller. If there is a large - * gap in the optimal path implying complete path cannot be found in UBDOT, nullptr is returned - */ - C_Path *construct_complete_path(O_Path *path){ - CPC_DEBUG(2) std::cout<<"-----------------------"<<'\n'; - CPC_DEBUG(2) std::cout<<"Construct complete path"<<'\n'; - if (path==nullptr) return nullptr; - C_Path *edges= new C_Path(); - int N = path->size(); - edges->push_back((*path)[0]->edge->id); - for(int i=0;iedge->id!=b->edge->id) || (a->offset>b->offset)) { - 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){ - CPC_DEBUG(1) std::cout<<"----- Warning: Large gap detected from "<< a->edge->target <<" to "<< b->edge->source <<'\n'; - CPC_DEBUG(1) std::cout<<"----- Construct complete path skipped"<<'\n'; - delete edges; // free the memory of edges - return nullptr; - } - for (int e:segs){ - edges->push_back(e); - } - edges->push_back(b->edge->id); - } - } - CPC_DEBUG(2) std::cout<<"Construct complete path finished "<<'\n'; - return edges; - }; + /** + * Return a shortest path (SP) containing edges from source to target. + * In case that SP is not found, empty is returned. + */ + C_PathIndex look_sp_path(NodeIndex source,NodeIndex target){ + C_PathIndex edges; + if (source==target) {return edges;} + Record *r=look_up(source,target); + // No transition exist from source to target + if (r==NULL) {return edges;} + while(r->first_n!=target) { + edges.push_back(r->next_e); + r=look_up(r->first_n,target); + } + edges.push_back(r->next_e); + return edges; + }; - /** - * Construct a traversed path from the optimal path. - * It is different with cpath in that the edges traversed between consecutive GPS observations are Recorded. - * It returns a traversed path including the cpath and the index of matched edge for each point in the GPS trajectory. - */ - T_Path *construct_traversed_path(O_Path *path){ - CPC_DEBUG(2) std::cout<<"-----------------------"<<'\n'; - CPC_DEBUG(2) std::cout<<"Construct traversed path begin"<<'\n'; - if (path==nullptr) return nullptr; - int N = path->size(); - // T_Path *edges= new T_Path(); - T_Path *t_path = new T_Path(); - t_path->cpath.push_back((*path)[0]->edge->id); - int current_idx = 0; - t_path->indices.push_back(current_idx); - for(int i=0;iedge->id!=b->edge->id) || (a->offset>b->offset)) { - 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){ - CPC_DEBUG(1) std::cout<<"----- Warning: Large gap detected from "<< a->edge->target <<" to "<< b->edge->source <<'\n'; - CPC_DEBUG(1) std::cout<<"----- Construct complete path skipped"<<'\n'; - delete t_path; // free the memory of edges - return nullptr; - } - for (int e:segs){ - t_path->cpath.push_back(e); - ++current_idx; - } - t_path->cpath.push_back(b->edge->id); - ++current_idx; - t_path->indices.push_back(current_idx); - } else { - // b stays on the same edge - t_path->indices.push_back(current_idx); - } + /** + * Construct the complete path (a vector of edge ID) from an optimal path + * (a vector of candidates) + * + * @param path, an optimal path + * @param edges, a vector of edges + * @return a complete path (spatially contiguous). + * If there is a large gap in the optimal + * 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(); } - CPC_DEBUG(2) std::cout<<"Construct traversed path finish"<<'\n'; - return t_path; - }; - /** - * Print statistics of the hashtable to a file - */ - void print_statictics(const char*filename){ - /* - Iterate through all buckets to get statistics - Bucket size, counts - */ - std::map statistics; - for (int i=0;inext; - ++count; - } - statistics[count]=statistics[count]+1; + for (int e:segs) { + cpath.push_back(edges[e].id); } - std::ofstream outputfile(filename); - if (outputfile.is_open()) - { - outputfile<<"BucketElements;Counts\n"; - for (std::map::iterator it=statistics.begin(); it!=statistics.end(); ++it) - outputfile<< it->first << ";" << it->second << '\n'; - outputfile.close(); + cpath.push_back(b->edge->id); + } + } + return cpath; + }; + + /** + * Construct a traversed path from the optimal path. + * It is different with cpath in that the edges traversed between + * consecutive GPS observations are Recorded. + * It returns a traversed path including the cpath and the index of + * matched edge for each point in the GPS trajectory. + */ + T_Path construct_traversed_path(O_Path &path, Network &network){ + 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); + 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(); } - else std::cout << "Unable to write statistics to file"<<'\n'; - }; - double get_delta(){ - return delta; - }; - inline int cal_bucket_index(int source,int target){ - return (source*multiplier+target)%buckets; - }; - // inline int cal_bucket_index(int source,int target){ - // std::size_t seed = source; - // boost::hash_combine(seed, target); - // return seed%buckets; - // }; - ~UBODT(){ - /* Clean hashtable */ - std::cout<< "Clean UBODT" << '\n'; - int i; - for (i=0;inext; // advance head to next element. - free(curr); // delete saved pointer. - } + for (int e:segs) { + t_path.cpath.push_back(edges[e].id); + ++current_idx; } - // Destory hash table pointer - free(hashtable); - std::cout<< "Clean UBODT finished" << '\n'; - }; - // Insert a Record into the hash table - void insert(Record *r) + t_path.cpath.push_back(b->edge->id); + ++current_idx; + t_path.indices.push_back(current_idx); + } else { + // b stays on the same edge + t_path.indices.push_back(current_idx); + } + } + return t_path; + }; + + /** + * Print statistics of the hashtable to a file + */ + void print_statictics(const char*filename){ + std::map statistics; + for (int i=0; inext; + ++count; + } + statistics[count]=statistics[count]+1; + } + std::ofstream outputfile(filename); + if (outputfile.is_open()) { - //int h = (r->source*multiplier+r->target)%buckets ; - int h = cal_bucket_index(r->source,r->target); - r->next = hashtable[h]; - hashtable[h]=r; - if (r->cost > delta) delta = r->cost; - }; + outputfile<<"BucketElements;Counts\n"; + for (std::map::iterator it=statistics.begin(); + it!=statistics.end(); ++it) + outputfile<< it->first << ";" << it->second << '\n'; + outputfile.close(); + } + else std::cout << "Unable to write statistics to file"<<'\n'; + }; + double get_delta(){ + return delta; + }; + inline unsigned int cal_bucket_index(NodeIndex source,NodeIndex target){ + return (source*multiplier+target)%buckets; + }; + + // Insert a Record into the hash table + void insert(Record *r) + { + //int h = (r->source*multiplier+r->target)%buckets ; + int h = cal_bucket_index(r->source,r->target); + r->next = hashtable[h]; + hashtable[h]=r; + if (r->cost > delta) delta = r->cost; + }; + private: - const long long multiplier; // multiplier to get a unique ID - const int buckets; // number of buckets - double delta = 0.0; - //int maxnode=0; - Record** hashtable; + const long long multiplier; // multiplier to get a unique ID + const int buckets; // number of buckets + double delta = 0.0; + //int maxnode=0; + Record** hashtable; }; +// Constant values used in UBODT. + double LOAD_FACTOR = 2.0; int BUFFER_LINE = 1024; @@ -238,39 +234,41 @@ int BUFFER_LINE = 1024; * @Returns the number of rows */ int estimate_ubodt_rows(const std::string &filename){ - struct stat stat_buf; - int rc = stat(filename.c_str(), &stat_buf); - if (rc==0) { - int file_bytes = stat_buf.st_size; - std::cout<<"UBODT file size is "< prime_numbers = { - 5003,10039,20029,50047,100669,200003,500000, - 1000039,2000083,5000101,10000103,20000033}; - int N = prime_numbers.size(); - for (int i=0;i prime_numbers = { + 5003,10039,20029,50047,100669,200003,500000, + 1000039,2000083,5000101,10000103,20000033}; + int N = prime_numbers.size(); + for (int i=0; isource, - &r->target, - &r->first_n, - &r->prev_n, - &r->next_e, - &r->cost - ); - r->next=NULL; - if (NUM_ROWS%progress_step==0) printf("Read rows: %d\n",NUM_ROWS); - /* Insert into the hash table */ - table->insert(r); - }; - fclose(stream); - std::cout<<" Number of rows read " << NUM_ROWS << '\n'; - double lf = NUM_ROWS/(double)buckets; - std::cout<<" Estimated load factor #elements/#tablebuckets "<10) std::cout<<" *** Warning, load factor is too large.\n"; - std::cout<<"Finish reading UBODT.\n"; - return table; + std::cout<<"Reading UBODT file (CSV format) from: " << filename << '\n'; + int rows = estimate_ubodt_rows(filename); + std::cout<<"Estimated rows is : " << rows << '\n'; + int progress_step = rows/10; + if (progress_step < 1) progress_step = 1; + int buckets = find_prime_number(rows/LOAD_FACTOR); + UBODT *table = new UBODT(buckets, multiplier); + FILE* stream = fopen(filename.c_str(), "r"); + int NUM_ROWS = 0; + char line[BUFFER_LINE]; + if(fgets(line, BUFFER_LINE, stream)) { + printf(" Header line skipped.\n"); + }; + while (fgets(line, BUFFER_LINE, stream)) + { + ++NUM_ROWS; + Record *r =(Record *) malloc(sizeof(Record)); + /* Parse line into a Record */ + sscanf( + line,"%d;%d;%d;%d;%d;%lf", + &r->source, + &r->target, + &r->first_n, + &r->prev_n, + &r->next_e, + &r->cost + ); + r->next=NULL; + if (NUM_ROWS%progress_step==0) printf("Read rows: %d\n",NUM_ROWS); + /* Insert into the hash table */ + table->insert(r); + }; + fclose(stream); + std::cout<<" Number of rows read " << NUM_ROWS << '\n'; + double lf = NUM_ROWS/(double)buckets; + std::cout<<" Estimated load factor #elements/#tablebuckets "<10) std::cout<<" *** Warning, load factor is too large.\n"; + std::cout<<"Finish reading UBODT.\n"; + return table; }; /** @@ -324,42 +322,41 @@ UBODT *read_ubodt_csv(const std::string &filename, int multiplier=50000) */ UBODT *read_ubodt_binary(const std::string &filename, int multiplier=50000) { - std::cout<<"Reading UBODT file (binary format) from: " << filename << '\n'; - int rows = estimate_ubodt_rows(filename); - int progress_step = rows/10; - if (progress_step < 1) progress_step = 1; - std::cout<<"Estimated rows is : " << rows << '\n'; - int buckets = find_prime_number(rows/LOAD_FACTOR); - UBODT *table = new UBODT(buckets,multiplier); - int NUM_ROWS = 0; - std::ifstream ifs(filename.c_str()); - // Check byte offset - std::streampos archiveOffset = ifs.tellg(); - std::streampos streamEnd = ifs.seekg(0, std::ios_base::end).tellg(); - ifs.seekg(archiveOffset); - boost::archive::binary_iarchive ia(ifs); - while (ifs.tellg() < streamEnd) - { - ++NUM_ROWS; - Record *r =(Record *) malloc(sizeof(Record)); - ia >> r->source; - ia >> r->target; - ia >> r->first_n; - ia >> r->prev_n; - ia >> r->next_e; - ia >> r->cost; - r->next=NULL; - if (NUM_ROWS%progress_step==0) printf("Read rows: %d\n",NUM_ROWS); - /* Insert into the hash table */ - table->insert(r); - } - ifs.close(); - std::cout<<" Number of rows read " << NUM_ROWS << '\n'; - double lf = NUM_ROWS/(double)buckets; - std::cout<<" Estimated load factor #elements/#tablebuckets "<10) std::cout<<" *** Warning, load factor is too large.\n"; - std::cout<<"Finish reading UBODT.\n"; - return table; + std::cout<<"Reading UBODT file (binary format) from: " << filename << '\n'; + int rows = estimate_ubodt_rows(filename); + int progress_step = rows/10; + if (progress_step < 1) progress_step = 1; + std::cout<<"Estimated rows is : " << rows << '\n'; + int buckets = find_prime_number(rows/LOAD_FACTOR); + UBODT *table = new UBODT(buckets,multiplier); + int NUM_ROWS = 0; + std::ifstream ifs(filename.c_str()); + // Check byte offset + std::streampos archiveOffset = ifs.tellg(); + std::streampos streamEnd = ifs.seekg(0, std::ios_base::end).tellg(); + ifs.seekg(archiveOffset); + boost::archive::binary_iarchive ia(ifs); + while (ifs.tellg() < streamEnd) + { + ++NUM_ROWS; + Record *r =(Record *) malloc(sizeof(Record)); + ia >> r->source; + ia >> r->target; + ia >> r->first_n; + ia >> r->prev_n; + ia >> r->next_e; + ia >> r->cost; + r->next=NULL; + if (NUM_ROWS%progress_step==0) printf("Read rows: %d\n",NUM_ROWS); + table->insert(r); + } + ifs.close(); + std::cout<<" Number of rows read " << NUM_ROWS << '\n'; + double lf = NUM_ROWS/(double)buckets; + std::cout<<" Estimated load factor #elements/#tablebuckets "<10) std::cout<<" *** Warning, load factor is too large.\n"; + std::cout<<"Finish reading UBODT.\n"; + return table; }; } diff --git a/src/util.hpp b/src/util.hpp index b1ba4ab..6f48e8e 100644 --- a/src/util.hpp +++ b/src/util.hpp @@ -3,13 +3,13 @@ * Utility functions * * @author: Can Yang - * @version: 2017.11.11 + * @version: 2020.01.31 */ #ifndef MM_UTIL_HPP #define MM_UTIL_HPP -#include // C++ API for GDAL +#include #include #include #include @@ -19,7 +19,7 @@ #include #include "types.hpp" -#include "multilevel_debug.h" +#include "debug.h" namespace MM { @@ -30,17 +30,46 @@ namespace UTIL */ bool fileExists(const char *filename) { - struct stat buf; - if (stat(filename, &buf) != -1) - { - return true; - } - return false; + struct stat buf; + if (stat(filename, &buf) != -1) + { + return true; + } + return false; }; bool fileExists(std::string &filename) { - return fileExists(filename.c_str()); + return fileExists(filename.c_str()); +}; + +bool folderExists(std::string &folder_name) +{ + if (folder_name.empty()) return true; + struct stat sb; + if (stat(folder_name.c_str(),&sb) == 0 && S_ISDIR(sb.st_mode)) { + return true; + } + return false; +} + +// Check extension of the file, 0 for CSV and 1 for Binary +int get_file_extension(std::string &fn) { + std::string fn_extension = fn.substr(fn.find_last_of(".") + 1); + if (fn_extension == "csv" || fn_extension == "txt") { + return 0; + } else if (fn_extension == "bin" || fn_extension == "binary") { + return 1; + } + return 2; +}; + +std::string get_file_directory(std::string &fn){ + std::size_t found = fn.find_last_of("/"); + if (found!=std::string::npos){ + return fn.substr(0,found); + } + return {}; }; /** @@ -48,85 +77,85 @@ bool fileExists(std::string &filename) * step;offset;distance;edge_id */ void print_traj_candidates(Traj_Candidates &tr_cs) { - std::cout << "step;offset;distance;id;edge_id_attr" << '\n'; - Traj_Candidates::iterator tr_cs_iter; - Point_Candidates::iterator p_cs_iter; - for (tr_cs_iter = tr_cs.begin(); tr_cs_iter != tr_cs.end(); ++tr_cs_iter) { - for (p_cs_iter = tr_cs_iter->begin(); p_cs_iter != tr_cs_iter->end(); ++p_cs_iter) { - std::cout << std::distance(tr_cs.begin(), tr_cs_iter) << ";" << p_cs_iter->offset << ";" << p_cs_iter->dist << ";" << p_cs_iter->edge->id << ";" << p_cs_iter->edge->id_attr<< '\n'; - } + std::cout << "step;offset;distance;id;edge_id_attr" << '\n'; + Traj_Candidates::iterator tr_cs_iter; + Point_Candidates::iterator p_cs_iter; + for (tr_cs_iter = tr_cs.begin(); tr_cs_iter != tr_cs.end(); ++tr_cs_iter) { + for (p_cs_iter = tr_cs_iter->begin(); + p_cs_iter != tr_cs_iter->end(); ++p_cs_iter) { + std::cout << std::distance(tr_cs.begin(), tr_cs_iter) << ";" + << p_cs_iter->offset << ";" + << p_cs_iter->dist << ";" + << p_cs_iter->edge->index + << ";" << p_cs_iter->edge->id<< '\n'; } + } } void print_traj_candidates_summary(Traj_Candidates &tr_cs) { - std::cout << "point_idx;candidate_count" << '\n'; - Traj_Candidates::iterator tr_cs_iter; - Point_Candidates::iterator p_cs_iter; - for (tr_cs_iter = tr_cs.begin(); tr_cs_iter != tr_cs.end(); ++tr_cs_iter) { - std::cout << std::distance(tr_cs.begin(), tr_cs_iter) << ";" << tr_cs_iter->size() << '\n'; - } + std::cout << "point_idx;candidate_count" << '\n'; + Traj_Candidates::iterator tr_cs_iter; + Point_Candidates::iterator p_cs_iter; + for (tr_cs_iter = tr_cs.begin(); tr_cs_iter != tr_cs.end(); ++tr_cs_iter) { + std::cout << std::distance(tr_cs.begin(), tr_cs_iter) << ";" + << tr_cs_iter->size() << '\n'; + } } // Print a complete path void print_c_path(C_Path *c_path_ptr) { - std::cout<<"Complete path elements:"; - int N = c_path_ptr->size(); - for(int i=0; isize(); + for(int i=0; isize() <<" "; - } - std::cout<< '\n'; + Traj_Candidates::iterator tr_cs_iter; + std::cout<<"Summary of transition graph: "<<'\n'; + std::cout<<"Count of candidates"<<'\n'; + for (tr_cs_iter = tr_cs.begin(); tr_cs_iter != tr_cs.end(); ++tr_cs_iter) { + std::cout<< tr_cs_iter->size() <<" "; + } + std::cout<< '\n'; }; /** * Print the OGRLineString in WKT format */ void print_geometry(LineString *geom){ - if (geom==nullptr) { - std::cout<<"Geometry in WKT: NULL"<<'\n'; - return; - } -#ifdef USE_BG_GEOMETRY - std::cout<< geom->exportToWkt()<<'\n'; -#else - char *wkt; - geom->exportToWkt(&wkt); - std::cout<<"Geometry in WKT: "<exportToWkt()<<'\n'; }; - // Get current timestamp std::chrono::time_point get_current_time(){ - return std::chrono::system_clock::now(); + return std::chrono::system_clock::now(); }; // Print a timestamp -void print_time(const std::chrono::time_point &start_time){ - std::time_t start_time_t = std::chrono::system_clock::to_time_t(start_time); - std::cout<<"Time "< &start_time){ + std::time_t start_time_t = std::chrono::system_clock::to_time_t(start_time); + std::cout<<"Time "< &start_time, -const std::chrono::time_point &end_time){ - std::chrono::duration elapsed_seconds = end_time-start_time; - return elapsed_seconds.count(); +double get_duration( + const std::chrono::time_point &start_time, + const std::chrono::time_point &end_time){ + std::chrono::duration elapsed_seconds = end_time-start_time; + return elapsed_seconds.count(); }; } // Util diff --git a/src/writer.hpp b/src/writer.hpp index dbf0085..98e76e0 100644 --- a/src/writer.hpp +++ b/src/writer.hpp @@ -4,15 +4,16 @@ * writing the results. * * @author: Can Yang - * @version: 2017.11.11 + * @version: 2020.01.31 */ -#ifndef MM_WRITER_HPP /* Currently not used */ +#ifndef MM_WRITER_HPP #define MM_WRITER_HPP #include #include + #include "types.hpp" -#include "multilevel_debug.h" +#include "debug.h" #include "config.hpp" #include "network.hpp" @@ -31,312 +32,307 @@ namespace IO { class ResultWriter { public: - /** - * Constructor - * @param result_file, the path to an output file - * @param network_ptr, a pointer to the network - */ - ResultWriter(const std::string &result_file, Network *network_ptr, ResultConfig &config_arg): - m_fstream_ptr(new std::ofstream(result_file)), - m_fstream(*m_fstream_ptr), m_network_ptr(network_ptr),config(config_arg) - { - std::cout << "Write result to file: " << result_file << '\n'; - write_header(); - }; - // Destructor - ~ResultWriter() { - delete m_fstream_ptr; - }; + /** + * Constructor + * @param result_file, the path to an output file + * @param network, a pointer to the network + */ + ResultWriter(const std::string &result_file, + Network &network_arg, ResultConfig &config_arg) : + fstream(result_file), + network(network_arg), + config(config_arg) + { + std::cout << "Write result to file: " << result_file << '\n'; + write_header(); + }; - /** - * Write map matching result for a trajectory - * @param tr_id: id of trajectory - * @param ogeom: original geometry of the trajectory OGRLineString* - * @param o_path_ptr: pointer to the optimal path (sequence of candidates) - * @param c_path_ptr: pointer to the complete path (sequence of edge ids) - * @param mgeom: the geometry of the matched path (untraversed path removed in complete path) - */ - void write_result(int tr_id, LineString *ogeom, O_Path *o_path_ptr, T_Path *t_path_ptr, LineString *mgeom){ - std::stringstream buf; - buf << tr_id; - if (config.write_ogeom){ - buf << ";"; - write_geometry(buf,ogeom); - } - if (config.write_opath) { - buf << ";"; - write_o_path(buf,o_path_ptr); - } - if (config.write_error) { - buf << ";"; - write_gps_error(buf,o_path_ptr); - } - if (config.write_offset) { - buf << ";"; - write_offset(buf,o_path_ptr); - } - // Distance traversed which could be more complicated. - if (config.write_spdist) { - buf << ";"; - write_spdist(buf,o_path_ptr); - } - if (config.write_pgeom){ - buf << ";"; - write_pgeom(buf,o_path_ptr); - } - // Write fields related with cpath - if (config.write_cpath) { - buf << ";"; - write_cpath(buf,t_path_ptr); - } - if (config.write_tpath) { - buf << ";"; - write_tpath(buf,t_path_ptr); - } - if (config.write_mgeom) { - buf << ";"; - write_geometry(buf,mgeom); - } - if (config.write_ep) { - buf << ";"; - write_ep(buf,o_path_ptr); - } - if (config.write_tp) { - buf << ";"; - write_tp(buf,o_path_ptr); - } - buf << '\n'; - // It seems that the one below is important to ensure the buffer control flow works - // as expected. - #pragma omp critical - m_fstream << buf.rdbuf(); - }; + /** + * Write map matching result for a trajectory + * @param tr_id: id of trajectory + * @param ogeom: original geometry of the trajectory OGRLineString* + * @param o_path: pointer to the optimal path (sequence of candidates) + * @param c_path: pointer to the complete path (sequence of edge ids) + * @param mgeom: the geometry of the matched path (untraversed part at the + * 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){ + std::stringstream buf; + buf << tr_id; + if (config.write_ogeom) { + buf << ";"; + write_geometry(buf,ogeom); + } + if (config.write_opath) { + buf << ";"; + write_o_path(buf,o_path); + } + if (config.write_error) { + buf << ";"; + write_gps_error(buf,o_path); + } + if (config.write_offset) { + buf << ";"; + write_offset(buf,o_path); + } + // Distance traversed which could be more complicated. + if (config.write_spdist) { + buf << ";"; + write_spdist(buf,o_path); + } + if (config.write_pgeom) { + buf << ";"; + write_pgeom(buf,o_path); + } + // Write fields related with cpath + if (config.write_cpath) { + buf << ";"; + write_cpath(buf,t_path); + } + if (config.write_tpath) { + buf << ";"; + write_tpath(buf,t_path); + } + if (config.write_mgeom) { + buf << ";"; + write_geometry(buf,mgeom); + } + if (config.write_ep) { + buf << ";"; + write_ep(buf,o_path); + } + if (config.write_tp) { + buf << ";"; + write_tp(buf,o_path); + } + buf << '\n'; + // Ensure that fstream is called corrected in OpenMP + #pragma omp critical + fstream << buf.rdbuf(); + }; - static std::string mkString(Network *network_ptr,O_Path *o_path_ptr, T_Path *t_path_ptr, LineString *mgeom, bool return_details = false){ - std::stringstream buf; - if (return_details){ - write_o_path(buf,o_path_ptr); - buf << ";"; - write_gps_error(buf,o_path_ptr); - buf << ";"; - write_offset(buf,o_path_ptr); - buf << ";"; - write_spdist(buf,o_path_ptr); - // Distance traversed which could be more complicated. - buf << ";"; - write_pgeom(buf,o_path_ptr); - buf << ";"; - write_cpath_network(buf,t_path_ptr,network_ptr); - buf << ";"; - write_cpath_network(buf,t_path_ptr,network_ptr); - buf << ";"; - } - if (mgeom != nullptr) { - write_geometry(buf,mgeom); - } - return buf.str(); - }; - void write_header() { - std::string header = "id"; - if (config.write_ogeom) header+=";ogeom"; - if (config.write_opath) header+=";opath"; - if (config.write_error) header+=";error"; - if (config.write_offset) header+=";offset"; - if (config.write_spdist) header+=";spdist"; - if (config.write_pgeom) header+=";pgeom"; - if (config.write_cpath) header+=";cpath"; - if (config.write_tpath) header+=";tpath"; - if (config.write_mgeom) header+=";mgeom"; - if (config.write_ep) header+=";ep"; - if (config.write_tp) header+=";tp"; - m_fstream << header << '\n'; - }; - static void write_geometry(std::stringstream &buf, LineString *line){ - if (line != nullptr) { -#ifdef USE_BG_GEOMETRY - buf << std::setprecision(12) << line->exportToWkt(); -#else - char *wkt; - line->exportToWkt(&wkt); - buf << wkt; - CPLFree(wkt); -#endif - } + static std::string mkString( + const Network &network, const O_Path &o_path, const T_Path &t_path, + const LineString &mgeom, bool return_details = false){ + std::stringstream buf; + if (return_details) { + write_o_path(buf,o_path); + buf << ";"; + write_gps_error(buf,o_path); + buf << ";"; + write_offset(buf,o_path); + buf << ";"; + write_spdist(buf,o_path); + buf << ";"; + write_pgeom(buf,o_path); + buf << ";"; + write_cpath_network(buf,t_path,network); + buf << ";"; + write_tpath_network(buf,t_path,network); + buf << ";"; + } + if (!mgeom.isEmpty()) { + write_geometry(buf,mgeom); + } + return buf.str(); + }; + void write_header() { + std::string header = "id"; + if (config.write_ogeom) header+=";ogeom"; + if (config.write_opath) header+=";opath"; + if (config.write_error) header+=";error"; + if (config.write_offset) header+=";offset"; + if (config.write_spdist) header+=";spdist"; + if (config.write_pgeom) header+=";pgeom"; + if (config.write_cpath) header+=";cpath"; + if (config.write_tpath) header+=";tpath"; + if (config.write_mgeom) header+=";mgeom"; + if (config.write_ep) header+=";ep"; + if (config.write_tp) header+=";tp"; + fstream << header << '\n'; + }; + static void write_geometry(std::stringstream &buf, const LineString &line){ + if (!line.isEmpty()) { + buf << std::setprecision(12) << line.exportToWkt(); + } + }; + // Write the optimal path + static void write_o_path(std::stringstream &buf,const O_Path &o_path) + { + if (o_path.empty()) { + return; }; - // Write the optimal path - static void write_o_path(std::stringstream &buf,O_Path *o_path_ptr) + int N = o_path.size(); + for (int i = 0; i < N - 1; ++i) { - if (o_path_ptr == nullptr) { - return; - }; - int N = o_path_ptr->size(); - for (int i = 0; i < N - 1; ++i) - { - buf << (*o_path_ptr)[i]->edge->id_attr << ","; - } - buf << (*o_path_ptr)[N - 1]->edge->id_attr; - }; + buf << o_path[i]->edge->id << ","; + } + buf << o_path[N - 1]->edge->id; + }; - static void write_offset(std::stringstream &buf,O_Path *o_path_ptr) - { - if (o_path_ptr == nullptr) { - return; - }; - int N = o_path_ptr->size(); - for (int i = 0; i < N - 1; ++i) - { - buf << (*o_path_ptr)[i]->offset<< ","; - } - buf << (*o_path_ptr)[N - 1]->offset; + static void write_offset(std::stringstream &buf,const O_Path &o_path) + { + if (o_path.empty()) { + return; }; - - static void write_spdist(std::stringstream &buf,O_Path *o_path_ptr) + int N = o_path.size(); + for (int i = 0; i < N - 1; ++i) { - if (o_path_ptr == nullptr) { - return; - }; - int N = o_path_ptr->size(); - for (int i = 0; i < N - 1; ++i) - { - buf << (*o_path_ptr)[i]->sp_dist<< ","; - } - buf << (*o_path_ptr)[N - 1]->sp_dist; - }; + buf << o_path[i]->offset<< ","; + } + buf << o_path[N - 1]->offset; + }; - static void write_ep(std::stringstream &buf,O_Path *o_path_ptr) - { - if (o_path_ptr == nullptr) { - return; - }; - int N = o_path_ptr->size(); - for (int i = 0; i < N - 1; ++i) - { - buf << (*o_path_ptr)[i]->obs_prob<< ","; - } - buf << (*o_path_ptr)[N - 1]->obs_prob; + static void write_spdist(std::stringstream &buf,const O_Path &o_path) + { + if (o_path.empty()) { + return; }; - - static void write_tp(std::stringstream &buf,O_Path *o_path_ptr) + int N = o_path.size(); + for (int i = 0; i < N - 1; ++i) { - if (o_path_ptr == nullptr) return; - int N = o_path_ptr->size(); - for (int i = 0; i < N - 1; ++i) - { - float ca = (*o_path_ptr)[i]->cumu_prob; - float cb = (*o_path_ptr)[i+1]->cumu_prob; - float tp = (cb - ca)/((*o_path_ptr)[i+1]->obs_prob+1e-7); - if (i==N-2) { - buf << tp; - } else { - buf << tp <<","; - } - } - }; + buf << o_path[i]->sp_dist<< ","; + } + buf << o_path[N - 1]->sp_dist; + }; - static void write_gps_error(std::stringstream &buf,O_Path *o_path_ptr) - { - if (o_path_ptr == nullptr) { - return; - }; - int N = o_path_ptr->size(); - for (int i = 0; i < N - 1; ++i) - { - buf << (*o_path_ptr)[i]->dist<< ","; - } - buf << (*o_path_ptr)[N - 1]->dist; + static void write_ep(std::stringstream &buf,const O_Path &o_path) + { + if (o_path.empty()) { + return; }; + int N = o_path.size(); + for (int i = 0; i < N - 1; ++i) + { + buf << o_path[i]->obs_prob<< ","; + } + buf << o_path[N - 1]->obs_prob; + }; - // Write the optimal path - static void write_pgeom(std::stringstream &buf,O_Path *o_path_ptr) + static void write_tp(std::stringstream &buf,const O_Path &o_path) + { + if (o_path.empty()) return; + int N = o_path.size(); + for (int i = 0; i < N - 1; ++i) { - if (o_path_ptr == nullptr) { - return; - }; - int N = o_path_ptr->size(); - LineString pline; - // Create a linestring from matched point - for (int i = 0; i < N; ++i) - { - LineString *edge_geom = (*o_path_ptr)[i]->edge->geom; - double px = 0; - double py = 0; - ALGORITHM::locate_point_by_offset(edge_geom,(*o_path_ptr)[i]->offset,&px,&py); - pline.addPoint(px,py); - } - if (!pline.IsEmpty()) { - write_geometry(buf,&pline); - } + 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); + if (i==N-2) { + buf << tp; + } else { + buf << tp <<","; + } + } + }; + + static void write_gps_error(std::stringstream &buf,const O_Path &o_path) + { + if (o_path.empty()) { + return; }; + int N = o_path.size(); + for (int i = 0; i < N - 1; ++i) + { + buf << o_path[i]->dist<< ","; + } + buf << o_path[N - 1]->dist; + }; - // Write the complete path - void write_cpath(std::stringstream &buf,T_Path *t_path_ptr) { - if (t_path_ptr == nullptr) return; - C_Path *c_path_ptr = &(t_path_ptr->cpath); - int N = c_path_ptr->size(); - for (int i = 0; i < N - 1; ++i) - { - buf << m_network_ptr->get_edge_id_attr((*c_path_ptr)[i]) << ","; - } - buf << m_network_ptr->get_edge_id_attr((*c_path_ptr)[N - 1]); + // Write the optimal path + static void write_pgeom(std::stringstream &buf,const O_Path &o_path) + { + if (o_path.empty()) { + return; }; + int N = o_path.size(); + LineString pline; + // 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 (!pline.isEmpty()) { + write_geometry(buf,pline); + } + }; + // Write the complete path + 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(); + for (int i = 0; i < N - 1; ++i) + { + buf << c_path[i] << ","; + } + buf << c_path[N - 1]; + }; - // Write the traversed path separated by vertical bar - void write_tpath(std::stringstream &buf,T_Path *t_path_ptr) { - if (t_path_ptr == nullptr) return; - // Iterate through consecutive indexes and write the traversed path - int J = t_path_ptr->indices.size(); - for (int j=0;jindices[j]; - int b = t_path_ptr->indices[j+1]; - for (int i=a;iget_edge_id_attr(t_path_ptr->cpath[i]); - buf << ","; - } - buf << m_network_ptr->get_edge_id_attr(t_path_ptr->cpath[b]); - if (jcpath); - int N = c_path_ptr->size(); - for (int i = 0; i < N - 1; ++i) - { - buf << network_ptr->get_edge_id_attr((*c_path_ptr)[i]) << ","; - } - buf << network_ptr->get_edge_id_attr((*c_path_ptr)[N - 1]); - }; + // Write the traversed path separated by vertical bar + 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(); + for (int j=0; jindices.size(); - for (int j=0;jindices[j]; - int b = t_path_ptr->indices[j+1]; - for (int i=a;iget_edge_id_attr(t_path_ptr->cpath[i]); - buf << ","; - } - buf << network_ptr->get_edge_id_attr(t_path_ptr->cpath[b]); - if (j