Cleanup and simulations.

Cleanup
Refactoring
Finish simulations framework: Add more places and paths
Add visualization script and a simulation results pickle
Add last profile for MDP VI (now fully in C++)
Minor fixes

osmnx_mdp/algorithms/algorithm.pyx

@@ -1,11 +1,5 @@
 # cython: language_level=3
 cdef class Algorithm:
-    def __init__(self, G):
-        pass
-
-    def __cinit__(self, G):
-        pass
-
     cdef setup(self, long start, long goal):
         pass
 

osmnx_mdp/algorithms/brtdp.pxd

@@ -3,6 +3,7 @@ from libcpp.vector cimport vector
 from libcpp.pair cimport pair
 from osmnx_mdp.algorithms.dense_hash_map cimport dense_hash_map
 from osmnx_mdp.algorithms.mdp cimport MDP
+cimport osmnx_mdp.algorithms.algorithm
 
 
 cdef extern from "cpp_brtdp.cpp":
@@ -24,21 +25,18 @@ cdef extern from "cpp_brtdp.cpp":
         int setup(long start, long goal)
         int run_sample_trial()
         int run_trials()
-        vector[long] get_path()
+        vector[long] get_path(dense_hash_map[long, long] diverge_policy)
 
 
-cdef class BRTDP:
+cdef class BRTDP(osmnx_mdp.algorithms.algorithm.Algorithm):
     cdef dense_hash_map[long, pair[float, float]] data
-    cdef vector[long] S
-    cdef dense_hash_map[long, vector[pair[long, long]]] A
-    cdef dense_hash_map[pair[long, long], float, pair_hash] C
-    cdef dense_hash_map[long, dense_hash_map[pair[long, long], vector[pair[long, double]], pair_hash]] P
-
     cdef dense_hash_map[long, double] vl
 
     cdef MDP mdp
     cdef CPP_BRTDP cpp
 
     cdef setup(self, long start, long goal)
     cdef run_trials(self)
-    cdef get_path(self)
+    cdef get_path(self, diverge_policy)
+    cdef solve(self)
+    cdef drive(self, policy, diverge_policy)

osmnx_mdp/algorithms/brtdp.pyx

@@ -2,8 +2,7 @@
 from osmnx_mdp.algorithms.mdp cimport MDP
 
 
-# TODO: extend Algorithm
-cdef class BRTDP:
+cdef class BRTDP(osmnx_mdp.algorithms.algorithm.Algorithm):
     def __init__(self, MDP mdp):
         self.mdp = mdp
 
@@ -13,32 +12,32 @@ cdef class BRTDP:
             node_id = node[0]
             self.data[node_id] = (node[1]['lat'], node[1]['lon'])
 
-        self.S = self.mdp.S
-
-        self.A.set_empty_key(0)
-        for k, v in self.mdp.A.items():
-            self.A[k] = v
-
-        self.C.set_empty_key((0, 0))
-        for k, v in self.mdp.C.items():
-            self.C[k] = v
-
-        self.P.set_empty_key(0)
-        cdef dense_hash_map[pair[long, long], vector[pair[long, double]], pair_hash] curr
-        curr.set_empty_key((0, 0))
-        for k, v in self.mdp.P.items():
-            for k_, v_ in v.items():
-                curr[k_] = v_
-            self.P[k] = curr
-            curr.clear()
-
         self.cpp = CPP_BRTDP()
-        self.cpp.init(&self.S, &self.A, &self.C, &self.P, &self.data)
+        self.cpp.init(
+                &self.mdp.S,
+                &self.mdp.A,
+                &self.mdp.C,
+                &self.mdp.P,
+                &self.data)
         self.cpp.setup(start, goal)
 
     cdef run_trials(self):
         self.cpp.run_trials()
         self.vl = self.cpp.vl
 
-    cdef get_path(self):
-        return self.cpp.get_path()
+    cdef get_path(self, diverge_policy):
+        cdef dense_hash_map[long, long] cpp_diverge_policy
+        cpp_diverge_policy.set_empty_key(0)
+
+        # Manually convert Python object to Cython object.
+        # Unfortunately, there is no other way other than to be explicit.
+        for k, v in diverge_policy.items():
+            cpp_diverge_policy[k] = v
+
+        return self.cpp.get_path(cpp_diverge_policy)
+
+    cdef solve(self):
+        self.run_trials()
+
+    cdef drive(self, policy, diverge_policy):
+        return self.get_path(diverge_policy)

osmnx_mdp/algorithms/cpp_brtdp.cpp

@@ -1,11 +1,7 @@
 #include "cpp_brtdp.hpp"
-#include <cmath>
 #include <stack>
 #include <unordered_set>
 
-#include <iostream>
-
-// TODO: Rename t to tau
 
 CPP_BRTDP::CPP_BRTDP() {};
 CPP_BRTDP::~CPP_BRTDP() {};
@@ -22,16 +18,6 @@ double CPP_BRTDP::get_Q_value(
     return immediate_cost + future_cost;
 }
 
-// TODO: COPIED from cpp_dstar_lite.cpp
-double aerial_dist(double lat1, double lon1, double lat2, double lon2, double R=6356.8) {
-    lon1 *= M_PI / 180.0;
-    lon2 *= M_PI / 180.0;
-    lat1 *= M_PI / 180.0;
-    lat2 *= M_PI / 180.0;
-    double d = pow(sin((lat2 - lat1) / 2), 2) + cos(lat1) * cos(lat2) * pow(sin((lon2 - lon1) / 2), 2);
-    return R * 2 * asin(pow(d, .5));
-}
-
 int CPP_BRTDP::init(
     std::vector<long> *S,
     google::dense_hash_map<long, std::vector<std::pair<long, long>>> *A,
@@ -72,6 +58,7 @@ int CPP_BRTDP::setup(long start, long goal) {
         double lat1 = x.second.first;
         double lon1 = x.second.second;
 
+        // TODO is this best choice ? 50 and 10 ?
         this->vl[x.first] = aerial_dist(lat1, lon1, lat2, lon2) / 50;
         this->vu[x.first] = aerial_dist(lat1, lon1, lat2, lon2) / 10;
     }
@@ -80,7 +67,7 @@ int CPP_BRTDP::setup(long start, long goal) {
 }
 
 // TODO RENAME
-std::pair<long, long> CPP_BRTDP::update(google::dense_hash_map<long, double> &v, const long &x) {
+std::pair<long, long> CPP_BRTDP::update_v(google::dense_hash_map<long, double> &v, const long &x) {
     std::pair<long, long> curr_min_action;
     double curr_min = INFINITY;
     for (auto &a : (*this->A)[x]) {
@@ -95,17 +82,16 @@ std::pair<long, long> CPP_BRTDP::update(google::dense_hash_map<long, double> &v,
 }
 
 // TODO See header for default values alpha=0.001, t=10
-int CPP_BRTDP::run_trials(double alpha, double t) {
+int CPP_BRTDP::run_trials(double alpha, double tau) {
     int i = 0;
     while (this->vu[this->start] - this->vl[this->start] > alpha) {
-        this->run_trial(t);
+        this->run_trial(tau);
         i += 1;
     }
-    std::cout << "CONVERGED " << i << std::endl;
     return 0;
 }
 
-int CPP_BRTDP::run_trial(double t) {
+int CPP_BRTDP::run_trial(double tau) {
     long x = this->start;
 
     std::stack<long> traj;
@@ -116,8 +102,8 @@ int CPP_BRTDP::run_trial(double t) {
 
         traj.push(x);
 
-        this->update(this->vu, x);
-        std::pair<long, long> curr_min_action = this->update(this->vl, x);
+        this->update_v(this->vu, x);
+        std::pair<long, long> curr_min_action = this->update_v(this->vl, x);
 
         std::vector<float> distribution_param;
         double B = 0;
@@ -127,7 +113,7 @@ int CPP_BRTDP::run_trial(double t) {
             B += cost;
         }
 
-        if (B < ((this->vu[this->start] - this->vl[this->start]) / t))
+        if (B < ((this->vu[this->start] - this->vl[this->start]) / tau))
             break;
 
         std::discrete_distribution<int> distribution(distribution_param.begin(), distribution_param.end());
@@ -138,22 +124,32 @@ int CPP_BRTDP::run_trial(double t) {
     while (!traj.empty()) {
         long x = traj.top();
         traj.pop();
-        this->update(this->vu, x);
-        this->update(this->vl, x);
+        this->update_v(this->vu, x);
+        this->update_v(this->vl, x);
     }
 
     return 0;
 }
 
-std::vector<long> CPP_BRTDP::get_path() {
+std::vector<long> CPP_BRTDP::get_path(google::dense_hash_map<long, long> diverge_policy) {
     std::vector<long> path;
 
+    std::unordered_set<long> visited;
+    visited.insert(this->start);
+
     long curr_node = this->start;
     while (curr_node != this->goal) {
         path.push_back(curr_node);
 
-        std::pair<long, long> curr_min_action = this->update(this->vl, curr_node);
-        curr_node = (*this->P)[curr_node][curr_min_action][0].first;
+        long diverged_node = diverge_policy[curr_node];
+        if (diverged_node == 0 || visited.find(diverged_node) != visited.end()) {
+            std::pair<long, long> curr_min_action = this->update_v(this->vl, curr_node);
+            curr_node = (*this->P)[curr_node][curr_min_action][0].first;
+        } else {
+            curr_node = diverged_node;
+        }
+
+        visited.insert(curr_node);
     }
 
     return path;

osmnx_mdp/algorithms/cpp_brtdp.hpp

@@ -1,7 +1,8 @@
 #include <google/dense_hash_map>
 #include <random>
 #include <vector>
-#include "cpp_lib.hpp"
+
+#include "../cpp_lib.hpp"
 
 
 class CPP_BRTDP {
@@ -21,7 +22,7 @@ class CPP_BRTDP {
         std::vector<long> *S;
         google::dense_hash_map<long, std::vector<std::pair<long, long>>> *A;
         google::dense_hash_map<std::pair<long, long>, float, pair_hash> *C;
-        // It is needed that this is DOUBLE. Especially (B)RTDP
+        // TODO It is needed that this is DOUBLE. Especially (B)RTDP
         // needs precision. Who knows how bad the effect in VI is.
         google::dense_hash_map<
             long,
@@ -37,7 +38,7 @@ class CPP_BRTDP {
                 google::dense_hash_map<long, double> &v,
                 const long &s,
                 const std::pair<long, long> &a);
-        std::pair<long, long> update(google::dense_hash_map<long, double> &v, const long &x);
+        std::pair<long, long> update_v(google::dense_hash_map<long, double> &v, const long &x);
         int init(
             std::vector<long> *S,
             google::dense_hash_map<long, std::vector<std::pair<long, long>>> *A,
@@ -54,5 +55,5 @@ class CPP_BRTDP {
         int setup(long start, long goal);
         int run_trial(double t);
         int run_trials(double alpha=1e-10, double t=10);
-        std::vector<long> get_path();
+        std::vector<long> get_path(google::dense_hash_map<long, long> diverge_policy);
 };