test

Signed-off-by: bruno.tissei <bruno.tissei@arculus.de>

README

+# Algorithm for Correction of Marker Rotation and Displacement Errors
+
+The algorithm is described and explained at report.pdf  
+
+# Get mid Points (First Step)
+
+Gets last PE point before each marker detection.  
+
+--md and --pe are the logs from marker detection and pose estimator from the path that needs to be optimized, this path needs the global pose update by markers turned on.  
+
+```
+$ python script/get_mid_points.py --marker=data/logs/20180516-134720/marker/pose.csv --pe=data/logs/20180516-134720/pe/pose.csv > mid_data
+```
+
+# Parse Marker Error (Second Step)
+
+Parse log from path without global position update by marker detection  
+
+```
+$ python script/parse_marker_error.py --map=data/gvz_ext.yml --md=data/logs/20180516-133414/marker/pose.csv > correction_data
+```  
+
+If no such path was captured, then the --md option should receive the same marker log that was used in the first step, in this case the "True values" in the algorithm's plot shall be ignored.
+
+
+# Run Algorithm
+
+Will plot the result and also create a file "fixed_gvz.yml", which is the gvz yml with the new values for the theta
+
+```
+$ python script/algorithm.py --inter=mid_data --correction=correction_data
+```
+
+
+# Plotter
+
+Plots a path. The --show option specifies which logs need to be included in the plot (m=marker, p=pose, c=ctrl, i=inter). The inter log can be obtained from the First Step.  
+
+```
+$ python script/plotter.py --map=data/gvz_ext.yml --log=data/logs/20180516-134720 --show=mpci
+```

graph/dijkstra.cpp

@@ -13,10 +13,10 @@ int dijkstra(int o, int d) {
   set<ii> pq;
   int u, v, wt;
 
-  memset(dist, inf, sizeof dist);
+  mset(dist, inf);
 
   dist[o] = 0;
-  pq.insert(make_pair(0, o));
+  pq.insert(ii(0, o));
 
   while (pq.size() != 0) {
     u = pq.begin()->second;
@@ -28,10 +28,10 @@ int dijkstra(int o, int d) {
 
       if (dist[v] > dist[u] + wt) {
         if (dist[v] != oo)
-          pq.erase(pq.find(make_pair(dist[v], v)));
+          pq.erase(pq.find(ii(dist[v], v)));
 
         dist[v] = dist[u] + wt;
-        pq.insert(make_pair(dist[v], v));
+        pq.insert(ii(dist[v], v));
       }
     }
   }

graph/kruskal.cpp

@@ -9,7 +9,7 @@
  * OBS: * = return maximum spanning tree
  */
 
-vector<iii> mst;
+vector<iii> mst; // Result
 vector<iii> edges;
 
 bool cmp(iii a, iii b) { 
@@ -20,7 +20,7 @@ bool cmp(iii a, iii b) {
 
 // Return value of MST and build mst vector with edges that belong to the tree
 int kruskal() {
-  sort(edges.begin(), edges.end(), cmp);
+  sort(all(edges), cmp);
 
   int size = 0;
   for (int i = 0; i < MAX; i++)

graph/topological_sort.cpp

@@ -31,7 +31,7 @@ bool dfs(int x) {
 // Returns if graph contains cycle or not, and fills tsort vector with
 // topological sort of the graph
 bool topological_sort(int n, vector<int> &tsort) {
-  memset(cont, 0, sizeof cont);
+  mset(cont, 0);
   
   bool cycle = false;
   for (int i = 0; i < n; ++i)
@@ -42,7 +42,7 @@ bool topological_sort(int n, vector<int> &tsort) {
     return true;
 
   while (!S.empty()) {
-    tsort.push_back(S.top());
+    tsort.pb(S.top());
     S.pop();
   }
 

structure/segment_tree.cpp

@@ -12,8 +12,7 @@ int N, tree[4 * MAX], v[MAX];
 
 // Build tree with elements from v
 void build_tree(int node = 1, int a = 0, int b = N) {
-  if (a > b)
-    return;
+  if (a > b) return;
 
   if (a == b) {
     tree[node] = v[a];
@@ -22,15 +21,13 @@ void build_tree(int node = 1, int a = 0, int b = N) {
 
   build_tree(node * 2, a, (a + b) / 2);
   build_tree(node * 2 + 1, 1 + (a + b) / 2, b);
-
   tree[node] = tree[node * 2] + tree[node * 2 + 1];
 }
 
 
 // Update position idx with value val
 void update_tree(int idx, int val, int node = 1, int a = 0, int b = N) {
-  if (a > b || a > idx || b < idx)
-    return;
+  if (a > b || a > idx || b < idx) return;
 
   if (a == b) {
     tree[node] = val;
@@ -39,21 +36,16 @@ void update_tree(int idx, int val, int node = 1, int a = 0, int b = N) {
 
   update_tree(idx, val, node * 2, a, (a + b) / 2);
   update_tree(idx, val, node * 2 + 1, 1 + (a + b) / 2, b);
-
   tree[node] = tree[node * 2] + tree[node * 2 + 1];
 }
 
 
 // Return sum of [i,j]
 int query_tree(int i, int j, int node = 1, int a = 0, int b = N) {
-  if (a > b || a > j || b < i)
-    return 0;
-
-  if (a >= i && b <= j)
-    return tree[node];
+  if (a > b || a > j || b < i) return 0;
 
-  int res1 = query_tree(i, j, node * 2, a, (a + b) / 2);
-  int res2 = query_tree(i, j, node * 2 + 1, 1 + (a + b) / 2, b);
+  if (a >= i && b <= j) return tree[node];
 
-  return res1 + res2;
+  return query_tree(i, j, node * 2, a, (a + b) / 2) + \
+         query_tree(i, j, node * 2 + 1, 1 + (a + b) / 2, b);
 }